Nibobee Linienverfolgung + Hindernis detektion
So ich bin auf der suche nach einem Programm, dass ermöglicht Linienverfolgung + Hindernisdetektion zu realisieren.
Zurzeit funktioniert die Linienverfolgung bei nicht zu scharfen kurven.
Grundlage ist das schon hier im Forum gepostete Programm
Code:
// kleines Linienfolgedemo (nachbearbeitet) 14.10.09 mic
// https://www.roboternetz.de/phpBB2/ze...=470433#470433
// https://www.roboternetz.de/phpBB2/ze...=468642#468642
#include <nibobee/iodefs.h>
#include <nibobee/motpwm.h>
#include <nibobee/analog.h>
#include <nibobee/line.h>
#include <nibobee/led.h>
#include <nibobee/delay.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
uint8_t i;
uint16_t nibobee_initialization;
int main(void) {
activate_output_group(IO_LEDS);
motpwm_init();
motpwm_setLeft(0);
motpwm_setRight(0);
analog_init();
line_readPersistent();
set_output_group(IO_SENS);
activate_output_bit(IO_LINE_EN);
int16_t speed_flt_l=0;
int16_t speed_flt_r=0;
// Countdown: LEDs blinken lassen
for (i=0; i<10; ++i) {
led_set(LED_L_RD, 1);
led_set(LED_R_RD, 1);
_delay_ms(10);
led_set(LED_L_RD, 0);
led_set(LED_R_RD, 0);
_delay_ms(990);
}
led_set(LED_L_YE, 1);
led_set(LED_R_YE, 1);
_delay_ms(1000);
led_set(LED_L_YE, 0);
led_set(LED_R_YE, 0);
// Hauptschleife:
while(1) {
sei();
_delay_ms(1);
int16_t speed_l=0;
int16_t speed_r=0;
int16_t lval = line_get(LINE_L);
int16_t cval = line_get(LINE_C);
int16_t rval = line_get(LINE_R);
if (lval+cval+rval < 20) {
led_set(LED_L_RD, 0);
led_set(LED_R_RD, 0);
speed_r=0, speed_l=0;
} else if ((lval<cval) && (lval<rval)) {
// lval is minimum
led_set(LED_L_RD, 1);
led_set(LED_R_RD, 0);
speed_r=350, speed_l=250-1*(cval-lval);
} else if ((rval<cval) && (rval<lval)) {
// rval is minimum
led_set(LED_L_RD, 0);
led_set(LED_R_RD, 1);
speed_r=250-1*(cval-rval), speed_l=350;
} else {
// cval is minimum
led_set(LED_L_RD, 1);
led_set(LED_R_RD, 1);
speed_r=450 + 1*(rval-cval), speed_l=450 + 1*(lval-cval);
}
speed_flt_l*=3; speed_flt_l+=speed_l; speed_flt_l/=4;
speed_flt_r*=3; speed_flt_r+=speed_r; speed_flt_r/=4;
motpwm_setLeft(speed_flt_l);
motpwm_setRight(speed_flt_r);
}
return 0;
}
Für die Hindernisdetektion habe ich bisher folgendes Programm
Code:
#include <nibobee/iodefs.h>
#include <nibobee/motpwm.h>
#include <nibobee/delay.h>
#include <nibobee/sens.h>
enum {
MODE_STOP,
MODE_DRIVE,
MODE_BACK,
MODE_STEER_R,
MODE_STEER_L,
MODE_AVOID_R,
MODE_AVOID_L
};
int16_t speed_l;
int16_t speed_r;
uint16_t counter_ms;
uint8_t perform_check(uint8_t mode);
uint8_t do_stop();
uint8_t do_drive();
uint8_t do_back();
uint8_t do_steer_r();
uint8_t do_steer_l();
uint8_t do_avoid_r();
uint8_t do_avoid_l();
int main() {
motpwm_init();
sens_init();
uint8_t mode;
while(1==1) {
enable_interrupts();
delay(1);
mode = perform_check(mode);
switch (mode) {
case MODE_STOP: mode = do_stop(); break;
case MODE_DRIVE: mode = do_drive(); break;
case MODE_BACK: mode = do_back(); break;
case MODE_STEER_R: mode = do_steer_r(); break;
case MODE_STEER_L: mode = do_steer_l(); break;
case MODE_AVOID_R: mode = do_avoid_r(); break;
case MODE_AVOID_L: mode = do_avoid_l(); break;
}
switch (mode) {
case MODE_STOP: speed_l = 0; speed_r = 0;
break;
case MODE_DRIVE: speed_l = 500; speed_r = 500;
break;
case MODE_BACK: speed_l = -500; speed_r = -500;
break;
case MODE_STEER_R: speed_l = 600; speed_r = 400;
break;
case MODE_STEER_L: speed_l = 400; speed_r = 600;
break;
case MODE_AVOID_R: speed_l = -400; speed_r = -600;
break;
case MODE_AVOID_L: speed_l = -600; speed_r = -400;
break;
}
motpwm_setLeft(speed_l);
motpwm_setRight(speed_r);
}
return 0;
}
uint8_t perform_check(uint8_t mode) {
if (sens_getLeft() && sens_getRight()) {
if ((sens_getLeft()==-1) && (sens_getRight()==-1)) {
mode = MODE_BACK;
} else {
mode = MODE_STOP;
}
}
return mode;
}
uint8_t do_stop() {
if ((sens_getLeft()==0) && (sens_getRight()==0)) {
return MODE_DRIVE;
}
return MODE_STOP;
}
uint8_t do_back() {
if (sens_getLeft()==0) {
return MODE_AVOID_L;
}
if (sens_getRight()==0) {
return MODE_AVOID_R;
}
return MODE_BACK;
}
uint8_t do_drive() {
if (sens_getRight()==1) {
return MODE_STEER_L;
}
if (sens_getLeft()==1) {
return MODE_STEER_R;
}
if (sens_getRight()==-1) {
return MODE_AVOID_L;
}
if (sens_getLeft()==-1) {
return MODE_AVOID_R;
}
return MODE_DRIVE;
}
uint8_t do_steer_r() {
if (sens_getLeft()==0) {
return MODE_DRIVE;
}
return MODE_STEER_R;
}
uint8_t do_steer_l() {
if (sens_getRight()==0) {
return MODE_DRIVE;
}
return MODE_STEER_L;
}
uint8_t do_avoid_r() {
if (counter_ms==0) {
counter_ms=1000;
} else {
counter_ms--;
}
if (counter_ms) {
return MODE_AVOID_R;
} else {
return MODE_DRIVE;
}
}
uint8_t do_avoid_l() {
if (counter_ms==0) {
counter_ms=1000;
} else {
counter_ms--;
}
if (counter_ms) {
return MODE_AVOID_L;
} else {
return MODE_DRIVE;
}
}
Mein Ziel ist es eine Art Verbindung aus beiden Programmen zu schaffen.
Damit der Nibobee dann in der Lage ist, einen Parkour aus Hindernissen und schwarzen Lininen zu befahren.
mfg Burak
danke im vorraus
