500 lines
16 KiB
C
500 lines
16 KiB
C
/*
|
|
* 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
|