diff options
Diffstat (limited to 'source/ct-Bot/bot-logic/behaviour_olympic.c')
| -rw-r--r-- | source/ct-Bot/bot-logic/behaviour_olympic.c | 428 |
1 files changed, 428 insertions, 0 deletions
diff --git a/source/ct-Bot/bot-logic/behaviour_olympic.c b/source/ct-Bot/bot-logic/behaviour_olympic.c new file mode 100644 index 0000000..2c6e853 --- /dev/null +++ b/source/ct-Bot/bot-logic/behaviour_olympic.c @@ -0,0 +1,428 @@ +/* + * 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_olympic.c + * @brief Bot sucht saeulen und faehrt dann slalom + * + * @author Benjamin Benz (bbe@heise.de) + * @date 03.11.06 +*/ + +#include "bot-logic/bot-logik.h" + +#ifdef BEHAVIOUR_OLYMPIC_AVAILABLE +#include <stdlib.h> + +/* Zustaende des bot_explore-Verhaltens */ + +#define EXPLORATION_STATE_GOTO_WALL 1 /*!< Zustand: Bot sucht eine Wand o.ae. Hinderniss */ +#define EXPLORATION_STATE_TURN_PARALLEL_LEFT 2 /*!< Zustand: Bot dreht sich nach links, bis er parallel zur Wand blickt. */ +#define EXPLORATION_STATE_TURN_PARALLEL_RIGHT 3 /*!< Zustand: Bot dreht sich nach rechts, bis er parallel zur Wand blickt. */ +#define EXPLORATION_STATE_DRIVE_PARALLEL_LEFT 4 /*!< Zustand: Bot faehrt parallel zur Wand links von sich. */ +#define EXPLORATION_STATE_DRIVE_PARALLEL_RIGHT 5 /*!< Zustand: Bot faehrt parallel zur Wand rechts von sich. */ +#define EXPLORATION_STATE_TURN_ORTHOGONAL_LEFT 6 /*!< Zustand: Bot dreht sich nach links, bis er senkrecht zur Wand steht. */ +#define EXPLORATION_STATE_TURN_ORTHOGONAL_RIGHT 7 /*!< Zustand: Bot dreht sich nach rechts, bis er senkrecht zur Wand steht. */ +#define EXPLORATION_STATE_DRIVE_ARC 8 /*!< Zustand: Bot faehrt einen Bogen. Der Winkel des Bogens sollte in einer + *!< weiteren static Variablen (z.B. curve) gespeichert sein. */ + +/* Zustaende des bot_olympic_behaviour-Verhaltens */ + +#define CB_STATE_EXPLORATION 0 /*!< Zustand: Bot erforscht die Umgebung. */ +#define CB_STATE_DOING_SLALOM 1 /*!< Zustand: Bot ist dabei Slalom zu fahren. */ + +/* Zustaende des bot_do_slalom-Verhaltens */ + +#define SLALOM_STATE_START 0 /*!< Zustand: Bot startet eine Slalomlauf und positioniert sich vor der Saeule. */ +#define SLALOM_STATE_TURN_1 1 /*!< Zustand: Bot dreht sich um 90�. */ +#define SLALOM_STATE_DRIVE_ARC 2 /*!< Zustand: Bot faehrt den Bogen um die Saeule. */ +#define SLALOM_STATE_TURN_2 3 /*!< Zustand: Bot dreht sich fuer den Sweep um 45�. */ +#define SLALOM_STATE_SWEEP_RUNNING 4 /*!< Zustand: Bot macht den Sweep. */ +#define SLALOM_STATE_SWEEP_DONE 5 /*!< Zustand: Bot ist fertig mit dem Sweep. */ +#define SLALOM_STATE_CHECK_PILLAR 6 /*!< Zustand: Bot ueberprueft, ob er den Slalom fortsetzen kann. */ + +#define SLALOM_ORIENTATION_LEFT 0 +#define SLALOM_ORIENTATION_RIGHT 1 + +/* Parameter fuer das bot_explore_behaviour() */ +int8 (*exploration_check_function)(void); /*!< Die Funktion, mit der das bot_explore_behaviour() feststellt, ob es etwas gefunden hat. + * Die Funktion muss True (1) zurueck geben, wenn dem so ist, sonst False (0). + * Beispiele fuer eine solche Funktion sind check_for_light, is_good_pillar_ahead etc.*/ + + + + +/*! + * Das Verhalten dreht den Bot so, dass er auf eine Lichtquelle zufaehrt. */ +void bot_goto_light(void){ + int16 speed, curve = (sensLDRL - sensLDRR)/1.5; + + if(curve < -127) curve = -127; + if(curve > 127) curve = 127; + + if(abs(sensLDRL - sensLDRR) < 20){ + speed = BOT_SPEED_MAX; + }else if(abs(sensLDRL - sensLDRR) < 150) { + speed = BOT_SPEED_FAST; + }else { + speed = BOT_SPEED_NORMAL; + } + + bot_drive(curve, speed); +} + + + +/*! + * Gibt aus, ob der Bot Licht sehen kann. + * @return True, wenn der Bot Licht sieht, sonst False. */ +int8 check_for_light(void){ + // Im Simulator kann man den Bot gut auf den kleinsten Lichtschein + // reagieren lassen, in der Realitaet gibt es immer Streulicht, so dass + // hier ein hoeherer Schwellwert besser erscheint. + // Simulator: + if(sensLDRL >= 1023 && sensLDRR >= 1023) return False; + // Beim echten Bot eher: + // if(sensLDRL >= 100 && sensLDRR >= 100) return False; + + else return True; +} + +/*! + * Die Funktion gibt aus, ob sich innerhalb einer gewissen Entfernung ein Objekt-Hindernis befindet. + * @param distance Entfernung in mm, bis zu welcher ein Objekt gesichtet wird. + * @return Gibt False (0) zurueck, wenn kein Objekt innerhalb von distance gesichtet wird. Ansonsten die Differenz + * zwischen dem linken und rechten Sensor. Negative Werte besagen, dass das Objekt naeher am linken, positive, dass + * es naeher am rechten Sensor ist. Sollten beide Sensoren den gleichen Wert haben, gibt die Funktion 1 zurueck, um + * von False unterscheiden zu koennen. */ +int16 is_obstacle_ahead(int16 distance){ + if(sensDistL > distance && sensDistR > distance) return False; + if(sensDistL - sensDistR == 0) return 1; + else return (sensDistL - sensDistR); +} + +/*! + * Gibt aus, ob der Bot eine fuer sein Slalomverhalten geeignete Saeule vor sich hat. + * @return True, wenn er eine solche Saeule vor sich hat, sonst False.*/ +int8 is_good_pillar_ahead(void){ + if(is_obstacle_ahead(COL_NEAR) != False && sensLDRL < 600 && sensLDRR < 600) return True; + else return False; +} + +/*! + * Das Verhalten verhindert, dass dem Bot boese Dinge wie Kollisionen oder Abstuerze widerfahren. + * @return Bestand Handlungsbedarf? True, wenn das Verhalten ausweichen musste, sonst False. + * TODO: Parameter einfuegen, der dem Verhalten vorschlaegt, wie zu reagieren ist. + * */ +int8 bot_avoid_harm(void){ + if(is_obstacle_ahead(COL_CLOSEST) != False || sensBorderL > BORDER_DANGEROUS || sensBorderR > BORDER_DANGEROUS){ + speedWishLeft = -BOT_SPEED_NORMAL; + speedWishRight = -BOT_SPEED_NORMAL; + return True; + } else return False; +} + +/*! + * Das Verhalten laesst den Roboter den Raum durchsuchen. + * Das Verhalten hat mehrere unterschiedlich Zustaende: + * 1. Zu einer Wand oder einem anderen Hindernis fahren. + * 2. Zu einer Seite drehen, bis der Bot parallel zur Wand ist. + * Es macht vielleicht Sinn, den Maussensor auszulesen, um eine Drehung um + * einen bestimmten Winkel zu realisieren. Allerdings muesste dafuer auch der + * Winkel des Bots zur Wand bekannt sein. + * 3. Eine feste Strecke parallel zur Wand vorwaerts fahren. + * Da bot_glance_behaviour abwechselnd zu beiden Seiten schaut, ist es fuer die Aufgabe, + * einer Wand auf einer Seite des Bots zu folgen, nur bedingt gewachsen und muss + * evtl. erweitert werden. + * 4. Senkrecht zur Wand drehen. + * Siehe 2. + * 5. Einen Bogen fahren, bis der Bot wieder auf ein Hindernis stoesst. + * Dann das Ganze von vorne beginnen, nur in die andere Richtung und mit einem + * weiteren Bogen. So erforscht der Bot einigermassen systematisch den Raum. + * + * Da das Verhalten jeweils nach 10ms neu aufgerufen wird, muss der Bot sich + * 'merken', in welchem Zustand er sich gerade befindet. + * */ +void bot_explore_behaviour(Behaviour_t *data){ + static int8 curve = 0,state = EXPLORATION_STATE_GOTO_WALL, running_curve = False; + + if((*exploration_check_function)()) return_from_behaviour(data); + + switch(state){ + // Volle Fahrt voraus, bis ein Hindernis erreicht ist. + case EXPLORATION_STATE_GOTO_WALL: + // Der Bot steht jetzt vor einem Hindernis und soll sich nach rechts drehen + if(bot_avoid_harm()) { + state = EXPLORATION_STATE_TURN_PARALLEL_RIGHT; + #ifdef BEHAVIOUR_AVOID_COL_AVAILABLE + deactivateBehaviour(bot_avoid_col_behaviour); + #endif + } + // Es befindet sich kein Hindernis direkt vor dem Bot. + else { + if(sensDistL < COL_NEAR || sensDistR < COL_NEAR){ + bot_drive(0,BOT_SPEED_FAST); + } else { + bot_drive(0,BOT_SPEED_MAX); + } + } + break; + // Nach links drehen, bis der Bot parallel zum Hindernis auf der rechten Seite steht. + /* TODO: Aufgabe: Entwickle ein Verhalten, dass auch bei Loechern funktioniert. + * Tipp dazu: Drehe den Roboter auf das Loch zu, bis beide Bodensensoren das Loch 'sehen'. Anschliessend drehe den Bot um 90 Grad. + * Es ist noetig, neue Zustaende zu definieren, die diese Zwischenschritte beschreiben. + * TODO: Drehung mit dem Maussensor ueberwachen. */ + case EXPLORATION_STATE_TURN_PARALLEL_LEFT: + if(sensDistR < COL_FAR){ + // Volle Drehung nach links mit ca. 3Grad/10ms + bot_drive(-127,BOT_SPEED_FAST); + } else { + // Nachdem das Hindernis nicht mehr in Sicht ist, dreht der Bot noch ca. 3 Grad weiter. + // Im Zweifelsfall dreht das den Bot zu weit, aber das ist besser, als ihn zu kurz zu drehen. + bot_drive(-127,BOT_SPEED_FAST); + state = EXPLORATION_STATE_DRIVE_PARALLEL_RIGHT; + } + break; + // Nach rechts drehen, bis der Bot parallel zum Hindernis auf der linken Seite steht. + /* Aufgabe: siehe EXPLORATION_STATE_TURN_PARALLEL_LEFT */ + case EXPLORATION_STATE_TURN_PARALLEL_RIGHT: + if(sensDistL < COL_FAR){ + // Volle Drehung nach rechts mit ca. 3Grad/10ms + bot_drive(127,BOT_SPEED_FAST); + } else { + /* Nachdem das Hindernis nicht mehr in Sicht ist, dreht der Bot noch ca. 3 Grad weiter. + * Im Zweifelsfall dreht das den Bot zu weit, aber das ist besser, als ihn zu kurz zu drehen. */ + bot_drive(127,BOT_SPEED_FAST); + state = EXPLORATION_STATE_DRIVE_PARALLEL_LEFT; + } + break; + case EXPLORATION_STATE_DRIVE_PARALLEL_LEFT: + bot_drive_distance(data,0,BOT_SPEED_FAST,15); + state = EXPLORATION_STATE_TURN_ORTHOGONAL_RIGHT; + break; + case EXPLORATION_STATE_DRIVE_PARALLEL_RIGHT: + bot_drive_distance(data,0,BOT_SPEED_FAST,15); + state = EXPLORATION_STATE_TURN_ORTHOGONAL_LEFT; + break; + case EXPLORATION_STATE_TURN_ORTHOGONAL_LEFT: + // Drehe den Bot um 90 Grad nach links. + /* Da der Bot sich immer ein bisschen zu weit von der Wand weg dreht, soll er sich + * hier nur um 85 Grad drehen. Nicht schoen, aber klappt.*/ + bot_turn(data,85); + state = EXPLORATION_STATE_DRIVE_ARC; + #ifdef BEHAVIOUR_AVOID_COL_AVAILABLE + activateBehaviour(bot_avoid_col_behaviour); + #endif + break; + case EXPLORATION_STATE_TURN_ORTHOGONAL_RIGHT: + // Drehe den Bot um 90 Grad nach rechts. + /* Da der Bot sich immer ein bisschen zu weit von der Wand weg dreht, soll er sich + * hier nur um 85 Grad drehen. Nicht schoen, aber klappt.*/ + bot_turn(data,-85); + state = EXPLORATION_STATE_DRIVE_ARC; + #ifdef BEHAVIOUR_AVOID_COL_AVAILABLE + activateBehaviour(bot_avoid_col_behaviour); + #endif + break; + case EXPLORATION_STATE_DRIVE_ARC: + /* Fahre einen Bogen. + * Der Bot soll im Wechsel Links- und Rechtsboegen fahren. Daher muss das Vorzeichen von curve wechseln. + * Ausserdem soll der Bogen zunehmend weiter werden, so dass der absolute Wert von curve abnehmen muss. + * Ist der Wert 0, wird er auf den engsten Bogen initialisiert. Da der Bot am Anfang nach rechts abbiegt, + * muss der Wert positiv sein. + * Aufgabe: Manchmal kann es passieren, dass der Bot bei einer kleinen Kurve zu weit weg von der Wand + * startet und dann nur noch im Kreis faehrt. Unterbinde dieses Verhalten. + */ + if(curve == 0){ + curve = 25; + running_curve = True; + } else if (running_curve == False){ + curve *= -0.9; + running_curve = True; + } + /* Sobald der Bot auf ein Hindernis stoesst, wird der naechste Zyklus eingeleitet. + * Auf einen Rechtsbogen (curve > 0) folgt eine Linksdrehung und auf einen Linksbogen eine Rechtsdrehung. + * Wenn der Wert von curve (durch Rundungsfehler bei int) auf 0 faellt, beginnt das Suchverhalten erneut.*/ + if(bot_avoid_harm()) { + state = (curve > 0) ? EXPLORATION_STATE_TURN_PARALLEL_LEFT : EXPLORATION_STATE_TURN_PARALLEL_RIGHT; + running_curve = False; + #ifdef BEHAVIOUR_AVOID_COL_AVAILABLE + deactivateBehaviour(bot_avoid_col_behaviour); + #endif + } else { + bot_drive(curve, BOT_SPEED_MAX); + } + break; + default: + state = EXPLORATION_STATE_GOTO_WALL; + curve = 0; + #ifdef BEHAVIOUR_AVOID_COL_AVAILABLE + activateBehaviour(bot_avoid_col_behaviour); + #endif + } + +} + +/*! + * Aktiviert bot_explore_behaviour. */ +void bot_explore(Behaviour_t *caller, int8 (*check)(void)){ + exploration_check_function = check; + switch_to_behaviour(caller,bot_explore_behaviour,NOOVERRIDE); +} + +/*! + * Das Verhalten laesst den Bot einen Slalom fahren. + * @see bot_do_slalom() + * */ +void bot_do_slalom_behaviour(Behaviour_t *data){ + static int8 state = SLALOM_STATE_CHECK_PILLAR; + static int8 orientation = SLALOM_ORIENTATION_RIGHT; + static int8 sweep_state; + static int8 sweep_steps = 0; + int16 turn; + int8 curve; + + switch(state){ + case SLALOM_STATE_CHECK_PILLAR: + // Der Bot sollte jetzt Licht sehen koennen... + if(check_for_light()){ + // Wenn der Bot direkt vor der Saeule steht, kann der Slalom anfangen, sonst zum Licht fahren + if(bot_avoid_harm()){ + state = SLALOM_STATE_START; + } else bot_goto_light(); + } else {// ... sonst muss er den Slalom-Kurs neu suchen. + #ifdef BEHAVIOUR_AVOID_COL_AVAILABLE + activateBehaviour(bot_avoid_col_behaviour); + #endif + return_from_behaviour(data); + } + break; + case SLALOM_STATE_START: + // Hier ist Platz fuer weitere Vorbereitungen, falls noetig. + #ifdef BEHAVIOUR_AVOID_COL_AVAILABLE + deactivateBehaviour(bot_avoid_col_behaviour); + #endif + state = SLALOM_STATE_TURN_1; + // break; + case SLALOM_STATE_TURN_1: + turn = (orientation == SLALOM_ORIENTATION_LEFT) ? 90 : -90; + bot_turn(data,turn); + state = SLALOM_STATE_DRIVE_ARC; + break; + case SLALOM_STATE_DRIVE_ARC: + // Nicht wundern: Bei einem Links-Slalom faehrt der Bot eine Rechtskurve. + curve = (orientation == SLALOM_ORIENTATION_LEFT) ? 25 : -25; + bot_drive_distance(data,curve,BOT_SPEED_FAST,20); + state = SLALOM_STATE_TURN_2; + break; + case SLALOM_STATE_TURN_2: + turn = (orientation == SLALOM_ORIENTATION_LEFT) ? 45 : -45; + bot_turn(data,turn); + state = SLALOM_STATE_SWEEP_RUNNING; + break; + case SLALOM_STATE_SWEEP_RUNNING: + if(sweep_steps == 0){ + sweep_state = SWEEP_STATE_CHECK; + } + // Insgesamt 6 Schritte drehen + if(sweep_steps < 6) { + if(sweep_state == SWEEP_STATE_CHECK){ + // Phase 1: Pruefen, ob vor dem Bot eine gute Saeule ist + if(is_good_pillar_ahead() == True){ + // Wenn die Saeule gut ist, drauf zu und Slalom anders rum fahren. + state = SLALOM_STATE_CHECK_PILLAR; + orientation = (orientation == SLALOM_ORIENTATION_LEFT) ? SLALOM_ORIENTATION_RIGHT : SLALOM_ORIENTATION_LEFT; + sweep_steps = 0; + } else { + // Sonst drehen. + sweep_state = SWEEP_STATE_TURN; + } + } + if(sweep_state == SWEEP_STATE_TURN) { + // Phase 2: Bot um 15 Grad drehen + turn = (orientation == SLALOM_ORIENTATION_LEFT) ? 15 : -15; + bot_turn(data,turn); + sweep_state = SWEEP_STATE_CHECK; + sweep_steps++; + } + } else { + turn = (orientation == SLALOM_ORIENTATION_LEFT) ? -90 : 90; + bot_turn(data,turn); + state = SLALOM_STATE_SWEEP_DONE; + sweep_steps = 0; + } + break; + case SLALOM_STATE_SWEEP_DONE: + turn = (orientation == SLALOM_ORIENTATION_LEFT) ? -135 : 135; + bot_turn(data,turn); + state = SLALOM_STATE_CHECK_PILLAR; + break; + default: + state = SLALOM_STATE_CHECK_PILLAR; + } + +} + +/*! + * Das Verhalten laesst den Bot zwischen einer Reihe beleuchteter Saeulen Slalom fahren. + * Das Verhalten ist wie bot_explore() in eine Anzahl von Teilschritten unterteilt. + * 1. Vor die aktuelle Saeule stellen, so dass sie zentral vor dem Bot und ungefaehr + * COL_CLOSEST (100 mm) entfernt ist. + * 2. 90 Grad nach rechts drehen. + * 3. In einem relativ engen Bogen 20 cm weit fahren. + * 4. Auf der rechten Seite des Bot nach einem Objekt suchen, dass + * a) im rechten Sektor des Bot liegt, also zwischen -45 Grad und -135 Grad zur Fahrtrichtung liegt, + * b) beleuchtet und + * c) nicht zu weit entfernt ist. + * Wenn es dieses Objekt gibt, wird es zur aktuellen Saeule und der Bot faehrt jetzt Slalom links. + * 5. Sonst zurueck drehen, 90 Grad drehen und Slalom rechts fahren. + * In diesem Schritt kann der Bot das Verhalten auch abbrechen, falls er gar kein Objekt mehr findet. + */ +void bot_do_slalom(Behaviour_t *caller){ + switch_to_behaviour(caller, bot_do_slalom_behaviour,NOOVERRIDE); +} + +/*! + * Das Verhalten setzt sich aus 3 Teilverhalten zusammen: + * Nach Licht suchen, auf das Licht zufahren, im Licht Slalom fahren. */ +void bot_olympic_behaviour(Behaviour_t *data){ + if(check_for_light()){ + /* Sobald der Bot auf ein Objekt-Hinderniss stoesst, versucht er, Slalom zu fahren. + * Aufgabe: Wenn der Bot vor einem Loch steht, hinter welchem sich die Lichtquelle + * befindet, wird er daran haengen bleiben. Schreibe ein Verhalten, dass das verhindert. */ + if(bot_avoid_harm() && is_obstacle_ahead(COL_NEAR)){ + bot_do_slalom(data); + } else bot_goto_light(); + } else bot_explore(data,check_for_light); +} + +/*! + * Initialisiert das Olympische Verhalten + * @param prio_main Prioritaet des Olympischen Verhalten (typ. 100) + * @param prio_helper Prioritaet der Hilfsfunktionen (typ. 52) + * @param active ACTIVE wenn es sofort starten soll, sonst INACTIVE + */ +void bot_olympic_init(int8 prio_main,int8 prio_helper, int8 active){ + // unwichtigere Hilfsverhalten + insert_behaviour_to_list(&behaviour, new_behaviour(prio_helper--, bot_explore_behaviour,INACTIVE)); + insert_behaviour_to_list(&behaviour, new_behaviour( prio_helper, bot_do_slalom_behaviour,INACTIVE)); + // Demo-Verhalten fuer aufwendiges System, inaktiv + insert_behaviour_to_list(&behaviour, new_behaviour(prio_main, bot_olympic_behaviour, active)); +} + +#endif |