/*! 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 }