From 56d9bdd39ed36c36e9a61411b86c76d5228b2133 Mon Sep 17 00:00:00 2001 From: sicarius Date: Sun, 11 Feb 2007 18:32:03 +0000 Subject: Added lot's of code-files used during work --- source/ct-Bot/bot-logic/behaviour_avoid_border.c | 41 ++ source/ct-Bot/bot-logic/behaviour_avoid_col.c | 142 +++++ source/ct-Bot/bot-logic/behaviour_catch_pillar.c | 106 ++++ source/ct-Bot/bot-logic/behaviour_drive_distance.c | 120 ++++ source/ct-Bot/bot-logic/behaviour_drive_square.c | 76 +++ source/ct-Bot/bot-logic/behaviour_follow_line.c | 176 ++++++ source/ct-Bot/bot-logic/behaviour_goto.c | 141 +++++ source/ct-Bot/bot-logic/behaviour_gotoxy.c | 141 +++++ source/ct-Bot/bot-logic/behaviour_olympic.c | 428 +++++++++++++ source/ct-Bot/bot-logic/behaviour_remotecall.c | 407 +++++++++++++ source/ct-Bot/bot-logic/behaviour_scan.c | 144 +++++ source/ct-Bot/bot-logic/behaviour_servo.c | 68 +++ source/ct-Bot/bot-logic/behaviour_simple.c | 139 +++++ source/ct-Bot/bot-logic/behaviour_solve_maze.c | 660 +++++++++++++++++++++ source/ct-Bot/bot-logic/behaviour_turn.c | 361 +++++++++++ source/ct-Bot/bot-logic/bot-logik.c | 500 ++++++++++++++++ 16 files changed, 3650 insertions(+) create mode 100644 source/ct-Bot/bot-logic/behaviour_avoid_border.c create mode 100644 source/ct-Bot/bot-logic/behaviour_avoid_col.c create mode 100644 source/ct-Bot/bot-logic/behaviour_catch_pillar.c create mode 100644 source/ct-Bot/bot-logic/behaviour_drive_distance.c create mode 100644 source/ct-Bot/bot-logic/behaviour_drive_square.c create mode 100644 source/ct-Bot/bot-logic/behaviour_follow_line.c create mode 100644 source/ct-Bot/bot-logic/behaviour_goto.c create mode 100644 source/ct-Bot/bot-logic/behaviour_gotoxy.c create mode 100644 source/ct-Bot/bot-logic/behaviour_olympic.c create mode 100644 source/ct-Bot/bot-logic/behaviour_remotecall.c create mode 100644 source/ct-Bot/bot-logic/behaviour_scan.c create mode 100644 source/ct-Bot/bot-logic/behaviour_servo.c create mode 100644 source/ct-Bot/bot-logic/behaviour_simple.c create mode 100644 source/ct-Bot/bot-logic/behaviour_solve_maze.c create mode 100644 source/ct-Bot/bot-logic/behaviour_turn.c create mode 100644 source/ct-Bot/bot-logic/bot-logik.c (limited to 'source/ct-Bot/bot-logic') diff --git a/source/ct-Bot/bot-logic/behaviour_avoid_border.c b/source/ct-Bot/bot-logic/behaviour_avoid_border.c new file mode 100644 index 0000000..9665b8d --- /dev/null +++ b/source/ct-Bot/bot-logic/behaviour_avoid_border.c @@ -0,0 +1,41 @@ +/* + * 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_avoid_border.c + * @brief Vermeide Abgruende + * + * @author Benjamin Benz (bbe@heise.de) + * @date 03.11.06 +*/ + +#include "bot-logic/bot-logik.h" + +#ifdef BEHAVIOUR_AVOID_BORDER_AVAILABLE +/*! + * Verhindert, dass der Bot in Graeben faellt + * @param *data der Verhaltensdatensatz + */ +void bot_avoid_border_behaviour(Behaviour_t *data){ + if (sensBorderL > BORDER_DANGEROUS) + speedWishLeft=-BOT_SPEED_NORMAL; + + if (sensBorderR > BORDER_DANGEROUS) + speedWishRight=-BOT_SPEED_NORMAL; +} +#endif diff --git a/source/ct-Bot/bot-logic/behaviour_avoid_col.c b/source/ct-Bot/bot-logic/behaviour_avoid_col.c new file mode 100644 index 0000000..a5c889a --- /dev/null +++ b/source/ct-Bot/bot-logic/behaviour_avoid_col.c @@ -0,0 +1,142 @@ +/* + * 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_avoid_col.c + * @brief Vermeide Kollisionen + * + * @author Benjamin Benz (bbe@heise.de) + * @date 03.11.06 +*/ + + +#include "bot-logic/bot-logik.h" + +#ifdef BEHAVIOUR_AVOID_COL_AVAILABLE + +#define ZONE_CLOSEST 0 /*!< Zone fuer extremen Nahbereich */ +#define ZONE_NEAR 1 /*!< Zone fuer Nahbereich */ +#define ZONE_FAR 2 /*!< Zone fuer Fernbereich */ +#define ZONE_CLEAR 3 /*!< Zone fuer Freien Bereich */ + +#define BRAKE_CLOSEST -1.0 /*!< Bremsfaktor fuer extremen Nahbereich ( <1 ==> bremsen <0 ==> rueckwaerts) */ +#define BRAKE_NEAR 0.6 /*!< Bremsfaktor fuer Nahbereich ( <1 ==> bremsen <0 ==> rueckwaerts) */ +#define BRAKE_FAR 0.8 /*!< Bremsfaktor fuer Fernbereich ( <1 ==> bremsen <0 ==> rueckwaerts) */ + +uint8 col_zone_l=ZONE_CLEAR; /*!< Kollisionszone, in der sich der linke Sensor befindet */ +uint8 col_zone_r=ZONE_CLEAR; /*!< Kollisionszone, in der sich der rechte Sensor befindet */ + + +/*! + * Passt auf, dass keine Kollision mit Hindernissen an der Front des Roboters + * geschieht. + * TODO: Diese Funktion ist nur ein Dummy-Beispiel, wie eine Kollisionsvermeidung aussehen + * koennte. Hier ist ein guter Einstiegspunkt fuer eigene Experimente und Algorithmen! + * @param *data der Verhaltensdatensatz + */ +void bot_avoid_col_behaviour(Behaviour_t *data){ + if (sensDistR < COL_CLOSEST) /* sehr nah */ + col_zone_r=ZONE_CLOSEST; /* dann auf jeden Fall CLOSEST Zone */ + else + /* sind wir naeher als NEAR und nicht in der inneren Zone gewesen */ + if ((sensDistR < COL_NEAR) && (col_zone_r > ZONE_CLOSEST)) + col_zone_r=ZONE_NEAR; /* dann auf in die NEAR-Zone */ + else + /* sind wir naeher als FAR und nicht in der NEAR-Zone gewesen */ + if ((sensDistR < COL_FAR) && (col_zone_r > ZONE_NEAR)) + col_zone_r=ZONE_FAR; /* dann auf in die FAR-Zone */ + else + /* wir waren in einer engeren Zone und verlassen sie in Richtung NEAR */ + if (sensDistR < (COL_NEAR * 0.50)) + col_zone_r=ZONE_NEAR; /* dann auf in die NEAR-Zone */ + else + if (sensDistR < (COL_FAR * 0.50)) + col_zone_r=ZONE_FAR; /* dann auf in die NEAR-Zone */ + else + col_zone_r=ZONE_CLEAR; /* dann auf in die CLEAR-Zone */ + + if (sensDistL < COL_CLOSEST) /* sehr nah */ + col_zone_l=ZONE_CLOSEST; /* dann auf jeden Fall CLOSEST-Zone */ + else + /* sind wir naeher als NEAR und nicht in der inneren Zone gewesen */ + if ((sensDistL < COL_NEAR) && (col_zone_l > ZONE_CLOSEST)) + col_zone_l=ZONE_NEAR; /* dann auf in die NEAR-Zone */ + else + /* sind wir naeher als FAR und nicht in der NEAR-Zone gewesen */ + if ((sensDistL < COL_FAR) && (col_zone_l > ZONE_NEAR)) + col_zone_l=ZONE_FAR; /* dann auf in die FAR-Zone */ + else + /* wir waren in einer engeren Zone und verlassen sie in Richtung NEAR */ + if (sensDistL < (COL_NEAR * 0.50)) + col_zone_l=ZONE_NEAR; /* dann auf in die NEAR-Zone */ + else + if (sensDistL < (COL_FAR * 0.50)) + col_zone_l=ZONE_FAR; /* dann auf in die NEAR-Zone */ + else + col_zone_l=ZONE_CLEAR; /* dann auf in die CLEAR-Zone */ + + // Nur reagieren, wenn der Bot vorwaerts faehrt + if ((speed_l > 0) && (speed_r >0)) { + // wenn auf beiden Seiten in der Kollisionszone + if ((col_zone_r == ZONE_CLOSEST)&&(col_zone_l == ZONE_CLOSEST)){ + // Drehe Dich zur Seite, wo mehr Platz ist + if (sensDistR < sensDistL) + bot_turn(data,20); + else + bot_turn(data,-20); + return; + } + + switch (col_zone_l){ + case ZONE_CLOSEST: + faktorWishRight = BRAKE_CLOSEST; + break; + case ZONE_NEAR: + faktorWishRight = BRAKE_NEAR; + break; + case ZONE_FAR: + faktorWishRight = BRAKE_FAR; + break; + case ZONE_CLEAR: + faktorWishRight = 1; + break; + default: + col_zone_l = ZONE_CLEAR; + break; + } + + switch (col_zone_r){ + case ZONE_CLOSEST: + faktorWishLeft = BRAKE_CLOSEST; + break; + case ZONE_NEAR: + faktorWishLeft = BRAKE_NEAR; + break; + case ZONE_FAR: + faktorWishLeft = BRAKE_FAR; + break; + case ZONE_CLEAR: + faktorWishLeft = 1; + break; + default: + col_zone_r = ZONE_CLEAR; + break; + } + } +} +#endif diff --git a/source/ct-Bot/bot-logic/behaviour_catch_pillar.c b/source/ct-Bot/bot-logic/behaviour_catch_pillar.c new file mode 100644 index 0000000..efc0a4e --- /dev/null +++ b/source/ct-Bot/bot-logic/behaviour_catch_pillar.c @@ -0,0 +1,106 @@ +/* + * 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_catch_pillar.c + * @brief sucht nach einer Dose und fängt sie ein + * + * @author Benjamin Benz (bbe@heise.de) + * @date 08.12.06 +*/ + + +#include "bot-logic/bot-logik.h" +#ifdef BEHAVIOUR_CATCH_PILLAR_AVAILABLE +#include + +#define MAX_PILLAR_DISTANCE 350 + +#define START 0 +#define LEFT_FOUND 1 +#define TURN_MIDDLE 2 +#define GO 3 + +static uint8 pillar_state=START; +/*! + * Fange eine Dose ein + * @param *data der Verhaltensdatensatz + */ +void bot_catch_pillar_behaviour(Behaviour_t *data){ + static float angle; + + switch (pillar_state){ + case START: + if (sensDistL < MAX_PILLAR_DISTANCE){ // sieht der linke Sensor schon was? + angle=heading; + pillar_state=LEFT_FOUND; + } else + speedWishLeft=-BOT_SPEED_SLOW; + speedWishRight=+BOT_SPEED_SLOW; + //bot_turn(data,5); // Ein Stueck drehen + break; + case LEFT_FOUND: + if (sensDistR < MAX_PILLAR_DISTANCE){ // sieht der rechte Sensor schon was? + angle= heading- angle; + if (angle < 0) + angle+=360; + angle= heading - angle/2; + pillar_state=TURN_MIDDLE; +// bot_turn(data,-angle/2); + }else + speedWishLeft=-BOT_SPEED_SLOW; + speedWishRight=+BOT_SPEED_SLOW; +// bot_turn(data,5); // Eins Stueck drehen + break; + case TURN_MIDDLE: + if (fabs(heading - angle) > 2){ + speedWishLeft=+BOT_SPEED_SLOW; + speedWishRight=-BOT_SPEED_SLOW; + } else { + bot_servo(data,SERVO1,DOOR_OPEN); // Klappe auf + pillar_state=GO; + } + break; + case GO: + if (sensTrans ==0){ + speedWishLeft=+BOT_SPEED_SLOW; + speedWishRight=+BOT_SPEED_SLOW; + } else { + bot_servo(data,SERVO1,DOOR_CLOSE); + pillar_state++; + } + break; + + default: + pillar_state=START; + return_from_behaviour(data); + break; + } +} + +/*! + * Fange ein Objekt ein + * @param caller Der obligatorische Verhaltensdatensatz des Aufrufers + */ +void bot_catch_pillar(Behaviour_t * caller){ + pillar_state=0; + // Zielwerte speichern + switch_to_behaviour(caller,bot_catch_pillar_behaviour,OVERRIDE); +} +#endif diff --git a/source/ct-Bot/bot-logic/behaviour_drive_distance.c b/source/ct-Bot/bot-logic/behaviour_drive_distance.c new file mode 100644 index 0000000..bb29e66 --- /dev/null +++ b/source/ct-Bot/bot-logic/behaviour_drive_distance.c @@ -0,0 +1,120 @@ +/* + * 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_drive_distance.c + * @brief Bot faehrt ein Stueck + * + * @author Benjamin Benz (bbe@heise.de) + * @date 03.11.06 +*/ + +#include "bot-logic/bot-logik.h" +#include "log.h" + +#ifdef BEHAVIOUR_DRIVE_DISTANCE_AVAILABLE + + +/* Parameter fuer das bot_drive_distance_behaviour() */ +int16 drive_distance_target; /*!< Zu fahrende Distanz bzw. angepeilter Stand der Radencoder sensEncL bzw. sensEncR */ +int8 drive_distance_curve; /*!< Kruemmung der zu fahrenden Strecke. */ +int16 drive_distance_speed; /*!< Angepeilte Geschwindigkeit. */ + + +/*! + * laesst den Bot in eine Richtung fahren. + * Es handelt sich hierbei nicht im eigentlichen Sinn um ein Verhalten, sondern ist nur eine Abstraktion der Motorkontrollen. + * @param curve Gibt an, ob der Bot eine Kurve fahren soll. Werte von -127 (So scharf wie moeglich links) ueber 0 (gerade aus) bis 127 (so scharf wie moeglich rechts) + * @param speed Gibt an, wie schnell der Bot fahren soll. */ +void bot_drive(int8 curve, int16 speed){ + // Wenn etwas ausgewichen wurde, bricht das Verhalten hier ab, sonst wuerde es evtl. die Handlungsanweisungen von bot_avoid_harm() stoeren. + //if(bot_avoid_harm()) return; + if(curve < 0) { + speedWishLeft = speed * (1.0 + 2.0*((float)curve/127)); + speedWishRight = speed; + } else if (curve > 0) { + speedWishRight = speed * (1.0 - 2.0*((float)curve/127)); + speedWishLeft = speed; + } else { + speedWishLeft = speed; + speedWishRight = speed; + } +} + +/*! + * Das Verhalten laesst den Bot eine vorher festgelegte Strecke fahren. + * @param *data der Verhaltensdatensatz + * @see bot_drive_distance() + */ +void bot_drive_distance_behaviour(Behaviour_t* data){ + int16 *encoder; + int16 to_drive; + + if (drive_distance_curve > 0){ + // Es handelt sich um eine Rechtskurve, daher wird mit dem linken Encoder gerechnet + encoder = (int16*)&sensEncL; + } else { + encoder = (int16*)&sensEncR; + } + + to_drive = drive_distance_target - *encoder; + if(drive_distance_speed < 0) to_drive = -to_drive; + + if(to_drive <= 0){ + return_from_behaviour(data); + } else { + if((drive_distance_speed > BOT_SPEED_SLOW || drive_distance_speed < -BOT_SPEED_SLOW) && to_drive < (0.1 * ENCODER_MARKS)) bot_drive(drive_distance_curve, drive_distance_speed / 2); + else bot_drive(drive_distance_curve, drive_distance_speed); + } +} + + + +/*! + * Das Verhalten laesst den Bot eine vorher festgelegte Strecke fahren. Dabei legt die Geschwindigkeit fest, ob der Bot vorwaerts oder rueckwaerts fahren soll. + * @param curve Gibt an, ob der Bot eine Kurve fahren soll. Werte von -127 (So scharf wie moeglich links) ueber 0 (gerade aus) bis 127 (so scharf wie moeglich rechts) + * @param speed Gibt an, wie schnell der Bot fahren soll. Negative Werte lassen den Bot rueckwaerts fahren. + * @param cm Gibt an, wie weit der Bot fahren soll. In cm :-) Die Strecke muss positiv sein, die Fahrtrichtung wird ueber speed geregelt. + */ +void bot_drive_distance(Behaviour_t* caller,int8 curve, int16 speed, int16 cm){ +// LOG_DEBUG(("curve= %d, speed= %d, cm =%d",curve, speed, cm)); + + int32 tmp = cm; + tmp*= 10 * ENCODER_MARKS; + tmp /= WHEEL_PERIMETER; + int16 marks_to_drive = (int16) tmp; + + int16 *encoder; + drive_distance_curve = curve; + drive_distance_speed = speed; + + if (curve > 0){ + // Es handelt sich um eine Rechtskurve, daher wird mit dem linken Encoder gerechnet + encoder = (int16*)&sensEncL; + } else { + encoder = (int16*)&sensEncR; + } + if(speed < 0){ + // Es soll rueckwaerts gefahren werden. Der Zielwert ist also kleiner als der aktuelle Encoder-Stand. + drive_distance_target = *encoder - marks_to_drive; + } else { + drive_distance_target = *encoder + marks_to_drive; + } + switch_to_behaviour(caller, bot_drive_distance_behaviour,NOOVERRIDE); +} +#endif diff --git a/source/ct-Bot/bot-logic/behaviour_drive_square.c b/source/ct-Bot/bot-logic/behaviour_drive_square.c new file mode 100644 index 0000000..7c2a924 --- /dev/null +++ b/source/ct-Bot/bot-logic/behaviour_drive_square.c @@ -0,0 +1,76 @@ +/* + * 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_drive_square.c + * @brief Bot faehrt im Quadrat + * + * @author Benjamin Benz (bbe@heise.de) + * @date 03.11.06 +*/ + + +#include "bot-logic/bot-logik.h" + +#ifdef BEHAVIOUR_DRIVE_SQUARE_AVAILABLE +/*! + * Laesst den Roboter ein Quadrat abfahren. + * Einfaches Beispiel fuer ein Verhalten, das einen Zustand besitzt. + * Es greift auf andere Behaviours zurueck und setzt daher + * selbst keine speedWishes. + * @param *data der Verhaltensdatensatz + */ +void bot_drive_square_behaviour(Behaviour_t *data){ + #define STATE_TURN 1 + #define STATE_FORWARD 0 + #define STATE_INTERRUPTED 2 + + static uint8 state = STATE_FORWARD; + + if (data->subResult == SUBFAIL) // letzter Auftrag schlug fehl? + state= STATE_INTERRUPTED; + + switch (state) { + case STATE_FORWARD: // Vorwaerts + bot_goto(data,100,100); + state = STATE_TURN; + break; + case STATE_TURN: // Drehen + bot_goto(data,22,-22); + state=STATE_FORWARD; + break; + case STATE_INTERRUPTED: + return_from_behaviour(data); // Beleidigt sein und sich selbst deaktiviern + break; + + default: /* Sind wir fertig, dann Kontrolle zurueck an Aufrufer */ + return_from_behaviour(data); + break; + } +} + +/*! + * Laesst den Roboter ein Quadrat abfahren. + * @param caller Der obligatorische Verhaltensdatensatz des aufrufers + */ +void bot_drive_square(Behaviour_t* caller){ + switch_to_behaviour(caller, bot_drive_square_behaviour,OVERRIDE); +} + + +#endif diff --git a/source/ct-Bot/bot-logic/behaviour_follow_line.c b/source/ct-Bot/bot-logic/behaviour_follow_line.c new file mode 100644 index 0000000..e35b16f --- /dev/null +++ b/source/ct-Bot/bot-logic/behaviour_follow_line.c @@ -0,0 +1,176 @@ +/* + * 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_follow_line.c + * @brief Linienverfolger + * + * @author Benjamin Benz (bbe@heise.de) + * @date 03.11.06 +*/ + +#include "bot-logic/bot-logik.h" + +#ifdef BEHAVIOUR_FOLLOW_LINE_AVAILABLE + +/*! Folgt einer Linie, sobald beide Liniensensoren ausloesen + * Die Linie sollte in etwa die Breite beider CNY70 haben + */ +void bot_follow_line_behaviour(Behaviour_t *data) { + /* Konstanten fuer das Verhalten */ + #define CORNER_LEFT 1 + #define CORNER_RIGHT 2 + /* Zustaende fuer das Verhalten */ + #define CHECK_LINE 0 /* !< Feststellen ob wir ueber einer Linie sind */ + #define FOLLOW_LINE 1 /* !< Folgen einer geraden oder leicht gekruemmten Linie */ + #define CHECK_BORDER 2 /* !< Abgrundsensoren haben Alarm geschlagen. Feststellen ob wirklich Abgrund oder Ecke */ + #define CORNER_TURN 3 /* !< Drehung in Richtun detektiertem Abgrund */ + #define CORRECT_POS 4 /* !< Nach der Drehung 1cm vorfahren zum Ausgleichen */ + #define ADVANCE_CORNER 5 /* !< Auf die Ecke zufahren, bis die Linie verschwindet */ + #define RETREAT_AND_STOP 6 /* !< Zurueckfahren mit voller Geschwindigkeit, dann Stop und Verhalten verlassen */ + + /* Status- und Hilfsvariablen */ + static int8 lineState=CHECK_LINE; + static int8 cornerDetected=False; + + switch(lineState) { + case CHECK_LINE: /* sind beide Sensoren ueber einer Linie? */ + if (sensLineL>=LINE_SENSE && sensLineR>=LINE_SENSE) { + /* zunaechst alle Hilfsverhalten ausschalten, die den Algorithmus stoeren */ + /* 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 + /* bot_glance() stoert bot_turn() */ + //deactivateBehaviour(bot_glance_behaviour); + /* losfahren und nach FOLLOW_LINE wechseln */ + speedWishLeft=BOT_SPEED_FOLLOW; + speedWishRight=BOT_SPEED_FOLLOW; + lineState=FOLLOW_LINE; + } + break; + + case FOLLOW_LINE: + /* Pruefen, ob die Abgrundsensoren einen Abgrund sehen */ + if (sensBorderL>BORDER_DANGEROUS || sensBorderR>BORDER_DANGEROUS) { + /* Abgrund erkannt, das kann jetzt eine Linie sein oder ein richtiger Abgrund.*/ + if (sensBorderL>BORDER_DANGEROUS && sensBorderR>BORDER_DANGEROUS) { + /* Wenn beidseitig erkannt, koennen wir damit nicht umgehen -> + * Ende des Verhaltens */ + speedWishLeft=BOT_SPEED_STOP; + speedWishRight=BOT_SPEED_STOP; + lineState=CHECK_LINE; /* Verhaltensstatus zuruecksetzen */ + //LOG_INFO(("Stopp in FOLLOW_LINE")); + return_from_behaviour(data); + break; + } + /* nachsehen, ob der linke oder rechte Liniensensor ohne Kontakt zur Linie ist + * und ggfs. gegensteuern */ + if (sensBorderL>BORDER_DANGEROUS) { + cornerDetected=CORNER_LEFT; + } else { + cornerDetected=CORNER_RIGHT; + } + /* nun zur vermuteten Ecke vorfahren */ + lineState=CHECK_BORDER; + bot_drive_distance(data,0,BOT_SPEED_FOLLOW,3); + break; + } + if (sensLineLLINE_SENSE) { + /* links von der Linie abgekommen, daher nach rechts drehen */ + //LOG_DEBUG(("Drehe rechts")); + speedWishLeft=BOT_SPEED_FOLLOW; + speedWishRight=-BOT_SPEED_FOLLOW; + } else if (sensLineL>LINE_SENSE && sensLineRLINE_SENSE && sensLineR>LINE_SENSE) { + /* noch ueber der Linie, also einfach geradeaus weiter */ + //LOG_DEBUG(("Fahre geradeaus")); + speedWishLeft=BOT_SPEED_FOLLOW; + speedWishRight=BOT_SPEED_FOLLOW; + } + break; + + case CHECK_BORDER: + /* wir sollten jetzt direkt an der Kante zum Abgrund stehen, wenn es + * denn wirklich eine ist. In dem Fall fahren wir ein Stueck zurueck. + * sonst gehen wir von einer Linie aus, drehen uns in die Richtung, + * in der wir den "Abgrund" entdeckt haben und machen dann weiter mit + * der Linienverfolgung */ + if (sensBorderL>BORDER_DANGEROUS || sensBorderR>BORDER_DANGEROUS) { + /* scheint wirklich ein Abgrund zu sein */ + lineState=RETREAT_AND_STOP; + speedWishLeft=-BOT_SPEED_MAX; + speedWishRight=-BOT_SPEED_MAX; + break; + } + /* war nur eine Ecke, noch weiter darauf zu bis kein Kontakt mehr zur Linie */ + lineState=ADVANCE_CORNER; + speedWishLeft=BOT_SPEED_FOLLOW; + speedWishRight=BOT_SPEED_FOLLOW; + break; + + case ADVANCE_CORNER: + /* auf die Ecke zufahren, bis die Linie verschwindet */ + if (sensLineL + + +#define MOT_GOTO_MAX 4 /*!< Richtungsaenderungen, bis goto erreicht sein muss */ +#define GOTO_REACHED 2 /*!< Wenn Encoder-Distanz <= GOTO_REACHED dann stop */ +#define GOTO_SLOW 4 /*!< Wenn Encoder-Distanz < GOTO_SLOW dann langsame Fahrt */ +#define GOTO_NORMAL 10 /*!< Wenn Encoder-Distanz < GOTO_NORMAL dann normale Fahrt */ +#define GOTO_FAST 40 /*!< Wenn Encoder-Distanz < GOTO_FAST dann schnelle Fahrt, sonst maximale Fahrt */ + + +int16 mot_l_goto=0; /*!< Speichert wie weit der linke Motor drehen soll */ +int16 mot_r_goto=0; /*!< Speichert wie weit der rechte Motor drehen soll */ + + +/*! + * Kuemmert sich intern um die Ausfuehrung der goto-Kommandos, + * @param *data der Verhaltensdatensatz + * @see bot_goto() + */ +void bot_goto_behaviour(Behaviour_t *data){ + static int16 mot_goto_l=0; /*!< Muss der linke Motor noch drehen? */ + static int16 mot_goto_r=0; /*!< Muss der rechte Motor noch drehen? */ + + int16 diff_l; /* Restdistanz links */ + int16 diff_r; /* Restdistanz rechts */ + + /* Sind beide Zaehler Null und ist die Funktion active + * -- sonst waeren wir nicht hier -- + * so ist dies der erste Aufruf ==> initialisieren */ + if (( mot_goto_l ==0) && ( mot_goto_r ==0)){ + /* Zaehler einstellen */ + if (mot_l_goto !=0) + mot_goto_l= MOT_GOTO_MAX; + if (mot_r_goto !=0) + mot_goto_r=MOT_GOTO_MAX; + + /* Encoder zuruecksetzen */ + sensEncL=0; + sensEncR=0; + } + + /* Motor L hat noch keine MOT_GOTO_MAX Nulldurchgaenge gehabt */ + if (mot_goto_l >0){ + diff_l = sensEncL - mot_l_goto; /* Restdistanz links */ + + if (abs(diff_l) <= GOTO_REACHED){ /* 2 Encoderstaende Genauigkeit reicht */ + speedWishLeft = BOT_SPEED_STOP; /* Stop */ + mot_goto_l--; /* wie Nulldurchgang behandeln */ + }else if (abs(diff_l) < GOTO_SLOW) + speedWishLeft= BOT_SPEED_SLOW; + else if (abs(diff_l) < GOTO_NORMAL) + speedWishLeft= BOT_SPEED_NORMAL; + else if (abs(diff_l) < GOTO_FAST) + speedWishLeft= BOT_SPEED_FAST; + else speedWishLeft= BOT_SPEED_MAX; + + // Richtung + if (diff_l>0) { // Wenn uebersteuert, + speedWishLeft= -speedWishLeft; //Richtung umkehren + } + + // Wenn neue Richtung ungleich alter Richtung + if (((speedWishLeft<0)&& (direction.left == DIRECTION_FORWARD))|| ( (speedWishLeft>0) && (direction.left == DIRECTION_BACKWARD) ) ) + mot_goto_l--; // Nulldurchgang merken + } + + // Motor R hat noch keine MOT_GOTO_MAX Nulldurchgaenge gehabt + if (mot_goto_r >0){ + diff_r = sensEncR - mot_r_goto; /* Restdistanz rechts */ + + if (abs(diff_r) <= GOTO_REACHED){ // 2 Encoderstaende Genauigkeit reicht + speedWishRight = BOT_SPEED_STOP; //Stop + mot_goto_r--; // wie Nulldurchgang behandeln + }else if (abs(diff_r) < GOTO_SLOW) + speedWishRight= BOT_SPEED_SLOW; + else if (abs(diff_r) < GOTO_NORMAL) + speedWishRight= BOT_SPEED_NORMAL; + else if (abs(diff_r) < GOTO_FAST) + speedWishRight= BOT_SPEED_FAST; + else speedWishRight= BOT_SPEED_MAX; + + // Richtung + if (diff_r>0) { // Wenn uebersteurt, + speedWishRight= -speedWishRight; //Richtung umkehren + } + + // Wenn neue Richtung ungleich alter Richtung + if (((speedWishRight<0)&& (direction.right == DIRECTION_FORWARD))|| ( (speedWishRight>0) && (direction.right == DIRECTION_BACKWARD) ) ) + mot_goto_r--; // Nulldurchgang merken + } + + /* Sind wir fertig, dann Regel deaktivieren */ + if ((mot_goto_l == 0) && (mot_goto_r == 0)){ + return_from_behaviour(data); + } +} + +/*! + * Drehe die Raeder um die gegebene Zahl an Encoder-Schritten weiter + * @param left Schritte links + * @param right Schritte rechts + */ +void bot_goto(Behaviour_t * caller, int16 left, int16 right){ + // Zielwerte speichern + mot_l_goto=left; + mot_r_goto=right; + + switch_to_behaviour(caller,bot_goto_behaviour,OVERRIDE); +} +#endif diff --git a/source/ct-Bot/bot-logic/behaviour_gotoxy.c b/source/ct-Bot/bot-logic/behaviour_gotoxy.c new file mode 100644 index 0000000..f5190ae --- /dev/null +++ b/source/ct-Bot/bot-logic/behaviour_gotoxy.c @@ -0,0 +1,141 @@ +/* + * 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_gotoxy.c + * @brief Bot faehrt eine Position an + * + * @author Benjamin Benz (bbe@heise.de) + * @date 03.11.06 +*/ + +#include "bot-logic/bot-logik.h" + +#ifdef BEHAVIOUR_GOTOXY_AVAILABLE +#include + + + + +/* Parameter fuer bot_gotoxy_behaviour-Verhalten */ +float target_x; /*!< Zielkoordinate X */ +float target_y; /*!< Zielkoordinate Y */ +float initialDiffX; /*!< Anfangsdifferenz in X-Richtung */ +float initialDiffY; /*!< Anfangsdifferenz in Y-Richtung */ + + +/*! + * Auslagerung der Berechnung der benoetigten Drehung aus dem gotoxy_behaviour + * @param xDiff + * @param yDiff + */ +int16 bot_gotoxy_calc_turn(float xDiff, float yDiff){ + float newHeading; + if(fabs(yDiff)>0.001 && fabs(xDiff)>0.001){ + newHeading=atan(yDiff/xDiff)*180/(M_PI); + if (xDiff<0) + newHeading+=180; + } else{ + if(fabs(xDiff)<=0.001) + newHeading=(yDiff>0)?90:(-90); + else + newHeading=(xDiff>0)?0:(180); + } + + int16 toTurn=(int16)newHeading-heading; + if (toTurn > 180) toTurn-=360; + if (toTurn < -180) toTurn+=360; + + return toTurn; +} + +/*! + * Das Verhalten faehrt von der aktuellen Position zur angegebenen Position (x/y) + * @param *data der Verhaltensdatensatz + * Verbessert von Thomas Noll, Jens Schoemann, Ben Horst (Philipps-Universitaet Marburg) + */ +void bot_gotoxy_behaviour(Behaviour_t *data){ + #define INITIAL_TURN 0 + #define GOTO_LOOP 1 + #define CORRECT_HEAD 2 + #define REACHED_POS 3 + static int16 speedLeft=BOT_SPEED_FOLLOW; + static int16 speedRight=BOT_SPEED_FOLLOW; + static int8 gotoState=INITIAL_TURN; + /* aus aktueller Position und Ziel neuen Zielwinkel berechnen */ + float xDiff=target_x-x_pos; + float yDiff=target_y-y_pos; + + if(xDiff*initialDiffX <0 || yDiff*initialDiffY <0){ // Hier kann noch verbessert werden + gotoState=REACHED_POS; // z.B. Abfragen statt *-Operation + speedWishLeft=BOT_SPEED_STOP; // bzw. neue Drehung statt Stehenbleiben + speedWishRight=BOT_SPEED_STOP; + } + + + switch(gotoState) { + case INITIAL_TURN: + gotoState=GOTO_LOOP; + bot_turn(data,bot_gotoxy_calc_turn(xDiff,yDiff)); + break; + + case GOTO_LOOP: + /* Position erreicht? */ + if ((xDiff)<10 || (yDiff)<10) { + gotoState=CORRECT_HEAD; + bot_turn(data,bot_gotoxy_calc_turn(xDiff,yDiff)); + break; + } + speedWishLeft=speedLeft; + speedWishRight=speedRight; + break; + + case CORRECT_HEAD: + /* Position erreicht? */ + if ((xDiff)<3 && (yDiff)<3) { + gotoState=REACHED_POS; + speedWishLeft=BOT_SPEED_STOP; + speedWishRight=BOT_SPEED_STOP; + break; + } + speedWishLeft=BOT_SPEED_SLOW; + speedWishRight=BOT_SPEED_SLOW; + break; + + case REACHED_POS: + gotoState=INITIAL_TURN; + return_from_behaviour(data); + break; + } + +} + +/*! + * Botenfunktion: Das Verhalten faehrt von der aktuellen Position zur angegebenen Position (x/y) + * @param caller Aufrufendes Verhalten + * @param x X-Ordinate an die der Bot fahren soll + * @param y Y-Ordinate an die der Bot fahren soll + */ +void bot_gotoxy(Behaviour_t *caller, float x, float y){ + target_x=x; + target_y=y; + initialDiffX=x-x_pos; + initialDiffY=y-y_pos; + switch_to_behaviour(caller, bot_gotoxy_behaviour, NOOVERRIDE); +} +#endif 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 + +/* 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 diff --git a/source/ct-Bot/bot-logic/behaviour_remotecall.c b/source/ct-Bot/bot-logic/behaviour_remotecall.c new file mode 100644 index 0000000..6e0125f --- /dev/null +++ b/source/ct-Bot/bot-logic/behaviour_remotecall.c @@ -0,0 +1,407 @@ +/* + * 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_remotecall_behaviour.c + * @brief ruft auf ein Kommando hin andere Verhalten auf und bestaetigt dann ihre Ausfuehrung + * + * @author Benjamin Benz (bbe@heise.de) + * @date 07.12.06 +*/ + +#include "bot-logic/bot-logik.h" +#ifdef BEHAVIOUR_REMOTECALL_AVAILABLE +#include +#include +#include +#include "log.h" +#include "command.h" + +#include "bot-logic/remote_calls.h" + +#define REMOTE_CALL_IDLE 0 +#define REMOTE_CALL_SCHEDULED 1 +#define REMOTE_CALL_RUNNING 2 + +/*! Uebergabevariable fuer Remotecall-Verhalten */ +static uint8 running_behaviour =REMOTE_CALL_IDLE; + +static uint8 function_id = 255; +static uint8 parameter_count = 0; /*!< Anzahl der Paramter (ohne Zeiger auf Aufrufer) */ +static uint8 parameter_data[8] = {0}; /*!< Hier liegen die eigentlichen Parameter, derzeit brauchen wir maximal 8 Byte (2 floats, 4 (u)int16 oder 4 (u)int8 */ +#ifdef MCU + static uint8 parameter_length[REMOTE_CALL_MAX_PARAM] = {0}; /*!< Hier speichern wir die Laenge der jeweiligen Parameter */ +#else + static uint8* parameter_length = NULL; /*!< Hier speichern wir die Laenge der jeweiligen Parameter */ +#endif + +#ifdef MCU + #include +#else + #define PROGMEM // Alibideklaration hat keine Funktion, verhindert aber eine Warning + #define strcmp_P strcmp // Auf dem PC gibt es keinen Flash, also auch kein eigenes Compare +#endif + +/*! + * Hier muessen alle Boten-Funktionen rein, die Remote aufgerufen werden sollen + * Diese stoßen dann das zugehoerige Verhalten an + * Ein Eintrag erfolgt so: + * PREPARE_REMOTE_CALL(BOTENFUNKTION,NUMBER_OF_PARAMS, STRING DER DIE PARAMETER BESCHREIBT,laenge der jeweiligen Parameter in Byte) + * + * Alle Botenfunktionen muessen folgendem Schema entsprechen + * void bot_xxx(Behaviour_t * caller, ...); + * + * Erklaerung am Bsp: + * PREPARE_REMOTE_CALL(bot_gotoxy, 2, "float x, float y", 4, 4), + * Name der Botenfunktion --^ ^ ^ ^ ^ + * Anzahl der Parameter --------- | | | + * Beschreibung der Parameter -------- | | + * Anzahl der Bytes Parameter 1 ------------------------ | + * Anzahl der Bytes Parameter 2 --------------------------- + * + * Zur Info: + * 1 Byte brauchen: uint8, int8,char + * 2 Byte brauchen: uint16, int16 + * 4 Byte brauchen: uint32, int32, float + */ +const call_t calls[] PROGMEM = { + #ifdef BEHAVIOUR_DRIVE_DISTANCE_AVAILABLE + PREPARE_REMOTE_CALL(bot_drive_distance,3, "int8 curve, int16 speed, int16 cm", 1,2,2), + #endif + #ifdef BEHAVIOUR_GOTOXY_AVAILABLE + PREPARE_REMOTE_CALL(bot_gotoxy, 2, "float x, float y", 4, 4), + #endif + #ifdef BEHAVIOUR_SOLVE_MAZE_AVAILABLE + PREPARE_REMOTE_CALL(bot_solve_maze,0,""), + #endif + #ifdef BEHAVIOUR_CATCH_PILLAR_AVAILABLE + PREPARE_REMOTE_CALL(bot_catch_pillar,0,""), + #endif + #ifdef BEHAVIOUR_DRIVE_SQUARE_AVAILABLE + PREPARE_REMOTE_CALL(bot_drive_square,0,""), + #endif + #ifdef BEHAVIOUR_FOLLOW_LINE_AVAILABLE + PREPARE_REMOTE_CALL(bot_follow_line,0,""), + #endif + #ifdef BEHAVIOUR_GOTO_AVAILABLE + PREPARE_REMOTE_CALL(bot_goto,2," int16 left, int16 right",2,2), + #endif + #ifdef BEHAVIOUR_OLYMPIC_AVAILABLE + PREPARE_REMOTE_CALL(bot_do_slalom,0,""), + #endif + #ifdef BEHAVIOUR_SCAN_AVAILABLE + PREPARE_REMOTE_CALL(bot_scan,0,""), + #endif + #ifdef BEHAVIOUR_SERVO_AVAILABLE + PREPARE_REMOTE_CALL(bot_servo,2,"uint8 servo, uint8 pos",1,1), + #endif + #ifdef BEHAVIOUR_SIMPLE_AVAILABLE + PREPARE_REMOTE_CALL(bot_simple,0,""), + PREPARE_REMOTE_CALL(bot_simple2,1,"int16 light",2), + #endif + #ifdef BEHAVIOUR_TURN_AVAILABLE + PREPARE_REMOTE_CALL(bot_turn,1,"int16 degrees",2) + #endif +}; + +#define STORED_CALLS (sizeof(calls)/sizeof(call_t)) /*!< Anzahl der Remote calls im Array */ + +/*! + * Sucht den Index des Remote-Calls heraus + * @param call String mit dem namen der gesuchten fkt + * @return Index in das calls-Array. Wenn nicht gefunden, dann 255 + */ +uint8 getRemoteCall(char * call){ +// LOG_DEBUG(("Suche nach Funktion: %s",call)); + + uint8 i; + for (i=0; i< (STORED_CALLS); i++){ + if (!strcmp_P (call, calls[i].name)){ + LOG_DEBUG(("calls[%d].name=%s passt",i,call)); + return i; + } + } + return 255; +} + +#ifdef MCU + /*! + * Hilfsfunktion fuer bot_remotecall() + * Baut einen AVR-kompatiblen Parameterstream aus einem uint32-Parameterarray und einem Infoarray ueber die Parameter + * @param dest Zeiger auf das Ausgabearray (len[0]*2 Byte gross!) + * @param count Anzahl der Parameter + * @param len Zeiger auf ein Array, das die Anzahl der Bytes fuer die jeweiligen Parameter enthaelt + * @param data Zeiger auf die Daten (32 Bit, Laenge 8) + * @author Timo Sandmann (mail@timosandmann.de) + * @date 12.01.2007 + */ + void remotecall_convert_params(uint8* dest, uint8 count, uint8* len, uint8* data){ + uint8 i; + /* jeden Parameter behandeln */ + for (i=0; i=0; j--) + *dest++ = data[j]; + data += 4; // data-Array ist immer in 32 Bit + } + } +#endif // MCU + +/*! + * Dieses Verhalten kuemmert sich darum die Verhalten, die von aussen angefragt wurden zu starten und liefert ein feedback zurueck, wenn sie beendet sind. + * @param *data der Verhaltensdatensatz + */ +void bot_remotecall_behaviour(Behaviour_t *data){ + uint8 call_id =255; + +// LOG_DEBUG(("Enter bot_remotecall_behaviour")); + void (* func) (struct _Behaviour_t *data); + + switch (running_behaviour) { + case REMOTE_CALL_SCHEDULED: // Es laueft kein Auftrag, aber es steht ein neuer an +// LOG_DEBUG(("REMOTE_CALL_SCHEDULED")); + + call_id=function_id; + if (call_id >= STORED_CALLS){ +// LOG_DEBUG(("keine Funktion gefunden. Exit")); + running_behaviour=REMOTE_CALL_IDLE; + return; + } + + #ifdef PC + // Auf dem PC liegt die calls-Struktur im RAM + func = (void*) calls[call_id].func; + #else + // Auf dem MCU liegt die calls-Struktur im Flash und muss erst geholt werden + func = (void*) pgm_read_word (& calls[call_id].func); + #endif + + if (parameter_count ==0 ){ // Kommen wir ohne Parameter aus? +// LOG_DEBUG(("call_id=%u",call_id)); + func(data); // Die aufgerufene Botenfunktion starten + running_behaviour=REMOTE_CALL_RUNNING; + } else if (parameter_count <= REMOTE_CALL_MAX_PARAM){ // Es gibt gueltige Parameter + // TODO: Ja hier wird es spannend, denn jetzt muessen die Parameter auf den Stack + LOG_DEBUG(("call_id=%u",call_id)); + LOG_DEBUG(("parameter_count=%u", parameter_count)); + // asm-hacks here ;) + #ifdef PC + /* Prinzip auf dem PC: Wir legen alle Parameter einzeln auf den Stack, springen in die Botenfunktion und raeumen anschliessend den Stack wieder auf */ + uint32 tmp; + uint8 i; + volatile uint8 td=1; // verwenden wir nur, damit der Compiler unsere inline-asm-Bloecke nicht umordnet + for (i=0; i0; i+=4,td++){ // Debug-Info ausgeben +// LOG_DEBUG(("parameter_data[%u-%u] = %lu",i, i+3, *(uint32*)(parameter_data+i))); + } + /* Erster Wert in parameter_length ist die Anzahl der Parameter (ohne Zeiger des Aufrufers) */ + for (i=0; i1; i++,td++){ // Check von td eigentlich sinnlos, aber wir brauchen eine echte Datenabhaengigkeit auf dieses Codestueck + /* cdecl-Aufrufkonvention => Parameter von rechts nach links auf den Stack */ + tmp = *(uint32*)(parameter_data+(parameter_count-i-1)*4); + /* Parameter 2 bis n pushen */ + asm volatile( // IA32-Support only + "pushl %0 # parameter i auf stack " + :: "g" (tmp) + : "memory" + ); + } + /* Parameter 1 (data) und Funktionsaufruf */ + asm volatile( + "pushl %0 # push data \n\t" + "movl %1, %%eax # adresse laden \n\t" + "call *%%eax # jump to callee \n\t" + :: "m" (data), "m" (func) + : "eax", "memory" + ); + /* caller rauemt den Stack wieder auf */ + for (i=0; i<=parameter_count && td>2; i++){ // Check von td erzwingt, dass das Aufraeumen erst jetzt passiert + asm volatile( + "pop %%eax # stack aufraeumen " + ::: "eax", "memory" + ); + } + #else + /* Prinzip auf der MCU: Keine komplizierten Rechnungen, sondern einfach alle Register ueberschreiben. + * Achtung: Derzeit braucht kein Verhalten mehr als 8 Register (2*float oder 4*int16 oder 4*int8), aendert sich das, + * muss man den Code hier erweitern! + * Die AVR-Konvention beim Funktionsaufruf: + * Die Groesse in Byte wird zur naechsten geraden Zahl aufgerundet, falls sie ungerade ist. + * Der Registerort faengt mit 26 an. + * Vom Registerort wird die berechete Groesse abgezogen und das Argument in diesen Registern uebergeben (LSB first). + * In r24/r25 legt der Compiler spaeter den Zeiger des Aufrufers, koennen wir hier also weglassen. */ +// LOG_DEBUG(("r22:r23 = %u:%u", parameter_data[1], parameter_data[0])); +// LOG_DEBUG(("r21:r20 = %u:%u", parameter_data[3], parameter_data[2])); +// LOG_DEBUG(("r18:r19 = %u:%u", parameter_data[5], parameter_data[4])); +// LOG_DEBUG(("r16:r17 = %u:%u", parameter_data[7], parameter_data[6])); + asm volatile( // remotecall_convert_params() hat den Speicher bereits richtig sortiert, nur noch Werte laden + "ld r23, Z+ \n\t" + "ld r22, Z+ \n\t" + "ld r21, Z+ \n\t" + "ld r20, Z+ \n\t" + "ld r19, Z+ \n\t" + "ld r18, Z+ \n\t" + "ld r17, Z+ \n\t" + "ld r16, Z " + :: "z" (parameter_data) + : "r23", "r22", "r21", "r20", "r19", "r18", "r17", "r16" + ); + func(data); // Die aufgerufene Botenfunktion starten + #endif + + running_behaviour=REMOTE_CALL_RUNNING; + return; + } else { +// LOG_DEBUG(("Parameteranzahl unzulaessig!")); + } + break; + + case REMOTE_CALL_RUNNING: // Es lief ein Verhalten und ist nun zuende (sonst waeren wir nicht hier) + { + // Antwort schicken + + char * function_name; + + #ifdef PC + function_name=(char*) &calls[function_id].name; + #else + // Auf dem MCU muessen wir die Daten erstmal aus dem Flash holen + + char tmp[REMOTE_CALL_FUNCTION_NAME_LEN+1]; + function_name=(char*)&tmp; + + uint8* from= (uint8*)& calls[function_id].name; + + uint8 i; + for (i=0; isubResult; + command_write_data(CMD_REMOTE_CALL,SUB_REMOTE_CALL_DONE,&result,&result,function_name); + #endif + +// LOG_DEBUG(("Remote-call %s beendet",function_name)); + + // Aufrauemen + function_id=255; + //parameter_length=NULL; + //parameter_data=NULL; + running_behaviour=REMOTE_CALL_IDLE; + + return_from_behaviour(data); // und Verhalten auch aus + break; + } + default: + return_from_behaviour(data); // und Verhalten auch aus + break; + + } +} + +/*! + * Fuehre einen remote_call aus. Es gibt KEIN aufrufendes Verhalten!! + * @param func Zeiger auf den Namen der Fkt + * @param data Zeiger auf die Daten + */ +void bot_remotecall(char* func, remote_call_data_t* data){ + + function_id= getRemoteCall(func); + if (function_id >= STORED_CALLS){ +// LOG_DEBUG(("Funktion %s nicht gefunden. Exit!",func)); + return; + } + + // parameter_length: Zeiger auf ein Array, das zuerst die Anzahl der Parameter und danach die Anzahl der Bytes fuer die jeweiligen Parameter enthaelt + #ifdef PC + parameter_count = calls[function_id].param_count; + parameter_length = (uint8*)calls[function_id].param_len; + #else + // Auf dem MCU muessen wir die Daten erstmal aus dem Flash holen + uint8* from= (uint8*)& calls[function_id].param_len; + uint8 i; + parameter_count = pgm_read_byte(&calls[function_id].param_count); + for (i=0; iname,call->param_info)); + } +} + +#endif diff --git a/source/ct-Bot/bot-logic/behaviour_scan.c b/source/ct-Bot/bot-logic/behaviour_scan.c new file mode 100644 index 0000000..6f6869c --- /dev/null +++ b/source/ct-Bot/bot-logic/behaviour_scan.c @@ -0,0 +1,144 @@ +/* + * 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_scan.c + * @brief Scannt die Umgebung und traegt sie in die Karte ein + * + * @author Benjamin Benz (bbe@heise.de) + * @date 03.11.06 +*/ + +#include "bot-logic/bot-logik.h" +#include "map.h" +#include + +#ifdef BEHAVIOUR_SCAN_AVAILABLE +/*! + * Der Roboter faehrt einen Vollkreis und scannt dabei die Umgebung + * @param *data der Verhaltensdatensatz + */ +void bot_scan_onthefly_behaviour(Behaviour_t *data){ + #define ONTHEFLY_DIST_RESOLUTION 0.02 /*!< Alle wieviel gefahrene Strecke [m] soll die Karte aktualisiert werden. Achtung er prueft x und y getrennt, daher ist die tatsaechlich zurueckgelegte Strecke im worst case sqrt(2)*ONTHEFLY_DIST_RESOLUTION */ + #define ONTHEFLY_ANGLE_RESOLUTION 10 /*!< Alle wieviel Gerad Drehung [m] soll die Karte aktualisiert werden */ + + + + #ifdef MAP_AVAILABLE + static float last_x, last_y, last_head; + + float diff_x = fabs(x_pos-last_x); + float diff_y = fabs(y_pos-last_y); + + // Wenn der Bot faehrt, aktualisieren wir alles + if ((diff_x > ONTHEFLY_DIST_RESOLUTION) ||( diff_y > ONTHEFLY_DIST_RESOLUTION)){ + update_map_location(x_pos,y_pos); + update_map(x_pos,y_pos,heading,sensDistL,sensDistR); + + last_x=x_pos; + last_y=y_pos; + last_head=heading; + return; + } + + float diff_head = fabs(last_head-heading); + // Wenn der bot nur dreht, aktualisieren wir nur die Blickstrahlen + if ( diff_head > ONTHEFLY_ANGLE_RESOLUTION ){ + update_map(x_pos,y_pos,heading,sensDistL,sensDistR); + last_head=heading; + } + /* if ((diff_x*diff_x + diff_y*diff_y > ONTHEFLY_DIST_RESOLUTION)||fabs(last_head-heading) > ONTHEFLY_ANGLE_RESOLUTION ){ + last_x=x_pos; + last_y=y_pos; + last_head=heading; + print_map(); + } + */ + #endif +} + + +#define BOT_SCAN_STATE_START 0 +uint8 bot_scan_state = BOT_SCAN_STATE_START; /*!< Zustandsvariable fuer bot_scan_behaviour */ + +/*! + * Der Roboter faehrt einen Vollkreis und scannt dabei die Umgebung + * @param *data der Verhaltensdatensatz + */ +void bot_scan_behaviour(Behaviour_t *data){ + #define BOT_SCAN_STATE_SCAN 1 + + #define ANGLE_RESOLUTION 5 /*!< Aufloesung fuer den Scan in Grad */ + +// static uint16 bot_scan_start_angle; /*!< Winkel, bei dem mit dem Scan begonnen wurde */ + static float turned; /*!< Winkel um den bereits gedreht wurde */ + + static float last_scan_angle; /*!< Winkel bei dem zuletzt gescannt wurde */ + + float diff; + + switch (bot_scan_state){ + case BOT_SCAN_STATE_START: + + turned=0; + last_scan_angle=heading-ANGLE_RESOLUTION; + bot_scan_state=BOT_SCAN_STATE_SCAN; + break; + case BOT_SCAN_STATE_SCAN: + diff = heading - last_scan_angle; + if (diff < -180) + diff+=360; + if (diff*1.15 >= ANGLE_RESOLUTION){ + turned+= diff; + last_scan_angle=heading; + + #ifdef MAP_AVAILABLE + // Eigentlicher Scan hier + update_map(x_pos,y_pos,heading,sensDistL,sensDistR); + //////////// + #endif + + } + + if (turned >= 360-ANGLE_RESOLUTION) // Ende erreicht + bot_scan_state++; + break; + default: + bot_scan_state = BOT_SCAN_STATE_START; + #ifdef MAP_AVAILABLE + print_map(); + #endif + return_from_behaviour(data); + break; + } +} + +/*! + * Der Roboter faehrt einen Vollkreis und scannt dabei die Umgebung + * @param Der aufrufer + */ +void bot_scan(Behaviour_t* caller){ + + bot_scan_state = BOT_SCAN_STATE_START; + bot_turn(caller,360); + switch_to_behaviour(0, bot_scan_behaviour,OVERRIDE); + +// update_map(x_pos,y_pos,heading,sensDistL,sensDistR); +// print_map(); +} +#endif diff --git a/source/ct-Bot/bot-logic/behaviour_servo.c b/source/ct-Bot/bot-logic/behaviour_servo.c new file mode 100644 index 0000000..77583cc --- /dev/null +++ b/source/ct-Bot/bot-logic/behaviour_servo.c @@ -0,0 +1,68 @@ +/* + * 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_servo.c + * @brief kontrolliert die Servos + * + * @author Benjamin Benz (bbe@heise.de) + * @date 07.12.06 +*/ + + +#include "bot-logic/bot-logik.h" +#ifdef BEHAVIOUR_SERVO_AVAILABLE + +#include "motor.h" +#include "timer.h" +/*! Uebergabevariable fuer Servo-Verhalten */ +static uint16 servo_time; +static uint8 servo_nr; +static uint8 servo_pos; +uint8 servo_active = 0; /*!< 0, wenn kein Servo aktiv, sonst Bit der gerade aktiven Servos gesetzt */ + +/*! + * Dieses Verhalten fuehrt ein Servo-Kommando aus und schaltet danach den Servo wieder ab + * + * @param *data der Verhaltensdatensatz + */ +void bot_servo_behaviour(Behaviour_t *data){ + /* Servo ausschalten, falls Klappe zu oder Countdown abgelaufen */ + if ( (servo_pos == DOOR_CLOSE && sensDoor == 0) || (TIMER_GET_TICKCOUNT_16 - servo_time > MS_TO_TICKS(1000L)) ){ + servo_set(servo_nr, SERVO_OFF); // Servo aus + servo_active &= ~servo_nr; + return_from_behaviour(data); // und Verhalten auch aus + } +} + +/*! + * Fahre den Servo an eine Position + * @param servo Nummer des Servos + * @param pos Zielposition des Servos + */ +void bot_servo(Behaviour_t * caller, uint8 servo, uint8 pos){ + servo_active |= servo; + servo_set(servo, pos); // Servo-PWM einstellen + servo_pos = pos; // Zielposition merken + servo_nr = servo; // Servo-Nr speichern + servo_time = TIMER_GET_TICKCOUNT_16; // Der Count down verschafft dem Servo etwas Zeit + + switch_to_behaviour(caller,bot_servo_behaviour,OVERRIDE); // Warte-Verahlten an +} +#endif diff --git a/source/ct-Bot/bot-logic/behaviour_simple.c b/source/ct-Bot/bot-logic/behaviour_simple.c new file mode 100644 index 0000000..5c897e9 --- /dev/null +++ b/source/ct-Bot/bot-logic/behaviour_simple.c @@ -0,0 +1,139 @@ +/* + * 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_simple.c + * @brief ganz einfache Beispielverhalten + * Diese Datei sollte der Einstiegspunkt fuer eigene Experimente sein, + * den Roboter zu steuern. + * + * @author Benjamin Benz (bbe@heise.de) + * @date 03.11.06 +*/ + + +#include "bot-logic/bot-logik.h" +#ifdef BEHAVIOUR_SIMPLE_AVAILABLE + + +/*! + * Ein ganz einfaches Verhalten, es hat maximale Prioritaet + * Hier kann man auf ganz einfache Weise die ersten Schritte wagen. + * Wer die Moeglichkeiten des ganzen Verhaltensframeworks ausschoepfen will, kann diese Funktion getrost auskommentieren + * und findet dann in bot_behave_init() und bot_behave() weitere Hinweise fuer elegante Bot-Programmierung.... + * + * Das Verhalten ist per default abgeschaltet. Daher muss man es erst in bot_behave_init() aktivieren. + * Dort steht aber bereits eine auskommentierte Zeile dazu, von der man nur die zwei Kommentarzeichen wegnehmen muss. + * Achtung, da bot_simple_behaviour() maximale Prioritaet hat, kommt es vor den anderen Regeln, wie dem Schutz vor Abgruenden, etc. zum Zuge + * Das sollte am Anfang nicht stoeren, spaeter sollte man jedoch die Prioritaet herabsetzen. + * + * @param *data der Verhaltensdatensatz + */ +void bot_simple_behaviour(Behaviour_t *data){ + static uint8 state=0; + + switch (state){ + case 0: + bot_drive_distance(data ,0 , BOT_SPEED_MAX, 14); + state++; + break; + case 1: + bot_turn(data , 90); + state=0; + break; + default: + state=0; + return_from_behaviour(data); + break; + } +} + + +/*! + * Rufe das Simple-Verhalten auf + * @param caller Der obligatorische Verhaltensdatensatz des Aufrufers + */ +void bot_simple(Behaviour_t * caller, int16 light){ + switch_to_behaviour(caller,bot_simple_behaviour,OVERRIDE); +} + + +/*! Uebergabevariable fuer SIMPLE2 */ +static int16 simple2_light=0; + +/*! + * Ein ganz einfaches Beispiel fuer ein Hilfsverhalten, + * das selbst SpeedWishes aussert und + * nach getaner Arbeit die aufrufende Funktion wieder aktiviert + * Zudem prueft es, ob eine Uebergabebedingung erfuellt ist. + * + * Zu diesem Verhalten gehoert die Botenfunktion bot_simple2() + * + * Hier kann man auf ganz einfache Weise die ersten Schritte wagen. + * Wer die Moeglichkeiten des ganzen Verhaltensframeworks ausschoepfen will, kann diese Funktion getrost auskommentieren + * und findet dann in bot_behave_init() und bot_behave() weitere Hinweise fuer elegante Bot-Programmierung.... + * + * Das Verhalten ist per default abgeschaltet. Daher muss man es erst in bot_behave_init() aktivieren. + * Dort steht aber bereits eine auskommentierte Zeile dazu, von der man nur die zwei Kommentarzeichen wegnehmen muss. + * Achtung, da bot_simple2_behaviour() maximale Prioritaet hat, kommt es vor den anderen Regeln, wie dem Schutz vor Abgruenden, etc. zum Zuge + * Das sollte am Anfang nicht stoeren, spaeter sollte man jedoch die Prioritaet herabsetzen. + * + * bot_simple2_behaviour faehrt den Bot solange geradeaus, bis es dunkler als im Uebergabeparameter spezifiziert ist wird + * + * @param *data der Verhaltensdatensatz + */ +void bot_simple2_behaviour(Behaviour_t *data){ + #define STATE_SIMPLE2_INIT 0 + #define STATE_SIMPLE2_WORK 1 + #define STATE_SIMPLE2_DONE 2 + static uint8 state = 0; + + switch (state) { + case STATE_SIMPLE2_INIT: + // Nichts zu tun + state=STATE_SIMPLE2_WORK; + break; + case STATE_SIMPLE2_WORK: + // Fahre ganz schnell + speedWishLeft = BOT_SPEED_FAST; + speedWishRight = BOT_SPEED_FAST; + if (sensLDRL< simple2_light) // Beispielbedingung + // Wenn es dunkler als angegeben wird, dann haben wir unserd Ziel erreicht + state=STATE_SIMPLE2_DONE; + break; + + case STATE_SIMPLE2_DONE: /* Sind wir fertig, dann Kontrolle zurueck an Aufrufer */ + state=STATE_SIMPLE2_INIT; + return_from_behaviour(data); + break; + } +} + +/*! + * Rufe das Simple2-Verhalten auf und uebergebe light + * @param caller Der obligatorische Verhaltensdatensatz des Aufrufers + * @param light Uebergabeparameter + */ +void bot_simple2(Behaviour_t * caller, int16 light){ + simple2_light=light; + + // Zielwerte speichern + switch_to_behaviour(caller,bot_simple2_behaviour,OVERRIDE); +} +#endif 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 +#include +#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 (sensorOPTIMAL_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 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_moustart_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 && sensLineLGROUND_GOAL-20 && sensLineR +#endif +#include +#include + +/* Parameter fuer das bot_turn_behaviour() */ +#ifndef MEASURE_MOUSE_AVAILABLE +int16 turn_targetR; /*!< Zu drehender Winkel bzw. angepeilter Stand des Radencoders sensEncR */ +int16 turn_targetL; /*!< Zu drehender Winkel bzw. angepeilter Stand des Radencoders sensEncL */ +#else +int8 angle_correct=0; /*!< Drehabschnitt 0=0-15Grad, 1=16-45 Grad, 2= >45 Grad */ +int16 to_turn; /*!< Wieviel Grad sind noch zu drehen? */ + #ifdef MCU + uint8 __attribute__ ((section (".eeprom"))) err15=1; /*!< Fehler bei Drehungen unter 15 Grad */ + uint8 __attribute__ ((section (".eeprom"))) err45=2; /*!< Fehler bei Drehungen zwischen 15 und 45 Grad */ + uint8 __attribute__ ((section (".eeprom"))) err_big=4; /*!< Fehler bei groesseren Drehungen */ + #else + uint8 err15=0; + uint8 err45=0; + uint8 err_big=0; + #endif +#endif +int8 turn_direction; /*!< Richtung der Drehung */ + + +/* Hilfsfunktion zur Berechnung einer Winkeldifferenz */ +inline int16 calc_turned_angle(int8 direction, int16 angle1, int16 angle2) { + int16 diff_angle=0; + + if (direction>0){ + // Drehung in mathematisch positivem Sinn + if (angle1>angle2) { + // Es gab einen Ueberlauf + diff_angle=360-angle1+angle2; + } else { + diff_angle=angle2-angle1; + } + } else { + // Drehung in mathematisch negativem Sinn + if (angle1 15 Grad zunaechst mit hoeherer Geschwindigkeit ausgefuehrt. Bei kleineren ++ * Winkeln oder wenn nur noch 15 Grad zu drehen sind, nur noch mit geringer Geschwindigkeit + * @param *data der Verhaltensdatensatz + * @see bot_turn() + */ +void bot_turn_behaviour(Behaviour_t *data){ + // Zustaende fuer das bot_turn_behaviour-Verhalten + #define NORMAL_TURN 0 + #define STOP_TURN 1 + #define FULL_STOP 2 + static int8 turnState=NORMAL_TURN; + static int16 old_heading=-1; + static int16 head_count=0; + uint8 e15; + uint8 e45; + uint8 ebig; + + // seit dem letzten mal gedrehte Grad + int16 turned=0; + // aktueller Winkel als int16 + int16 akt_heading=(int16)heading_mou; + + // erster Aufruf? -> alter Winkel==neuer Winkel + if (old_heading==-1) old_heading=akt_heading; + + // berechnen, wieviel Grad seit dem letzten Aufruf gedreht wurden + turned=calc_turned_angle(turn_direction,old_heading,akt_heading); + if (turned > 300) turned -= 360; // hier ging etwas schief + + // aktueller Winkel wird alter Winkel + old_heading=akt_heading; + // aktuelle Drehung von zu drehendem Winkel abziehen + to_turn-=turned; + + switch(turnState) { + case NORMAL_TURN: + // Solange drehen, bis Drehwinkel erreicht ist + // oder gar zu weit gedreht wurde + if (to_turn<1) { + /* Nachlauf abwarten */ + speedWishLeft=BOT_SPEED_STOP; + speedWishRight=BOT_SPEED_STOP; + turnState=STOP_TURN; + break; + } + speedWishLeft = (turn_direction > 0) ? -BOT_SPEED_NORMAL : BOT_SPEED_NORMAL; speedWishLeft = (turn_direction > 0) ? -BOT_SPEED_SLOW : BOT_SPEED_SLOW; + speedWishRight = (turn_direction > 0) ? BOT_SPEED_NORMAL : -BOT_SPEED_NORMAL; speedWishRight = (turn_direction > 0) ? BOT_SPEED_SLOW : -BOT_SPEED_SLOW; + break; + + case STOP_TURN: + // Abwarten, bis Nachlauf beendet + if (akt_heading!=old_heading){ + head_count=0; + speedWishLeft=BOT_SPEED_STOP; + speedWishRight=BOT_SPEED_STOP; + break; + } + if (head_count<10) { + head_count++; + speedWishLeft=BOT_SPEED_STOP; + speedWishRight=BOT_SPEED_STOP; + break; + } + #ifdef MCU + e15=eeprom_read_byte(&err15); + e45=eeprom_read_byte(&err45); + ebig=eeprom_read_byte(&err_big); + #else + e15=err15; + e45=err45; + ebig=err_big; + #endif + + // Neue Abweichung mit alter vergleichen und ggfs neu bestimmen + + switch(angle_correct) { + case 0: + if (abs(to_turn)-e15>1) { + e15=(int8)(abs(to_turn)+e15)/2; + #ifdef MCU + eeprom_write_byte(&err15,e15); + #else + err15=e15; + #endif + } + break; + + case 1: + if (abs(to_turn)-e45>1) { + e45=(int8)(abs(to_turn)+e45)/2; + #ifdef MCU + eeprom_write_byte(&err45,e45); + #else + err45=e45; + #endif + } + break; + + case 2: + if (abs(to_turn)-ebig>1) { + ebig=(int8)(abs(to_turn)+ebig)/2; + #ifdef MCU + eeprom_write_byte(&err_big,ebig); + #else + err_big=ebig; + #endif + } + break; + } + // ok, verhalten beenden + speedWishLeft=BOT_SPEED_STOP; + speedWishRight=BOT_SPEED_STOP; + turnState=NORMAL_TURN; + old_heading=-1; + return_from_behaviour(data); + break; + } +} + +void bot_turn(Behaviour_t *caller, int16 degrees) +{ + // Richtungsgerechte Umrechnung in den Zielwinkel + if(degrees < 0) turn_direction = -1; + else turn_direction = 1; + to_turn=abs(degrees); + #ifdef MCU + if (eeprom_read_byte(&err15)==255 && eeprom_read_byte(&err45)==255 && eeprom_read_byte(&err_big)==255) { + eeprom_write_byte(&err15,1); + eeprom_write_byte(&err45,2); + eeprom_write_byte(&err_big,4); + } + if (to_turn>45) { + to_turn-=eeprom_read_byte(&err_big); + angle_correct=2; + } else if (to_turn<=45 && to_turn>15) { + to_turn-=eeprom_read_byte(&err45); + angle_correct=1; + } else { + to_turn-=eeprom_read_byte(&err15); + angle_correct=0; + } + #else + if (to_turn>45) { + to_turn-=err_big; + angle_correct=2; + } else if (to_turn<=45 && to_turn>15) { + to_turn-=err45; + angle_correct=1; + } else { + to_turn-=err15; + angle_correct=0; + } + #endif + switch_to_behaviour(caller, bot_turn_behaviour,NOOVERRIDE); + } +#else +/*! + * Das Verhalten laesst den Bot eine Punktdrehung durchfuehren. + * @see bot_turn() + * */ +void bot_turn_behaviour(Behaviour_t* data){ + /* Drehen findet in vier Schritten statt. Die Drehung wird dabei + * bei Winkeln > 90 Grad zunaechst mit maximaler Geschwindigkeit ausgefuehrt. Bei kleineren + * Winkeln oder wenn nur noch 90 Grad zu drehen sind, nur noch mit normaler Geschwindigkeit + */ + /* Zustaende fuer das bot_turn_behaviour-Verhalten */ + #define NORMAL_TURN 0 + #define SHORT_REVERSE 1 + #define CORRECT_POSITION 2 + #define FULL_STOP 3 + static int8 turnState=NORMAL_TURN; + /* zu drehende Schritte in die korrekte Drehrichtung korrigieren */ + int16 to_turnR = turn_direction*(turn_targetR - sensEncR); + int16 to_turnL = -turn_direction*(turn_targetL - sensEncL); + + switch(turnState) { + case NORMAL_TURN: + /* Solange drehen, bis beide Encoder nur noch zwei oder weniger Schritte zu fahren haben */ + + if (to_turnL <= 2 && to_turnR<=2){ + /* nur noch 2 Schritte oder weniger, abbremsen einleiten */ + turnState=SHORT_REVERSE; + break; + } + + /* Bis 90 Grad kann mit maximaler Geschwindigkeit gefahren werden, danach auf Normal reduzieren */ + /* Geschwindigkeit fuer beide Raeder getrennt ermitteln */ + if(abs(to_turnL) < ANGLE_CONSTANT*0.25) { + speedWishLeft = (turn_direction > 0) ? -BOT_SPEED_MEDIUM : BOT_SPEED_MEDIUM; + } else { + speedWishLeft = (turn_direction > 0) ? -BOT_SPEED_NORMAL : BOT_SPEED_NORMAL; + } + + if(abs(to_turnR) < ANGLE_CONSTANT*0.25) { + speedWishRight = (turn_direction > 0) ? BOT_SPEED_MEDIUM : -BOT_SPEED_MEDIUM; + } else { + speedWishRight = (turn_direction > 0) ? BOT_SPEED_NORMAL : -BOT_SPEED_NORMAL; + } + + break; + + case SHORT_REVERSE: + /* Ganz kurz durch umpolen anbremsen */ + speedWishLeft = (turn_direction > 0) ? BOT_SPEED_SLOW : -BOT_SPEED_SLOW; + speedWishRight = (turn_direction > 0) ? -BOT_SPEED_SLOW : BOT_SPEED_SLOW; + turnState=CORRECT_POSITION; + break; + + case CORRECT_POSITION: + /* Evtl. etwas zuruecksetzen, falls wir zu weit gefahren sind */ + if (to_turnR<0) { + /* rechts zu weit gefahren..langsam zurueck */ + speedWishRight = (turn_direction > 0) ? -BOT_SPEED_SLOW : BOT_SPEED_SLOW; + } else if (to_turnR>0) { + /* rechts noch nicht weit genug...langsam vor */ + speedWishRight = (turn_direction > 0) ? BOT_SPEED_SLOW : -BOT_SPEED_SLOW; + } else { + /* Endposition erreicht, rechtes Rad anhalten */ + speedWishRight = BOT_SPEED_STOP; + } + + if (to_turnL<0) { + /* links zu weit gefahren..langsam zurueck */ + speedWishLeft = (turn_direction > 0) ? BOT_SPEED_SLOW : -BOT_SPEED_SLOW; + } else if (to_turnL>0) { + /* links noch nicht weit genug...langsam vor */ + speedWishLeft = (turn_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 */ + turnState=FULL_STOP; + } + break; + + + default: + /* ist gleichzeitig FULL_STOP, da gleiche Aktion + * Stoppen, State zuruecksetzen und Verhalten beenden */ + speedWishLeft = BOT_SPEED_STOP; + speedWishRight = BOT_SPEED_STOP; + turnState=NORMAL_TURN; + return_from_behaviour(data); + break; + } +} + +/*! + * Dreht den Bot im mathematisch positiven Sinn. + * @param degrees Grad, um die der Bot gedreht wird. Negative Zahlen drehen im (mathematisch negativen) Uhrzeigersinn. + * Die Aufloesung betraegt rund 3 Grad + */ +void bot_turn(Behaviour_t* caller,int16 degrees){ + /* Umrechnung von Grad in Encoder-Markierungen. + * Hinweis: Eigentlich muessten der Umfang von Bot und Rad verwendet werden. Die Rechnung wird + * allerdings viel einfacher, wenn man Pi auskuerzt. + * Ist degrees negativ, ist die Drehung negativ und der rechte Encoder muss kleiner werden. + */ + + if(degrees < 0) turn_direction = -1; + else turn_direction = 1; + /* Anzahl zu fahrender Encoderschritte berechnen */ + turn_targetR=(degrees*ANGLE_CONSTANT)/360; + /* linkes Rad dreht entgegengesetzt, daher negativer Wert */ + turn_targetL=-turn_targetR; + + /* aktuellen Sensorwert zu zu drehenden Encoderschritten addieren */ + turn_targetR+=sensEncR; + turn_targetL+=sensEncL; + switch_to_behaviour(caller, bot_turn_behaviour,OVERRIDE); +} +#endif +#endif + diff --git a/source/ct-Bot/bot-logic/bot-logik.c b/source/ct-Bot/bot-logic/bot-logik.c new file mode 100644 index 0000000..3274375 --- /dev/null +++ b/source/ct-Bot/bot-logic/bot-logik.c @@ -0,0 +1,500 @@ +/* + * c't-Sim - Robotersimulator fuer den 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 bot-logik.c + * @brief High-Level Routinen fuer die Steuerung des c't-Bots. + * Diese Datei sollte der Einstiegspunkt fuer eigene Experimente sein, + * den Roboter zu steuern. + * + * bot_behave() arbeitet eine Liste von Verhalten ab. + * Jedes Verhalten kann entweder absolute Werte setzen, dann kommen niedrigerpriorisierte nicht mehr dran. + * Alternativ dazu kann es Modifikatoren aufstellen, die bei niedriger priorisierten angewendet werden. + * bot_behave_init() baut diese Liste auf. + * Jede Verhaltensfunktion bekommt einen Verhaltensdatensatz uebergeben, in den Sie ihre Daten eintraegt + * + * @author Benjamin Benz (bbe@heise.de) + * @author Christoph Grimmer (c.grimmer@futurio.de) + * @date 01.12.05 +*/ + + +#include "bot-logic/bot-logik.h" + +#ifdef BEHAVIOUR_AVAILABLE + +#include "display.h" +#include "rc5.h" + + + +#include + +int16 speedWishLeft; /*!< Puffervariablen fuer die Verhaltensfunktionen absolut Geschwindigkeit links*/ +int16 speedWishRight; /*!< Puffervariablen fuer die Verhaltensfunktionen absolut Geschwindigkeit rechts*/ + +float faktorWishLeft; /*!< Puffervariablen fuer die Verhaltensfunktionen Modifikationsfaktor links*/ +float faktorWishRight; /*!< Puffervariablen fuer die Verhaltensfunktionen Modifikationsfaktor rechts */ + +int16 target_speed_l=BOT_SPEED_STOP; /*!< Sollgeschwindigkeit linker Motor - darum kuemmert sich bot_base()*/ +int16 target_speed_r=BOT_SPEED_STOP; /*!< Sollgeschwindigkeit rechter Motor - darum kuemmert sich bot_base() */ + + +/*! Liste mit allen Verhalten */ +Behaviour_t *behaviour = NULL; + +/*! + * Das einfachste Grundverhalten + * @param *data der Verhaltensdatensatz + */ +void bot_base_behaviour(Behaviour_t *data){ + speedWishLeft=target_speed_l; + speedWishRight=target_speed_r; +} + +/*! + * Initialisert das ganze Verhalten + */ +void bot_behave_init(void){ + #ifdef BEHAVIOUR_REMOTECALL_AVAILABLE + // Dieses Verhalten kann andere Starten + insert_behaviour_to_list(&behaviour, new_behaviour(254, bot_remotecall_behaviour,INACTIVE)); + #endif + + #ifdef BEHAVIOUR_SERVO_AVAILABLE + insert_behaviour_to_list(&behaviour, new_behaviour(253, bot_servo_behaviour,INACTIVE)); + #endif + + // Demo-Verhalten, ganz einfach, inaktiv + // Achtung, im Moment hat es eine hoehere Prioritaet als die Gefahrenerkenner!!! + #ifdef BEHAVIOUR_SIMPLE_AVAILABLE + insert_behaviour_to_list(&behaviour, new_behaviour(252, bot_simple_behaviour,INACTIVE)); + insert_behaviour_to_list(&behaviour, new_behaviour(251, bot_simple2_behaviour,INACTIVE)); + #endif + + + // Hoechste Prioritate haben die Notfall Verhalten + + // Verhalten zum Schutz des Bots, hohe Prioritaet, Aktiv + #ifdef BEHAVIOUR_AVOID_BORDER_AVAILABLE + insert_behaviour_to_list(&behaviour, new_behaviour(250, bot_avoid_border_behaviour,ACTIVE)); + #endif + #ifdef BEHAVIOUR_AVOID_COL_AVAILABLE + insert_behaviour_to_list(&behaviour, new_behaviour(249, bot_avoid_col_behaviour,ACTIVE)); + #endif + + #ifdef BEHAVIOUR_SCAN_AVAILABLE + // Verhalten, das die Umgebung des Bots on-the fly beim fahren scannt + insert_behaviour_to_list(&behaviour, new_behaviour(155, bot_scan_onthefly_behaviour,ACTIVE)); + + // Verhalten, das einmal die Umgebung des Bots scannt + insert_behaviour_to_list(&behaviour, new_behaviour(152, bot_scan_behaviour,INACTIVE)); + #endif + + // Alle Hilfsroutinen sind relativ wichtig, da sie auch von den Notverhalten her genutzt werden + // Hilfsverhalten, die Befehle von Boten-Funktionen ausfuehren, erst inaktiv, werden von Boten aktiviert + #ifdef BEHAVIOUR_TURN_AVAILABLE + insert_behaviour_to_list(&behaviour, new_behaviour(150, bot_turn_behaviour,INACTIVE)); + #endif + #ifdef BEHAVIOUR_DRIVE_DISTANCE_AVAILABLE + insert_behaviour_to_list(&behaviour, new_behaviour(149, bot_drive_distance_behaviour,INACTIVE)); + #endif + #ifdef BEHAVIOUR_GOTO_AVAILABLE + insert_behaviour_to_list(&behaviour, new_behaviour(148, bot_goto_behaviour,INACTIVE)); + #endif + + // Hilfsverhalten zum Anfahren von Positionen + #ifdef BEHAVIOUR_GOTOXY_AVAILABLE + insert_behaviour_to_list(&behaviour, new_behaviour(147, bot_gotoxy_behaviour,INACTIVE)); + #endif + + + #ifdef BEHAVIOUR_CATCH_PILLAR_AVAILABLE + insert_behaviour_to_list(&behaviour, new_behaviour(44, bot_catch_pillar_behaviour,INACTIVE)); + #endif + + + #ifdef BEHAVIOUR_OLYMPIC_AVAILABLE + bot_olympic_init(52,100,INACTIVE); + #endif + + #ifdef BEHAVIOUR_FOLLOW_LINE_AVAILABLE + // Verhalten um einer Linie zu folgen + insert_behaviour_to_list(&behaviour, new_behaviour(70, bot_follow_line_behaviour, ACTIVE)); + #endif + + #ifdef BEHAVIOUR_SOLVE_MAZE_AVAILABLE + bot_solve_maze_init(100,43,INACTIVE); + #endif + + #ifdef BEHAVIOUR_DRIVE_SQUARE_AVAILABLE + // Demo-Verhalten, etwas komplexer, inaktiv + insert_behaviour_to_list(&behaviour, new_behaviour(51, bot_drive_square_behaviour,INACTIVE)); + #endif + + + + // Grundverhalten, setzt aeltere FB-Befehle um, aktiv + insert_behaviour_to_list(&behaviour, new_behaviour(2, bot_base_behaviour, ACTIVE)); + + // Um das Simple-Behaviour zu nutzen, die Kommentarzeichen vor der folgenden Zeile entfernen!!! + // activateBehaviour(bot_simple_behaviour); + // activateBehaviour(bot_simple2_behaviour); + + #ifdef PC + #ifdef DISPLAY_AVAILABLE + /* Anzeigen der geladenen Verhalten */ + Behaviour_t *ptr = behaviour; + + display_cursor(5,1); + display_printf("Verhaltensstack:\n"); + while(ptr != NULL) { + display_printf("Prioritaet: %d.\n", ptr->priority); + ptr = ptr->next; + } + #endif + #endif +} + + +/*! + * Aktiviert eine Regel mit gegebener Funktion + * @param function Die Funktion, die das Verhalten realisiert. + */ +void activateBehaviour(BehaviourFunc function){ + Behaviour_t *job; // Zeiger auf ein Verhalten + + // Einmal durch die Liste gehen, bis wir den gewuenschten Eintrag haben + for (job = behaviour; job; job = job->next) { + if (job->work == function) { + job->active = ACTIVE; + break; + } + } +} + + +/*! + * Deaktiviert eine Regel mit gegebener Funktion + * @param function Die Funktion, die das Verhalten realisiert. + */ +void deactivateBehaviour(BehaviourFunc function){ + Behaviour_t *job; // Zeiger auf ein Verhalten + + // Einmal durch die Liste gehen, bis wir den gewuenschten Eintrag haben + for (job = behaviour; job; job = job->next) { + if (job->work == function) { + job->active = INACTIVE; + break; + } + } +} + +/*! + * Ruft ein anderes Verhalten auf und merkt sich den Ruecksprung + * return_from_behaviour() kehrt dann spaeter wieder zum aufrufenden Verhalten zurueck + * @param from aufrufendes Verhalten + * @param to aufgerufenes Verhalten + * @param override Hier sind zwei Werte Moeglich: + * 1. OVERRIDE : Das Zielverhalten to wird aktiviert, auch wenn es noch aktiv ist. + * Das Verhalten, das es zuletzt aufgerufen hat wird dadurch automatisch + * wieder aktiv und muss selbst sein eigenes Feld subResult auswerten, um zu pruefen, ob das + * gewuenschte Ziel erreicht wurde, oder vorher ein Abbruch stattgefunden hat. + * 2. NOOVERRIDE : Das Zielverhalten wird nur aktiviert, wenn es gerade nichts zu tun hat. + * In diesem Fall kann der Aufrufer aus seinem eigenen subResult auslesen, + * ob seibem Wunsch Folge geleistet wurde. + */ +void switch_to_behaviour(Behaviour_t * from, void *to, uint8 override ){ + Behaviour_t *job; // Zeiger auf ein Verhalten + + // Einmal durch die Liste gehen, bis wir den gewuenschten Eintrag haben + for (job = behaviour; job; job = job->next) { + if (job->work == to) { + break; // Abbruch der Schleife, job zeigt nun auf die Datenstruktur des Zielverhaltens + } + } + + if (job->caller){ // Ist das auzurufende Verhalten noch beschaeftigt? + if (override==NOOVERRIDE){ // nicht ueberschreiben, sofortige Rueckkehr + if (from) + from->subResult=SUBFAIL; + return; + } + // Wir wollen also ueberschreiben, aber nett zum alten Aufrufer sein und ihn darueber benachrichtigen + job->caller->active=ACTIVE; // alten Aufrufer reaktivieren + job->caller->subResult=SUBFAIL; // er bekam aber nicht das gewuenschte Resultat + } + + if (from) { + // laufendes Verhalten abschalten + from->active=INACTIVE; + from->subResult=SUBRUNNING; + } + + // neues Verhalten aktivieren + job->active=ACTIVE; + // Aufrufer sichern + job->caller = from; +} + +/*! + * Kehrt zum aufrufenden Verhalten zurueck + * @param running laufendes Verhalten + */ +void return_from_behaviour(Behaviour_t * data){ + data->active=INACTIVE; // Unterverhalten deaktivieren + if (data->caller){ + data->caller->active=ACTIVE; // aufrufendes Verhalten aktivieren + data->caller->subResult=SUBSUCCESS; // Unterverhalten war erfolgreich + } + data->caller=NULL; // Job erledigt, Verweis loeschen +} + +/*! + * Deaktiviert alle Verhalten bis auf Grundverhalten. Bei Verhaltensauswahl werden die Aktivitaeten vorher + * in die Verhaltens-Auswahlvariable gesichert. + */ +void deactivateAllBehaviours(void){ + Behaviour_t *job; // Zeiger auf ein Verhalten + + #ifdef DISPLAY_BEHAVIOUR_AVAILABLE + // bei Verhaltensanzeige in Aktivitaets-Auswahl-Variable sichern + #ifndef DISPLAY_DYNAMIC_BEHAVIOUR_AVAILABLE + // bei Verhaltensanzeige in Aktivitaets-Auswahl-Variable sichern + // nicht bei dynamischer Anzeige und Selektion + set_behaviours_equal(); + #endif + #endif + + // Einmal durch die Liste gehen und (fast) alle deaktivieren, Grundverhalten nicht + for (job = behaviour; job; job = job->next) { + if ((job->priority >= PRIO_VISIBLE_MIN) &&(job->priority <= PRIO_VISIBLE_MAX)) { + // Verhalten deaktivieren + job->active = INACTIVE; + } + } +} + +/*! + * Zentrale Verhaltens-Routine, wird regelmaessig aufgerufen. + * Dies ist der richtige Platz fuer eigene Routinen, um den Bot zu steuern. + */ +void bot_behave(void){ + Behaviour_t *job; // Zeiger auf ein Verhalten + + float faktorLeft = 1.0; // Puffer fuer Modifkatoren + float faktorRight = 1.0; // Puffer fuer Modifkatoren + + #ifdef RC5_AVAILABLE + rc5_control(); // Abfrage der IR-Fernbedienung + #endif + + /* Solange noch Verhalten in der Liste sind... + (Achtung: Wir werten die Jobs sortiert nach Prioritaet aus. Wichtige zuerst einsortieren!!!) */ + for (job = behaviour; job; job = job->next) { + if (job->active) { + /* WunschVariablen initialisieren */ + speedWishLeft = BOT_SPEED_IGNORE; + speedWishRight = BOT_SPEED_IGNORE; + + faktorWishLeft = 1.0; + faktorWishRight = 1.0; + + job->work(job); /* Verhalten ausfuehren */ + /* Modifikatoren sammeln */ + faktorLeft *= faktorWishLeft; + faktorRight *= faktorWishRight; + /* Geschwindigkeit aendern? */ + if ((speedWishLeft != BOT_SPEED_IGNORE) || (speedWishRight != BOT_SPEED_IGNORE)){ + if (speedWishLeft != BOT_SPEED_IGNORE) + speedWishLeft *= faktorLeft; + if (speedWishRight != BOT_SPEED_IGNORE) + speedWishRight *= faktorRight; + + motor_set(speedWishLeft, speedWishRight); + break; /* Wenn ein Verhalten Werte direkt setzen will, nicht weitermachen */ + } + + } + /* Dieser Punkt wird nur erreicht, wenn keine Regel im System die Motoren beeinflusen will */ + if (job->next == NULL) { + motor_set(BOT_SPEED_IGNORE, BOT_SPEED_IGNORE); + } + } +} + +/*! + * Erzeugt ein neues Verhalten + * @param priority Die Prioritaet + * @param *work Den Namen der Funktion, die sich drum kuemmert + */ +Behaviour_t *new_behaviour(uint8 priority, void (*work) (struct _Behaviour_t *data), int8 active){ + Behaviour_t *newbehaviour = (Behaviour_t *) malloc(sizeof(Behaviour_t)); + + if (newbehaviour == NULL) + return NULL; + + newbehaviour->priority = priority; + newbehaviour->active=active; + newbehaviour->next= NULL; + newbehaviour->work=work; + newbehaviour->caller=NULL; + newbehaviour->subResult=SUBSUCCESS; + return newbehaviour; +} + +/*! + * Fuegt ein Verhalten der Verhaltenliste anhand der Prioritaet ein. + * @param list Die Speicherstelle an der die globale Verhaltensliste anfaengt + * @param behave Einzufuegendes Verhalten + */ +void insert_behaviour_to_list(Behaviour_t **list, Behaviour_t *behave){ + Behaviour_t *ptr = *list; + Behaviour_t *temp = NULL; + + /* Kein Eintrag dabei? */ + if (behave == NULL) + return; + + /* Erster Eintrag in der Liste? */ + if (ptr == NULL){ + ptr = behave; + *list = ptr; + } else { + /* Gleich mit erstem Eintrag tauschen? */ + if (ptr->priority < behave->priority) { + behave->next = ptr; + ptr = behave; + *list = ptr; + } else { + /* Mit dem naechsten Eintrag vergleichen */ + while(NULL != ptr->next) { + if (ptr->next->priority < behave->priority) + break; + + /* Naechster Eintrag */ + ptr = ptr->next; + } + + temp = ptr->next; + ptr->next = behave; + behave->next = temp; + } + } +} + + + +#ifdef DISPLAY_BEHAVIOUR_AVAILABLE + +/*! + * ermittelt ob noch eine weitere Verhaltensseite existiert + */ + int8 another_behaviour_page(void) { + int16 max_behaviours ; + Behaviour_t *ptr ; + + /* dazu muss ich auch gueltige Screenseite sein */ + #ifdef DISPLAY_SCREENS_AVAILABLE + if (display_screen != 2) + return 0; + #endif + + ptr = behaviour; + max_behaviours = 0; + +// zuerst alle Verhalten ermitteln ausser Grundverhalten + while(ptr != NULL) { + if ((ptr->priority >= PRIO_VISIBLE_MIN) &&(ptr->priority <= PRIO_VISIBLE_MAX)) + max_behaviours++; + + ptr = ptr->next; + } + + return (behaviour_page * 6) < max_behaviours; +} + + +/*! + * toggled ein Verhalten der Verhaltensliste an Position pos, + * die Aenderung erfolgt nur auf die Puffervariable + * @param pos Listenposition, entspricht der Taste 1-6 der gewaehlten Verhaltensseite + */ +void toggleNewBehaviourPos(int8 pos){ + Behaviour_t *job; // Zeiger auf ein Verhalten + int8 i; + + // nur aendern, wenn ich richtige Screenseite bin + if (display_screen != 2) + return ; + + // richtigen Index je nach Seite ermitteln + pos = (behaviour_page - 1) * 6 + pos; + i = 0; + + // durch die Liste gehen, bis wir den gewuenschten Index erreicht haben + for (job = behaviour; job; job = job->next) { + if ((job->priority >= PRIO_VISIBLE_MIN) &&(job->priority <= PRIO_VISIBLE_MAX)) { + i++; + if (i == pos) { + // bei dynamischer Wahl wird direkt die Zustandsvariable geaendert + #ifdef DISPLAY_DYNAMIC_BEHAVIOUR_AVAILABLE + job->active = !job->active; + #else + job->active_new = !job->active_new; + #endif + + break; + } + } + } +} + +#ifndef DISPLAY_DYNAMIC_BEHAVIOUR_AVAILABLE +/*! + * Startschuss, die gewaehlten neuen Verhaltensaktivitaeten werden in die + * Verhaltensliste geschrieben und die Verhalten damit scharf geschaltet + */ +void set_behaviours_active_to_new(void) { + + Behaviour_t *job; + for (job = behaviour; job; job = job->next) { + + if ((job->priority >= PRIO_VISIBLE_MIN) &&(job->priority <= PRIO_VISIBLE_MAX)) + job->active = job->active_new; + + } + +} + +/*! + * Die Aktivitaeten der Verhalten werden in die Puffervariable geschrieben, + * welche zur Anzeige und Auswahl verwendet wird + */ +void set_behaviours_equal(void) { + Behaviour_t *job; + for (job = behaviour; job; job = job->next) { + + if ((job->priority >= PRIO_VISIBLE_MIN) &&(job->priority <= PRIO_VISIBLE_MAX)) + job->active_new = job->active; + + } +} +#endif +#endif +#endif -- cgit v1.2.3