neocturne/rc2007-soccer
Archived
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Added lot's of code-files used during work

Browse source
This commit is contained in:
sicarius 2007-02-11 18:32:03 +00:00
parent 644121b478
commit 56d9bdd39e
155 changed files with 23423 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

41
source/ct-Bot/bot-logic/behaviour_avoid_border.c Normal file
View file

@ -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

142
source/ct-Bot/bot-logic/behaviour_avoid_col.c Normal file
View file

@ -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

106
source/ct-Bot/bot-logic/behaviour_catch_pillar.c Normal file
View file

@ -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

120
source/ct-Bot/bot-logic/behaviour_drive_distance.c Normal file
View file

@ -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

76
source/ct-Bot/bot-logic/behaviour_drive_square.c Normal file
View file

@ -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

176
source/ct-Bot/bot-logic/behaviour_follow_line.c Normal file
View file

@ -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<39> 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

141
source/ct-Bot/bot-logic/behaviour_goto.c Normal file
View file

@ -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

141
source/ct-Bot/bot-logic/behaviour_gotoxy.c Normal file
View file

@ -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

428
source/ct-Bot/bot-logic/behaviour_olympic.c Normal file
View file

@ -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<39>. */
#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<34>. */
#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

407
source/ct-Bot/bot-logic/behaviour_remotecall.c Normal file
View file

@ -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

144
source/ct-Bot/bot-logic/behaviour_scan.c Normal file
View file

@ -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

68
source/ct-Bot/bot-logic/behaviour_servo.c Normal file
View file

@ -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

139
source/ct-Bot/bot-logic/behaviour_simple.c Normal file
View file

@ -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

660
source/ct-Bot/bot-logic/behaviour_solve_maze.c Normal file
View file

@ -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<36> 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

361
source/ct-Bot/bot-logic/behaviour_turn.c Normal file
View file

@ -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

500
source/ct-Bot/bot-logic/bot-logik.c Normal file
View file

@ -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