Code:
#include "RP6ControlLib.h"
#include "RP6I2CmasterTWI.h"
#include "RP6Control_MultiIOLib.h"
#include "RP6Control_I2CMasterLib.h"
#include "RP6Control_LFSBumperLib.h"
#include "RP6ControlServoLib.h"
#include "RP6Control_OrientationLib.h"
#include "RP6Stopwatch0Lib.h"
#include "standard.h"
/***************************variablen******************************/
uint8_t RP6data[32];
uint8_t i, j, t;
int16_t x, y, z;
uint16_t anfang, ende;
uint16_t lfs_l, lfs_m, lfs_r;
uint8_t nord; //nord gefunden
uint8_t IR_wert[1];
uint8_t ch;
char item[12];
char dir[3];
int32_t new_dir; //neue richtung
int32_t old_dir; //gemessene richtung
int32_t temp; //berechnung korrektur richtung
int16_t dev, rot;// i;//berechnung rotation
char rdir;
/********************* ULTRASCHALL & HC-SR-04 ******************************/
double distanz;
volatile uint16_t zeit;
volatile uint16_t timestamp_last;
/****************************** STANDARD *********************************/
/*******************I2C Error handler*********************/
void I2C_transmissionError(uint8_t errorState)
{
writeString_P("\nI2C ERROR --> TWI STATE IS: 0x");
writeInteger(errorState, HEX);
writeChar('\n');
}
/******* The I2C_requestedDataReady Event Handler*********/
void I2C_requestedDataReady(uint8_t dataRequestID)
{
checkRP6Status(dataRequestID);
}
/**** Write a floating point number to the LCD or terminal. ****/
void writeDoubleLCD(double number, uint8_t width, uint8_t prec)
{
char buffer[width + 1];
dtostrf(number, width, prec, &buffer[0]);
writeStringLCD(&buffer[0]);
}
void writeDouble(double number, uint8_t width, uint8_t prec)
{
char buffer[width + 1];
dtostrf(number, width, prec, &buffer[0]);
writeString(&buffer[0]);
}
/***********************accuspannungsanzeige******************/
void accuspannung(void) // accuspannung ausgeben
{
multiio_init();
LTC2990_measure();
setCursorPosLCD(0, 0);
writeStringLCD(" accu: ");
writeDoubleLCD(vbat, 4, 2);
writeStringLCD( " V");
}
/************************acculadungsanzeige*****************/
void acculadung(void)
{
clearLCD();
setCursorPosLCD(0, 0);
writeStringLCD(" ADC2: ");
uint16_t adc2 = readADC(ADC_2); // Read ADC Channel 2
setCursorPosLCD(0, 9);
writeIntegerLCD(adc2, DEC);
if (adc2 > 650)
{
setCursorPosLCD(2, 0);
writeStringLCD(" ladespannung ok ");
setMultiIOLED1(1);
}
}
/************************accuzustand***********************/
void accuzustand(void) // accuspannung abfragen und signalisieren
{
LTC2990_measure();
if (vbat < 6.0)
{
buzzer(330);
mSleep(200);
buzzer(330);
mSleep(200);
//bake_suche();
}
}
/***********************watchdog_request******************/
void watchDogRequest(void)
{
static uint8_t heartbeat2 = false;
if(heartbeat2)
{
clearPosLCD(1, 14, 1);
heartbeat2 = false;
}
else
{
setCursorPosLCD(1, 14);
writeStringLCD_P("#");
heartbeat2 = true;
}
}
/******************* Heartbeat function********************/
void task_LCDHeartbeat(void)
{
if(getStopwatch1() > 500)
{
static uint8_t heartbeat = false;
if(heartbeat)
{
clearPosLCD(0, 15, 1);
heartbeat = false;
}
else
{
setCursorPosLCD(0, 15);
writeStringLCD_P("*");
heartbeat = true;
}
setStopwatch1(0);
}
}
/*******************fahr_bis_schwarz************************/
void fahr_bis_schwarz (void)
{
moveAtSpeed(80, 80);
lfs_l = 0;
lfs_m = 0;
lfs_r = 0;
lfs_l = getLFS(CH_LFS_L);
lfs_m = getLFS(CH_LFS_M);
lfs_r = getLFS(CH_LFS_R);
if(lfs_l && lfs_m && lfs_r <300) //stop();//break;
{
stop();
setCursorPosLCD(0, 0);
writeStringLCD(" li mi re ");
setCursorPosLCD(1, 0);
lfs_l = getLFS(CH_LFS_L);
writeIntegerLCD(lfs_l, DEC);
setCursorPosLCD(1, 8);
lfs_m = getLFS(CH_LFS_M);
writeIntegerLCD(lfs_m, DEC);
setCursorPosLCD(1, 16);
lfs_r = getLFS(CH_LFS_R);
writeIntegerLCD(lfs_r, DEC);
writeIntegerLength(lfs_l, DEC, 4);
writeString(" ");
writeIntegerLength(lfs_m, DEC, 4);
writeString(" ");
writeIntegerLength(lfs_r, DEC, 4);
writeString(" ");
writeChar('\n');
mSleep(2000);
}
}
/*********************************** IR-bake***************************************/
/********************readAllRegister*******************/
void readAllRegisters(void)
{
I2CTWI_transmitByte(I2C_RP6_BASE_ADR, 0);
I2CTWI_readBytes(I2C_RP6_BASE_ADR,RP6data, 31);
writeString_P("\nREADING ALL RP6 REGISTERS:");
uint8_t i = 0;
for(i = 0; i < 31; i++)
{
if(i % 8 == 0)
writeChar('\n');
else
writeString_P(" | ");
writeChar('#');
writeIntegerLength(i,DEC,2);
writeChar(':');
writeIntegerLength(RP6data[i],DEC,3);
}
writeChar('\n');
}
/***************print_register_30********************/
void print_Register_30(void)
{
writeString_P("\nPRINTING REGISTER 30:");
uint8_t i = 0;
for(i = 0; i < 31; i++)
{
I2CTWI_transmitByte(I2C_RP6_BASE_ADR, 30);
IR_wert[0] = I2CTWI_readByte(I2C_RP6_BASE_ADR);
if(i % 8 == 0)
writeChar('\n');
else
writeString_P(" | ");
writeChar('#');
writeIntegerLength(i,DEC,2);
writeChar(':');
writeIntegerLength(IR_wert[0],DEC,3);
}
writeChar('\n');
}
/******************read_register_30********************/
void read_Register_30(void)
{
I2CTWI_transmitByte(I2C_RP6_BASE_ADR, 30);
IR_wert[0] = I2CTWI_readByte(I2C_RP6_BASE_ADR);
}
/*******************bake_suche**************************/
void bake_suche(void)
{
setLEDs(0b0100);
writeString_P("bake_suche_1 \n");
writeChar('\n');
initRP6Control();
initLCD();
startStopwatch3();
t=0;
do
{
if(getStopwatch3() > 50)
{
read_Register_30();
if (IR_wert[0] !=0)
{
setMultiIOLED1(1);
setMultiIOLED1(0);
rotate(80, RIGHT, 5, false);
read_Register_30();
if (IR_wert[0] == 0) stop();
}
read_Register_30();
if (IR_wert[0] == 0)
{
x = getStopwatch3();
setMultiIOLED3(1);
setMultiIOLED3(0);
if (t<10)
{
t++;
if (t == 10)
{
y = getStopwatch3();
z = y-x;
writeInteger(x, DEC);
writeChar('\n');
writeInteger(y, DEC);
writeChar('\n');
writeInteger(z, DEC);
writeChar('\n');
t=0;
setStopwatch3(0);
if (z< 600)
{
move(100, FWD, DIST_MM(100), false);
setStopwatch3(0);
t=0;
mSleep(400);
}
}
}
}
}
}
while(!bumper_left && !bumper_right);
stop();
}
/*************************** GYRO & NORDSUCHE ******************************************/
/**********************test rdir**************************/
void test_rdir(void)
{
if (rot < 0)
rdir = LEFT;
else rdir = RIGHT;
}
/************korrekrur richtung*************************/
void korrekrur_richtung(void)
// Wertebereich new_dir und old_dir: 0..359
// Ergebnis in rot! Positiv: Rechtsdrehung, negativ: Linksdrehung!
{
dev = new_dir - old_dir;
rot = dev;
if (abs(dev) > 180)
{
{
if (dev < 0)
{
rot = 360 + dev;
}
else
{
rot = -360 + dev;
}
}
}
}
/******************sensorwerte_holen********************/
void sensorwerte_holen(void)
{
task_I2CTWI();
readLSM303DLHC_M(); // Get sensor values
task_I2CTWI();
normalizeLSM303DLHC_M(); // Normalize data
headingm = headingLSM303DLHC_M(); // Calculate heading
calculateDetailDir(dir, headingm);
}
/*********************nordsuche************************/
void nordsuche (void)
{
nord=0;
// while (true)
// {
new_dir = 360;
sensorwerte_holen();
old_dir = headingm;
korrekrur_richtung();
test_rdir();
rotate(60, rdir, ((abs(rot)/2)), true);
if isMovementComplete()
{
for (i= 0; i<9; i++);
{
sensorwerte_holen();
old_dir = headingm;
korrekrur_richtung();
test_rdir();
rotate(60, rdir, ((abs(rot)/2)), true);
if (headingm <= 5 || headingm >= 355)
{
nord=1;
// if (nord==1) break;
}
task_I2CTWI();
}
}
}
//}
/*********************nordsuche links************************/
void nordsuche_links (void)
{
clearLCD();
new_dir = 360;
sensorwerte_holen();
old_dir = headingm;
// korrekrur_richtung();
// test_rdir();
rotate(60, LEFT, ((abs(old_dir)/2)), true); //0
for (i= 0; i<9; i++);
{
sensorwerte_holen();
old_dir = headingm;
// korrekrur_richtung();
// test_rdir();
rotate(60, LEFT, ((abs(old_dir)/2)), true); //0
// }
if (abs(old_dir) < 003) stop();
}
// break;
// task_I2CTWI();
}
/***********************calculate_dir**********************/
void calculateDir(char* dir, uint16_t heading) //setzt headingwerte grob in himmelsrichtungen um
{
dir[1] = ' ';
dir[2] = '\0';
if ((heading <= 22) || (heading >=338)) dir[0] = 'N';
if ((heading >= 23) && (heading <= 67))
{
dir[0] = 'N';
dir[1] = 'E';
}
if ((heading >= 68) && (heading <= 112)) dir[0] = 'E';
if ((heading >= 113) && (heading <= 157))
{
dir[0] = 'S';
dir[1] = 'E';
}
if ((heading >= 158) && (heading <= 202)) dir[0] = 'S';
if ((heading >= 203) && (heading <= 247))
{
dir[0] = 'S';
dir[1] = 'W';
}
if ((heading >= 248) && (heading <= 292)) dir[0] = 'W';
if ((heading >= 293) && (heading <= 337))
{
dir[0] = 'N';
dir[1] = 'W';
}
}
/************************calculateDetailDir*************************************/
void calculateDetailDir(char *dir, uint16_t heading)
{
dir[1] = ' ';
dir[2] = '\0';
dir[3] = '\0';
if ((heading <= 11) || (heading >=349)) dir[0] = 'N';
if ((heading >= 12) && (heading <= 33))
{
dir[0] = 'N';
dir[1] = 'N';
dir[2] = 'E';
}
if ((heading >= 34) && (heading <= 56))
{
dir[0] = 'N';
dir[1] = 'E';
}
if ((heading >= 57) && (heading <= 78))
{
dir[0] = 'E';
dir[1] = 'N';
dir[2] = 'E';
}
if ((heading >= 79) && (heading <= 101)) dir[0] = 'E';
if ((heading >= 102) && (heading <= 123))
{
dir[0] = 'E';
dir[1] = 'S';
dir[2] = 'E';
}
if ((heading >= 124) && (heading <= 146))
{
dir[0] = 'S';
dir[1] = 'E';
}
if ((heading >= 147) && (heading <= 168))
{
dir[0] = 'S';
dir[1] = 'S';
dir[2] = 'E';
}
if ((heading >= 169) && (heading <= 191)) dir[0] = 'S';
if ((heading >= 192) && (heading <= 213))
{
dir[0] = 'S';
dir[1] = 'S';
dir[2] = 'W';
}
if ((heading >= 214) && (heading <= 236))
{
dir[0] = 'S';
dir[1] = 'W';
}
if ((heading >= 237) && (heading <= 258))
{
dir[0] = 'W';
dir[1] = 'S';
dir[2] = 'W';
}
if ((heading >= 259) && (heading <= 281)) dir[0] = 'W';
if ((heading >= 282) && (heading <= 303))
{
dir[0] = 'W';
dir[1] = 'N';
dir[2] = 'N';
}
if ((heading >= 304) && (heading <= 326))
{
dir[0] = 'N';
dir[1] = 'W';
}
if ((heading >= 327) && (heading <= 348))
{
dir[0] = 'N';
dir[1] = 'N';
dir[2] = 'W';
}
}
/********************* ULTRASCHALL & HC-SR-04 ******************************/
//uint16_t distanz;
//volatile uint16_t zeit;
//volatile uint16_t timestamp_last;
volatile uint16_t dauer;
ISR(TIMER1_CAPT_vect)
{
//Wenn steigende Flanke
if(TCCR1B & (1<<ICES1))
{
//Flankenerkennung auf fallend
TCCR1B ^= (1<<ICES1);
//aktuelen timer-wert speichern
timestamp_last = ICR1;
}
//fallende Flanke
else
{
//Flankenerkennung auf steigend
TCCR1B ^= (1<<ICES1);
//Laufzeit = aktueller timerwert - vorheriger timerwert
zeit = ICR1 - timestamp_last;
}
}
/*************** trig ***************************/
void trig(void)
{
PORTC |= (1<<PC6);//Trig high
_delay_us(12);
PORTC &= ~(1<<PC6);//TRIG auf low
}
/*********************trig PC6**********************/
void trig_PC6(void)
{
PORTC |= (1<<PC6);//Trig high
_delay_us(12);
PORTC &= ~(1<<PC6);//TRIG auf low
}
/********************read_PC5**********************/
void read_PC5(void)
{
loop_until_bit_is_set(PINC, PC5);
setStopwatch02(0);
loop_until_bit_is_clear(PINC, PC5);
dauer = getStopwatch02();
//distanz_schleife = (dauer*34.3)/2;
distanz = ((dauer/2)*3.43);
}
/***************** messung_SR_04 ********************/
void messung_SR_04 (void)
{
DDRC |= (1 << PC6);//Trig als Ausgang
PORTC &= ~(1<<PC6);//TRIG auf low
DDRD &= ~(1<<PD6);//Echo als Eingang
PORTD &= ~(1<<PD6);//ECHO pullup AUS
//Timer konfigurieren
TCCR1A = 0; // normal mode, keine PWM Ausgänge
//Noise Canceler aktivieren, Flankenerkennung auf steigende, Prescaler auf 64
TCCR1B |= (1<<ICNC1) | (1<<ICES1) | (1<<CS11) |(1<<CS10);
//ICP Interrupt aktivieren
TIMSK |= (1<<TICIE1);
//Globale Interrupts aktivieren
sei();
distanz = (zeit*4)/58;
}
und eine standard.h" in dieser form:
Lesezeichen