Programm für Roboter Eins
//written by Martin Wewior, Sascha Germer
#include <conio.h>
#include <unistd.h>
#include <dsensor.h>
#include <dmotor.h>
#include <sys/lcd.h>
#include <lnp.h>
#include <lnp-logical.h>
#define LIGHT_R SENSOR_1
#define LIGHT_L SENSOR_3
// MOTOR_A ->> Antrieb
// MOTOR_C ->> Lenkung
//Korrekturfaktor, linker Sensor
#define CORL 0
//Korrekturfaktor, rechter Sensor
#define CORR 0
//min. Zeitdifferenz zwischen Erkennen von Silber und ermöglichen erneuter Silbererkennung
#define SILVDIFF 3000
//min. Zeitdifferenz, für das entlangfahren am blauen "Endstreifen"
#define BLUDIFF 1300
#define IS_GREEN(x) (x<70)
#define IS_BLUE(x) ((x>76)&&(x<93))
#define IS_WHITE(x) ((x>95)&&(x<124))
#define IS_SILVER(x) (x>128)
#define SLOW_SPEED (MAX_SPEED/2)
#define NORMAL_SPEED (2*MAX_SPEED/3)
//ein paar Farben
#define GREEN 100
#define BLUE 101
#define WHITE 102
#define SILVER 103
#define UNKNOWN 104
//ein paar Zustände
#define PANIC 0 // :-)
#define ON_LINE 1
#define LEFT_OFF 2
#define RIGHT_OFF 3
#define ON_SILVER 4
#define ON_SILVER_LEFT 5
#define ON_SILVER_RIGHT 6
#define IN_TURN 7
#define IN_TURN_RIGHT_OFF 8
#define IN_TURN_LEFT_OFF 9
#define EXIT_TURN 10
#define EXIT_TURN_RIGHT 11
#define EXIT_TURN_LEFT 12
#define CROSSING 13
#define CROSSING_LEFT_OFF 14
#define CROSSING_RIGHT_OFF 15
#define ON_BLUE_LEFT 16
#define ON_BLUE_RIGHT 17
#define ON_BLUE 18
#define EXIT_CROSSING 19
#define EXIT_CROSSING_RIGHT 20
#define EXIT_CROSSING_LEFT 21
#define CHECK_BLUE 22
#define ENTE 23
//etc.
#define CURVES 255 //veranschlagen wir, daß nicht mehr als 255 Kreuzungen existieren!
char route[CURVES]; //in diesem Array wird der Weg zum Ziel gespeichert
int ptr=0; //ein "Pointer" auf ein best. Element des Array
int received=0; //Empfangsbestätigung, (s. unten)
void initialize(void); //Funktionsprototypen..
void stop(void);
void forward(int);
void turn_left(int,int);
void turn_right(int,int);
int colour(int);
void detected_crossing(void);
void detected_dead_end(void);
void ir_handler(const unsigned char *, unsigned char);
void initialize(void){
int i=0;
ds_active(&LIGHT_L);
ds_active(&LIGHT_R);
lnp_logical_range(1);
lnp_integrity_set_handler(ir_handler);
for(i=0;i<CURVES;i++)
{ route[i]=0;
}
route[0]=4; //4 definiert den Anfang des Feldes
ptr=1;
msleep(50);
}
void ir_handler(const unsigned char *buffer, unsigned char length) { //wenn Empfangsbestätigung vom zweiten Roboter erhalten, wird Senden abgebrochen und Programm beendet (s. unten)
if((buffer[0]=='O')&&(buffer[1]=='K')) {
received=1;
}
}
//Steuerungskontrollen..
void stop(void){
motor_a_speed(MAX_SPEED/5);
motor_c_speed(MAX_SPEED);
motor_a_dir(brake);
motor_c_dir(brake);
msleep(70);
}
void forward(int a){
motor_a_speed(a);
motor_c_speed(NORMAL_SPEED);
motor_a_dir(rev);
motor_c_dir(brake);
}
void turn_right(int a, int c){
motor_a_speed(a);
motor_c_speed(c);
motor_a_dir(rev);
motor_c_dir(fwd);
}
void turn_left(int a, int c){
motor_a_speed(a);
motor_c_speed(c);
motor_a_dir(rev);
motor_c_dir(rev);
}
int colour(int cl){ //Lichtwerten an den Sensoren werden Farben zugeordnet
if(IS_GREEN(cl)){
return GREEN;
}
else if(IS_BLUE(cl)){
return BLUE;
}
else if(IS_WHITE(cl)){
return WHITE;
}
else if(IS_SILVER(cl)){
return SILVER;
}
else{
return UNKNOWN; //unbekannte Farbwerte werden ignoriert...
}
}
void detected_crossing(void) //Was geschieht bei Befahren von Kreuzungen:
{
if(route[ptr]<2) //erstes und zweites Befahren einer Kreuzung-> Zähler wird um 1 erhöht und Pointer auf nächstes Element des Arrays gesetzt (nächste Kreuzung)
//eine 1 im Array steht für "linker Ast der Kreuzung wird abgefahren", eine 2 für "rechter Ast wird abgefahren"
{
route[ptr]++;
ptr++;
}
else //wenn Kreuzung zum dritten Mal befahren wird, also alle linken und rechten Teiläste Sackgassen waren, wird Kreuzung aus dem Array gelöscht
{
route[ptr]=0;
ptr--;
}
}
void detected_dead_end(void) //Sackgasse... was da zu tun ist
{
ptr--; //zur letzten Kreuzung wird zurückgefahren, also Pointer wieder auf diese Kreuzung gesetzt
}
int main(int argc, char *argv[]) {
//debug -> z.Z. deaktiviert
int debug1=0; //Zustände
int debug2=0; //Knotenliste
int old=0;
int current=ON_LINE;
int lr=0;
int ll=0;
long sil_enter_time=0; //RCX speichert Systemzeit in ms seit Aktivierung...
long sil_leave_time=0;
long blu_enter_time=0;
int notjuhu=1;
initialize(); //init der Sensoren etc.
while(notjuhu){
//Zustand bestimmen
if(debug2)lcd_int(((route[0])*1000)+((route[1])*100)+((route[2])*10)+(route[3]));
old=current;
lr=LIGHT(LIGHT_R);
ll=LIGHT(LIGHT_L);
lr=colour(lr+(CORR));
ll=colour(ll+(CORL));
//Aktionen festlegen
//"normale Fahrt"
if (old==ON_LINE){
if(debug1) cputs("ONLN");
if ((ll==SILVER)&&(lr==SILVER)&&((sys_time-sil_leave_time)>(SILVDIFF))){ //Bevor eine EndSchleife befahren wird, wird geprüft, ob nicht gerade eine verlassen wurde.
current=ON_SILVER;
}
else if((ll==BLUE)&&(lr==BLUE)){
current=ON_BLUE;
blu_enter_time=sys_time;
}
else if ((ll!=GREEN)&&(lr!=GREEN)){
if(debug1==1) cputs("ONL");
current=ON_LINE;
}
else if((ll==GREEN)&&(lr!=GREEN)){
current=LEFT_OFF;
}
else if ((ll!=GREEN)&&(lr==GREEN)){
current=RIGHT_OFF;
}
}
else if (old==LEFT_OFF){
if((ll!=GREEN)&&(lr!=GREEN)){
current=ON_LINE;
}
}
else if (old==RIGHT_OFF){
if((ll!=GREEN)&&(lr!=GREEN)){
current=ON_LINE;
}
}
//Auffahren auf Kreuzung / Starte Enderkennung
else if ((old==ON_BLUE)||(old==ON_BLUE_LEFT)||(old==ON_BLUE_RIGHT)){
if(debug1) cputs("BLU");
if((ll==WHITE)||(lr==WHITE)){
current=CROSSING;
detected_crossing();
}
else if((ll!=GREEN)&&(lr==GREEN)){
current=ON_BLUE_RIGHT;
}
else if((ll==GREEN)&&(lr!=GREEN)){
current=ON_BLUE_LEFT;
}
else if((ll==GREEN)&&(lr==GREEN)&&((sys_time-blu_enter_time)>(BLUDIFF))){
notjuhu=0;
route[ptr]=5;
current=ENTE;
}
else{
current=ON_BLUE;
}
}
//Befahren der "Endschleife"
else if ((old==ON_SILVER)||(old==ON_SILVER_LEFT)||(old==ON_SILVER_RIGHT)){
if(debug1) cputs("SIL");
if((ll==WHITE)||(lr==WHITE)){
current=IN_TURN;
detected_dead_end();
sil_enter_time=sys_time; //Zeitnahme, wenn Eintritt in Schleife
}
else if((lr==GREEN)&&(ll!=GREEN)){
current=ON_SILVER_RIGHT;
}
else if((ll==GREEN)&&(lr!=GREEN)){
current=ON_SILVER_LEFT;
}
else{
current=ON_SILVER;
}
}
//in der Kreuzung
else if((old==CROSSING)||(old==CROSSING_LEFT_OFF)||(old==CROSSING_RIGHT_OFF)){
if(debug1) cputs("CRS");
if((ll==GREEN)&&(lr==BLUE)){
current=CHECK_BLUE; //da der Übergang zwischen Grün und Weiß manchmal als Blau erkannt wird, wird über diesen Zustand geprüft, ob tatsächlich Blau vorliegt (geschieht weiter unten im Programm)
}
else if((ll==GREEN)&&(lr!=GREEN)){
current=CROSSING;
}
else if((ll!=GREEN)&&(lr!=GREEN)){
current=CROSSING_RIGHT_OFF;
}
else if((ll==GREEN)&&(lr==GREEN)){
current=CROSSING_LEFT_OFF;
}
}
//Fahrt auf "Endschleife"
else if ((old==IN_TURN)||(old==IN_TURN_LEFT_OFF)||(old==IN_TURN_RIGHT_OFF)){
if(debug1) cputs("TRN");
if (((ll==SILVER)||(lr==SILVER))&&((sys_time-sil_enter_time)>(SILVDIFF))){ //eine gewisse Zeitdifferenz muß zwischen Eintritt und Austritt aus der Schleife liegen...
current=EXIT_TURN;
sil_leave_time=sys_time; //Zeitpunkt des Verlassens der Endschleife
}
else if((ll!=GREEN)&&(lr==GREEN)){
current=IN_TURN;
}
else if((ll!=GREEN)&&(lr!=GREEN)){
current=IN_TURN_LEFT_OFF;
}
else if((ll==GREEN)&&(lr==GREEN)){
current=IN_TURN_RIGHT_OFF;
}
}
//Verlassen der Kreuzung
else if ((old==EXIT_CROSSING)||(old==EXIT_CROSSING_RIGHT)||(old==EXIT_CROSSING_LEFT)){
if(debug1) cputs("EXC");
if((ll==WHITE)&&(lr==WHITE)){
current=ON_LINE;
}
else if((ll!=GREEN)&&(lr==GREEN)){
current=EXIT_CROSSING_RIGHT;
}
else if((ll==GREEN)&&(lr!=GREEN)){
current=EXIT_CROSSING_LEFT;
}
else{
current=EXIT_CROSSING;
}
}
//Verlassen der "Endschleife"
else if ((old==EXIT_TURN)||(old==EXIT_TURN_RIGHT)||(old==EXIT_TURN_LEFT)){
if(debug1) cputs("EXT");
if((ll==WHITE)&&(lr==WHITE)){
current=ON_LINE;
}
else if(lr==GREEN){
current=EXIT_TURN_RIGHT;
}
else if(ll==GREEN){
current=EXIT_TURN_LEFT;
}
else{
current=EXIT_TURN;
}
}
//Aktionen starten entsprechend des vorliegenden Zustandes (selbsterklärend ;-})
if((current==ON_LINE)||(current==IN_TURN)){
forward(NORMAL_SPEED);
}
else if((current==LEFT_OFF)||(current==ON_SILVER_LEFT)||(current==EXIT_TURN_LEFT)||(current==CROSSING_LEFT_OFF)){
turn_right(MAX_SPEED/4, MAX_SPEED/2);
}
else if((current==RIGHT_OFF)||(current==ON_SILVER_RIGHT)||(current==EXIT_TURN_RIGHT)||(current==CROSSING_RIGHT_OFF)){
turn_left(MAX_SPEED/4, MAX_SPEED/2);
}
else if((current==EXIT_TURN)||(current==CROSSING)){
forward(SLOW_SPEED/2);
}
else if((current==IN_TURN_RIGHT_OFF)||(current==EXIT_CROSSING_RIGHT)||(current==ON_BLUE_RIGHT)){
turn_left(MAX_SPEED/8, MAX_SPEED/3);
}
else if((current==IN_TURN_LEFT_OFF)||(current==EXIT_CROSSING_LEFT)||(current==ON_BLUE_LEFT)){
turn_right(MAX_SPEED/8, MAX_SPEED/3);
}
else if((current==ON_BLUE)||(current==ON_SILVER)||(current==EXIT_CROSSING)){
forward(SLOW_SPEED);
}
else if(current==CHECK_BLUE){ //hier wird geprüft, ob beim Verlassen einer Kreuzung tatsächlich auf BEIDEN Sensoren Blau gemessen wird
ll=0;
lr=0;
if(debug1) cputs("CHK");
motor_a_dir(brake);
turn_right(0, MAX_SPEED/3);
while(ll<80){
ll=LIGHT(LIGHT_L)+(CORL);
lr=LIGHT(LIGHT_R)+(CORR);
if(IS_BLUE(lr)&&IS_BLUE(ll)) {
current=EXIT_CROSSING;
}
else current=CROSSING;
}
stop();
}
}
if(debug1) cputs("ZIEL");
//nachdem das Ziel gefunden wurde, wird der kürzeste Weg an den zweiten Roboter übertragen
//um Übertragung zu garantieren, wird mehrfach gesendet und der Roboter im Kreis bewegt, um alle Richtungen beim Senden abzudecken
//das Senden wird erst abgebrochen, wenn Empfangsbestätigung vom zweiten Roboter erhalten wurde
turn_left(SLOW_SPEED, SLOW_SPEED);
msleep(400);
stop();
while(!received) {
lnp_integrity_write(route,(ptr+1));
msleep(20);
forward(NORMAL_SPEED);
msleep(400);
lnp_integrity_write(route,(ptr+1));
msleep(20);
turn_left(SLOW_SPEED, SLOW_SPEED);
msleep(100);
stop();
}
stop();
if(debug1) cputs("done");
return 0;
}
Programm Roboter 2
//written by Martin Wewior, Sascha Germer
#include <conio.h>
#include <unistd.h>
#include <dsensor.h>
#include <dmotor.h>
#include <sys/lcd.h>
#include <lnp.h>
#include <lnp-logical.h>
#define LIGHT_R SENSOR_1
#define LIGHT_L SENSOR_3
// MOTOR_A ->> Antrieb
// MOTOR_C ->> Lenkung
//Korrekturfaktor, linker Sensor
#define CORL 5
//Korrekturfaktor, rechter Sensor
#define CORR 0
#define IS_GREEN(x) (x<70)
#define IS_BLUE(x) ((x>76)&&(x<93))
#define IS_WHITE(x) ((x>95)&&(x<124))
#define IS_SILVER(x) (x>128)
#define SLOW_SPEED (MAX_SPEED/2)
#define NORMAL_SPEED (2*MAX_SPEED/3)
//ein paar Farben
#define GREEN 100
#define BLUE 101
#define WHITE 102
#define SILVER 103
#define UNKNOWN 104
//ein paar Zustände
#define PANIC 0 // :-)
#define ON_LINE 1
#define LEFT_OFF 2
#define RIGHT_OFF 3
#define ON_BLUE_LEFT 4
#define ON_BLUE_RIGHT 5
#define ON_BLUE 6
#define CROSSING_RIGHT 7
#define CROSSING_RIGHT_LEFT_OFF 8
#define CROSSING_RIGHT_RIGHT_OFF 9
#define CROSSING_LEFT 10
#define CROSSING_LEFT_LEFT_OFF 11
#define CROSSING_LEFT_RIGHT_OFF 12
#define EXIT_CROSSING 13
#define EXIT_CROSSING_RIGHT 14
#define EXIT_CROSSING_LEFT 15
#define CHECK_BLUE_RIGHT 16
#define CHECK_BLUE_LEFT 17
#define ENTE 18
#define CURVES 255 //veranschlagen wir, daß nicht mehr als 255 Kreuzungen existieren!
char route[CURVES];
int ptr=0; //ein "Pointer" auf ein best. Element des Array
int trmcpl=0; //Übertragung komplett
void initialize(void);
void stop(void);
void forwrd(int);
void trn_lft(int,int);
void trn_rgt(int,int);
int colour(int);
void ir_handler(const unsigned char *, unsigned char);
void initialize(void){
ds_active(&LIGHT_L);
ds_active(&LIGHT_R);
lnp_logical_range(1);
lnp_integrity_set_handler(ir_handler);
msleep(50);
}
//Funktionen für die Motoransteuerung
void stop(void){
motor_a_speed(MAX_SPEED/5);
motor_c_speed(MAX_SPEED);
motor_a_dir(brake);
motor_c_dir(brake);
msleep(70);
}
void forwrd(int a){
motor_a_speed(a);
motor_c_speed(NORMAL_SPEED);
motor_a_dir(rev);
motor_c_dir(brake);
}
void trn_rgt(int a, int c){
motor_a_speed(a);
motor_c_speed(c);
motor_a_dir(rev);
motor_c_dir(fwd);
}
void trn_lft(int a, int c){
motor_a_speed(a);
motor_c_speed(c);
motor_a_dir(rev);
motor_c_dir(rev);
}
//Funktion zur Bestimmung der Farben aus den Lichtwerten
int colour(int cl){
if(IS_GREEN(cl)){
return GREEN;
}
else if(IS_BLUE(cl)){
return BLUE;
}
else if(IS_WHITE(cl)){
return WHITE;
}
else if(IS_SILVER(cl)){
return SILVER;
}
else{
return UNKNOWN;
}
}
//Handler für den IR-Empfang, kopiert empfangene Daten in 'lokales' Array und setzt Status
void ir_handler(const unsigned char *buffer, unsigned char length) {
int i;
for(i=0;i<length;i++)
{ route[i]=buffer[i];
}
trmcpl=1;
}
int main(int argc, char *argv[]) {
//____________________debug_____________
int debug1=1; //zeigt aktuellen Zustand
int debug2=0; //zeigt aktuelle Abzweigrichtung
int old=0; //Startzustand: auf der Linie
int i=9;
int current=ON_LINE; //Startzustand: auf der Linie
int lr=0; //die üblichen inits...
int ll=0; //Lichtsensorvariablen auf Null
int notjuhu=1; //Status für Nichterreichen des Zieles
ptr=1; //Array-Pointer auf erstes 'echtes' Element
initialize(); //init der Sensoren, IR-Handler...
while(!trmcpl) //Warten auf Übermittlung der Daten
{ cputs("WAIT");
msleep(10);
}
msleep(250);
cputs("");
lnp_integrity_write("OK",3); //Empfangsbestätigung senden
while(notjuhu){
//Empfangsbestätigung mehrfach senden
if((i--)>0)
{ lnp_integrity_write("OK",3);
}
if(debug2) lcd_int(route[ptr]);
//Zustand bestimmen
old=current;
lr=LIGHT(LIGHT_R);
ll=LIGHT(LIGHT_L);
lr=colour(lr+(CORR));
ll=colour(ll+(CORL));
//Aktionen festlegen
//"normale Fahrt"
if (old==ON_LINE){
if(debug1) cputs("ONLN");
if((ll==BLUE)&&(lr==BLUE)){
if(route[ptr]==5){ //Enderkennung anhand entsprechendem Arraywert
notjuhu=0;
current=ENTE;
}
else {
current=ON_BLUE; //Blau erkannt...
}
}
else if ((ll!=GREEN)&&(lr!=GREEN)){ //Korrekturbewegungen
if(debug1) cputs("ONL");
current=ON_LINE;
}
else if((ll==GREEN)&&(lr!=GREEN)){
current=LEFT_OFF;
}
else if ((ll!=GREEN)&&(lr==GREEN)){
current=RIGHT_OFF;
}
}
else if (old==LEFT_OFF){
if((ll!=GREEN)&&(lr!=GREEN)){
current=ON_LINE;
}
}
else if (old==RIGHT_OFF){
if((ll!=GREEN)&&(lr!=GREEN)){
current=ON_LINE;
}
}
//Auffahren auf Kreuzung
else if ((old==ON_BLUE)||(old==ON_BLUE_LEFT)||(old==ON_BLUE_RIGHT)){
if(debug1) cputs("BLU");
if((ll==WHITE)||(lr==WHITE)){ //nach l&r=blau. l&r=weiß -> Kreuzung
if(route[ptr]==2) //aus Array die Abbiegerichtung lesen
{ current=CROSSING_RIGHT;
ptr++;
}
else if(route[ptr]==1)
{ current=CROSSING_LEFT;
ptr++;
}
}
else if((ll!=GREEN)&&(lr==GREEN)){ //Korrekturbewegungen
current=ON_BLUE_RIGHT;
}
else if((ll==GREEN)&&(lr!=GREEN)){
current=ON_BLUE_LEFT;
}
else{
current=ON_BLUE;
}
}
//in der Kreuzung - Linksabbiegen
else if((old==CROSSING_LEFT)||(old==CROSSING_LEFT_LEFT_OFF)||(old==CROSSING_LEFT_RIGHT_OFF)){
if(debug1) cputs("CRL");
if((ll==GREEN)&&(lr==BLUE)){ //Wechsel zu Kreuzungsendtest
current=CHECK_BLUE_LEFT;
}
else if((ll==GREEN)&&(lr!=GREEN)){ //Korrekturbewegungen
current=CROSSING_LEFT;
}
else if((ll!=GREEN)&&(lr!=GREEN)){
current=CROSSING_LEFT_RIGHT_OFF;
}
else if((ll==GREEN)&&(lr==GREEN)){
current=CROSSING_LEFT_LEFT_OFF;
}
}
//in der Kreuzung - Rechtsabbiegen
else if((old==CROSSING_RIGHT)||(old==CROSSING_RIGHT_LEFT_OFF)||(old==CROSSING_RIGHT_RIGHT_OFF)){
if(debug1) cputs("CRR");
if((ll==BLUE)&&(lr==GREEN)){ //Wechsel zu Kreuzungsendtest
current=CHECK_BLUE_RIGHT;
}
else if((ll!=GREEN)&&(lr==GREEN)){ //Korrekturbewegungen
current=CROSSING_RIGHT;
}
else if((ll!=GREEN)&&(lr!=GREEN)){
current=CROSSING_RIGHT_LEFT_OFF;
}
else if((ll==GREEN)&&(lr==GREEN)){
current=CROSSING_RIGHT_RIGHT_OFF;
}
}
//Verlassen der Kreuzung
else if ((old==EXIT_CROSSING)||(old==EXIT_CROSSING_RIGHT)||(old==EXIT_CROSSING_LEFT)){
if(debug1) cputs("EXC");
if((ll==WHITE)&&(lr==WHITE)){ //wenn statt bunt- weiß, dann Kreuzung zuende
current=ON_LINE;
}
else if((ll!=GREEN)&&(lr==GREEN)){ //Korrekturbewegungen
current=EXIT_CROSSING_RIGHT;
}
else if((ll==GREEN)&&(lr!=GREEN)){
current=EXIT_CROSSING_LEFT;
}
else{
current=EXIT_CROSSING;
}
}
//Aktionen starten entsprechend des vorliegenden Zustands (selbsterklärend ;-})
if(current==ON_LINE){
forwrd(NORMAL_SPEED);
}
else if((current==LEFT_OFF)||(current==CROSSING_LEFT_LEFT_OFF)||(current==CROSSING_RIGHT_LEFT_OFF)||(current==EXIT_CROSSING_LEFT)){
trn_rgt(MAX_SPEED/4, MAX_SPEED/2);
}
else if((current==RIGHT_OFF)||(current==CROSSING_RIGHT_RIGHT_OFF)||(current==CROSSING_LEFT_RIGHT_OFF)||(current==EXIT_CROSSING_RIGHT)){
trn_lft(MAX_SPEED/4, MAX_SPEED/2);
}
else if((current==CROSSING_RIGHT)||(current==CROSSING_LEFT)){
forwrd(SLOW_SPEED/2);
}
else if(current==ON_BLUE_RIGHT){
trn_lft(MAX_SPEED/8, MAX_SPEED/3);
}
else if(current==ON_BLUE_LEFT){
trn_rgt(MAX_SPEED/8, MAX_SPEED/3);
}
else if((current==ON_BLUE)||(current==EXIT_CROSSING)){
forwrd(SLOW_SPEED);
}
else if(current==CHECK_BLUE_LEFT){ //testen, ob angezeigtes Kreuzungsende wirklich eins ist (Grün+Weiß=blau-Problem)
ll=0; //kurzzeitiges Verlassen der Zustandssteuerung
lr=0; //beide Sensoren werden auf Bahn gelenkt und getestet, ob beide blau lesen
if(debug1) cputs("CHKL");
motor_a_dir(brake);
trn_rgt(0, MAX_SPEED/3);
while(ll<80){
ll=LIGHT(LIGHT_L)+(CORL);
lr=LIGHT(LIGHT_R)+(CORR);
if(IS_BLUE(lr)&&IS_BLUE(ll)) {
current=EXIT_CROSSING;
}
else
{ current=old; //wenn Kreuzung noch nicht verlassen wurde, schaltet er in den alten Zustand zurück...
}
}
stop();
}
else if(current==CHECK_BLUE_RIGHT){ //wie oben, nur diesmal beim Rechtsabbiegen...
ll=0;
lr=0;
if(debug1) cputs("CHKR");
motor_a_dir(brake);
trn_lft(0, MAX_SPEED/3);
while(lr<80){
ll=LIGHT(LIGHT_L)+(CORL);
lr=LIGHT(LIGHT_R)+(CORR);
if(IS_BLUE(lr)&&IS_BLUE(ll)) {
current=EXIT_CROSSING;
}
else
{ current=old;
}
}
stop();
}
}
stop();
cputs("Ziel");
return 0;
}