Nibobee Linienverfolgung + Hindernis detektion

**HHBurak** · 22.04.2010, 11:14

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

**s.frings** · 05.05.2010, 10:54

Und wo ist da das Problem?

**HHBurak** · 06.05.2010, 11:52

Aber unserer Roboter hat ein Problem mit den Rädern. Funktioniert das Programm bei euch und was macht das Programm

Code:

#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>
#include <nibobee/sens.h>


uint8_t i;
uint16_t nibobee_initialization;
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();

enum {
MODE_STOP,
MODE_DRIVE,
MODE_BACK,
MODE_STEER_R,
MODE_STEER_L,
MODE_AVOID_R,
MODE_AVOID_L
};



int main(void) {
 
  sens_init(); 
  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);
  uint8_t mode ; 
  

  
	while(1==1) {

		enable_interrupts();
		int16_t speed_r=550;
		int16_t speed_l=500;


		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);


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_l*=3; speed_l+=speed_l; speed_l/=4;
    speed_r*=3; speed_r+=speed_r; speed_r/=4;

    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;
}
}

**s.frings** · 06.05.2010, 14:05

Ein Problem mit den Rädern? Welches denn? konkrete Infos kann alles Mögliche los sein, z.B:

- Räder fallen ab
- Sie bleiben an Kaugummi kleben
- Die Räder rutschen durch
- Sie passen nicht auf die Achsen

**HHBurak** · 06.05.2010, 14:43

Wenn ich den Roboter in der Luft halte drehen sich die Räder ab sobald ich ihn auf den Boden legen dreht sich nur ein Rad das andere nicht. -
Das ist aber nicht so wichtig mir geht es eher um das Programm weil wir dass der Klasse vorstellen müssen.
Das Programm wird mit einer Warning compiliert, wenn ich "uint8_t mod = 1 " mache geht es sogar ohne Warning, ich wollte nur wissen ob das Programm bei euch fehlerfrei läuft.

**Rabenauge** · 06.05.2010, 20:34

Was genau soll es denn "machen"?
Habs mal eben meiner Biene aufgegeben:
1. sie fährt.
2. sie reagiert auf hellen/dunklen Untergrund mit blinken der roten LED`s (von Lenken hab ich allerdings nix mitbekommen)
3. wenn sie mit nem Fühler anstösst (hab da gerade keine Kabelbinder dran) will sie offenbar ein zu kleines Stück zurückfahren oder sowas.
Aber nur, so lange die Fühler auch wirklich betätigt werden.

Hab leider keine Zeit, mir den Code anzusehen, aber wenn dir das weiterhilft?

Thema: Nibobee Linienverfolgung + Hindernis detektion

Themen-Optionen

Thema durchsuchen

Anzeige

Nibobee Linienverfolgung + Hindernis detektion

Lesezeichen

Lesezeichen

Berechtigungen