summaryrefslogtreecommitdiffstats
path: root/source/ct-Bot/bot-logic
diff options
context:
space:
mode:
Diffstat (limited to 'source/ct-Bot/bot-logic')
-rw-r--r--source/ct-Bot/bot-logic/behaviour_avoid_border.c41
-rw-r--r--source/ct-Bot/bot-logic/behaviour_avoid_col.c142
-rw-r--r--source/ct-Bot/bot-logic/behaviour_catch_pillar.c106
-rw-r--r--source/ct-Bot/bot-logic/behaviour_drive_distance.c120
-rw-r--r--source/ct-Bot/bot-logic/behaviour_drive_square.c76
-rw-r--r--source/ct-Bot/bot-logic/behaviour_follow_line.c176
-rw-r--r--source/ct-Bot/bot-logic/behaviour_goto.c141
-rw-r--r--source/ct-Bot/bot-logic/behaviour_gotoxy.c141
-rw-r--r--source/ct-Bot/bot-logic/behaviour_olympic.c428
-rw-r--r--source/ct-Bot/bot-logic/behaviour_remotecall.c407
-rw-r--r--source/ct-Bot/bot-logic/behaviour_scan.c144
-rw-r--r--source/ct-Bot/bot-logic/behaviour_servo.c68
-rw-r--r--source/ct-Bot/bot-logic/behaviour_simple.c139
-rw-r--r--source/ct-Bot/bot-logic/behaviour_solve_maze.c660
-rw-r--r--source/ct-Bot/bot-logic/behaviour_turn.c361
-rw-r--r--source/ct-Bot/bot-logic/bot-logik.c500
16 files changed, 3650 insertions, 0 deletions
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 <math.h>
+
+#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 (sensLineL<LINE_SENSE && sensLineR>LINE_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 && sensLineR<LINE_SENSE) {
+ /* andersrum, also links drehen */
+ //LOG_DEBUG(("Drehe links"));
+ speedWishLeft=-BOT_SPEED_FOLLOW;
+ speedWishRight=BOT_SPEED_FOLLOW;
+ } else if (sensLineL>LINE_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<LINE_SENSE && sensLineR<LINE_SENSE) {
+ /* Linie weg, also Stop, kurz zurueck und drehen */
+ lineState=CORNER_TURN;
+ speedWishLeft=-BOT_SPEED_SLOW;
+ speedWishRight=-BOT_SPEED_SLOW;
+ break;
+ }
+ speedWishLeft=BOT_SPEED_FOLLOW;
+ speedWishRight=BOT_SPEED_FOLLOW;
+ break;
+
+ case CORNER_TURN:
+ /* 90� in Richtung des detektierten Abgrunds drehen */
+ lineState=CORRECT_POS;
+ bot_turn(data,(cornerDetected==CORNER_LEFT)?90:-90);
+ cornerDetected=False;
+ break;
+
+ case CORRECT_POS:
+ lineState=FOLLOW_LINE;
+ bot_drive_distance(data,0,BOT_SPEED_SLOW,2);
+ break;
+
+ case RETREAT_AND_STOP:
+ /* wir sind an einem Abgrund, Stop und Ende des Verhaltens */
+ speedWishLeft=BOT_SPEED_STOP;
+ speedWishRight=BOT_SPEED_STOP;
+ cornerDetected=False;
+ lineState=CHECK_LINE;
+ return_from_behaviour(data);
+ break;
+ }
+}
+
+/*! Folgt einer Linie, sobald beide Liniensensoren ausloesen
+ * Die Linie sollte in etwa die Breite beider CNY70 haben
+ */
+void bot_follow_line(Behaviour_t *caller) {
+ switch_to_behaviour(caller, bot_follow_line_behaviour,NOOVERRIDE);
+}
+#endif
diff --git a/source/ct-Bot/bot-logic/behaviour_goto.c b/source/ct-Bot/bot-logic/behaviour_goto.c
new file mode 100644
index 0000000..9b655a4
--- /dev/null
+++ b/source/ct-Bot/bot-logic/behaviour_goto.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_goto.c
+ * @brief Bot faehrt einen punkt an
+ *
+ * @author Benjamin Benz (bbe@heise.de)
+ * @date 03.11.06
+*/
+
+
+
+#include "bot-logic/bot-logik.h"
+
+#ifdef BEHAVIOUR_GOTO_AVAILABLE
+#include <stdlib.h>
+
+
+#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 <math.h>
+
+
+
+
+/* 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 <stdlib.h>
+
+/* Zustaende des bot_explore-Verhaltens */
+
+#define EXPLORATION_STATE_GOTO_WALL 1 /*!< Zustand: Bot sucht eine Wand o.ae. Hinderniss */
+#define EXPLORATION_STATE_TURN_PARALLEL_LEFT 2 /*!< Zustand: Bot dreht sich nach links, bis er parallel zur Wand blickt. */
+#define EXPLORATION_STATE_TURN_PARALLEL_RIGHT 3 /*!< Zustand: Bot dreht sich nach rechts, bis er parallel zur Wand blickt. */
+#define EXPLORATION_STATE_DRIVE_PARALLEL_LEFT 4 /*!< Zustand: Bot faehrt parallel zur Wand links von sich. */
+#define EXPLORATION_STATE_DRIVE_PARALLEL_RIGHT 5 /*!< Zustand: Bot faehrt parallel zur Wand rechts von sich. */
+#define EXPLORATION_STATE_TURN_ORTHOGONAL_LEFT 6 /*!< Zustand: Bot dreht sich nach links, bis er senkrecht zur Wand steht. */
+#define EXPLORATION_STATE_TURN_ORTHOGONAL_RIGHT 7 /*!< Zustand: Bot dreht sich nach rechts, bis er senkrecht zur Wand steht. */
+#define EXPLORATION_STATE_DRIVE_ARC 8 /*!< Zustand: Bot faehrt einen Bogen. Der Winkel des Bogens sollte in einer
+ *!< weiteren static Variablen (z.B. curve) gespeichert sein. */
+
+/* Zustaende des bot_olympic_behaviour-Verhaltens */
+
+#define CB_STATE_EXPLORATION 0 /*!< Zustand: Bot erforscht die Umgebung. */
+#define CB_STATE_DOING_SLALOM 1 /*!< Zustand: Bot ist dabei Slalom zu fahren. */
+
+/* Zustaende des bot_do_slalom-Verhaltens */
+
+#define SLALOM_STATE_START 0 /*!< Zustand: Bot startet eine Slalomlauf und positioniert sich vor der Saeule. */
+#define SLALOM_STATE_TURN_1 1 /*!< Zustand: Bot dreht sich um 90�. */
+#define SLALOM_STATE_DRIVE_ARC 2 /*!< Zustand: Bot faehrt den Bogen um die Saeule. */
+#define SLALOM_STATE_TURN_2 3 /*!< Zustand: Bot dreht sich fuer den Sweep um 45�. */
+#define SLALOM_STATE_SWEEP_RUNNING 4 /*!< Zustand: Bot macht den Sweep. */
+#define SLALOM_STATE_SWEEP_DONE 5 /*!< Zustand: Bot ist fertig mit dem Sweep. */
+#define SLALOM_STATE_CHECK_PILLAR 6 /*!< Zustand: Bot ueberprueft, ob er den Slalom fortsetzen kann. */
+
+#define SLALOM_ORIENTATION_LEFT 0
+#define SLALOM_ORIENTATION_RIGHT 1
+
+/* Parameter fuer das bot_explore_behaviour() */
+int8 (*exploration_check_function)(void); /*!< Die Funktion, mit der das bot_explore_behaviour() feststellt, ob es etwas gefunden hat.
+ * Die Funktion muss True (1) zurueck geben, wenn dem so ist, sonst False (0).
+ * Beispiele fuer eine solche Funktion sind check_for_light, is_good_pillar_ahead etc.*/
+
+
+
+
+/*!
+ * Das Verhalten dreht den Bot so, dass er auf eine Lichtquelle zufaehrt. */
+void bot_goto_light(void){
+ int16 speed, curve = (sensLDRL - sensLDRR)/1.5;
+
+ if(curve < -127) curve = -127;
+ if(curve > 127) curve = 127;
+
+ if(abs(sensLDRL - sensLDRR) < 20){
+ speed = BOT_SPEED_MAX;
+ }else if(abs(sensLDRL - sensLDRR) < 150) {
+ speed = BOT_SPEED_FAST;
+ }else {
+ speed = BOT_SPEED_NORMAL;
+ }
+
+ bot_drive(curve, speed);
+}
+
+
+
+/*!
+ * Gibt aus, ob der Bot Licht sehen kann.
+ * @return True, wenn der Bot Licht sieht, sonst False. */
+int8 check_for_light(void){
+ // Im Simulator kann man den Bot gut auf den kleinsten Lichtschein
+ // reagieren lassen, in der Realitaet gibt es immer Streulicht, so dass
+ // hier ein hoeherer Schwellwert besser erscheint.
+ // Simulator:
+ if(sensLDRL >= 1023 && sensLDRR >= 1023) return False;
+ // Beim echten Bot eher:
+ // if(sensLDRL >= 100 && sensLDRR >= 100) return False;
+
+ else return True;
+}
+
+/*!
+ * Die Funktion gibt aus, ob sich innerhalb einer gewissen Entfernung ein Objekt-Hindernis befindet.
+ * @param distance Entfernung in mm, bis zu welcher ein Objekt gesichtet wird.
+ * @return Gibt False (0) zurueck, wenn kein Objekt innerhalb von distance gesichtet wird. Ansonsten die Differenz
+ * zwischen dem linken und rechten Sensor. Negative Werte besagen, dass das Objekt naeher am linken, positive, dass
+ * es naeher am rechten Sensor ist. Sollten beide Sensoren den gleichen Wert haben, gibt die Funktion 1 zurueck, um
+ * von False unterscheiden zu koennen. */
+int16 is_obstacle_ahead(int16 distance){
+ if(sensDistL > distance && sensDistR > distance) return False;
+ if(sensDistL - sensDistR == 0) return 1;
+ else return (sensDistL - sensDistR);
+}
+
+/*!
+ * Gibt aus, ob der Bot eine fuer sein Slalomverhalten geeignete Saeule vor sich hat.
+ * @return True, wenn er eine solche Saeule vor sich hat, sonst False.*/
+int8 is_good_pillar_ahead(void){
+ if(is_obstacle_ahead(COL_NEAR) != False && sensLDRL < 600 && sensLDRR < 600) return True;
+ else return False;
+}
+
+/*!
+ * Das Verhalten verhindert, dass dem Bot boese Dinge wie Kollisionen oder Abstuerze widerfahren.
+ * @return Bestand Handlungsbedarf? True, wenn das Verhalten ausweichen musste, sonst False.
+ * TODO: Parameter einfuegen, der dem Verhalten vorschlaegt, wie zu reagieren ist.
+ * */
+int8 bot_avoid_harm(void){
+ if(is_obstacle_ahead(COL_CLOSEST) != False || sensBorderL > BORDER_DANGEROUS || sensBorderR > BORDER_DANGEROUS){
+ speedWishLeft = -BOT_SPEED_NORMAL;
+ speedWishRight = -BOT_SPEED_NORMAL;
+ return True;
+ } else return False;
+}
+
+/*!
+ * Das Verhalten laesst den Roboter den Raum durchsuchen.
+ * Das Verhalten hat mehrere unterschiedlich Zustaende:
+ * 1. Zu einer Wand oder einem anderen Hindernis fahren.
+ * 2. Zu einer Seite drehen, bis der Bot parallel zur Wand ist.
+ * Es macht vielleicht Sinn, den Maussensor auszulesen, um eine Drehung um
+ * einen bestimmten Winkel zu realisieren. Allerdings muesste dafuer auch der
+ * Winkel des Bots zur Wand bekannt sein.
+ * 3. Eine feste Strecke parallel zur Wand vorwaerts fahren.
+ * Da bot_glance_behaviour abwechselnd zu beiden Seiten schaut, ist es fuer die Aufgabe,
+ * einer Wand auf einer Seite des Bots zu folgen, nur bedingt gewachsen und muss
+ * evtl. erweitert werden.
+ * 4. Senkrecht zur Wand drehen.
+ * Siehe 2.
+ * 5. Einen Bogen fahren, bis der Bot wieder auf ein Hindernis stoesst.
+ * Dann das Ganze von vorne beginnen, nur in die andere Richtung und mit einem
+ * weiteren Bogen. So erforscht der Bot einigermassen systematisch den Raum.
+ *
+ * Da das Verhalten jeweils nach 10ms neu aufgerufen wird, muss der Bot sich
+ * 'merken', in welchem Zustand er sich gerade befindet.
+ * */
+void bot_explore_behaviour(Behaviour_t *data){
+ static int8 curve = 0,state = EXPLORATION_STATE_GOTO_WALL, running_curve = False;
+
+ if((*exploration_check_function)()) return_from_behaviour(data);
+
+ switch(state){
+ // Volle Fahrt voraus, bis ein Hindernis erreicht ist.
+ case EXPLORATION_STATE_GOTO_WALL:
+ // Der Bot steht jetzt vor einem Hindernis und soll sich nach rechts drehen
+ if(bot_avoid_harm()) {
+ state = EXPLORATION_STATE_TURN_PARALLEL_RIGHT;
+ #ifdef BEHAVIOUR_AVOID_COL_AVAILABLE
+ deactivateBehaviour(bot_avoid_col_behaviour);
+ #endif
+ }
+ // Es befindet sich kein Hindernis direkt vor dem Bot.
+ else {
+ if(sensDistL < COL_NEAR || sensDistR < COL_NEAR){
+ bot_drive(0,BOT_SPEED_FAST);
+ } else {
+ bot_drive(0,BOT_SPEED_MAX);
+ }
+ }
+ break;
+ // Nach links drehen, bis der Bot parallel zum Hindernis auf der rechten Seite steht.
+ /* TODO: Aufgabe: Entwickle ein Verhalten, dass auch bei Loechern funktioniert.
+ * Tipp dazu: Drehe den Roboter auf das Loch zu, bis beide Bodensensoren das Loch 'sehen'. Anschliessend drehe den Bot um 90 Grad.
+ * Es ist noetig, neue Zustaende zu definieren, die diese Zwischenschritte beschreiben.
+ * TODO: Drehung mit dem Maussensor ueberwachen. */
+ case EXPLORATION_STATE_TURN_PARALLEL_LEFT:
+ if(sensDistR < COL_FAR){
+ // Volle Drehung nach links mit ca. 3Grad/10ms
+ bot_drive(-127,BOT_SPEED_FAST);
+ } else {
+ // Nachdem das Hindernis nicht mehr in Sicht ist, dreht der Bot noch ca. 3 Grad weiter.
+ // Im Zweifelsfall dreht das den Bot zu weit, aber das ist besser, als ihn zu kurz zu drehen.
+ bot_drive(-127,BOT_SPEED_FAST);
+ state = EXPLORATION_STATE_DRIVE_PARALLEL_RIGHT;
+ }
+ break;
+ // Nach rechts drehen, bis der Bot parallel zum Hindernis auf der linken Seite steht.
+ /* Aufgabe: siehe EXPLORATION_STATE_TURN_PARALLEL_LEFT */
+ case EXPLORATION_STATE_TURN_PARALLEL_RIGHT:
+ if(sensDistL < COL_FAR){
+ // Volle Drehung nach rechts mit ca. 3Grad/10ms
+ bot_drive(127,BOT_SPEED_FAST);
+ } else {
+ /* Nachdem das Hindernis nicht mehr in Sicht ist, dreht der Bot noch ca. 3 Grad weiter.
+ * Im Zweifelsfall dreht das den Bot zu weit, aber das ist besser, als ihn zu kurz zu drehen. */
+ bot_drive(127,BOT_SPEED_FAST);
+ state = EXPLORATION_STATE_DRIVE_PARALLEL_LEFT;
+ }
+ break;
+ case EXPLORATION_STATE_DRIVE_PARALLEL_LEFT:
+ bot_drive_distance(data,0,BOT_SPEED_FAST,15);
+ state = EXPLORATION_STATE_TURN_ORTHOGONAL_RIGHT;
+ break;
+ case EXPLORATION_STATE_DRIVE_PARALLEL_RIGHT:
+ bot_drive_distance(data,0,BOT_SPEED_FAST,15);
+ state = EXPLORATION_STATE_TURN_ORTHOGONAL_LEFT;
+ break;
+ case EXPLORATION_STATE_TURN_ORTHOGONAL_LEFT:
+ // Drehe den Bot um 90 Grad nach links.
+ /* Da der Bot sich immer ein bisschen zu weit von der Wand weg dreht, soll er sich
+ * hier nur um 85 Grad drehen. Nicht schoen, aber klappt.*/
+ bot_turn(data,85);
+ state = EXPLORATION_STATE_DRIVE_ARC;
+ #ifdef BEHAVIOUR_AVOID_COL_AVAILABLE
+ activateBehaviour(bot_avoid_col_behaviour);
+ #endif
+ break;
+ case EXPLORATION_STATE_TURN_ORTHOGONAL_RIGHT:
+ // Drehe den Bot um 90 Grad nach rechts.
+ /* Da der Bot sich immer ein bisschen zu weit von der Wand weg dreht, soll er sich
+ * hier nur um 85 Grad drehen. Nicht schoen, aber klappt.*/
+ bot_turn(data,-85);
+ state = EXPLORATION_STATE_DRIVE_ARC;
+ #ifdef BEHAVIOUR_AVOID_COL_AVAILABLE
+ activateBehaviour(bot_avoid_col_behaviour);
+ #endif
+ break;
+ case EXPLORATION_STATE_DRIVE_ARC:
+ /* Fahre einen Bogen.
+ * Der Bot soll im Wechsel Links- und Rechtsboegen fahren. Daher muss das Vorzeichen von curve wechseln.
+ * Ausserdem soll der Bogen zunehmend weiter werden, so dass der absolute Wert von curve abnehmen muss.
+ * Ist der Wert 0, wird er auf den engsten Bogen initialisiert. Da der Bot am Anfang nach rechts abbiegt,
+ * muss der Wert positiv sein.
+ * Aufgabe: Manchmal kann es passieren, dass der Bot bei einer kleinen Kurve zu weit weg von der Wand
+ * startet und dann nur noch im Kreis faehrt. Unterbinde dieses Verhalten.
+ */
+ if(curve == 0){
+ curve = 25;
+ running_curve = True;
+ } else if (running_curve == False){
+ curve *= -0.9;
+ running_curve = True;
+ }
+ /* Sobald der Bot auf ein Hindernis stoesst, wird der naechste Zyklus eingeleitet.
+ * Auf einen Rechtsbogen (curve > 0) folgt eine Linksdrehung und auf einen Linksbogen eine Rechtsdrehung.
+ * Wenn der Wert von curve (durch Rundungsfehler bei int) auf 0 faellt, beginnt das Suchverhalten erneut.*/
+ if(bot_avoid_harm()) {
+ state = (curve > 0) ? EXPLORATION_STATE_TURN_PARALLEL_LEFT : EXPLORATION_STATE_TURN_PARALLEL_RIGHT;
+ running_curve = False;
+ #ifdef BEHAVIOUR_AVOID_COL_AVAILABLE
+ deactivateBehaviour(bot_avoid_col_behaviour);
+ #endif
+ } else {
+ bot_drive(curve, BOT_SPEED_MAX);
+ }
+ break;
+ default:
+ state = EXPLORATION_STATE_GOTO_WALL;
+ curve = 0;
+ #ifdef BEHAVIOUR_AVOID_COL_AVAILABLE
+ activateBehaviour(bot_avoid_col_behaviour);
+ #endif
+ }
+
+}
+
+/*!
+ * Aktiviert bot_explore_behaviour. */
+void bot_explore(Behaviour_t *caller, int8 (*check)(void)){
+ exploration_check_function = check;
+ switch_to_behaviour(caller,bot_explore_behaviour,NOOVERRIDE);
+}
+
+/*!
+ * Das Verhalten laesst den Bot einen Slalom fahren.
+ * @see bot_do_slalom()
+ * */
+void bot_do_slalom_behaviour(Behaviour_t *data){
+ static int8 state = SLALOM_STATE_CHECK_PILLAR;
+ static int8 orientation = SLALOM_ORIENTATION_RIGHT;
+ static int8 sweep_state;
+ static int8 sweep_steps = 0;
+ int16 turn;
+ int8 curve;
+
+ switch(state){
+ case SLALOM_STATE_CHECK_PILLAR:
+ // Der Bot sollte jetzt Licht sehen koennen...
+ if(check_for_light()){
+ // Wenn der Bot direkt vor der Saeule steht, kann der Slalom anfangen, sonst zum Licht fahren
+ if(bot_avoid_harm()){
+ state = SLALOM_STATE_START;
+ } else bot_goto_light();
+ } else {// ... sonst muss er den Slalom-Kurs neu suchen.
+ #ifdef BEHAVIOUR_AVOID_COL_AVAILABLE
+ activateBehaviour(bot_avoid_col_behaviour);
+ #endif
+ return_from_behaviour(data);
+ }
+ break;
+ case SLALOM_STATE_START:
+ // Hier ist Platz fuer weitere Vorbereitungen, falls noetig.
+ #ifdef BEHAVIOUR_AVOID_COL_AVAILABLE
+ deactivateBehaviour(bot_avoid_col_behaviour);
+ #endif
+ state = SLALOM_STATE_TURN_1;
+ // break;
+ case SLALOM_STATE_TURN_1:
+ turn = (orientation == SLALOM_ORIENTATION_LEFT) ? 90 : -90;
+ bot_turn(data,turn);
+ state = SLALOM_STATE_DRIVE_ARC;
+ break;
+ case SLALOM_STATE_DRIVE_ARC:
+ // Nicht wundern: Bei einem Links-Slalom faehrt der Bot eine Rechtskurve.
+ curve = (orientation == SLALOM_ORIENTATION_LEFT) ? 25 : -25;
+ bot_drive_distance(data,curve,BOT_SPEED_FAST,20);
+ state = SLALOM_STATE_TURN_2;
+ break;
+ case SLALOM_STATE_TURN_2:
+ turn = (orientation == SLALOM_ORIENTATION_LEFT) ? 45 : -45;
+ bot_turn(data,turn);
+ state = SLALOM_STATE_SWEEP_RUNNING;
+ break;
+ case SLALOM_STATE_SWEEP_RUNNING:
+ if(sweep_steps == 0){
+ sweep_state = SWEEP_STATE_CHECK;
+ }
+ // Insgesamt 6 Schritte drehen
+ if(sweep_steps < 6) {
+ if(sweep_state == SWEEP_STATE_CHECK){
+ // Phase 1: Pruefen, ob vor dem Bot eine gute Saeule ist
+ if(is_good_pillar_ahead() == True){
+ // Wenn die Saeule gut ist, drauf zu und Slalom anders rum fahren.
+ state = SLALOM_STATE_CHECK_PILLAR;
+ orientation = (orientation == SLALOM_ORIENTATION_LEFT) ? SLALOM_ORIENTATION_RIGHT : SLALOM_ORIENTATION_LEFT;
+ sweep_steps = 0;
+ } else {
+ // Sonst drehen.
+ sweep_state = SWEEP_STATE_TURN;
+ }
+ }
+ if(sweep_state == SWEEP_STATE_TURN) {
+ // Phase 2: Bot um 15 Grad drehen
+ turn = (orientation == SLALOM_ORIENTATION_LEFT) ? 15 : -15;
+ bot_turn(data,turn);
+ sweep_state = SWEEP_STATE_CHECK;
+ sweep_steps++;
+ }
+ } else {
+ turn = (orientation == SLALOM_ORIENTATION_LEFT) ? -90 : 90;
+ bot_turn(data,turn);
+ state = SLALOM_STATE_SWEEP_DONE;
+ sweep_steps = 0;
+ }
+ break;
+ case SLALOM_STATE_SWEEP_DONE:
+ turn = (orientation == SLALOM_ORIENTATION_LEFT) ? -135 : 135;
+ bot_turn(data,turn);
+ state = SLALOM_STATE_CHECK_PILLAR;
+ break;
+ default:
+ state = SLALOM_STATE_CHECK_PILLAR;
+ }
+
+}
+
+/*!
+ * Das Verhalten laesst den Bot zwischen einer Reihe beleuchteter Saeulen Slalom fahren.
+ * Das Verhalten ist wie bot_explore() in eine Anzahl von Teilschritten unterteilt.
+ * 1. Vor die aktuelle Saeule stellen, so dass sie zentral vor dem Bot und ungefaehr
+ * COL_CLOSEST (100 mm) entfernt ist.
+ * 2. 90 Grad nach rechts drehen.
+ * 3. In einem relativ engen Bogen 20 cm weit fahren.
+ * 4. Auf der rechten Seite des Bot nach einem Objekt suchen, dass
+ * a) im rechten Sektor des Bot liegt, also zwischen -45 Grad und -135 Grad zur Fahrtrichtung liegt,
+ * b) beleuchtet und
+ * c) nicht zu weit entfernt ist.
+ * Wenn es dieses Objekt gibt, wird es zur aktuellen Saeule und der Bot faehrt jetzt Slalom links.
+ * 5. Sonst zurueck drehen, 90 Grad drehen und Slalom rechts fahren.
+ * In diesem Schritt kann der Bot das Verhalten auch abbrechen, falls er gar kein Objekt mehr findet.
+ */
+void bot_do_slalom(Behaviour_t *caller){
+ switch_to_behaviour(caller, bot_do_slalom_behaviour,NOOVERRIDE);
+}
+
+/*!
+ * Das Verhalten setzt sich aus 3 Teilverhalten zusammen:
+ * Nach Licht suchen, auf das Licht zufahren, im Licht Slalom fahren. */
+void bot_olympic_behaviour(Behaviour_t *data){
+ if(check_for_light()){
+ /* Sobald der Bot auf ein Objekt-Hinderniss stoesst, versucht er, Slalom zu fahren.
+ * Aufgabe: Wenn der Bot vor einem Loch steht, hinter welchem sich die Lichtquelle
+ * befindet, wird er daran haengen bleiben. Schreibe ein Verhalten, dass das verhindert. */
+ if(bot_avoid_harm() && is_obstacle_ahead(COL_NEAR)){
+ bot_do_slalom(data);
+ } else bot_goto_light();
+ } else bot_explore(data,check_for_light);
+}
+
+/*!
+ * Initialisiert das Olympische Verhalten
+ * @param prio_main Prioritaet des Olympischen Verhalten (typ. 100)
+ * @param prio_helper Prioritaet der Hilfsfunktionen (typ. 52)
+ * @param active ACTIVE wenn es sofort starten soll, sonst INACTIVE
+ */
+void bot_olympic_init(int8 prio_main,int8 prio_helper, int8 active){
+ // unwichtigere Hilfsverhalten
+ insert_behaviour_to_list(&behaviour, new_behaviour(prio_helper--, bot_explore_behaviour,INACTIVE));
+ insert_behaviour_to_list(&behaviour, new_behaviour( prio_helper, bot_do_slalom_behaviour,INACTIVE));
+ // Demo-Verhalten fuer aufwendiges System, inaktiv
+ insert_behaviour_to_list(&behaviour, new_behaviour(prio_main, bot_olympic_behaviour, active));
+}
+
+#endif
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 <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#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 <avr/pgmspace.h>
+#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<count; i++){
+ int8 j;
+ if (len[i] == 1) *dest++ = 0; // auch (u)int8 beginnen immer in geraden Registern
+ /* pro Parameter LSB zuerst nach dest kopieren */
+ for (j=len[i]-1; j>=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; i<parameter_count*4 && td>0; 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; i<parameter_count && td>1; 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; i<REMOTE_CALL_FUNCTION_NAME_LEN+1; i++)
+ *function_name++ = (uint8) pgm_read_byte ( from++ );
+ function_name=(char*)&tmp;
+ #endif
+
+ #ifdef COMMAND_AVAILABLE
+ int16 result = data->subResult;
+ 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; i<REMOTE_CALL_MAX_PARAM; i++)
+ parameter_length[i] = (uint8) pgm_read_byte ( from++ );
+ #endif
+// LOG_DEBUG(("func=%s param_count=%d Len= %u %u %u %u",func,parameter_count,parameter_length[0],parameter_length[1],parameter_length[2]));
+// if (data != NULL){
+// LOG_DEBUG(("data= %u %u %u %u",data[0],data[1],data[2],data[3]));
+// }
+
+ #ifdef MCU // Die MCU legt die Parameter nach einem anderen Verfahren ab, diese Funktion konvertiert sie deshalb
+ remotecall_convert_params(parameter_data, parameter_count, parameter_length, (uint8*)data);
+ #else // Auf dem PC kopieren wir die Daten einfach
+ memcpy(parameter_data, data, parameter_count*4);
+ #endif
+
+ running_behaviour=REMOTE_CALL_SCHEDULED;
+ activateBehaviour(bot_remotecall_behaviour);
+}
+
+/*!
+ * Fuehre einen remote_call aus. Es gibt KEIN aufrufendes Verhalten!!
+ * @param data Zeiger die Payload eines Kommandos. Dort muss zuerst ein String mit dem Fkt-Namen stehen. ihm folgen die Nutzdaten
+ */
+void bot_remotecall_from_command(uint8 * data){
+ char * function_name = (char*)data;
+ remote_call_data_t * params = (remote_call_data_t *)(data+ strlen(function_name)+1);
+ bot_remotecall(function_name,params);
+}
+
+
+/*!
+ * Listet alle verfuegbaren Remote-Calls auf und verschickt sie als einzelne Kommanods
+ */
+void remote_call_list(void){
+ #ifdef MCU
+ call_t call_storage;
+ uint8* to;
+ uint8* from;
+ #endif
+ call_t* call;
+
+ int16 i;
+ for (i=0; i< (STORED_CALLS); i++){
+ #ifdef MCU
+ // Auf dem MCU muessen die Daten erstmal aus dem Flash ins RAM
+ from= (uint8*)&calls[i];
+ to= (uint8*)&call_storage;
+ uint8 j;
+ for (j=0; j< sizeof(call_t); j++){
+ *to = (uint8) pgm_read_byte ( from++ );
+ to++;
+ }
+ call = &call_storage;
+ #else
+ call = (call_t*)&calls[i];
+ #endif
+
+ #ifdef COMMAND_AVAILABLE
+ // und uebertragen
+ command_write_rawdata(CMD_REMOTE_CALL,SUB_REMOTE_CALL_ENTRY,&i,&i, sizeof(call_t),(uint8*)call);
+ #endif
+
+// LOG_DEBUG(("%s(%s)",call->name,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 <math.h>
+
+#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 <math.h>
+#include <stdlib.h>
+#include "log.h"
+
+#ifdef BEHAVIOUR_SOLVE_MAZE_AVAILABLE
+
+/* Parameter fuer das check_wall_behaviour() */
+int8 wall_detected; /*!< enthaelt True oder False, je nach Ergebnis des Verhaltens */
+int8 check_direction; /*!< enthaelt CHECK_WALL_LEFT oder CHECK_WALL_RIGHT */
+int16 wall_distance; /*!< enthaelt gemessene Entfernung */
+
+/* Konstanten fuer check_wall_behaviour-Verhalten */
+#define CHECK_WALL_RIGHT 0
+#define CHECK_WALL_LEFT 1
+
+/* Parameter fuer das measure_angle_behaviour() */
+int8 measure_direction; /*!< enthaelt MEASURE_RIGHT oder MEASURE_LEFT */
+int16 measure_distance; /*!< enthaelt maximale Messentfernung, enthaelt nach der Messung die Entfernung */
+int16 measured_angle; /*!< enthaelt gedrehten Winkel oder 0, falls nichts entdeckt */
+#ifdef MEASURE_MOUSE_AVAILABLE
+ int16 start_heading; /*!< Blickwinkel des Bots zu Beginn der Messung */
+#else
+ int16 startEncL; /*!< enthaelt Encoderstand zu Beginn der Messung */
+ int16 startEncR; /*!< enthaelt Encoderstand zu Beginn der Messung */
+#endif
+/* Konstanten fuer measure_angle_behaviour-Verhalten */
+#define MEASURE_LEFT 1
+#define MEASURE_RIGHT -1
+
+
+/*!
+ * Das Verhalten dreht sich um 45 Grad in die angegebene Richtung (0=rechts, 1=links)
+ * und prueft, ob auf dem Sensor auf der Seite der angegebenen Richtung eine Wand
+ * im Abstand von 12cm zu sehen ist. Wenn dem so ist, wird die Variable wall_present
+ * auf True gesetzt, sonst False */
+void bot_check_wall_behaviour(Behaviour_t *data) {
+ /* Konstantenfuer check_wall_behaviour-Verhalten */
+ #define CORRECT_NEAR 0
+ #define CORRECT_NONE 1
+ #define CORRECT_FAR 2
+ /* Zustaende fuer check_wall_behaviour-Verhalten */
+ #define CHECK_WALL_TURN 0
+ #define CHECK_WALL_SENS 1
+ #define CHECK_WALL_TURN_BACK 2
+ #define CHECK_WALL_FINISHED 3
+ #define CHECK_WALL_CORRECT 4
+
+ static int8 checkState=CHECK_WALL_TURN;
+ /* wenn die Wand noch da ist aber aus dem Blickfeld rueckt, Entfernung und Winkel korrigieren */
+ static int8 correctDistance=CORRECT_NONE;
+ /* letzte, gueltige Distanz fuer Abweichungsberechnung */
+ static int16 lastDistance=0;
+ /* enthaelt anzahl der +/-5 identischen Messungen */
+ static int8 measureCount=0;
+ /* letzter Messwert */
+ static int16 lastSensor=0;
+
+ int16 sensor; /* fuer temporaer benutzte Senorwerte */
+
+ switch(checkState) {
+ case CHECK_WALL_TURN:
+ /* haben wir links oder rechts eine Wand? */
+ if (check_direction==CHECK_WALL_LEFT) {
+ checkState=CHECK_WALL_SENS;
+ bot_turn(data,45);
+ break;
+ } else {
+ checkState=CHECK_WALL_SENS;
+ bot_turn(data,-45);
+ break;
+ }
+
+ case CHECK_WALL_SENS:
+ if (check_direction==CHECK_WALL_LEFT) {
+ sensor=sensDistL;
+ } else {
+ sensor=sensDistR;
+ }
+ /* dafuer sorgen, dass wir nur verlaessliche Werte haben
+ * dazu muss der wert dreimal nacheinander max. um +/- 5
+ * unterschiedlich sein */
+ if (measureCount==0) {
+ lastSensor=sensor;
+ measureCount++;
+ break;
+ }
+ if (sensor>=lastSensor-5 && sensor<=lastSensor+5 && measureCount<4) {
+ /* Messwert ist ok */
+ measureCount++;
+ break;
+ } else if (measureCount<4) {
+ /* Messwert weicht zu doll ab -> von Neuem messen */
+ measureCount=0;
+ break;
+ }
+ /* ok, wir hatten drei Messungen mit nahezu identischen Werten */
+
+ /* keine wand in eingestellter Maximalentfernung? */
+ if (sensor>IGNORE_DISTANCE) {
+ correctDistance=CORRECT_NONE; /* dann auch keine Korrektur */
+ lastDistance=0; /* auch kein Vergleichswert */
+ wall_detected=False; /* keine Wand da */
+ } else if (sensor<OPTIMAL_DISTANCE-ADJUST_DISTANCE) {
+ /* bot hat den falschen Abstand zur Wand, zu nah dran */
+ wall_detected=True;
+ correctDistance=CORRECT_NEAR;
+ } else if (sensor>OPTIMAL_DISTANCE+ADJUST_DISTANCE) {
+ /* bot hat den falschen Abstand zur Wand, zu weit weg */
+ wall_detected=True;
+ correctDistance=CORRECT_FAR;
+ } else {
+ /* perfekter Abstand */
+ correctDistance=CORRECT_NONE;
+ wall_detected=True;
+ }
+ wall_distance=sensor;
+ /* Wenn Korrektur noetig, dann durchfuehren, sonst gleich zurueckdrehen */
+ if (correctDistance==CORRECT_NONE) checkState=CHECK_WALL_TURN_BACK;
+ else checkState=CHECK_WALL_CORRECT;
+ break;
+
+
+ case CHECK_WALL_TURN_BACK:
+ checkState=CHECK_WALL_FINISHED;
+ int16 turnAngle=45;
+
+ /* welcher Sensorwert wird gebraucht? */
+ if (check_direction==CHECK_WALL_LEFT) {
+ sensor=sensDistL;
+ } else {
+ sensor=sensDistR;
+ }
+ /* wenn vorhanden, aus letztem Abstand und aktuellem Abstand
+ * Korrekturwinkel berechnen, falls vorheriger Abstand da.
+ * wird durch Abstand>IGNORE_DISTANCE zurueckgesetzt */
+ if (lastDistance!=0) {
+ turnAngle=turnAngle+(lastDistance-wall_distance)/10;
+ }
+ if (sensor<IGNORE_DISTANCE) lastDistance=sensor;
+ if (check_direction==CHECK_WALL_LEFT) {
+ bot_turn(data,-turnAngle);
+ } else {
+ bot_turn(data,turnAngle);
+ }
+ break;
+
+ case CHECK_WALL_FINISHED:
+ /* ok, check beendet -> Verhalten verlassen */
+ checkState=CHECK_WALL_TURN;
+ return_from_behaviour(data);
+ break;
+
+ case CHECK_WALL_CORRECT:
+ /* Abhaengig von der Seite, auf der die Wand ist, Entfernung korrigieren */
+ if (check_direction==CHECK_WALL_LEFT) {
+ sensor=sensDistL;
+ } else {
+ sensor=sensDistR;
+ }
+ if (sensor-OPTIMAL_DISTANCE<0) {
+ speedWishLeft=-BOT_SPEED_SLOW;
+ speedWishRight=-BOT_SPEED_SLOW;
+ } else if (sensor-OPTIMAL_DISTANCE>0) {
+ speedWishLeft=BOT_SPEED_SLOW;
+ speedWishRight=BOT_SPEED_SLOW;
+ }
+ else {
+ checkState=CHECK_WALL_TURN_BACK;
+ speedWishLeft=BOT_SPEED_STOP;
+ speedWishRight=BOT_SPEED_STOP;
+ }
+ break;
+
+
+ default:
+ checkState=CHECK_WALL_TURN;
+ return_from_behaviour(data);
+ break;
+ }
+}
+
+
+/*!
+ * Das Verhalten dreht sich um 45 Grad in die angegebene Richtung (0=rechts, 1=links)
+ * und prueft, ob auf dem Sensor auf der Seite der angegebenen Richtung eine Wand
+ * im Abstand von 12-22cm zu sehen ist. Wenn dem so ist, wird die Variable wall_present
+ * auf True gesetzt, sonst False */
+void bot_check_wall(Behaviour_t *caller,int8 direction) {
+ check_direction=direction;
+ wall_detected=False;
+ switch_to_behaviour(caller, bot_check_wall_behaviour,NOOVERRIDE);
+}
+
+#ifdef MEASURE_MOUSE_AVAILABLE
+ /*!
+ * Das Verhalten dreht den Bot in die angegebene Richtung bis ein Hindernis
+ * im Sichtbereich erscheint, das eine Entfernung bis max. zur angegebenen
+ * Distanz zum Bot hat.
+ */
+
+ void bot_measure_angle_behaviour(Behaviour_t *caller) {
+ /* Zustaende measure_angle_behaviour-Verhalten */
+ #define MEASURE_TURN 0
+ #define FOUND_OBSTACLE 1
+ #define MEASUREMENT_DONE 2
+
+ static int8 measureState=MEASURE_TURN;
+
+ /* enthaelt anzahl der +/-5 identischen Messungen */
+ static int8 measureCount=0;
+ /* letzter Messwert */
+ static int16 lastSensor=0;
+
+ /* bereits gedrehten Winkel */
+ int16 turned_angle=0;
+ if (measure_direction>0) {
+ if ((int16)heading_mou<start_heading) {
+ /* war ein ueberlauf */
+ turned_angle=360-start_heading+(int16)heading_mou;
+ } else {
+ /* sonst normale differenz berechnen */
+ turned_angle=(int16)heading_mou-start_heading;
+ }
+ } else {
+ if ((int16)heading_mou>start_heading) {
+ /* war ein ueberlauf */
+ turned_angle=360-(int16)heading_mou+start_heading;
+ } else {
+ turned_angle=start_heading-(int16)heading_mou;
+ }
+
+ }
+
+ /* sensorwert abhaengig von der Drehrichtung abnehmen */
+ int16 sensor=(measure_direction==MEASURE_LEFT)?sensDistL:sensDistR;
+ /* solange drehen, bis Hindernis innerhalb Messstrecke oder 360 Grad komplett */
+ switch(measureState){
+ case MEASURE_TURN:
+ /* nicht mehr als eine komplette Drehung machen! */
+ if (turned_angle>=360) {
+ measure_direction=-measure_direction;
+ measureState=MEASUREMENT_DONE;
+ bot_turn(caller,measure_direction*turned_angle);
+ measured_angle=0; /* kein Hindernis gefunden */
+ break;
+ }
+ /* Ist ein Objekt in Reichweite? */
+ if (sensor<=measure_distance) {
+ speedWishLeft=BOT_SPEED_STOP;
+ speedWishRight=BOT_SPEED_STOP;
+ measureState=FOUND_OBSTACLE;
+ break;
+ }
+ /* Beginnen, zurueckzudrehen */
+ speedWishLeft = (measure_direction > 0) ? -BOT_SPEED_FOLLOW : BOT_SPEED_FOLLOW;
+ speedWishRight = (measure_direction > 0) ? BOT_SPEED_FOLLOW : -BOT_SPEED_FOLLOW;
+ break;
+
+ case FOUND_OBSTACLE:
+ /* dafuer sorgen, dass wir nur verlaessliche Werte haben
+ * dazu muss der wert dreimal nacheinander max. um +/- 5
+ * unterschiedlich sein */
+ if (measureCount==0) {
+ lastSensor=sensor;
+ measureCount++;
+ break;
+ }
+ if (sensor>=lastSensor-5 && sensor<=lastSensor+5 && measureCount<4) {
+ /* Messwert ist ok */
+ measureCount++;
+ break;
+ } else if (measureCount<4) {
+ /* Messwert weicht zu doll ab -> von Neuem messen */
+ measureCount=0;
+ break;
+ }
+ /* ok, wir hatten drei Messungen mit nahezu identischen Werten */
+ measure_distance=sensor;
+ /* Hindernis gefunden, nun Bot wieder in Ausgangsstellung drehen */
+ measure_direction=-measure_direction;
+ measured_angle=turned_angle;
+ measureState=MEASUREMENT_DONE;
+ bot_turn(caller,measure_direction*turned_angle);
+ break;
+
+ case MEASUREMENT_DONE:
+ measureState=MEASURE_TURN;
+ measureCount=0;
+ return_from_behaviour(caller);
+ break;
+ }
+ }
+
+ /*!
+ * Das Verhalten dreht den Bot in die angegebene Richtung bis ein Hindernis
+ * im Sichtbereich erscheint, das eine Entfernung bis max. zur angegebenen
+ * Distanz zum Bot hat.
+ */
+
+ void bot_measure_angle(Behaviour_t *caller, int8 direction, int16 distance) {
+ /* maximale Messentfernung und Richtung setzen */
+ measure_direction=direction;
+ measure_distance=distance;
+ /* Heading zu Anfang des Verhaltens merken */
+ start_heading=(int16)heading_mou;
+ switch_to_behaviour(caller, bot_measure_angle_behaviour,NOOVERRIDE);
+ }
+#else
+/*!
+ * Das Verhalten dreht den Bot in die angegebene Richtung bis ein Hindernis
+ * im Sichtbereich erscheint, das eine Entfernung bis max. zur angegebenen
+ * Distanz zum Bot hat.
+ */
+
+void bot_measure_angle_behaviour(Behaviour_t *caller) {
+ /* Zustaende measure_angle_behaviour-Verhalten */
+ #define MEASURE_TURN 0
+ #define FOUND_OBSTACLE 1
+ #define TURN_COMPLETED 2
+ #define TURN_BACK 3
+ #define CORRECT_ANGLE 4
+ #define MEASUREMENT_DONE 5
+
+ static int8 measureState=MEASURE_TURN;
+
+ /* bereits gedrehte Strecke errechnen */
+ int16 turnedLeft=(measure_direction>0)?-(sensEncL-startEncL):(sensEncL-startEncL);
+ int16 turnedRight=(measure_direction>0)?(sensEncR-startEncR):-(sensEncR-startEncR);
+ int16 turnedSteps=(abs(turnedLeft)+abs(turnedRight))/2;
+
+ /* sensorwert abhaengig von der Drehrichtung abnehmen */
+ int16 sensor=(measure_direction==MEASURE_LEFT)?sensDistL:sensDistR;
+ /* solange drehen, bis Hindernis innerhalb Messstrecke oder 360� komplett */
+ switch(measureState){
+ case MEASURE_TURN:
+ /* nicht mehr als eine komplette Drehung machen! */
+ if (turnedSteps>=ANGLE_CONSTANT) {
+ speedWishLeft=BOT_SPEED_STOP;
+ speedWishRight=BOT_SPEED_STOP;
+ measureState=TURN_COMPLETED;
+ measured_angle=0; /* kein Hindernis gefunden */
+ break;
+ }
+ /* Ist ein Objekt in Reichweite? */
+ if (sensor<=measure_distance) {
+ measure_distance=sensor;
+ speedWishLeft=BOT_SPEED_STOP;
+ speedWishRight=BOT_SPEED_STOP;
+ measureState=FOUND_OBSTACLE;
+ break;
+ }
+ /* Beginnen, zurueckzudrehen */
+ speedWishLeft = (measure_direction > 0) ? -BOT_SPEED_SLOW : BOT_SPEED_SLOW;
+ speedWishRight = (measure_direction > 0) ? BOT_SPEED_SLOW : -BOT_SPEED_SLOW;
+ break;
+
+ case FOUND_OBSTACLE:
+ /* Hindernis gefunden, nun Bot wieder in Ausgangsstellung drehen */
+ measure_direction=-measure_direction;
+ measured_angle=(int16)((long)(turnedSteps*360)/ANGLE_CONSTANT);
+ measureState=TURN_BACK;
+ speedWishLeft = (measure_direction > 0) ? -BOT_SPEED_SLOW : BOT_SPEED_SLOW;
+ speedWishRight = (measure_direction > 0) ? BOT_SPEED_SLOW : -BOT_SPEED_SLOW;
+ break;
+
+ case TURN_COMPLETED:
+ /* bot steht wieder in Ausgangsrichtung, Verhalten beenden */
+ speedWishLeft=BOT_SPEED_STOP;
+ speedWishRight=BOT_SPEED_STOP;
+ measureState=CORRECT_ANGLE;
+ break;
+
+ case TURN_BACK:
+ /* Bot in Ausgangsposition drehen */
+ if ((turnedLeft+turnedRight)/2>=0) {
+ speedWishLeft=BOT_SPEED_STOP;
+ speedWishRight=BOT_SPEED_STOP;
+ measureState=TURN_COMPLETED;
+ break;
+ }
+ speedWishLeft = (measure_direction > 0) ? -BOT_SPEED_SLOW : BOT_SPEED_SLOW;
+ speedWishRight = (measure_direction > 0) ? BOT_SPEED_SLOW : -BOT_SPEED_SLOW;
+ break;
+
+ case CORRECT_ANGLE:
+ /* Evtl. etwas zuruecksetzen, falls wir zu weit gefahren sind */
+ if (turnedRight>0) {
+ /* rechts zu weit gefahren..langsam zurueck */
+ speedWishRight = (measure_direction > 0) ? -BOT_SPEED_SLOW : BOT_SPEED_SLOW;
+ } else if (turnedRight<0) {
+ /* rechts noch nicht weit genug...langsam vor */
+ speedWishRight = (measure_direction > 0) ? BOT_SPEED_SLOW : -BOT_SPEED_SLOW;
+ } else {
+ /* Endposition erreicht, rechtes Rad anhalten */
+ speedWishRight = BOT_SPEED_STOP;
+ }
+ if (turnedLeft<0) {
+ /* links zu weit gefahren..langsam zurueck */
+ speedWishLeft = (measure_direction > 0) ? -BOT_SPEED_SLOW : BOT_SPEED_SLOW;
+ } else if (turnedLeft>0) {
+ /* links noch nicht weit genug...langsam vor */
+ speedWishLeft = (measure_direction > 0) ? BOT_SPEED_SLOW : -BOT_SPEED_SLOW;
+ } else {
+ /* Endposition erreicht, linkes Rad anhalten */
+ speedWishLeft = BOT_SPEED_STOP;
+ }
+ if (speedWishLeft == BOT_SPEED_STOP && speedWishRight == BOT_SPEED_STOP) {
+ /* beide Raeder haben nun wirklich die Endposition erreicht, daher anhalten */
+ measureState=MEASUREMENT_DONE;
+ }
+ break;
+
+ case MEASUREMENT_DONE:
+ measureState=MEASURE_TURN;
+ return_from_behaviour(caller);
+ break;
+ }
+}
+
+/*!
+ * Das Verhalten dreht den Bot in die angegebene Richtung bis ein Hindernis
+ * im Sichtbereich erscheint, das eine Entfernung bis max. zur angegebenen
+ * Distanz zum Bot hat.
+ */
+
+void bot_measure_angle(Behaviour_t *caller, int8 direction, int16 distance) {
+ /* maximale Messentfernung und Richtung setzen */
+ measure_direction=direction;
+ measure_distance=distance;
+ /* Encoderwerte zu Anfang des Verhaltens merken */
+ startEncL=sensEncL;
+ startEncR=sensEncR;
+ switch_to_behaviour(caller, bot_measure_angle_behaviour,NOOVERRIDE);
+}
+#endif
+/*!
+ * Das Verhalten findet seinen Weg durch ein Labyrinth, das nach gewissen Grundregeln gebaut ist
+ * in nicht immer optimaler Weise aber in jedem Fall. Es arbeitet nach dem Hoehlenforscher-Algorithmus.
+ * Einschraenkung: Objekte im Labyrinth, die Endlossschleifen verursachen koennen, z.b. ein einzeln
+ * stehender Pfeiler im Labyrinth um den der Bot dann immer wieder herum fahren wuerde.
+ */
+void bot_solve_maze_behaviour(Behaviour_t *data){
+ /* Zustaende fuer das bot_solve_maze_behaviour-Verhalten */
+ #define CHECK_FOR_STARTPAD 0
+ #define CHECK_FOR_WALL_RIGHT 1
+ #define CHECK_FOR_WALL_LEFT 2
+ #define CHECK_WALL_PRESENT 3
+ #define SOLVE_MAZE_LOOP 4
+ #define SOLVE_TURN_WALL 5
+ #define CHECK_CONDITION 6
+ #define TURN_TO_BRANCH 7
+ #define DETECTED_CROSS_BRANCH 8
+ #define APPROACH_CORNER 9
+ #define AVOID_ABYSS 10
+ #define REACHED_GOAL 11
+ static int8 mazeState=CHECK_FOR_STARTPAD;
+ static int8 followWall=-1;
+ static int8 checkedWalls=0;
+
+ int16 distance;
+ double x;
+ switch(mazeState) {
+ case CHECK_FOR_STARTPAD:
+ /* Wo beginnen wir, nach einer Wand zu suchen?
+ * Abgrund- und Kollisions-Verhalten ausschalten */
+ #ifdef BEHAVIOUR_AVOID_COL_AVAILABLE
+ deactivateBehaviour(bot_avoid_col_behaviour);
+ #endif
+ #ifdef BEHAVIOUR_AVOID_BORDER_AVAILABLE
+ deactivateBehaviour(bot_avoid_border_behaviour);
+ #endif
+ /* sieht nach, ob der Bot auf einem definierten Startpad steht und
+ * beginnt dann mit der Suche gleich an der richtigen Wand */
+ /* Zuserst bei nach Startpad1 gucken */
+ checkedWalls=0;
+ if ((sensLineL>=STARTPAD1-10 && sensLineL<=STARTPAD1+10) ||
+ (sensLineR>=STARTPAD1-10 && sensLineR<=STARTPAD1+10)) {
+ mazeState=CHECK_FOR_WALL_LEFT;
+ break;
+ }
+ mazeState=CHECK_FOR_WALL_RIGHT;
+ break;
+
+ case CHECK_FOR_WALL_RIGHT:
+
+ mazeState=CHECK_WALL_PRESENT;
+ followWall=CHECK_WALL_RIGHT;
+ bot_check_wall(data,followWall);
+ break;
+
+ case CHECK_FOR_WALL_LEFT:
+
+ followWall=CHECK_WALL_LEFT;
+ bot_check_wall(data,followWall);
+ mazeState=CHECK_WALL_PRESENT;
+ break;
+
+
+ case CHECK_WALL_PRESENT:
+ /* wenn keine Wand gefunden aber links noch nicht nachgesehen, andere
+ * Richtung checken, sonst vorfahren */
+ checkedWalls++;
+ if (wall_detected==False){
+ /* Wand noch nicht entdeckt...haben wir schon beide gecheckt? */
+ if (checkedWalls<2) {
+ if (followWall==CHECK_WALL_RIGHT) {
+ mazeState=CHECK_FOR_WALL_LEFT;
+ break;
+ } else {
+ mazeState=CHECK_FOR_WALL_RIGHT;
+ break;
+ }
+ } else {
+ /* keine wand? dann vorfahren und selbes prozedere nochmal */
+ bot_drive_distance(data,0,BOT_SPEED_NORMAL,BOT_DIAMETER);
+ mazeState=CHECK_FOR_WALL_RIGHT;
+ checkedWalls=0;
+ break;
+ }
+ }
+ /* ok, wir haben unsere richtung im labyrinth gefunden jetzt dieser
+ * nur noch folgen bis Ziel, Abzweig oder Abgrund */
+ mazeState=SOLVE_MAZE_LOOP;
+ break;
+
+ case SOLVE_MAZE_LOOP:
+ /* Einen Schritt (=halbe BOT-Groesse) vorwaerts */
+ mazeState=SOLVE_TURN_WALL;
+ bot_drive_distance(data,0,BOT_SPEED_NORMAL,BOT_DIAMETER);
+ break;
+
+ case SOLVE_TURN_WALL:
+ /* Ziel erreicht? */
+ if ((sensLineL>GROUND_GOAL-20 && sensLineL<GROUND_GOAL+20) ||
+ (sensLineR>GROUND_GOAL-20 && sensLineR<GROUND_GOAL+20)) {
+ /* Bot hat Ziel erreicht...aus Freude einmal um die Achse drehen */
+ bot_turn(data,360);
+ mazeState=REACHED_GOAL;
+ break;
+ }
+ /* checken, ob wand vor uns (Abstand 2.5*Bot-Durchmesser in mm) */
+ distance=(sensDistL+sensDistR)/2;
+ if (distance<=25*BOT_DIAMETER) { // umrechnen 10*BOT_DIAMETER, da letzteres in cm angegeben wird
+ /* berechnete Entfernung zur Wand abzueglich optimale Distanz fahren */
+ mazeState=DETECTED_CROSS_BRANCH;
+ bot_drive_distance(data,0,BOT_SPEED_NORMAL,(distance-OPTIMAL_DISTANCE)/10);
+ break;
+ }
+ /* Zur Wand drehen....ist die Wand noch da? */
+ mazeState=CHECK_CONDITION;
+ bot_check_wall(data,followWall);
+ break;
+
+ case CHECK_CONDITION:
+ /* Solange weiter, wie die Wand zu sehen ist */
+ if (wall_detected==True) {
+ mazeState=SOLVE_MAZE_LOOP;
+ break;
+ }
+ /* messen, wo genau die Ecke ist */
+ mazeState=APPROACH_CORNER;
+ if (followWall==CHECK_WALL_LEFT){
+ bot_measure_angle(data,MEASURE_LEFT,300);
+ } else {
+ bot_measure_angle(data,MEASURE_RIGHT,300);
+ }
+ break;
+
+ case TURN_TO_BRANCH:
+ /* nun in Richtung Abzweig drehen , dann mit Hauptschleife weiter*/
+ mazeState=SOLVE_MAZE_LOOP;
+ if (followWall==CHECK_WALL_RIGHT) {
+ bot_turn(data,-90);
+ } else {
+ bot_turn(data,90);
+ }
+ break;
+
+ case DETECTED_CROSS_BRANCH:
+ /* Bot faehrt auf eine Ecke zu, in Richtung Gang drehen */
+ if (followWall==CHECK_WALL_LEFT) {
+ mazeState=SOLVE_MAZE_LOOP;
+ bot_turn(data,-90);
+ } else {
+ mazeState=SOLVE_MAZE_LOOP;
+ bot_turn(data,90);
+ }
+ break;
+
+ case APPROACH_CORNER:
+ /* ok, nun strecke bis zur Kante berechnen */
+ x=measure_distance*cos(measured_angle*M_PI/180)/10+BOT_DIAMETER*1.5;
+ mazeState=TURN_TO_BRANCH;
+ bot_drive_distance(data,0,BOT_SPEED_NORMAL,(int16)x);
+ break;
+
+ case REACHED_GOAL:
+ mazeState=CHECK_WALL_RIGHT;
+ speedWishLeft=BOT_SPEED_STOP;
+ speedWishRight=BOT_SPEED_STOP;
+ return_from_behaviour(data);
+ break;
+ }
+}
+
+
+/*!
+ * Das Verhalten findet seinen Weg durch ein Labyrinth, das nach gewissen Grundregeln gebaut ist
+ * in nicht immer optimaler Weise aber in jedem Fall. Es arbeitet nach dem Hoehlenforscher-Algorithmus.
+ * Einschraenkung: Objekte im Labyrinth, die Endlossschleifen verursachen koennen, z.b. ein einzeln
+ * stehender Pfeiler im Labyrinth um den der Bot dann immer wieder herum fahren wuerde.
+ */
+
+void bot_solve_maze(Behaviour_t *caller){
+ LOG_DEBUG(("bot_solve_maze()"));
+ switch_to_behaviour(caller, bot_solve_maze_behaviour,NOOVERRIDE);
+}
+
+/*!
+ * Initialisiert den Hoelenforscher
+ * @param prio_main Prioritaet des Hoehlenforschers (typ. 100)
+ * @param prio_helper Prioritaet der Hilfsfunktionen (typ. 42)
+ * @param active ACTIVE wenn der hoehlenforcher sofort starten soll, sonst INACTIVE
+ */
+void bot_solve_maze_init(int8 prio_main,int8 prio_helper, int8 active){
+ // Verhalten, um ein Labyrinth nach der Hoehlenforscher-Methode loesen
+ insert_behaviour_to_list(&behaviour, new_behaviour(prio_main, bot_solve_maze_behaviour,active));
+
+ insert_behaviour_to_list(&behaviour, new_behaviour(prio_helper--, bot_measure_angle_behaviour,INACTIVE));
+ insert_behaviour_to_list(&behaviour, new_behaviour(prio_helper, bot_check_wall_behaviour,INACTIVE));
+
+}
+#endif
diff --git a/source/ct-Bot/bot-logic/behaviour_turn.c b/source/ct-Bot/bot-logic/behaviour_turn.c
new file mode 100644
index 0000000..5b53c89
--- /dev/null
+++ b/source/ct-Bot/bot-logic/behaviour_turn.c
@@ -0,0 +1,361 @@
+/*
+ * 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_turn.c
+ * @brief Drehe den Bot
+ *
+ * @author Benjamin Benz (bbe@heise.de)
+ * @date 03.11.06
+*/
+
+
+#include "bot-logic/bot-logik.h"
+
+#ifdef BEHAVIOUR_TURN_AVAILABLE
+#ifdef MCU
+ #include <avr/eeprom.h>
+#endif
+#include <stdlib.h>
+#include <math.h>
+
+/* 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<angle2) {
+ // Es gab einen Ueberlauf
+ diff_angle=angle1+360-angle2;
+ } else {
+ diff_angle=angle1-angle2;
+ }
+ }
+ return diff_angle;
+}
+
+#ifdef MEASURE_MOUSE_AVAILABLE
+ /*!
+ * Das Verhalten laesst den Bot eine Punktdrehung durchfuehren.
++ * Drehen findet in drei Schritten statt. Die Drehung wird dabei
++ * bei Winkeln > 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 <stdlib.h>
+
+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
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-22 13:23:10 +0100