Projekte
ccaero
Hinderniserkennung_beta
main.c
Projekte
Forum
Doku
Öffentliche Projekte
Startseite
Beispielprogramme
Projekte von anderen
Welcome
Username
Passwort
Eingeloggt bleiben
Zugangsdaten vergessen?
Registrieren
Projektverwaltung
⇨ Please choose! ⇦
——————————————————
✎ Create new project...
★ Browse existing projects...
——————————————————
⚬ MotorTest#1
⚬ C Tutorial 8#1
⚬ NIBO2 C Project#1
⚙ C Tutorial 15#1
⚬ 2010_11_18_el_test001#1
Hindernis...nung_beta
main.c
Project details
Compiler settings
Nachrichten
Sie sind nicht eingeloggt.
Neuigkeiten
★
NiboRoboLib 3.6
2017-01-17: Neue Version 3.6
★
NiboRoboLib 3.4.1
2016-04-16: Neue Version 3.4.1
★
Coding Tutorial
2015-11-22: Jetzt auch für den NIBO burger!
Site-Statistic
7426 private projects
385 public projects
16180353 lines compiled
58212 builds
NIBO
@
hinderniserkennung_beta/main.c [read only]
//************************************************************************************************** // Bibliotheken einbinden //************************************************************************************************** #include <nibobee/iodefs.h> #include <nibobee/clock.h> #include <nibobee/motpwm.h> #include <nibobee/delay.h> #include <nibobee/led.h> #include <nibobee/line.h> #include <nibobee/sens.h> #include <nibobee/odometry.h> #include <nibobee/motpid.h> extern int16_t motpwm_motor_l; extern int16_t motpwm_motor_r; //************************************************************************************************** // Funktionen Vordefinieren //************************************************************************************************** uint32_t get_clock_time(); uint8_t do_drive(uint8_t mode); uint8_t do_stop(uint8_t mode); uint8_t test_sens(uint8_t mode); uint8_t do_back(uint8_t mode); int16_t test_distance(uint8_t reset); uint8_t do_turn(uint8_t mode); uint8_t test_pwm(uint8_t reset); uint8_t do_sterring(uint8_t mode); uint8_t do_alarm(uint8_t mode); uint8_t do_alarmdrive(uint8_t mode); uint8_t watch_move(uint8_t mode); //************************************************************************************************** // Aufzählung für Betriebs-Modus //************************************************************************************************** enum { STOP, DRIVE, BACK, TURN, STERRING, ALARM, ALARM_DRIVE_F, ALARM_DRIVE_R }; //************************************************************************************************** // Globale Variablen Vordefinieren //************************************************************************************************** volatile uint8_t do_ = 0; //Variable speichert ob Roboter gerade eine Aktion ausführt volatile uint8_t first_sens = 0; //Variable speichert den Ort einer Kollision volatile int8_t turn_val=0; //Variable speichert den Drehungsgrad für die Turn-Funktion volatile uint8_t turn_col=0; //Variable speichert ob während einer Drehung eine Kollision festgestellt wurde volatile int8_t sens_l=0; //Variable speichert den Status des linken Fühlers volatile int8_t sens_r=0; //Variable speichert den Status des rechten Fühlers //************************************************************************************************** // Hauptprogramm //************************************************************************************************** int main() { // Timer0 Konfiguration für Fühler-Entprellung TCCR0 |= (1<<WGM01) | (1<<CS02) | (1<<CS00); //Compare-Modus, Prescaler 1024 OCR0 |= 147; //Compare-Wert TIMSK |= (1<<OCIE0); //Timer0 im Compare-Modus starten // Initialisierung enable_interrupts(); // Interrupts einschalten motpwm_init(); // Motorcontroller vorbereitebn odometry_init(); // Odometry vorbereiten clock_inc_irq(0,0); // interne Uhr starten led_init(); // LED-Ausgabe vorbereiten sens_init(); // Fühler vorbereiten line_init(); // Bodensendor vorbereiten motpid_init(); // Motor-Software-Regler aktiveren delay(1000); // Pause als INIT-Zeit //Variablen für Hauptprogramm setzen uint8_t mode = STOP; //Hauptschleife ******************************************************************************* while(1==1) { //Sensoren auslesen und entsprechende Aktionen setzen mode = test_sens(mode); mode = watch_move(mode); //gesetzen Modus ausführen switch(mode) { case STOP: mode=do_stop(mode); break; //Modus "STOP" case DRIVE: mode=do_drive(mode); break; //Modus "FAHREN" case BACK: mode=do_back(mode); break; //Modus "Zurück" case TURN: mode=do_turn(mode); break; //Modus "Drehen" case STERRING: mode=do_sterring(mode); break; //Modus "Lenken" case ALARM: mode=do_alarm(mode); break; //Modus "ALARM" case ALARM_DRIVE_F: case ALARM_DRIVE_R: mode=do_alarmdrive(mode); break; } } return 0; } //************************************************************************************************** // Funktion zum testen und auslesen der Sensoren //************************************************************************************************** uint8_t test_sens(uint8_t mode) { // STOP-SIGNAL PRÜFEN: //Testen ob beide Fühler nach vorne gedrückt wurden, wenn ja Motoren stoppen, Modus "STOP" setzen, LEDs ausschalten und Aktions-Variable zurücksetzen if (sens_l == 1 && sens_r == 1 && mode != STOP) { //Motoren Stoppen motpid_stop(1); //Aktions-Variable zurücksetzen do_=0; watch_move(mode); //gelbe LEDs Ausschalten led_set(LED_L_YE,0); led_set(LED_R_YE,0); //rote LEDs Ausschalten led_set(LED_L_RD,0); led_set(LED_R_RD,0); // Modus "STOP" setzen" und Funktion beenden return STOP; } // AUF SEITLICHE KOLLISION WÄHREND DEM VORWÄRTSFAHREN PRÜFEN: //Testen ob nur ein Fühler nach vorne gedrückt wird, während der Modus "Fahren" aktiv ist else if ((sens_l == 1 || sens_r == 1) && mode == DRIVE) { do_=0; //Aktionsvariable zurücksetzen; watch_move(mode); return STERRING; } //AUF FRONTAL-KOLLISION PRÜFEN //Test ob ein Fühler nach hinten gedrückt wurden, wenn ja Motoren stoppen, Modus "ZURÜCK" setzen, Aktions-.Variable zurücksetzen und Kollision mit roten LEDs anzeigen if ((sens_l == -1 || sens_r == -1) && mode != BACK && mode != TURN && mode != ALARM) { //Motoren Stoppen motpid_stop(1); //Aktions-Variable zurücksetzen do_=0; watch_move(mode); //gelbe LEDs Ausschalten led_set(LED_L_YE,0); led_set(LED_R_YE,0); // Ort der Kollision feststellen, in Variable speichern und Rote LEDs setzen if (sens_l == -1 && sens_r != -1) { first_sens=1; led_set(LED_L_RD,1); led_set(LED_R_RD,0); } if (sens_r == -1 && sens_l != -1) { first_sens=2; led_set(LED_L_RD,0); led_set(LED_R_RD,1); } if (sens_r == -1 && sens_l == -1) { first_sens=0; led_set(LED_L_RD,1); led_set(LED_R_RD,1); } //kurze Pause delay(150); //Modus "ZURÜCK" setzen und Funktion beenden return BACK; } //AUF SEITLICHE KOLLISION WÄHREND DREHUNG PRÜFEN if ((sens_l == 1 || sens_r == 1) && mode == TURN && turn_col == 0) { do_=0; //Aktions-Variable zurücksetzen watch_move(mode); turn_col=1; //Kollisions-Variable setzen motpid_stop(1); //Motoren Stoppen turn_val=turn_val*-1; //Drehrichtung umkehren if (turn_val < 0) { turn_val=turn_val+test_distance(0); } else if (turn_val > 0) { turn_val=turn_val+(test_distance(0)*-1); } led_set(LED_L_YE,0); //Linke Gelbe LED ausschalten led_set(LED_R_YE,0); //Rechte Gelbe LED ausschalten return TURN; //Modus auf "Drehen" stellen (sicherheitshalber) } return mode; } //************************************************************************************************** // Funktion zum Lenken //************************************************************************************************** uint8_t do_sterring(uint8_t mode) { //Konstanten definieren const uint8_t max_speed=30; //Gibt die maximale Fahrgeschwindigkeit an; //Variablen definieren static uint32_t last_clock_time2=0; static uint8_t led_stat=0; //Zeitsteuerung für Blinklicht if ( get_clock_time()-last_clock_time2 > 100 ) { led_stat=!led_stat; last_clock_time2=get_clock_time(); } //Steuerung nach Rechts if (sens_l == 1) { motpid_setSpeed(max_speed,0); do_=1; //Linke Gelbe LED blinken lassen led_set(LED_L_YE,led_stat); led_set(LED_R_YE,1); return mode; // Steuerung nach Links } else if (sens_r == 1) { motpid_setSpeed(0,max_speed); do_=1; //Rechte Gelbe LED blinken lassen led_set(LED_R_YE,led_stat); led_set(LED_L_YE,1); return mode; // Wenn keine Kollision mehr erkennbar ist, zum Modus "Fahren" wechseln } else { do_=0; watch_move(mode); return DRIVE; } } //************************************************************************************************** // Funktion zum Drehen //************************************************************************************************** uint8_t do_turn(uint8_t mode) { //Konstanten definieren const uint8_t max_speed=100; //Gibt die maximale Drehgeschwindigkeit an; //Wenn keine seitliche Kollision stattfindet, Kollsions-Variable zurücksetzen if (sens_l != 1 && sens_r != 1) { turn_col=0; } // Wenn Drehung noch nicht gestartet ... if (do_ == 0) { //...Drehen nach Links einleiten if (turn_val < 0) { test_distance(1); //Distanz-Zähler zurücksetzen do_ = 1; //Aktions-Variable setzen motpid_setTargetRel(turn_val,turn_val*-1,max_speed); //Motoren auf Drehen stellen test_pwm(1); //PWM-Tester initen led_set(LED_L_YE,1); //...Drehen nach Rechts einleiten } else if (turn_val > 0) { test_distance(1); //Distanz-Zähler zurücksetzen do_ = 1; //Aktions-Variable setzen motpid_setTargetRel(turn_val,turn_val*-1,max_speed); //Motoren auf Drehen stellen test_pwm(1); //Fahrt-Tester initen led_set(LED_R_YE,1); //...Wenn Drehrichtung 0 ist, dann zum Modus "Fahren" wechseln } else { return DRIVE; } // Wenn Drehung eingeleitet, prüfen ob Drehung noch im Gange ist } else { //Wenn Drehung abgeschlossen if (test_pwm(0) == 0) { do_=0; //Aktions-Variable zurücksetzeb watch_move(mode); motpid_stop(1); //Motoren Stoppen (sicherheitshalber) turn_val=0; //Dreh-Variable zurücksetzen return DRIVE; } } return mode; } //************************************************************************************************** // Funktion zum Zurücksetzen //************************************************************************************************** uint8_t do_back(uint8_t mode) { // Konstanten definieren const uint8_t max_distance=50; //Gibt die maximale Rückwärtsstrecke an const uint8_t min_distance=10; //Gibt die minimale Rückwärtsstrecke an const uint8_t max_speed=50; //Gibt die Rückwärtsgeschwindigkeit an; const uint8_t min_dist_st=100; //Gibt die Minimale Strecke an, die gefahren werden musste, für den "Small-Turn" //Variablen definieren int16_t distance=0; static uint8_t test=0; static uint8_t pre_do=0; static uint32_t last_clock_time2=0; //Wenn Fühler nicht frei ist, dann so lange zurückfahren bis Fühler frei if (sens_l == -1 || sens_r == -1) { if (pre_do == 0) { motpid_setSpeed(max_speed*-1,max_speed*-1); pre_do=1; last_clock_time2=get_clock_time(); } else { // wenn Fühler nach 2 Sekunden noch nicht frei, dann Vollgas geben if ( get_clock_time()-last_clock_time2 > 2000 && get_clock_time()-last_clock_time2 < 5000) { motpid_setSpeed(-100,-100); led_set(LED_L_RD,1); led_set(LED_R_RD,1); // wenn Fühler nach 5 Sekunden noch nicht frei dann in Alarm-Modus wechseln } else if ( get_clock_time()-last_clock_time2 > 5000 ) { pre_do=0; test=0; do_=0; watch_move(mode); return ALARM; } } } else { if (pre_do == 1) { pre_do=0; do_=0; watch_move(mode); } } //Distanzabhängige Rückwärtsfahrt einleiten (sofern Fühler frei) und Aktions-Variable zurücksetzen wenn Rückwärtsfahrt beenden ist if (pre_do == 0) { //Rückwärtsfahr starten if (do_ == 0) { do_ = 1; //Aktions-Variable setzen distance=test_distance(0); if (distance < min_dist_st) { test=1; } else { test=0; } if (distance < max_distance) { distance=distance*-1; if (distance > (min_distance*-1)) { distance=min_distance*-1; } } else { distance=max_distance*-1; } motpid_setTargetRel(distance,distance,max_speed); //Motoren auf Rückwärtsfahrt stellen, mit bestimmten Position //delay(20); test_pwm(1); //Fahrt-Tester initen //Prüfen ob Rückwärtsfahrt beendet wurde, wenn ja dann Aktionsvariable zurücksetzen } else { if (test_pwm(0) == 0) { do_=0; //Aktions-Variable zurücksetzeb watch_move(mode); motpid_stop(1); //Motoren Stoppen (sicherheitshalber) } } } //Prüfen welcher Fühler als erstes entlastet wird (Ort der Kollision erkennen, sofern nicht schon durch Sensor-Funktion erledigt) if ((sens_l == -1 && sens_r != -1) && first_sens == 0) { first_sens=1; } if ((sens_r == -1 && sens_l != -1) && first_sens == 0) { first_sens=2; } //Rote LEDs Anhand erkannter Kollision setzen if (first_sens == 1) { led_set(LED_L_RD,1); led_set(LED_R_RD,0); } if (first_sens == 2) { led_set(LED_L_RD,0); led_set(LED_R_RD,1); } //Return-Variable setzen //Wenn Rückwärtsfahrt beendet wurde if (do_ == 0 && pre_do == 0) { //rote LEDs ausschalten led_set(LED_L_RD,0); led_set(LED_R_RD,0); //Drehung einleiten; if (test == 1) { if (first_sens == 1) { turn_val=16; return TURN; } else if (first_sens == 2) { turn_val=-16; return TURN; } else { turn_val=0; return TURN; } } else { if (first_sens == 1) { turn_val=8; return TURN; } else if (first_sens == 2) { turn_val=-8; return TURN; } else { turn_val=0; return TURN; } } //Wenn Rückwärtsfahrt noch läuft } else { return mode; } } //************************************************************************************************** // Funktion zum Stoppen //************************************************************************************************** uint8_t do_stop(uint8_t mode) { static uint32_t last_clock_time2=0; static uint8_t led_stat=0; // Testen ob Fühler nach vorne gedrückt sind, wenn nicht dann zum Modus "Fahren" wechseln if (sens_l != 1 || sens_r != 1) { //gelbe LEDs Ausschalten led_set(LED_L_YE,0); led_set(LED_R_YE,0); return DRIVE; } else { //gelbe LEDs abwechselnt blinken lassen if ( get_clock_time()-last_clock_time2 > 250 ) { led_stat=!led_stat; led_set(LED_L_YE,led_stat); led_set(LED_R_YE,!led_stat); last_clock_time2=get_clock_time(); } } return mode; } //************************************************************************************************** // ALARM-DRIVE - Funktion um steckendes/blockiertes Getriebe zu befreien // (Gibts kurz nach vorne oder hinten vollgas) //************************************************************************************************** uint8_t do_alarmdrive(uint8_t mode) { static int8_t dir=0; if (do_ == 0) { led_set(LED_L_YE,0); led_set(LED_R_YE,0); led_set(LED_L_RD,1); led_set(LED_R_RD,1); switch(mode) { case ALARM_DRIVE_F: dir=50; break; case ALARM_DRIVE_R: dir=-51; break; } do_ = 1; motpid_setTargetRel(dir,dir,100); test_pwm(1); return mode; } else { if (test_pwm(0) == 0) { do_=0; watch_move(mode); motpid_stop(1); led_set(LED_L_RD,0); led_set(LED_R_RD,0); return DRIVE; } else { return mode; } } } //************************************************************************************************** // Alarm-Funktion: Motoren Stopp, und LED blinken lassen //************************************************************************************************** uint8_t do_alarm(uint8_t mode) { static uint32_t last_clock_time2=0; static uint8_t led_stat=0; //gelbe LEDs deaktivieren led_set(LED_L_YE,0); led_set(LED_R_YE,0); //Motoren stoppen motpid_stop(1); //rote LEDs blinken lassen if ( get_clock_time()-last_clock_time2 > 250 ) { led_stat=!led_stat; led_set(LED_L_RD,led_stat); led_set(LED_R_RD,led_stat); last_clock_time2=get_clock_time(); } return ALARM; } //************************************************************************************************** // Funktion zum Fahren //************************************************************************************************** uint8_t do_drive(uint8_t mode) { // Konstanten festlegen const uint8_t min_speed=30; //Minimale Fahrgeschwindigkeit const uint8_t max_speed=100; //Maximale Fahrgeschwindigkeit const uint8_t speed_step=5; //Beschleunigung const uint16_t speed_step_time=1000; //Zeit bis zur nächsten Beschleunigung in Millisekunden // Statische Variablen festlegen static uint32_t last_clock_time=0; static uint8_t cur_speed=0; // Fahrt starten if (do_ == 0) { do_=1; last_clock_time=get_clock_time(); cur_speed=min_speed; test_distance(1); motpid_setSpeed(cur_speed,cur_speed); led_set(LED_L_YE,1); led_set(LED_R_YE,1); //Beschleunigen } else { if ( get_clock_time()-last_clock_time > speed_step_time ) { cur_speed=cur_speed+speed_step; if (cur_speed > max_speed) { cur_speed = max_speed; } motpid_setSpeed(cur_speed,cur_speed); last_clock_time=get_clock_time(); } } return mode; } //************************************************************************************************** // Funktion zum berechnen der vergangenen Millisekunden seit Systemstart //************************************************************************************************** uint32_t get_clock_time() { return (clock_sec*1000)+clock_ms; } //************************************************************************************************** // Funktion um zurückgelegte Strecke (in Ticks) zu messen //************************************************************************************************** int16_t test_distance(uint8_t reset) { static int32_t last_odo_pos_l=0; static int32_t last_odo_pos_r=0; int16_t left_distance=0; int16_t right_distance=0; if (reset == 1) { last_odo_pos_l=odometry_getLeft(0); last_odo_pos_r=odometry_getRight(0); return 0; } left_distance=odometry_getLeft(0)-last_odo_pos_l; right_distance=odometry_getRight(0)-last_odo_pos_r; if (left_distance < right_distance) { return left_distance; } else { return right_distance; } } //************************************************************************************************** // Funktion welche Feststellt ob Motor PWM-Signal gesetzt ist //************************************************************************************************** uint8_t test_pwm(uint8_t reset) { static uint8_t wait=0; static uint32_t last_clock_time=0; if (reset == 1) { last_clock_time=get_clock_time(); wait=1; } if (get_clock_time()-last_clock_time > 500) { wait=0; } if (wait==0) { if (motpwm_motor_l >= -25 && motpwm_motor_r >= -25 && motpwm_motor_l <= 25 && motpwm_motor_r <= 25) { return 0; } else { return 1; } } else { return 1; } } //************************************************************************************************** // Movement-Watchdog - Überprüft ob Bewegungen ausgeführt werden, ansonsten wird // in den Alarm-Drive Modus geschalten und wenn das nicht erfolgreich war ALARM gegeben //************************************************************************************************** uint8_t watch_move(uint8_t mode) { static uint8_t enable=0; //Variable gibt an ob der Watch-Dog aktiv ist static uint32_t last_change=0; //Variable gibt an, zu welcher zeit sich die Odometrie das letzte mal verändert hat static int32_t last_odo_pos_l=0; //Variable speichert den Odometrie-Wert zur letzten Ausführung static int32_t last_odo_pos_r=0; static uint8_t test=0; if (do_ == 0) { enable=0; last_change=0; last_odo_pos_l=0; last_odo_pos_r=0; return mode; } else { if (enable == 0) { enable=1; last_change=get_clock_time(); last_odo_pos_l=odometry_getLeft(0); last_odo_pos_r=odometry_getRight(0); return mode; } else { if (get_clock_time()-last_change > 3000 ) { if ( last_odo_pos_l != odometry_getLeft(0) || last_odo_pos_r != odometry_getRight(0)) { last_change=get_clock_time(); last_odo_pos_l=odometry_getLeft(0); last_odo_pos_r=odometry_getRight(0); if (test == 1) { test=0; } return mode; } else { do_=0; enable=0; last_change=0; last_odo_pos_l=0; last_odo_pos_r=0; if (test == 0) { test=1; motpid_stop(1); if (mode == DRIVE || mode == STERRING) { return ALARM_DRIVE_R; } else { return ALARM_DRIVE_F; } } else { return ALARM; } } } else { return mode; } } } } //************************************************************************************************** // Timer0 Compare Interrupt // Zur Entprellung der Fühler //************************************************************************************************** ISR(TIMER0_COMP_vect) { static int8_t ll=0; //Variable für letzten Status linker Fühler static uint8_t lcc=0; //Counter-Variable - zählt wie oft hintereinander linker Fühler gleichen Status hatte static int8_t rl=0; //Variable für letzten Status rechter Fühler static uint8_t rcc=0; //Counter-Variable - zählt wie oft hintereinander rechter Fühler gleichen Status hatte //-- Linker Fühler -- //aktuellen Status prüfen - wenn aktueller Status gleich letzten Status dann Counter hochzählen //ansonsten letzter Status auf aktuellen setzen und Counter zurücksetzen if (sens_getLeft() == ll) { lcc=lcc+1; } else { ll=sens_getLeft(); lcc=0; } //Wenn Counter den Wert 3 erreicht, dann entprellten Fühlerstatus "stat_l" auf letzen Status setzen if ( lcc == 3) { sens_l=ll; } //-- Rechter Fühler -- //aktuellen Status prüfen - wenn aktueller Status gleich letzten Status dann Counter hochzählen //ansonsten letzter Status auf aktuellen setzen und Counter zurücksetzen if (sens_getRight() == rl) { rcc=rcc+1; } else { rl=sens_getRight(); rcc=0; } //Wenn Counter den Wert 3 erreicht, dann entprellten Fühlerstatus "stat_r" auf letzen Status setzen if ( rcc == 3) { sens_r=rl; } }
Compiler results:
Werbung
Online
mattbrock489364684
mckinleysiddons
warrenhinder6456