diff options
Diffstat (limited to 'source/ct-Bot/bot-logic/behaviour_solve_maze.c')
| -rw-r--r-- | source/ct-Bot/bot-logic/behaviour_solve_maze.c | 660 |
1 files changed, 660 insertions, 0 deletions
diff --git a/source/ct-Bot/bot-logic/behaviour_solve_maze.c b/source/ct-Bot/bot-logic/behaviour_solve_maze.c new file mode 100644 index 0000000..5eb2fc4 --- /dev/null +++ b/source/ct-Bot/bot-logic/behaviour_solve_maze.c @@ -0,0 +1,660 @@ +/* + * c't-Bot + * + * This program is free software; you can redistribute it + * and/or modify it under the terms of the GNU General + * Public License as published by the Free Software + * Foundation; either version 2 of the License, or (at your + * option) any later version. + * This program is distributed in the hope that it will be + * useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + * PURPOSE. See the GNU General Public License for more details. + * You should have received a copy of the GNU General Public + * License along with this program; if not, write to the Free + * Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, + * MA 02111-1307, USA. + * + */ + +/*! @file behaviour_solve_maze.c + * @brief Wandfolger durchs Labyrinth + * + * @author Benjamin Benz (bbe@heise.de) + * @date 03.11.06 +*/ + +#include "bot-logic/bot-logik.h" +#include <math.h> +#include <stdlib.h> +#include "log.h" + +#ifdef BEHAVIOUR_SOLVE_MAZE_AVAILABLE + +/* Parameter fuer das check_wall_behaviour() */ +int8 wall_detected; /*!< enthaelt True oder False, je nach Ergebnis des Verhaltens */ +int8 check_direction; /*!< enthaelt CHECK_WALL_LEFT oder CHECK_WALL_RIGHT */ +int16 wall_distance; /*!< enthaelt gemessene Entfernung */ + +/* Konstanten fuer check_wall_behaviour-Verhalten */ +#define CHECK_WALL_RIGHT 0 +#define CHECK_WALL_LEFT 1 + +/* Parameter fuer das measure_angle_behaviour() */ +int8 measure_direction; /*!< enthaelt MEASURE_RIGHT oder MEASURE_LEFT */ +int16 measure_distance; /*!< enthaelt maximale Messentfernung, enthaelt nach der Messung die Entfernung */ +int16 measured_angle; /*!< enthaelt gedrehten Winkel oder 0, falls nichts entdeckt */ +#ifdef MEASURE_MOUSE_AVAILABLE + int16 start_heading; /*!< Blickwinkel des Bots zu Beginn der Messung */ +#else + int16 startEncL; /*!< enthaelt Encoderstand zu Beginn der Messung */ + int16 startEncR; /*!< enthaelt Encoderstand zu Beginn der Messung */ +#endif +/* Konstanten fuer measure_angle_behaviour-Verhalten */ +#define MEASURE_LEFT 1 +#define MEASURE_RIGHT -1 + + +/*! + * Das Verhalten dreht sich um 45 Grad in die angegebene Richtung (0=rechts, 1=links) + * und prueft, ob auf dem Sensor auf der Seite der angegebenen Richtung eine Wand + * im Abstand von 12cm zu sehen ist. Wenn dem so ist, wird die Variable wall_present + * auf True gesetzt, sonst False */ +void bot_check_wall_behaviour(Behaviour_t *data) { + /* Konstantenfuer check_wall_behaviour-Verhalten */ + #define CORRECT_NEAR 0 + #define CORRECT_NONE 1 + #define CORRECT_FAR 2 + /* Zustaende fuer check_wall_behaviour-Verhalten */ + #define CHECK_WALL_TURN 0 + #define CHECK_WALL_SENS 1 + #define CHECK_WALL_TURN_BACK 2 + #define CHECK_WALL_FINISHED 3 + #define CHECK_WALL_CORRECT 4 + + static int8 checkState=CHECK_WALL_TURN; + /* wenn die Wand noch da ist aber aus dem Blickfeld rueckt, Entfernung und Winkel korrigieren */ + static int8 correctDistance=CORRECT_NONE; + /* letzte, gueltige Distanz fuer Abweichungsberechnung */ + static int16 lastDistance=0; + /* enthaelt anzahl der +/-5 identischen Messungen */ + static int8 measureCount=0; + /* letzter Messwert */ + static int16 lastSensor=0; + + int16 sensor; /* fuer temporaer benutzte Senorwerte */ + + switch(checkState) { + case CHECK_WALL_TURN: + /* haben wir links oder rechts eine Wand? */ + if (check_direction==CHECK_WALL_LEFT) { + checkState=CHECK_WALL_SENS; + bot_turn(data,45); + break; + } else { + checkState=CHECK_WALL_SENS; + bot_turn(data,-45); + break; + } + + case CHECK_WALL_SENS: + if (check_direction==CHECK_WALL_LEFT) { + sensor=sensDistL; + } else { + sensor=sensDistR; + } + /* dafuer sorgen, dass wir nur verlaessliche Werte haben + * dazu muss der wert dreimal nacheinander max. um +/- 5 + * unterschiedlich sein */ + if (measureCount==0) { + lastSensor=sensor; + measureCount++; + break; + } + if (sensor>=lastSensor-5 && sensor<=lastSensor+5 && measureCount<4) { + /* Messwert ist ok */ + measureCount++; + break; + } else if (measureCount<4) { + /* Messwert weicht zu doll ab -> von Neuem messen */ + measureCount=0; + break; + } + /* ok, wir hatten drei Messungen mit nahezu identischen Werten */ + + /* keine wand in eingestellter Maximalentfernung? */ + if (sensor>IGNORE_DISTANCE) { + correctDistance=CORRECT_NONE; /* dann auch keine Korrektur */ + lastDistance=0; /* auch kein Vergleichswert */ + wall_detected=False; /* keine Wand da */ + } else if (sensor<OPTIMAL_DISTANCE-ADJUST_DISTANCE) { + /* bot hat den falschen Abstand zur Wand, zu nah dran */ + wall_detected=True; + correctDistance=CORRECT_NEAR; + } else if (sensor>OPTIMAL_DISTANCE+ADJUST_DISTANCE) { + /* bot hat den falschen Abstand zur Wand, zu weit weg */ + wall_detected=True; + correctDistance=CORRECT_FAR; + } else { + /* perfekter Abstand */ + correctDistance=CORRECT_NONE; + wall_detected=True; + } + wall_distance=sensor; + /* Wenn Korrektur noetig, dann durchfuehren, sonst gleich zurueckdrehen */ + if (correctDistance==CORRECT_NONE) checkState=CHECK_WALL_TURN_BACK; + else checkState=CHECK_WALL_CORRECT; + break; + + + case CHECK_WALL_TURN_BACK: + checkState=CHECK_WALL_FINISHED; + int16 turnAngle=45; + + /* welcher Sensorwert wird gebraucht? */ + if (check_direction==CHECK_WALL_LEFT) { + sensor=sensDistL; + } else { + sensor=sensDistR; + } + /* wenn vorhanden, aus letztem Abstand und aktuellem Abstand + * Korrekturwinkel berechnen, falls vorheriger Abstand da. + * wird durch Abstand>IGNORE_DISTANCE zurueckgesetzt */ + if (lastDistance!=0) { + turnAngle=turnAngle+(lastDistance-wall_distance)/10; + } + if (sensor<IGNORE_DISTANCE) lastDistance=sensor; + if (check_direction==CHECK_WALL_LEFT) { + bot_turn(data,-turnAngle); + } else { + bot_turn(data,turnAngle); + } + break; + + case CHECK_WALL_FINISHED: + /* ok, check beendet -> Verhalten verlassen */ + checkState=CHECK_WALL_TURN; + return_from_behaviour(data); + break; + + case CHECK_WALL_CORRECT: + /* Abhaengig von der Seite, auf der die Wand ist, Entfernung korrigieren */ + if (check_direction==CHECK_WALL_LEFT) { + sensor=sensDistL; + } else { + sensor=sensDistR; + } + if (sensor-OPTIMAL_DISTANCE<0) { + speedWishLeft=-BOT_SPEED_SLOW; + speedWishRight=-BOT_SPEED_SLOW; + } else if (sensor-OPTIMAL_DISTANCE>0) { + speedWishLeft=BOT_SPEED_SLOW; + speedWishRight=BOT_SPEED_SLOW; + } + else { + checkState=CHECK_WALL_TURN_BACK; + speedWishLeft=BOT_SPEED_STOP; + speedWishRight=BOT_SPEED_STOP; + } + break; + + + default: + checkState=CHECK_WALL_TURN; + return_from_behaviour(data); + break; + } +} + + +/*! + * Das Verhalten dreht sich um 45 Grad in die angegebene Richtung (0=rechts, 1=links) + * und prueft, ob auf dem Sensor auf der Seite der angegebenen Richtung eine Wand + * im Abstand von 12-22cm zu sehen ist. Wenn dem so ist, wird die Variable wall_present + * auf True gesetzt, sonst False */ +void bot_check_wall(Behaviour_t *caller,int8 direction) { + check_direction=direction; + wall_detected=False; + switch_to_behaviour(caller, bot_check_wall_behaviour,NOOVERRIDE); +} + +#ifdef MEASURE_MOUSE_AVAILABLE + /*! + * Das Verhalten dreht den Bot in die angegebene Richtung bis ein Hindernis + * im Sichtbereich erscheint, das eine Entfernung bis max. zur angegebenen + * Distanz zum Bot hat. + */ + + void bot_measure_angle_behaviour(Behaviour_t *caller) { + /* Zustaende measure_angle_behaviour-Verhalten */ + #define MEASURE_TURN 0 + #define FOUND_OBSTACLE 1 + #define MEASUREMENT_DONE 2 + + static int8 measureState=MEASURE_TURN; + + /* enthaelt anzahl der +/-5 identischen Messungen */ + static int8 measureCount=0; + /* letzter Messwert */ + static int16 lastSensor=0; + + /* bereits gedrehten Winkel */ + int16 turned_angle=0; + if (measure_direction>0) { + if ((int16)heading_mou<start_heading) { + /* war ein ueberlauf */ + turned_angle=360-start_heading+(int16)heading_mou; + } else { + /* sonst normale differenz berechnen */ + turned_angle=(int16)heading_mou-start_heading; + } + } else { + if ((int16)heading_mou>start_heading) { + /* war ein ueberlauf */ + turned_angle=360-(int16)heading_mou+start_heading; + } else { + turned_angle=start_heading-(int16)heading_mou; + } + + } + + /* sensorwert abhaengig von der Drehrichtung abnehmen */ + int16 sensor=(measure_direction==MEASURE_LEFT)?sensDistL:sensDistR; + /* solange drehen, bis Hindernis innerhalb Messstrecke oder 360 Grad komplett */ + switch(measureState){ + case MEASURE_TURN: + /* nicht mehr als eine komplette Drehung machen! */ + if (turned_angle>=360) { + measure_direction=-measure_direction; + measureState=MEASUREMENT_DONE; + bot_turn(caller,measure_direction*turned_angle); + measured_angle=0; /* kein Hindernis gefunden */ + break; + } + /* Ist ein Objekt in Reichweite? */ + if (sensor<=measure_distance) { + speedWishLeft=BOT_SPEED_STOP; + speedWishRight=BOT_SPEED_STOP; + measureState=FOUND_OBSTACLE; + break; + } + /* Beginnen, zurueckzudrehen */ + speedWishLeft = (measure_direction > 0) ? -BOT_SPEED_FOLLOW : BOT_SPEED_FOLLOW; + speedWishRight = (measure_direction > 0) ? BOT_SPEED_FOLLOW : -BOT_SPEED_FOLLOW; + break; + + case FOUND_OBSTACLE: + /* dafuer sorgen, dass wir nur verlaessliche Werte haben + * dazu muss der wert dreimal nacheinander max. um +/- 5 + * unterschiedlich sein */ + if (measureCount==0) { + lastSensor=sensor; + measureCount++; + break; + } + if (sensor>=lastSensor-5 && sensor<=lastSensor+5 && measureCount<4) { + /* Messwert ist ok */ + measureCount++; + break; + } else if (measureCount<4) { + /* Messwert weicht zu doll ab -> von Neuem messen */ + measureCount=0; + break; + } + /* ok, wir hatten drei Messungen mit nahezu identischen Werten */ + measure_distance=sensor; + /* Hindernis gefunden, nun Bot wieder in Ausgangsstellung drehen */ + measure_direction=-measure_direction; + measured_angle=turned_angle; + measureState=MEASUREMENT_DONE; + bot_turn(caller,measure_direction*turned_angle); + break; + + case MEASUREMENT_DONE: + measureState=MEASURE_TURN; + measureCount=0; + return_from_behaviour(caller); + break; + } + } + + /*! + * Das Verhalten dreht den Bot in die angegebene Richtung bis ein Hindernis + * im Sichtbereich erscheint, das eine Entfernung bis max. zur angegebenen + * Distanz zum Bot hat. + */ + + void bot_measure_angle(Behaviour_t *caller, int8 direction, int16 distance) { + /* maximale Messentfernung und Richtung setzen */ + measure_direction=direction; + measure_distance=distance; + /* Heading zu Anfang des Verhaltens merken */ + start_heading=(int16)heading_mou; + switch_to_behaviour(caller, bot_measure_angle_behaviour,NOOVERRIDE); + } +#else +/*! + * Das Verhalten dreht den Bot in die angegebene Richtung bis ein Hindernis + * im Sichtbereich erscheint, das eine Entfernung bis max. zur angegebenen + * Distanz zum Bot hat. + */ + +void bot_measure_angle_behaviour(Behaviour_t *caller) { + /* Zustaende measure_angle_behaviour-Verhalten */ + #define MEASURE_TURN 0 + #define FOUND_OBSTACLE 1 + #define TURN_COMPLETED 2 + #define TURN_BACK 3 + #define CORRECT_ANGLE 4 + #define MEASUREMENT_DONE 5 + + static int8 measureState=MEASURE_TURN; + + /* bereits gedrehte Strecke errechnen */ + int16 turnedLeft=(measure_direction>0)?-(sensEncL-startEncL):(sensEncL-startEncL); + int16 turnedRight=(measure_direction>0)?(sensEncR-startEncR):-(sensEncR-startEncR); + int16 turnedSteps=(abs(turnedLeft)+abs(turnedRight))/2; + + /* sensorwert abhaengig von der Drehrichtung abnehmen */ + int16 sensor=(measure_direction==MEASURE_LEFT)?sensDistL:sensDistR; + /* solange drehen, bis Hindernis innerhalb Messstrecke oder 360� komplett */ + switch(measureState){ + case MEASURE_TURN: + /* nicht mehr als eine komplette Drehung machen! */ + if (turnedSteps>=ANGLE_CONSTANT) { + speedWishLeft=BOT_SPEED_STOP; + speedWishRight=BOT_SPEED_STOP; + measureState=TURN_COMPLETED; + measured_angle=0; /* kein Hindernis gefunden */ + break; + } + /* Ist ein Objekt in Reichweite? */ + if (sensor<=measure_distance) { + measure_distance=sensor; + speedWishLeft=BOT_SPEED_STOP; + speedWishRight=BOT_SPEED_STOP; + measureState=FOUND_OBSTACLE; + break; + } + /* Beginnen, zurueckzudrehen */ + speedWishLeft = (measure_direction > 0) ? -BOT_SPEED_SLOW : BOT_SPEED_SLOW; + speedWishRight = (measure_direction > 0) ? BOT_SPEED_SLOW : -BOT_SPEED_SLOW; + break; + + case FOUND_OBSTACLE: + /* Hindernis gefunden, nun Bot wieder in Ausgangsstellung drehen */ + measure_direction=-measure_direction; + measured_angle=(int16)((long)(turnedSteps*360)/ANGLE_CONSTANT); + measureState=TURN_BACK; + speedWishLeft = (measure_direction > 0) ? -BOT_SPEED_SLOW : BOT_SPEED_SLOW; + speedWishRight = (measure_direction > 0) ? BOT_SPEED_SLOW : -BOT_SPEED_SLOW; + break; + + case TURN_COMPLETED: + /* bot steht wieder in Ausgangsrichtung, Verhalten beenden */ + speedWishLeft=BOT_SPEED_STOP; + speedWishRight=BOT_SPEED_STOP; + measureState=CORRECT_ANGLE; + break; + + case TURN_BACK: + /* Bot in Ausgangsposition drehen */ + if ((turnedLeft+turnedRight)/2>=0) { + speedWishLeft=BOT_SPEED_STOP; + speedWishRight=BOT_SPEED_STOP; + measureState=TURN_COMPLETED; + break; + } + speedWishLeft = (measure_direction > 0) ? -BOT_SPEED_SLOW : BOT_SPEED_SLOW; + speedWishRight = (measure_direction > 0) ? BOT_SPEED_SLOW : -BOT_SPEED_SLOW; + break; + + case CORRECT_ANGLE: + /* Evtl. etwas zuruecksetzen, falls wir zu weit gefahren sind */ + if (turnedRight>0) { + /* rechts zu weit gefahren..langsam zurueck */ + speedWishRight = (measure_direction > 0) ? -BOT_SPEED_SLOW : BOT_SPEED_SLOW; + } else if (turnedRight<0) { + /* rechts noch nicht weit genug...langsam vor */ + speedWishRight = (measure_direction > 0) ? BOT_SPEED_SLOW : -BOT_SPEED_SLOW; + } else { + /* Endposition erreicht, rechtes Rad anhalten */ + speedWishRight = BOT_SPEED_STOP; + } + if (turnedLeft<0) { + /* links zu weit gefahren..langsam zurueck */ + speedWishLeft = (measure_direction > 0) ? -BOT_SPEED_SLOW : BOT_SPEED_SLOW; + } else if (turnedLeft>0) { + /* links noch nicht weit genug...langsam vor */ + speedWishLeft = (measure_direction > 0) ? BOT_SPEED_SLOW : -BOT_SPEED_SLOW; + } else { + /* Endposition erreicht, linkes Rad anhalten */ + speedWishLeft = BOT_SPEED_STOP; + } + if (speedWishLeft == BOT_SPEED_STOP && speedWishRight == BOT_SPEED_STOP) { + /* beide Raeder haben nun wirklich die Endposition erreicht, daher anhalten */ + measureState=MEASUREMENT_DONE; + } + break; + + case MEASUREMENT_DONE: + measureState=MEASURE_TURN; + return_from_behaviour(caller); + break; + } +} + +/*! + * Das Verhalten dreht den Bot in die angegebene Richtung bis ein Hindernis + * im Sichtbereich erscheint, das eine Entfernung bis max. zur angegebenen + * Distanz zum Bot hat. + */ + +void bot_measure_angle(Behaviour_t *caller, int8 direction, int16 distance) { + /* maximale Messentfernung und Richtung setzen */ + measure_direction=direction; + measure_distance=distance; + /* Encoderwerte zu Anfang des Verhaltens merken */ + startEncL=sensEncL; + startEncR=sensEncR; + switch_to_behaviour(caller, bot_measure_angle_behaviour,NOOVERRIDE); +} +#endif +/*! + * Das Verhalten findet seinen Weg durch ein Labyrinth, das nach gewissen Grundregeln gebaut ist + * in nicht immer optimaler Weise aber in jedem Fall. Es arbeitet nach dem Hoehlenforscher-Algorithmus. + * Einschraenkung: Objekte im Labyrinth, die Endlossschleifen verursachen koennen, z.b. ein einzeln + * stehender Pfeiler im Labyrinth um den der Bot dann immer wieder herum fahren wuerde. + */ +void bot_solve_maze_behaviour(Behaviour_t *data){ + /* Zustaende fuer das bot_solve_maze_behaviour-Verhalten */ + #define CHECK_FOR_STARTPAD 0 + #define CHECK_FOR_WALL_RIGHT 1 + #define CHECK_FOR_WALL_LEFT 2 + #define CHECK_WALL_PRESENT 3 + #define SOLVE_MAZE_LOOP 4 + #define SOLVE_TURN_WALL 5 + #define CHECK_CONDITION 6 + #define TURN_TO_BRANCH 7 + #define DETECTED_CROSS_BRANCH 8 + #define APPROACH_CORNER 9 + #define AVOID_ABYSS 10 + #define REACHED_GOAL 11 + static int8 mazeState=CHECK_FOR_STARTPAD; + static int8 followWall=-1; + static int8 checkedWalls=0; + + int16 distance; + double x; + switch(mazeState) { + case CHECK_FOR_STARTPAD: + /* Wo beginnen wir, nach einer Wand zu suchen? + * Abgrund- und Kollisions-Verhalten ausschalten */ + #ifdef BEHAVIOUR_AVOID_COL_AVAILABLE + deactivateBehaviour(bot_avoid_col_behaviour); + #endif + #ifdef BEHAVIOUR_AVOID_BORDER_AVAILABLE + deactivateBehaviour(bot_avoid_border_behaviour); + #endif + /* sieht nach, ob der Bot auf einem definierten Startpad steht und + * beginnt dann mit der Suche gleich an der richtigen Wand */ + /* Zuserst bei nach Startpad1 gucken */ + checkedWalls=0; + if ((sensLineL>=STARTPAD1-10 && sensLineL<=STARTPAD1+10) || + (sensLineR>=STARTPAD1-10 && sensLineR<=STARTPAD1+10)) { + mazeState=CHECK_FOR_WALL_LEFT; + break; + } + mazeState=CHECK_FOR_WALL_RIGHT; + break; + + case CHECK_FOR_WALL_RIGHT: + + mazeState=CHECK_WALL_PRESENT; + followWall=CHECK_WALL_RIGHT; + bot_check_wall(data,followWall); + break; + + case CHECK_FOR_WALL_LEFT: + + followWall=CHECK_WALL_LEFT; + bot_check_wall(data,followWall); + mazeState=CHECK_WALL_PRESENT; + break; + + + case CHECK_WALL_PRESENT: + /* wenn keine Wand gefunden aber links noch nicht nachgesehen, andere + * Richtung checken, sonst vorfahren */ + checkedWalls++; + if (wall_detected==False){ + /* Wand noch nicht entdeckt...haben wir schon beide gecheckt? */ + if (checkedWalls<2) { + if (followWall==CHECK_WALL_RIGHT) { + mazeState=CHECK_FOR_WALL_LEFT; + break; + } else { + mazeState=CHECK_FOR_WALL_RIGHT; + break; + } + } else { + /* keine wand? dann vorfahren und selbes prozedere nochmal */ + bot_drive_distance(data,0,BOT_SPEED_NORMAL,BOT_DIAMETER); + mazeState=CHECK_FOR_WALL_RIGHT; + checkedWalls=0; + break; + } + } + /* ok, wir haben unsere richtung im labyrinth gefunden jetzt dieser + * nur noch folgen bis Ziel, Abzweig oder Abgrund */ + mazeState=SOLVE_MAZE_LOOP; + break; + + case SOLVE_MAZE_LOOP: + /* Einen Schritt (=halbe BOT-Groesse) vorwaerts */ + mazeState=SOLVE_TURN_WALL; + bot_drive_distance(data,0,BOT_SPEED_NORMAL,BOT_DIAMETER); + break; + + case SOLVE_TURN_WALL: + /* Ziel erreicht? */ + if ((sensLineL>GROUND_GOAL-20 && sensLineL<GROUND_GOAL+20) || + (sensLineR>GROUND_GOAL-20 && sensLineR<GROUND_GOAL+20)) { + /* Bot hat Ziel erreicht...aus Freude einmal um die Achse drehen */ + bot_turn(data,360); + mazeState=REACHED_GOAL; + break; + } + /* checken, ob wand vor uns (Abstand 2.5*Bot-Durchmesser in mm) */ + distance=(sensDistL+sensDistR)/2; + if (distance<=25*BOT_DIAMETER) { // umrechnen 10*BOT_DIAMETER, da letzteres in cm angegeben wird + /* berechnete Entfernung zur Wand abzueglich optimale Distanz fahren */ + mazeState=DETECTED_CROSS_BRANCH; + bot_drive_distance(data,0,BOT_SPEED_NORMAL,(distance-OPTIMAL_DISTANCE)/10); + break; + } + /* Zur Wand drehen....ist die Wand noch da? */ + mazeState=CHECK_CONDITION; + bot_check_wall(data,followWall); + break; + + case CHECK_CONDITION: + /* Solange weiter, wie die Wand zu sehen ist */ + if (wall_detected==True) { + mazeState=SOLVE_MAZE_LOOP; + break; + } + /* messen, wo genau die Ecke ist */ + mazeState=APPROACH_CORNER; + if (followWall==CHECK_WALL_LEFT){ + bot_measure_angle(data,MEASURE_LEFT,300); + } else { + bot_measure_angle(data,MEASURE_RIGHT,300); + } + break; + + case TURN_TO_BRANCH: + /* nun in Richtung Abzweig drehen , dann mit Hauptschleife weiter*/ + mazeState=SOLVE_MAZE_LOOP; + if (followWall==CHECK_WALL_RIGHT) { + bot_turn(data,-90); + } else { + bot_turn(data,90); + } + break; + + case DETECTED_CROSS_BRANCH: + /* Bot faehrt auf eine Ecke zu, in Richtung Gang drehen */ + if (followWall==CHECK_WALL_LEFT) { + mazeState=SOLVE_MAZE_LOOP; + bot_turn(data,-90); + } else { + mazeState=SOLVE_MAZE_LOOP; + bot_turn(data,90); + } + break; + + case APPROACH_CORNER: + /* ok, nun strecke bis zur Kante berechnen */ + x=measure_distance*cos(measured_angle*M_PI/180)/10+BOT_DIAMETER*1.5; + mazeState=TURN_TO_BRANCH; + bot_drive_distance(data,0,BOT_SPEED_NORMAL,(int16)x); + break; + + case REACHED_GOAL: + mazeState=CHECK_WALL_RIGHT; + speedWishLeft=BOT_SPEED_STOP; + speedWishRight=BOT_SPEED_STOP; + return_from_behaviour(data); + break; + } +} + + +/*! + * Das Verhalten findet seinen Weg durch ein Labyrinth, das nach gewissen Grundregeln gebaut ist + * in nicht immer optimaler Weise aber in jedem Fall. Es arbeitet nach dem Hoehlenforscher-Algorithmus. + * Einschraenkung: Objekte im Labyrinth, die Endlossschleifen verursachen koennen, z.b. ein einzeln + * stehender Pfeiler im Labyrinth um den der Bot dann immer wieder herum fahren wuerde. + */ + +void bot_solve_maze(Behaviour_t *caller){ + LOG_DEBUG(("bot_solve_maze()")); + switch_to_behaviour(caller, bot_solve_maze_behaviour,NOOVERRIDE); +} + +/*! + * Initialisiert den Hoelenforscher + * @param prio_main Prioritaet des Hoehlenforschers (typ. 100) + * @param prio_helper Prioritaet der Hilfsfunktionen (typ. 42) + * @param active ACTIVE wenn der hoehlenforcher sofort starten soll, sonst INACTIVE + */ +void bot_solve_maze_init(int8 prio_main,int8 prio_helper, int8 active){ + // Verhalten, um ein Labyrinth nach der Hoehlenforscher-Methode loesen + insert_behaviour_to_list(&behaviour, new_behaviour(prio_main, bot_solve_maze_behaviour,active)); + + insert_behaviour_to_list(&behaviour, new_behaviour(prio_helper--, bot_measure_angle_behaviour,INACTIVE)); + insert_behaviour_to_list(&behaviour, new_behaviour(prio_helper, bot_check_wall_behaviour,INACTIVE)); + +} +#endif |