/*! Sensor_update
* Kümmert sich um die Weiterverarbeitung der rohen Sensordaten
*/
void sensor_update(void){
#ifdef TIME_AVAILABLE
static int16 lastTime =0;
#endif
static int16 lastEncL =0;
static int16 lastEncR =0;
sensMouseY += sensMouseDY;
sensMouseX += sensMouseDX;
#ifdef TIME_AVAILABLE
if (timer_get_s() != lastTime) { // sollte genau 1x pro Sekunde zutreffen
#endif
v_left= ((sensEncL - lastEncL) * WHEEL_PERIMETER) / ENCODER_MARKS;
v_right= ((sensEncR - lastEncR) * WHEEL_PERIMETER) / ENCODER_MARKS;
lastEncL= sensEncL;
lastEncR= sensEncR;
#ifdef TIME_AVAILABLE
lastTime = timer_get_s();
#endif
#ifdef SRF10_AVAILABLE
sensSRF10 = srf10_get_measure(); /*!< Messung Ultraschallsensor */
#endif
#ifdef TIME_AVAILABLE
}
#endif
sensors_initialized=1;
}
/*!
* Delays for ms milliseconds
* Wenn RTC_AVAILABLE dann ueber rtc, sonst ueber delay_100ms
* ==> aufloesung ohne rtc: 100ms-schritte mir rtc: 5ms-Schritte
* @param ms Anzahl der Millisekunden
*/
void delay(uint16 ms){
#ifdef TIME_AVAILABLE
uint16 ms_start = timer_get_ms();
uint16 s_start = timer_get_s();
uint16 ms_stop = ms_start + ms;
uint16 s_stop = s_start + ms_stop/1000;
ms_stop %= 1000;
while ((s_stop != timer_get_s()) && (ms_stop != timer_get_ms())){asm volatile("nop");}
#else
uint16 ms_counter=0;
while (ms_counter <ms){
delay_100ms();
ms_counter+=100;
}
#endif
}
/**
* Zeigt ein paar Infos an, die man nicht naeher zuordnen kann
*/
void misc_display(void) {
/* Anzeige der Bot-Adresse (aenderbar) */
display_cursor(1, 1);
display_puts("bot_addr=");
#ifdef KEYPAD_AVAILABLE
if (RC5_Code == RC5_CODE_MUTE) {
gui_keypad_request(change_bot_addr_callback, 1, 1, 10);
#ifdef PC
display_cursor(1, 10);
#endif
display_puts(" "); // clean
new_address = 1;
RC5_Code = 0;
}
if (new_address == 0)
#endif // KEYPAD_AVAILABLE
{
#ifdef PC
display_cursor(1, 10);
#endif
display_printf("0x%x", get_bot_address());
}
#ifdef BEHAVIOUR_AVAILABLE
display_cursor(2, 1);
display_printf("TS=%+4d %+4d", target_speed_l, target_speed_r);
#endif
#ifdef SRF10_AVAILABLE
display_cursor(2, 15);
display_printf("US%4u", sensSRF10);
#endif
display_cursor(3, 1);
display_printf("RC=%+4d %+4d", sensEncL, sensEncR);
display_cursor(4, 1);
display_printf("Speed=%+4d", v_center);
display_cursor(4, 12);
display_printf("%4u:%03u", timer_get_s(), timer_get_ms());
}
double timer_get_us(const struct _timer *timer) {
return double(1000000*(timer_get_s(timer)));
}
/*!
* Direkter Zugriff auf den Motor
* @param left Geschwindigkeit fuer den linken Motor
* @param right Geschwindigkeit fuer den linken Motor
* Geschwindigkeit liegt zwischen -255 und +255.
* 0 bedeutet Stillstand, 255 volle Kraft voraus, -255 volle Kraft zurueck.
* Sinnvoll ist die Verwendung der Konstanten: BOT_SPEED_XXX,
* also z.B. motor_set(BOT_SPEED_LOW,-BOT_SPEED_LOW);
* fuer eine langsame Drehung
*/
void motor_set(int16 left, int16 right){
#ifdef SPEED_CONTROL_AVAILABLE
volatile static int16 old_mot_time_s=0;
volatile static int16 old_mot_time_ms=0;
#endif
if (left == BOT_SPEED_IGNORE)
left=BOT_SPEED_STOP;
if (right == BOT_SPEED_IGNORE)
right=BOT_SPEED_STOP;
// Haben wir ueberhaupt etwas zu tun?
if ((speed_l == left) && (speed_r == right)){
// Hier sitzt die eigentliche Regelung
#ifdef SPEED_CONTROL_AVAILABLE
if (timer_get_ms_since(old_mot_time_s,old_mot_time_ms) > SPEED_CONTROL_INTERVAL) {
speed_control();
old_mot_time_s= timer_get_s();
old_mot_time_ms= timer_get_ms();
}
#endif
return; // Keine Aenderung? Dann zuerueck
}
if (abs(left) > BOT_SPEED_MAX) // Nicht schneller fahren als moeglich
speed_l = BOT_SPEED_MAX;
else if (left == 0) // Stop wird nicht veraendert
speed_l = BOT_SPEED_STOP;
else if (abs(left) < BOT_SPEED_SLOW) // Nicht langsamer als die
speed_l = BOT_SPEED_SLOW; // Motoren koennen
else // Sonst den Wunsch uebernehmen
speed_l = abs(left);
if (abs(right) > BOT_SPEED_MAX)// Nicht schneller fahren als moeglich
speed_r = BOT_SPEED_MAX;
else if (abs(right) == 0) // Stop wird nicht veraendert
speed_r = BOT_SPEED_STOP;
else if (abs(right) < BOT_SPEED_SLOW) // Nicht langsamer als die
speed_r = BOT_SPEED_SLOW; // Motoren koennen
else // Sonst den Wunsch uebernehmen
speed_r = abs(right);
if (left < 0 )
speed_l=-speed_l;
if (right < 0 )
speed_r=-speed_r;
#ifdef SPEED_CONTROL_AVAILABLE
// TODO Hier koennten wir die Motorkennlinie heranziehen um gute Einstiegswerte fuer die Regelung zu haben
// Evtl. sogar eine im EEPROM kalibrierbare Tabelle??
encoderTargetRateL = left / SPEED_TO_ENCODER_RATE; // Geschwindigkeit [mm/s] umrechnen in [EncoderTicks/Aufruf]
encoderTargetRateR = right / SPEED_TO_ENCODER_RATE; // Geschwindigkeit [mm/s] umrechnen in [EncoderTicks/Aufruf]
#endif
// Zuerst einmal eine lineare Kennlinie annehmen
bot_motor(speed_l/2,speed_r/2);
}
/*!
* Zeigt ein paar Informationen an
*/
void display(void){
#ifdef DISPLAY_BEHAVIOUR_AVAILABLE
/*!
* Definitionen fuer die Verhaltensanzeige
*/
#undef TEST_AVAILABLE_COUNTER
Behaviour_t *ptr = behaviour;
int8 colcounter = 0;
int8 linecounter = 0;
int8 firstcol = 0;
#endif
#ifdef TEST_AVAILABLE_COUNTER
static int counter=0;
#endif
if (display_update >0)
#ifdef DISPLAY_SCREENS_AVAILABLE
switch (display_screen) {
case 0:
#endif
display_cursor(1,1);
display_printf("P=%03X %03X D=%03d %03d ",sensLDRL,sensLDRR,sensDistL,sensDistR);
display_cursor(2,1);
display_printf("B=%03X %03X L=%03X %03X ",sensBorderL,sensBorderR,sensLineL,sensLineR);
display_cursor(3,1);
display_printf("R=%2d %2d F=%d K=%d T=%d ",sensEncL % 10,sensEncR %10,sensError,sensDoor,sensTrans);
display_cursor(4,1);
display_printf("I=%04X M=%05d %05d",RC5_Code,sensMouseX,sensMouseY);
#ifdef DISPLAY_SCREENS_AVAILABLE
break;
case 1:
#ifdef TIME_AVAILABLE
display_cursor(1,1);
display_printf("Zeit: %04d:%03d", timer_get_s(), timer_get_ms());
#endif
display_cursor(2,1);
display_printf("TS=%+4d %+4d",target_speed_l,target_speed_r);
#ifdef SRF10_AVAILABLE
display_cursor(2,15);
display_printf("US%+4d",sensSRF10);
#endif
display_cursor(3,1);
display_printf("RC=%+4d %+4d",sensEncL,sensEncR);
display_cursor(4,1);
display_printf("Speed= %04d %04d",v_left,v_right);
break;
case 2:
display_cursor(1,1);
#ifdef DISPLAY_BEHAVIOUR_AVAILABLE
/*!
* zeilenweise Anzeige der Verhalten
*/
display_printf("Verhalten (Pri/Akt)%d",behaviour_page);
colcounter = 0; linecounter = 2;
/* je nach Seitenwahl die ersten Saetze ueberlesen bis richtige Seite */
firstcol = (behaviour_page -1)*6;
/*!
* max. 3 Zeilen mit 6 Verhalten anzeigbar wegen Ueberschrift
* Seitensteuerung bei mehr Verhalten
*/
while((ptr != NULL)&& (linecounter<5)) {
if ((ptr->priority > 2) &&(ptr->priority <= 200)) {
if (colcounter >= firstcol) {
display_cursor(linecounter,((colcounter % 2)* 12)+1);
display_printf(" %3d,%2d",ptr->priority,ptr->active_new);
colcounter++;
/* bei colcounter 0 neue Zeile */
if (colcounter % 2 == 0)
linecounter++;
}
else
colcounter ++;
}
ptr = ptr->next;
}
#endif
#ifdef TEST_AVAILABLE_COUNTER
display_printf("Screen 3");
display_cursor(2,1);
display_printf("count %d",counter++);
display_cursor(3,1);
display_printf("Reset-Counter %d",resets);
#endif
break;
case 3:
display_cursor(1,1);
#ifdef DISPLAY_SCREEN_RESETINFO
/* Zeige den Grund fuer Resets an */
display_printf("MCUCSR - Register");
display_cursor(2,1);
display_printf("PORF :%d WDRF :%d",binary(reset_flag,0),binary(reset_flag,3));
display_cursor(3,1);
display_printf("EXTRF:%d JTRF :%d",binary(reset_flag,1),binary(reset_flag,4));
display_cursor(4,1);
display_printf("BORF :%d",binary(reset_flag,2));
#else
display_printf("Screen 4");
#endif
break;
case 4:
/* Wird zur Ausgabe von Loggings verwendet. */
break;
}
#endif
}
