summaryrefslogtreecommitdiffstats
path: root/source/ct-Bot/bot-logic/bot-logik.c
diff options
context:
space:
mode:
Diffstat (limited to 'source/ct-Bot/bot-logic/bot-logik.c')
-rw-r--r--source/ct-Bot/bot-logic/bot-logik.c500
1 files changed, 500 insertions, 0 deletions
diff --git a/source/ct-Bot/bot-logic/bot-logik.c b/source/ct-Bot/bot-logic/bot-logik.c
new file mode 100644
index 0000000..3274375
--- /dev/null
+++ b/source/ct-Bot/bot-logic/bot-logik.c
@@ -0,0 +1,500 @@
+/*
+ * c't-Sim - Robotersimulator fuer den c't-Bot
+ *
+ * This program is free software; you can redistribute it
+ * and/or modify it under the terms of the GNU General
+ * Public License as published by the Free Software
+ * Foundation; either version 2 of the License, or (at your
+ * option) any later version.
+ * This program is distributed in the hope that it will be
+ * useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE. See the GNU General Public License for more details.
+ * You should have received a copy of the GNU General Public
+ * License along with this program; if not, write to the Free
+ * Software Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307, USA.
+ *
+ */
+
+/*! @file bot-logik.c
+ * @brief High-Level Routinen fuer die Steuerung des c't-Bots.
+ * Diese Datei sollte der Einstiegspunkt fuer eigene Experimente sein,
+ * den Roboter zu steuern.
+ *
+ * bot_behave() arbeitet eine Liste von Verhalten ab.
+ * Jedes Verhalten kann entweder absolute Werte setzen, dann kommen niedrigerpriorisierte nicht mehr dran.
+ * Alternativ dazu kann es Modifikatoren aufstellen, die bei niedriger priorisierten angewendet werden.
+ * bot_behave_init() baut diese Liste auf.
+ * Jede Verhaltensfunktion bekommt einen Verhaltensdatensatz uebergeben, in den Sie ihre Daten eintraegt
+ *
+ * @author Benjamin Benz (bbe@heise.de)
+ * @author Christoph Grimmer (c.grimmer@futurio.de)
+ * @date 01.12.05
+*/
+
+
+#include "bot-logic/bot-logik.h"
+
+#ifdef BEHAVIOUR_AVAILABLE
+
+#include "display.h"
+#include "rc5.h"
+
+
+
+#include <stdlib.h>
+
+int16 speedWishLeft; /*!< Puffervariablen fuer die Verhaltensfunktionen absolut Geschwindigkeit links*/
+int16 speedWishRight; /*!< Puffervariablen fuer die Verhaltensfunktionen absolut Geschwindigkeit rechts*/
+
+float faktorWishLeft; /*!< Puffervariablen fuer die Verhaltensfunktionen Modifikationsfaktor links*/
+float faktorWishRight; /*!< Puffervariablen fuer die Verhaltensfunktionen Modifikationsfaktor rechts */
+
+int16 target_speed_l=BOT_SPEED_STOP; /*!< Sollgeschwindigkeit linker Motor - darum kuemmert sich bot_base()*/
+int16 target_speed_r=BOT_SPEED_STOP; /*!< Sollgeschwindigkeit rechter Motor - darum kuemmert sich bot_base() */
+
+
+/*! Liste mit allen Verhalten */
+Behaviour_t *behaviour = NULL;
+
+/*!
+ * Das einfachste Grundverhalten
+ * @param *data der Verhaltensdatensatz
+ */
+void bot_base_behaviour(Behaviour_t *data){
+ speedWishLeft=target_speed_l;
+ speedWishRight=target_speed_r;
+}
+
+/*!
+ * Initialisert das ganze Verhalten
+ */
+void bot_behave_init(void){
+ #ifdef BEHAVIOUR_REMOTECALL_AVAILABLE
+ // Dieses Verhalten kann andere Starten
+ insert_behaviour_to_list(&behaviour, new_behaviour(254, bot_remotecall_behaviour,INACTIVE));
+ #endif
+
+ #ifdef BEHAVIOUR_SERVO_AVAILABLE
+ insert_behaviour_to_list(&behaviour, new_behaviour(253, bot_servo_behaviour,INACTIVE));
+ #endif
+
+ // Demo-Verhalten, ganz einfach, inaktiv
+ // Achtung, im Moment hat es eine hoehere Prioritaet als die Gefahrenerkenner!!!
+ #ifdef BEHAVIOUR_SIMPLE_AVAILABLE
+ insert_behaviour_to_list(&behaviour, new_behaviour(252, bot_simple_behaviour,INACTIVE));
+ insert_behaviour_to_list(&behaviour, new_behaviour(251, bot_simple2_behaviour,INACTIVE));
+ #endif
+
+
+ // Hoechste Prioritate haben die Notfall Verhalten
+
+ // Verhalten zum Schutz des Bots, hohe Prioritaet, Aktiv
+ #ifdef BEHAVIOUR_AVOID_BORDER_AVAILABLE
+ insert_behaviour_to_list(&behaviour, new_behaviour(250, bot_avoid_border_behaviour,ACTIVE));
+ #endif
+ #ifdef BEHAVIOUR_AVOID_COL_AVAILABLE
+ insert_behaviour_to_list(&behaviour, new_behaviour(249, bot_avoid_col_behaviour,ACTIVE));
+ #endif
+
+ #ifdef BEHAVIOUR_SCAN_AVAILABLE
+ // Verhalten, das die Umgebung des Bots on-the fly beim fahren scannt
+ insert_behaviour_to_list(&behaviour, new_behaviour(155, bot_scan_onthefly_behaviour,ACTIVE));
+
+ // Verhalten, das einmal die Umgebung des Bots scannt
+ insert_behaviour_to_list(&behaviour, new_behaviour(152, bot_scan_behaviour,INACTIVE));
+ #endif
+
+ // Alle Hilfsroutinen sind relativ wichtig, da sie auch von den Notverhalten her genutzt werden
+ // Hilfsverhalten, die Befehle von Boten-Funktionen ausfuehren, erst inaktiv, werden von Boten aktiviert
+ #ifdef BEHAVIOUR_TURN_AVAILABLE
+ insert_behaviour_to_list(&behaviour, new_behaviour(150, bot_turn_behaviour,INACTIVE));
+ #endif
+ #ifdef BEHAVIOUR_DRIVE_DISTANCE_AVAILABLE
+ insert_behaviour_to_list(&behaviour, new_behaviour(149, bot_drive_distance_behaviour,INACTIVE));
+ #endif
+ #ifdef BEHAVIOUR_GOTO_AVAILABLE
+ insert_behaviour_to_list(&behaviour, new_behaviour(148, bot_goto_behaviour,INACTIVE));
+ #endif
+
+ // Hilfsverhalten zum Anfahren von Positionen
+ #ifdef BEHAVIOUR_GOTOXY_AVAILABLE
+ insert_behaviour_to_list(&behaviour, new_behaviour(147, bot_gotoxy_behaviour,INACTIVE));
+ #endif
+
+
+ #ifdef BEHAVIOUR_CATCH_PILLAR_AVAILABLE
+ insert_behaviour_to_list(&behaviour, new_behaviour(44, bot_catch_pillar_behaviour,INACTIVE));
+ #endif
+
+
+ #ifdef BEHAVIOUR_OLYMPIC_AVAILABLE
+ bot_olympic_init(52,100,INACTIVE);
+ #endif
+
+ #ifdef BEHAVIOUR_FOLLOW_LINE_AVAILABLE
+ // Verhalten um einer Linie zu folgen
+ insert_behaviour_to_list(&behaviour, new_behaviour(70, bot_follow_line_behaviour, ACTIVE));
+ #endif
+
+ #ifdef BEHAVIOUR_SOLVE_MAZE_AVAILABLE
+ bot_solve_maze_init(100,43,INACTIVE);
+ #endif
+
+ #ifdef BEHAVIOUR_DRIVE_SQUARE_AVAILABLE
+ // Demo-Verhalten, etwas komplexer, inaktiv
+ insert_behaviour_to_list(&behaviour, new_behaviour(51, bot_drive_square_behaviour,INACTIVE));
+ #endif
+
+
+
+ // Grundverhalten, setzt aeltere FB-Befehle um, aktiv
+ insert_behaviour_to_list(&behaviour, new_behaviour(2, bot_base_behaviour, ACTIVE));
+
+ // Um das Simple-Behaviour zu nutzen, die Kommentarzeichen vor der folgenden Zeile entfernen!!!
+ // activateBehaviour(bot_simple_behaviour);
+ // activateBehaviour(bot_simple2_behaviour);
+
+ #ifdef PC
+ #ifdef DISPLAY_AVAILABLE
+ /* Anzeigen der geladenen Verhalten */
+ Behaviour_t *ptr = behaviour;
+
+ display_cursor(5,1);
+ display_printf("Verhaltensstack:\n");
+ while(ptr != NULL) {
+ display_printf("Prioritaet: %d.\n", ptr->priority);
+ ptr = ptr->next;
+ }
+ #endif
+ #endif
+}
+
+
+/*!
+ * Aktiviert eine Regel mit gegebener Funktion
+ * @param function Die Funktion, die das Verhalten realisiert.
+ */
+void activateBehaviour(BehaviourFunc function){
+ Behaviour_t *job; // Zeiger auf ein Verhalten
+
+ // Einmal durch die Liste gehen, bis wir den gewuenschten Eintrag haben
+ for (job = behaviour; job; job = job->next) {
+ if (job->work == function) {
+ job->active = ACTIVE;
+ break;
+ }
+ }
+}
+
+
+/*!
+ * Deaktiviert eine Regel mit gegebener Funktion
+ * @param function Die Funktion, die das Verhalten realisiert.
+ */
+void deactivateBehaviour(BehaviourFunc function){
+ Behaviour_t *job; // Zeiger auf ein Verhalten
+
+ // Einmal durch die Liste gehen, bis wir den gewuenschten Eintrag haben
+ for (job = behaviour; job; job = job->next) {
+ if (job->work == function) {
+ job->active = INACTIVE;
+ break;
+ }
+ }
+}
+
+/*!
+ * Ruft ein anderes Verhalten auf und merkt sich den Ruecksprung
+ * return_from_behaviour() kehrt dann spaeter wieder zum aufrufenden Verhalten zurueck
+ * @param from aufrufendes Verhalten
+ * @param to aufgerufenes Verhalten
+ * @param override Hier sind zwei Werte Moeglich:
+ * 1. OVERRIDE : Das Zielverhalten to wird aktiviert, auch wenn es noch aktiv ist.
+ * Das Verhalten, das es zuletzt aufgerufen hat wird dadurch automatisch
+ * wieder aktiv und muss selbst sein eigenes Feld subResult auswerten, um zu pruefen, ob das
+ * gewuenschte Ziel erreicht wurde, oder vorher ein Abbruch stattgefunden hat.
+ * 2. NOOVERRIDE : Das Zielverhalten wird nur aktiviert, wenn es gerade nichts zu tun hat.
+ * In diesem Fall kann der Aufrufer aus seinem eigenen subResult auslesen,
+ * ob seibem Wunsch Folge geleistet wurde.
+ */
+void switch_to_behaviour(Behaviour_t * from, void *to, uint8 override ){
+ Behaviour_t *job; // Zeiger auf ein Verhalten
+
+ // Einmal durch die Liste gehen, bis wir den gewuenschten Eintrag haben
+ for (job = behaviour; job; job = job->next) {
+ if (job->work == to) {
+ break; // Abbruch der Schleife, job zeigt nun auf die Datenstruktur des Zielverhaltens
+ }
+ }
+
+ if (job->caller){ // Ist das auzurufende Verhalten noch beschaeftigt?
+ if (override==NOOVERRIDE){ // nicht ueberschreiben, sofortige Rueckkehr
+ if (from)
+ from->subResult=SUBFAIL;
+ return;
+ }
+ // Wir wollen also ueberschreiben, aber nett zum alten Aufrufer sein und ihn darueber benachrichtigen
+ job->caller->active=ACTIVE; // alten Aufrufer reaktivieren
+ job->caller->subResult=SUBFAIL; // er bekam aber nicht das gewuenschte Resultat
+ }
+
+ if (from) {
+ // laufendes Verhalten abschalten
+ from->active=INACTIVE;
+ from->subResult=SUBRUNNING;
+ }
+
+ // neues Verhalten aktivieren
+ job->active=ACTIVE;
+ // Aufrufer sichern
+ job->caller = from;
+}
+
+/*!
+ * Kehrt zum aufrufenden Verhalten zurueck
+ * @param running laufendes Verhalten
+ */
+void return_from_behaviour(Behaviour_t * data){
+ data->active=INACTIVE; // Unterverhalten deaktivieren
+ if (data->caller){
+ data->caller->active=ACTIVE; // aufrufendes Verhalten aktivieren
+ data->caller->subResult=SUBSUCCESS; // Unterverhalten war erfolgreich
+ }
+ data->caller=NULL; // Job erledigt, Verweis loeschen
+}
+
+/*!
+ * Deaktiviert alle Verhalten bis auf Grundverhalten. Bei Verhaltensauswahl werden die Aktivitaeten vorher
+ * in die Verhaltens-Auswahlvariable gesichert.
+ */
+void deactivateAllBehaviours(void){
+ Behaviour_t *job; // Zeiger auf ein Verhalten
+
+ #ifdef DISPLAY_BEHAVIOUR_AVAILABLE
+ // bei Verhaltensanzeige in Aktivitaets-Auswahl-Variable sichern
+ #ifndef DISPLAY_DYNAMIC_BEHAVIOUR_AVAILABLE
+ // bei Verhaltensanzeige in Aktivitaets-Auswahl-Variable sichern
+ // nicht bei dynamischer Anzeige und Selektion
+ set_behaviours_equal();
+ #endif
+ #endif
+
+ // Einmal durch die Liste gehen und (fast) alle deaktivieren, Grundverhalten nicht
+ for (job = behaviour; job; job = job->next) {
+ if ((job->priority >= PRIO_VISIBLE_MIN) &&(job->priority <= PRIO_VISIBLE_MAX)) {
+ // Verhalten deaktivieren
+ job->active = INACTIVE;
+ }
+ }
+}
+
+/*!
+ * Zentrale Verhaltens-Routine, wird regelmaessig aufgerufen.
+ * Dies ist der richtige Platz fuer eigene Routinen, um den Bot zu steuern.
+ */
+void bot_behave(void){
+ Behaviour_t *job; // Zeiger auf ein Verhalten
+
+ float faktorLeft = 1.0; // Puffer fuer Modifkatoren
+ float faktorRight = 1.0; // Puffer fuer Modifkatoren
+
+ #ifdef RC5_AVAILABLE
+ rc5_control(); // Abfrage der IR-Fernbedienung
+ #endif
+
+ /* Solange noch Verhalten in der Liste sind...
+ (Achtung: Wir werten die Jobs sortiert nach Prioritaet aus. Wichtige zuerst einsortieren!!!) */
+ for (job = behaviour; job; job = job->next) {
+ if (job->active) {
+ /* WunschVariablen initialisieren */
+ speedWishLeft = BOT_SPEED_IGNORE;
+ speedWishRight = BOT_SPEED_IGNORE;
+
+ faktorWishLeft = 1.0;
+ faktorWishRight = 1.0;
+
+ job->work(job); /* Verhalten ausfuehren */
+ /* Modifikatoren sammeln */
+ faktorLeft *= faktorWishLeft;
+ faktorRight *= faktorWishRight;
+ /* Geschwindigkeit aendern? */
+ if ((speedWishLeft != BOT_SPEED_IGNORE) || (speedWishRight != BOT_SPEED_IGNORE)){
+ if (speedWishLeft != BOT_SPEED_IGNORE)
+ speedWishLeft *= faktorLeft;
+ if (speedWishRight != BOT_SPEED_IGNORE)
+ speedWishRight *= faktorRight;
+
+ motor_set(speedWishLeft, speedWishRight);
+ break; /* Wenn ein Verhalten Werte direkt setzen will, nicht weitermachen */
+ }
+
+ }
+ /* Dieser Punkt wird nur erreicht, wenn keine Regel im System die Motoren beeinflusen will */
+ if (job->next == NULL) {
+ motor_set(BOT_SPEED_IGNORE, BOT_SPEED_IGNORE);
+ }
+ }
+}
+
+/*!
+ * Erzeugt ein neues Verhalten
+ * @param priority Die Prioritaet
+ * @param *work Den Namen der Funktion, die sich drum kuemmert
+ */
+Behaviour_t *new_behaviour(uint8 priority, void (*work) (struct _Behaviour_t *data), int8 active){
+ Behaviour_t *newbehaviour = (Behaviour_t *) malloc(sizeof(Behaviour_t));
+
+ if (newbehaviour == NULL)
+ return NULL;
+
+ newbehaviour->priority = priority;
+ newbehaviour->active=active;
+ newbehaviour->next= NULL;
+ newbehaviour->work=work;
+ newbehaviour->caller=NULL;
+ newbehaviour->subResult=SUBSUCCESS;
+ return newbehaviour;
+}
+
+/*!
+ * Fuegt ein Verhalten der Verhaltenliste anhand der Prioritaet ein.
+ * @param list Die Speicherstelle an der die globale Verhaltensliste anfaengt
+ * @param behave Einzufuegendes Verhalten
+ */
+void insert_behaviour_to_list(Behaviour_t **list, Behaviour_t *behave){
+ Behaviour_t *ptr = *list;
+ Behaviour_t *temp = NULL;
+
+ /* Kein Eintrag dabei? */
+ if (behave == NULL)
+ return;
+
+ /* Erster Eintrag in der Liste? */
+ if (ptr == NULL){
+ ptr = behave;
+ *list = ptr;
+ } else {
+ /* Gleich mit erstem Eintrag tauschen? */
+ if (ptr->priority < behave->priority) {
+ behave->next = ptr;
+ ptr = behave;
+ *list = ptr;
+ } else {
+ /* Mit dem naechsten Eintrag vergleichen */
+ while(NULL != ptr->next) {
+ if (ptr->next->priority < behave->priority)
+ break;
+
+ /* Naechster Eintrag */
+ ptr = ptr->next;
+ }
+
+ temp = ptr->next;
+ ptr->next = behave;
+ behave->next = temp;
+ }
+ }
+}
+
+
+
+#ifdef DISPLAY_BEHAVIOUR_AVAILABLE
+
+/*!
+ * ermittelt ob noch eine weitere Verhaltensseite existiert
+ */
+ int8 another_behaviour_page(void) {
+ int16 max_behaviours ;
+ Behaviour_t *ptr ;
+
+ /* dazu muss ich auch gueltige Screenseite sein */
+ #ifdef DISPLAY_SCREENS_AVAILABLE
+ if (display_screen != 2)
+ return 0;
+ #endif
+
+ ptr = behaviour;
+ max_behaviours = 0;
+
+// zuerst alle Verhalten ermitteln ausser Grundverhalten
+ while(ptr != NULL) {
+ if ((ptr->priority >= PRIO_VISIBLE_MIN) &&(ptr->priority <= PRIO_VISIBLE_MAX))
+ max_behaviours++;
+
+ ptr = ptr->next;
+ }
+
+ return (behaviour_page * 6) < max_behaviours;
+}
+
+
+/*!
+ * toggled ein Verhalten der Verhaltensliste an Position pos,
+ * die Aenderung erfolgt nur auf die Puffervariable
+ * @param pos Listenposition, entspricht der Taste 1-6 der gewaehlten Verhaltensseite
+ */
+void toggleNewBehaviourPos(int8 pos){
+ Behaviour_t *job; // Zeiger auf ein Verhalten
+ int8 i;
+
+ // nur aendern, wenn ich richtige Screenseite bin
+ if (display_screen != 2)
+ return ;
+
+ // richtigen Index je nach Seite ermitteln
+ pos = (behaviour_page - 1) * 6 + pos;
+ i = 0;
+
+ // durch die Liste gehen, bis wir den gewuenschten Index erreicht haben
+ for (job = behaviour; job; job = job->next) {
+ if ((job->priority >= PRIO_VISIBLE_MIN) &&(job->priority <= PRIO_VISIBLE_MAX)) {
+ i++;
+ if (i == pos) {
+ // bei dynamischer Wahl wird direkt die Zustandsvariable geaendert
+ #ifdef DISPLAY_DYNAMIC_BEHAVIOUR_AVAILABLE
+ job->active = !job->active;
+ #else
+ job->active_new = !job->active_new;
+ #endif
+
+ break;
+ }
+ }
+ }
+}
+
+#ifndef DISPLAY_DYNAMIC_BEHAVIOUR_AVAILABLE
+/*!
+ * Startschuss, die gewaehlten neuen Verhaltensaktivitaeten werden in die
+ * Verhaltensliste geschrieben und die Verhalten damit scharf geschaltet
+ */
+void set_behaviours_active_to_new(void) {
+
+ Behaviour_t *job;
+ for (job = behaviour; job; job = job->next) {
+
+ if ((job->priority >= PRIO_VISIBLE_MIN) &&(job->priority <= PRIO_VISIBLE_MAX))
+ job->active = job->active_new;
+
+ }
+
+}
+
+/*!
+ * Die Aktivitaeten der Verhalten werden in die Puffervariable geschrieben,
+ * welche zur Anzeige und Auswahl verwendet wird
+ */
+void set_behaviours_equal(void) {
+ Behaviour_t *job;
+ for (job = behaviour; job; job = job->next) {
+
+ if ((job->priority >= PRIO_VISIBLE_MIN) &&(job->priority <= PRIO_VISIBLE_MAX))
+ job->active_new = job->active;
+
+ }
+}
+#endif
+#endif
+#endif
generated by cgit v1.2.3 (git 2.25.1) at 2024-05-06 11:34:40 +0200