Absender: Menzel, Frank IT-OO4
Datum: Fr, 24.08.2007 08:57:10
In-reply-to:
<46CD483E.10903@xxxxxxxx>
Hallo, Anbei mal ein nettes kleines Explorer-Verhalten. Der bot fährt solange geradeaus, bis er voraus ein Hindernis erkennt. An diesem dreht er sich gewisse Zeit weg und das Spiel geht von vorn los. Die zu drehende Richtung hängt einerseits vom Wandabstand der Sensoren ab als auch bei Wand senkrecht voraus von einem Zufallswert. Als Beschreibung habe ich als Kommentar folgendes eingefügt: * @brief Wandfolger Explorer; faehrt solange vorwaerts, bis er an eine Wand kommt, an die er sich gewisse Zeit wegdreht; * nach links dreht er sich, wenn rechts eine Wand als naeher erkannt wird sonst nach rechts; es erfolgt * hier auch eine Abgrundauswertung; wird erkannt dass entweder * beide Abgrundsensoren den Abgrund detektieren oder der bot senkrecht zur Wand steht, so wird via Zeitzaehler * ein Pseudo-Zufallswert bis 255 ausgewertet und danach die neue Drehrichtung links/ rechts ermittelt; * zur Mindestdrehzeit wird ebenfalls immer dieser Zufallswert zuaddiert * sehr sinnvoll mit diesem Explorer-Verhalten ist das hang_on-Verhalten, welches durch Vergleich mit Mausdaten * ein Haengenbleiben erkennt, rueckwaerts faehrt und das hier registrierte Notverhalten startet. Dieses wiederum * sorgt dafuer, dass der bot sich wegdreht und weiterfaehrt wie an einer Wand. Gleiches gilt fuer das Abgrundverhalten. Recht nett anzusehen... Mit freundlichen Grüßen Frank Menzel Confidentiality note: The information in this email and any attachment may contain confidential and proprietary information of Heidelberger Druckmaschinen AG and/or its affiliates and may be privileged or otherwise protected from disclosure. If you are not the intended recipient, you are hereby notified that any review, reliance or distribution by others or forwarding without express permission is strictly prohibited and may cause liability. In case you have received this message due to an error in transmission, we kindly ask you to notify the sender immediately and to delete this email and any attachment from your system.
Index: C:/temp/ct-bot/ct-Bot/bot-logic/behaviour_olympic.c =================================================================== --- C:/temp/ct-bot/ct-Bot/bot-logic/behaviour_olympic.c (revision 1224) +++ C:/temp/ct-bot/ct-Bot/bot-logic/behaviour_olympic.c (working copy) @@ -26,6 +26,22 @@ #include "bot-logic/bot-logik.h" +/*! + * 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. + * Routine vorgezogen, damit sie auch anderen Verhalten zur Verfuegung steht wenn Olympic nicht benutzt wird + */ +int16 is_obstacle_ahead(int16 distance){ + if(sensDistL > distance && sensDistR > distance) return False; + if(sensDistL - sensDistR == 0) return 1; + else return (sensDistL - sensDistR); +} + + #ifdef BEHAVIOUR_OLYMPIC_AVAILABLE #include <stdlib.h> @@ -103,18 +119,6 @@ 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. Index: C:/temp/ct-bot/ct-Bot/bot-logic/behaviour_follow_wall.c =================================================================== --- C:/temp/ct-bot/ct-Bot/bot-logic/behaviour_follow_wall.c (revision 0) +++ C:/temp/ct-bot/ct-Bot/bot-logic/behaviour_follow_wall.c (revision 0) @@ -0,0 +1,298 @@ +/* + * 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_wall.c + * @brief Wandfolger Explorer; faehrt solange vorwaerts, bis er an eine Wand kommt, an die er sich gewisse Zeit wegdreht; + * nach links dreht er sich, wenn rechts eine Wand als naeher erkannt wird sonst nach rechts; es erfolgt + * hier auch eine Abgrundauswertung; wird erkannt dass entweder + * beide Abgrundsensoren den Abgrund detektieren oder der bot senkrecht zur Wand steht, so wird via Zeitzaehler + * ein Pseudo-Zufallswert bis 255 ausgewertet und danach die neue Drehrichtung links/ rechts ermittelt; + * zur Mindestdrehzeit wird ebenfalls immer dieser Zufallswert zuaddiert + * sehr sinnvoll mit diesem Explorer-Verhalten ist das hang_on-Verhalten, welches durch Vergleich mit Mausdaten + * ein Haengenbleiben erkennt, rueckwaerts faehrt und das hier registrierte Notverhalten startet. Dieses wiederum + * sorgt dafuer, dass der bot sich wegdreht und weiterfaehrt wie an einer Wand. Gleiches gilt fuer das Abgrundverhalten. + * + * + * @author Frank Menzel (Menzelfr@xxxxxxx) + * @date 05.05.2007 +*/ + +#include "bot-logic/bot-logik.h" + +#include "timer.h" +#include <math.h> + +#ifdef BEHAVIOUR_FOLLOW_WALL_AVAILABLE + + /* Zustaende fuer check_wall_behaviour-Verhalten */ + #define CHECK_FOR_BACK 1 + #define CHECK_WALL_GO 2 + #define CHECK_WALL_TURN 3 + #define CHECK_FOR_READY 4 + + /* Verhaltens- und letzter gemerkter Status */ + static uint8 state=CHECK_FOR_BACK; + static uint8 laststate=0; + + /* Zeitzaehlerwert vor Neusetzen */ + static uint16 lastmsTime =0; + + /* Abgrund wurde erkannt */ + volatile int8 border_follow_wall_fired=False; + + /* Mindest-Wartezeiten fuer Drehung; auf diese Werte wird noch der Pseudo-Zufallswert aus + * TIMER_GET_TICKCOUNT_8 raufaddiert, kann also bis 255 mehr sein */ + #define DELAY_NORMAL 100 // normale Wartezeit + #ifdef PC + #define DELAY_AFTER_HOLE 300 // Wartezeit nach Abgrund + #define DELAY_AFTER_HOLE_VERT 400 + #else + #define DELAY_AFTER_HOLE 150 // Wartezeit nach Abgrund + #define DELAY_AFTER_HOLE_VERT 200 + #endif + /* Delay-Time, solange wird mindestens gedreht, gesetzt im Prog */ + static uint16 delay_time_turn = 0; + + static int16 wall_side=0; // <0 Hindernis links, sonst rechts + + /* Abbruchfunktion des Verhaltens() */ + int8 (*check_function)(void)=0; /*!< Die Funktion, mit der das bot_explore_behaviour() feststellt, ob es etwas gefunden hat. + * Die Funktion muss True (1) zurueckgeben, wenn dem so ist, sonst False (0). + * Beispiele fuer eine solche Funktion sind check_for_light, is_good_pillar_ahead etc.*/ + /*! ermittelt Zufallswert je nach Zeitzaehler + * @param Zeit-Vergleichswert + * @return liefert 1 oder -1 in Abhaengigkeit der abgelaufenen Zeit als Zufallskomponente + */ + int8 get_randomside(uint8 compval) { + return (TIMER_GET_TICKCOUNT_8 >compval) ? -1 : 1 ; + } + + /*! + * Notfallhandler, ausgefuehrt bei Abgrunderkennung oder Haengenbleiben; muss registriert werden und laesst + * den bot etwas rueckwaerts fahren und dreht ihn zur Seite + */ +void border_follow_wall_handler(void) { + // Routine muss zuerst checken, ob follow_wall auch gerade aktiv ist, da nur in diesem + // Fall etwas gemacht werden muss + if (!behaviour_is_activated(bot_follow_wall_behaviour)) + return; + + border_follow_wall_fired=True; // Setzen der Syncvar des Verhaltens, die immer abgefragt wird + + state = CHECK_FOR_BACK; + + if (sensBorderL > BORDER_DANGEROUS && sensBorderR > BORDER_DANGEROUS) + { + wall_side = get_randomside(127); // links fuer < 127 sonst rechts + delay_time_turn = DELAY_AFTER_HOLE_VERT + TIMER_GET_TICKCOUNT_8; // Mindest- und Zufallszeit + } + else + { + wall_side = (sensBorderL > BORDER_DANGEROUS) ? -1:1; // Drehseite festlegen + delay_time_turn = DELAY_AFTER_HOLE + TIMER_GET_TICKCOUNT_8; // Mindest- und Zufallszeit + } + + // rueckwaertsfahren; bei schon aktivem Verhalten durch Notfallverhalten selbst wird dieses + // hier nicht ausgefuehrt + bot_drive_distance(0,0,-BOT_SPEED_FOLLOW,10); +} + + + +/*! Check, ob eine bestimmte Zeit vergangen ist seit letztem Aufruf + * @param ms Ablaufzeit in mm + * @return True wenn Zeit ms abgelaufen ist sonst False +*/ +int8 time_reached(uint16 ms) { + + register uint16 ticks = TIMER_GET_TICKCOUNT_16; + if (ticks-lastmsTime >= MS_TO_TICKS((uint32)ms)) { + lastmsTime = ticks; + return True; // Rueckgabe von true, Zeit ist abgelaufen + } + else + return False; // False, Zeit noch nicht abgelaufen + +} + + +/*! Ermittlung, ob sich eine Wand voraus senkrecht befindet, beide Sensoren also denselben Abstand sehen + * Sensorwerte duerfen dabei nicht zu weit voraus blicken + * @return True: Wand senkrecht voraus zu bot, beide Sensoren haben (innerhalb Toleranz) denselben Wert + */ +int8 wall_is_vertical(void) { + return (sensDistL > COL_FAR && sensDistR > COL_FAR) ? False : (fabs(sensDistL-sensDistR)<20); +} + + + + +/*! Das eigentliche Wandfolgerverhalten; Bot faehrt gerade voraus bis zu einer Wand und dreht sich gewisse Zeit solange, + * bis keine Wand mehr in geringem Abstand gefunden wird; trifft er senkrecht auf eine Wand, wird per Zufall die Seite + * ermittelt, wohin sich der Bot wegdreht (gilt auch fuer Abgrund); das Speiel geht wieder von vorn los + * @param data der Verhaltensdatensatz + */ +void bot_follow_wall_behaviour(Behaviour_t *data){ + + int16 sensor = 0; // Merker fuer Abstandssensor je nach Wand links oder rechts + + if(check_function && (*check_function)()) // wenn ueberhaupt definiert und Abbruchbedingung erfuellt + //return_from_behaviour(data); + state = CHECK_FOR_READY; + + + switch (state){ + + case CHECK_FOR_BACK: // Check auf zu nah oder bei Abgrund mit Rueckwaertsfahren + // Der Bot erkennt Hindernis + if (border_follow_wall_fired || is_obstacle_ahead(OPTIMAL_DISTANCE+ADJUST_DISTANCE)/*bot_avoid_harm()*/) { // aus bot_olympic + /* Abhaengig von der Seite, auf der die Wand ist, Entfernung ermitteln fuer rueckwaerts */ + sensor = (is_obstacle_ahead(OPTIMAL_DISTANCE+ADJUST_DISTANCE)<0) ? sensDistL: sensDistR ; + + if (border_follow_wall_fired) { // gesetzt durch registriertes Abgrundverhalten + state = CHECK_WALL_TURN; // nach rueckwaerts immer zum drehen + border_follow_wall_fired=False; + lastmsTime = TIMER_GET_TICKCOUNT_16; // Zeitzaehler geht jetzt los, damit mindestens xxms gedreht wird + break; + } // Notfall erkannt + + } // Hindernis oder Abgrund vorhanden + + /* kam ich von Drehung, dann auch wieder dorthin sonst fahren */ + state = (laststate == CHECK_WALL_TURN) ? CHECK_WALL_TURN: CHECK_WALL_GO ; + + break; + + + case CHECK_WALL_GO: + + // Der Bot erkennt Hindernis oder Abgrund und muss gecheckt werden + if (is_obstacle_ahead(OPTIMAL_DISTANCE+ADJUST_DISTANCE)) { // innerhalb xx cm Hindernis + + /* naechsten Status setzen und aktuellen merken */ + state=CHECK_FOR_BACK; + laststate=CHECK_WALL_TURN; + + // Seite des Hindernisses ermitteln; <0 links, >= 0 rechts + wall_side = is_obstacle_ahead(OPTIMAL_DISTANCE+ADJUST_DISTANCE); // Seite ermitteln + + /* bei Wand senkrecht voraus, dann per Zufall nach links oder rechts wegdrehen */ + if (wall_is_vertical()) + wall_side = get_randomside(127); // < 127 links sonst rechts + + break; + } + + + // Hier befindet sich kein Hindernis direkt vor dem Bot, einfach fahren. + #ifdef PC + speedWishLeft=BOT_SPEED_MAX; + speedWishRight=BOT_SPEED_MAX; + + #else + // sehen Sensoren unendlich, dann schnell fahren sonst normal + if(sensDistL > COL_FAR && sensDistR > COL_FAR ) { + speedWishLeft=BOT_SPEED_FAST; + speedWishRight=BOT_SPEED_FAST; + } + else + { + speedWishLeft=BOT_SPEED_NORMAL; + speedWishRight=BOT_SPEED_NORMAL; + } + #endif + + laststate=CHECK_WALL_GO; // merken dass ich aus GO komme + + break; // immer weiter fahren in GO + + case CHECK_WALL_TURN: + // nun bestimmte Zeit von Wand wegdrehen, je nach bereits ermittelter Seite + + // Setzen der Drehgeschwindigkeiten + if (wall_side < 0) { // rechtsdrehen wenn Wand links + speedWishLeft = BOT_SPEED_MEDIUM; + speedWishRight = -BOT_SPEED_MEDIUM; + } + else { // sonst linksdrehen, wenn Wand rechts + speedWishLeft = -BOT_SPEED_MEDIUM; + speedWishRight = BOT_SPEED_MEDIUM; + } + + // solange drehen bis Zeit in ms ueberschritten wird; ist dann genug Abstand bei beiden Sensoren + // zur Wand vorhanden ist Drehung beendet + // Durch die Zeit kommt der bot den Wandecken weniger nah, da er sich dadurch nicht genau parallel ausrichtet + // und mit Drehen aufhoert wenn er ueber die Ecke schielt sondern sich wieder etwas von wegdreht; bei Abgrund dreht er + // sich damit auch sicher weg mit hoeherer Drehzeit + // Wartezeit hier nicht init. da dies dann noch zufaelliger wird + if (time_reached(delay_time_turn)) { + + delay_time_turn = DELAY_NORMAL + TIMER_GET_TICKCOUNT_8; // wieder mit normalen Pseudo-Zufallswert belegen + + if(sensDistL > COL_NEAR && sensDistR > COL_NEAR ) { // keine Wand mehr in Sichtweite + state=CHECK_FOR_BACK; // Abgrund-/ Abstandscheck und dann los + laststate=0; // Initialwert + break; + } + + } // Check auf Ablauf der Wartezeit + + break; + + default: + state=CHECK_FOR_BACK; + laststate=0; + return_from_behaviour(data); + break; + } +} + + + +/*! Faehrt vorwaerts bis zu einer Wand, von die er sich wegdreht + * @param check Abbruchfunktion; wenn diese True liefert wird das Verhalten beendet sonst endlos + * einfach 0 uebergeben, wenn keine definiert ist + * @param caller Verhaltensdatensatz + */ +void bot_follow_wall(Behaviour_t *caller,int8 (*check)(void)) { + + check_function = check; + + #ifdef BEHAVIOUR_AVOID_COL_AVAILABLE + // verwaltet selbst Abstand zur Wand + deactivateBehaviour(bot_avoid_col_behaviour); + #endif + + #ifdef BEHAVIOUR_AVOID_BORDER_AVAILABLE + // falls durch anderes Verhalten vorher ausgeschaltet wurde + activateBehaviour(bot_avoid_border_behaviour); + #endif + + // nach Notaus stehen diese Vars sonst undefiniert rum, also init. + state=CHECK_FOR_BACK; + laststate=0; + + // und Umschalten zum eigentlichen Verhalten + switch_to_behaviour(caller, bot_follow_wall_behaviour,NOOVERRIDE); +} + + +#endif + + Index: C:/temp/ct-bot/ct-Bot/bot-logic/bot-logik.c =================================================================== --- C:/temp/ct-bot/ct-Bot/bot-logic/bot-logik.c (revision 1224) +++ C:/temp/ct-bot/ct-Bot/bot-logic/bot-logik.c (working copy) @@ -1,34 +1,34 @@ /* * 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 + * 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 + * 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 + * 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, + * Diese Datei sollte der Einstiegspunkt fuer eigene Experimente sein, * den Roboter zu steuern. - * - * bot_behave() arbeitet eine Liste von Verhalten ab. + * + * 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@xxxxxxxx) * @author Christoph Grimmer (c.grimmer@xxxxxxxxxx) * @date 01.12.05 @@ -71,12 +71,12 @@ #endif -#define MAX_PROCS 3 /*!< Maximale Anzahl der registrierbaren Funktionen */ +#define MAX_PROCS 6 /*!< Maximale Anzahl der registrierbaren Funktionen */ static int8_t count_arr_emerg = 0; /*!< Anzahl der zurzeit registrierten Notfallfunktionen */ /*! hier liegen die Zeiger auf die auszufuehrenden Abgrund Notfall-Funktionen */ static void (* emerg_functions[MAX_PROCS])(void) = {NULL}; -/*! +/*! * Routine zum Registrieren einer Notfallfunktion, die beim Ausloesen eines Abgrundsensors * aufgerufen wird; hierdurch kann ein Verhalten vom Abgrund benachrichtigt werden und * entsprechend dem Verhalten reagieren @@ -90,9 +90,9 @@ return proc_nr; } -/*! - * Beim Ausloesen eines Abgrundes wird diese Routine am Ende des Notfall-Abgrundverhaltens angesprungen - * und ruft alle registrierten Prozeduren der Reihe nach auf +/*! + * Beim Ausloesen eines Abgrundes wird diese Routine am Ende des Notfall-Abgrundverhaltens angesprungen + * und ruft alle registrierten Prozeduren der Reihe nach auf */ void start_registered_emergency_procs(void) { uint8_t i=0; @@ -101,8 +101,8 @@ } } -/*! - * Das einfachste Grundverhalten +/*! + * Das einfachste Grundverhalten * @param *data der Verhaltensdatensatz */ void bot_base_behaviour(Behaviour_t *data){ @@ -142,13 +142,13 @@ // Hoechste Prioritate haben die Notfall Verhalten // Verhalten zum Schutz des Bots, hohe Prioritaet, Aktiv - #ifdef BEHAVIOUR_AVOID_BORDER_AVAILABLE + #ifdef BEHAVIOUR_AVOID_BORDER_AVAILABLE insert_behaviour_to_list(&behaviour, new_behaviour(250, bot_avoid_border_behaviour,ACTIVE)); #endif - #ifdef BEHAVIOUR_AVOID_COL_AVAILABLE + #ifdef BEHAVIOUR_AVOID_COL_AVAILABLE insert_behaviour_to_list(&behaviour, new_behaviour(249, bot_avoid_col_behaviour,ACTIVE)); #endif - #ifdef BEHAVIOUR_HANG_ON_AVAILABLE + #ifdef BEHAVIOUR_HANG_ON_AVAILABLE insert_behaviour_to_list(&behaviour, new_behaviour(245, bot_hang_on_behaviour,ACTIVE)); // Registrierung des Handlers zur Behandlung des Haengenbleibens zum Rueckwaertsfahren register_emergency_proc(&hang_on_handler); @@ -159,15 +159,15 @@ insert_behaviour_to_list(&behaviour, new_behaviour(202, bot_scan_onthefly_behaviour,ACTIVE)); // vom Notfallverhalten wird Position des Abgrundes in Map eingetragen durch // Aufruf dieser registrierten Proc - #ifdef MAP_AVAILABLE + #ifdef MAP_AVAILABLE register_emergency_proc(&border_in_map_handler); - #endif + #endif // 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 + // 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 @@ -184,9 +184,9 @@ #endif #ifdef BEHAVIOUR_MEASURE_DISTANCE_AVAILABLE - insert_behaviour_to_list(&behaviour, new_behaviour(145, bot_measure_distance_behaviour, INACTIVE)); + insert_behaviour_to_list(&behaviour, new_behaviour(145, bot_measure_distance_behaviour, INACTIVE)); #endif - + // Verhalten, um laut Map zu einem bestimmten Ziel zu fahren #ifdef BEHAVIOUR_MAP_GO_DESTINATION_AVAILABLE insert_behaviour_to_list(&behaviour, new_behaviour(139, bot_path_bestfirst_behaviour,INACTIVE)); @@ -196,12 +196,19 @@ register_emergency_proc(&border_mapgo_handler); #endif + #ifdef BEHAVIOUR_FOLLOW_WALL_AVAILABLE + // Explorer-Verhalten um einer Wand zu folgen + insert_behaviour_to_list(&behaviour, new_behaviour(48, bot_follow_wall_behaviour, INACTIVE)); + // Registrierung zur Behandlung des Notfallverhaltens zum Rueckwaertsfahren + register_emergency_proc(&border_follow_wall_handler); + #endif + #ifdef BEHAVIOUR_CATCH_PILLAR_AVAILABLE insert_behaviour_to_list(&behaviour, new_behaviour(44, bot_catch_pillar_behaviour,INACTIVE)); insert_behaviour_to_list(&behaviour, new_behaviour(43, bot_unload_pillar_behaviour,INACTIVE)); #endif - + #ifdef BEHAVIOUR_OLYMPIC_AVAILABLE bot_olympic_init(52,80,INACTIVE); #endif @@ -223,19 +230,19 @@ #ifdef BEHAVIOUR_FOLLOW_OBJECT_AVAILABLE insert_behaviour_to_list(&behaviour, new_behaviour(40, bot_follow_object_behaviour, INACTIVE)); #endif - + #ifdef BEHAVIOUR_CALIBRATE_PID_AVAILABLE - insert_behaviour_to_list(&behaviour, new_behaviour(30, bot_calibrate_pid_behaviour, INACTIVE)); + insert_behaviour_to_list(&behaviour, new_behaviour(30, bot_calibrate_pid_behaviour, INACTIVE)); #endif #ifdef BEHAVIOUR_CALIBRATE_SHARPS_AVAILABLE - insert_behaviour_to_list(&behaviour, new_behaviour(29, bot_calibrate_sharps_behaviour, INACTIVE)); + insert_behaviour_to_list(&behaviour, new_behaviour(29, bot_calibrate_sharps_behaviour, INACTIVE)); #endif #ifdef BEHAVIOUR_TURN_TEST_AVAILABLE insert_behaviour_to_list(&behaviour, new_behaviour(28, bot_turn_test_behaviour, INACTIVE)); #endif - + // Grundverhalten, setzt aeltere FB-Befehle um, aktiv insert_behaviour_to_list(&behaviour, new_behaviour(2, bot_base_behaviour, ACTIVE)); @@ -253,10 +260,10 @@ */ void activateBehaviour(BehaviourFunc function) { switch_to_behaviour(NULL, function, NOOVERRIDE); - + // Behaviour_t *job; // Zeiger auf ein Verhalten // -// // Einmal durch die Liste gehen, bis wir den gewuenschten Eintrag haben +// // 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; @@ -272,8 +279,8 @@ */ void deactivateBehaviour(BehaviourFunc function){ Behaviour_t *job; // Zeiger auf ein Verhalten - - // Einmal durch die Liste gehen, bis wir den gewuenschten Eintrag haben + + // 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; @@ -291,7 +298,7 @@ uint8_t behaviour_is_activated(BehaviourFunc function) { Behaviour_t *job; // Zeiger auf ein Verhalten - // Einmal durch die Liste gehen, bis wir den gewuenschten Eintrag haben + // Einmal durch die Liste gehen, bis wir den gewuenschten Eintrag haben for (job = behaviour; job; job = job->next) { if (job->work == function) return job->active; @@ -308,9 +315,9 @@ */ static uint8 isInCallHierarchy(Behaviour_t *job, BehaviourFunc function) { uint8 level = 0; - + if (job == NULL) return 0; // Liste ist leer - + for (; job->caller; job=job->caller){ level++; if (job->caller->work == function){ @@ -318,24 +325,24 @@ return level; // Direkter Aufrufender in Tiefe level gefunden } } - return level; + return level; } // O(n), n:=|Caller-Liste| /*! - * Deaktiviert alle von diesem Verhalten aufgerufenen Verhalten. + * Deaktiviert alle von diesem Verhalten aufgerufenen Verhalten. * Das Verhalten selbst bleibt Aktiv und bekommt ein SUBCANCEL in seine datanestruktur eingetragen. * @param function Die Funktion, die das Verhalten realisiert. */ void deactivateCalledBehaviours(BehaviourFunc function){ Behaviour_t *job; // Zeiger auf ein Verhalten uint8 level; - + LOG_DEBUG("Beginne mit dem Durchsuchen der Liste"); // Einmal durch die Liste gehen, und alle aktiven Funktionen pruefen, ob sie von dem uebergebenen Verhalten aktiviert wurden uint16 i=0; Behaviour_t* beh_of_function = NULL; for (job=behaviour; job; job=job->next) { // n mal - if (job->active == ACTIVE){ + if (job->active == ACTIVE){ i++; level = isInCallHierarchy(job, function); // O(n) LOG_DEBUG("Verhalten mit Prio = %u ist ACTIVE, Durchlauf %u", job->priority, i); @@ -347,7 +354,7 @@ Behaviour_t* tmp = job; job = job->caller; // zur naechsten Ebene tmp->caller = NULL; // Caller loeschen, damit Verhalten auch ohne OVERRIDE neu gestartet werden koennen - } + } } else if (job->work == function){ /* Verhalten von function fuer spaeter merken, wenn wir hier eh schon die ganze Liste absuchen */ beh_of_function = job; @@ -360,30 +367,30 @@ } } // O(n^2) -/*! - * Ruft ein anderes Verhalten auf und merkt sich den Ruecksprung +/*! + * 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 + * 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. + * 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)(Behaviour_t *), uint8 override ){ Behaviour_t *job; // Zeiger auf ein Verhalten - - // Einmal durch die Liste gehen, bis wir den gewuenschten Eintrag haben + + // 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) { /* Zielverhalten existiert gar nicht */ if (from) from->subResult=SUBFAIL; @@ -405,28 +412,28 @@ from->active=INACTIVE; from->subResult=SUBRUNNING; } - + // neues Verhalten aktivieren job->active=ACTIVE; // Aufrufer sichern job->caller = from; - + #ifdef DEBUG_BOT_LOGIC if (from) { LOG_DEBUG("Verhaltenscall: %d wurde von %d aufgerufen",job->priority,from->priority); - } else { + } else { LOG_DEBUG("Verhaltenscall: %d wurde direkt aufgerufen",job->priority); } #endif } -/*! +/*! * @brief Kehrt zum aufrufenden Verhalten zurueck * @param data laufendes Verhalten - */ + */ void return_from_behaviour(Behaviour_t * data){ data->active=INACTIVE; // Unterverhalten deaktivieren - if (data->caller){ + if (data->caller){ data->caller->active=ACTIVE; // aufrufendes Verhalten aktivieren data->caller->subResult=SUBSUCCESS; // Unterverhalten war erfolgreich } @@ -438,25 +445,25 @@ */ void deactivateAllBehaviours(void){ Behaviour_t *job; // Zeiger auf ein Verhalten - // Einmal durch die Liste gehen und (fast) alle deaktivieren, Grundverhalten nicht + // 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; + // Verhalten deaktivieren + job->active = INACTIVE; job->caller = NULL; // Caller loeschen, damit Verhalten auch ohne OVERRIDE neu gestartet werden koennen } - } + } } -/*! - * @brief Zentrale Verhaltens-Routine, wird regelmaessig aufgerufen. +/*! + * @brief Zentrale Verhaltens-Routine, wird regelmaessig aufgerufen. */ -void bot_behave(void){ +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 @@ -468,15 +475,15 @@ /* WunschVariablen initialisieren */ speedWishLeft = BOT_SPEED_IGNORE; speedWishRight = BOT_SPEED_IGNORE; - + faktorWishLeft = 1.0; faktorWishRight = 1.0; - + if (job->work) // hat das Verhalten eine Work-Routine job->work(job); /* Verhalten ausfuehren */ else // wenn nicht: Verhalten deaktivieren, da es nicht sinnvoll arbeiten kann - job->active=INACTIVE; - + job->active=INACTIVE; + /* Modifikatoren sammeln */ faktorLeft *= faktorWishLeft; faktorRight *= faktorWishRight; @@ -486,11 +493,11 @@ 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) { @@ -499,18 +506,18 @@ } } -/*! - * @brief Erzeugt ein neues Verhalten +/*! + * @brief Erzeugt ein neues Verhalten * @param priority Die Prioritaet * @param *work Den Namen der Funktion, die sich drum kuemmert * @param active Boolean, ob das Verhalten aktiv oder inaktiv erstellt wird */ 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) + Behaviour_t *newbehaviour = (Behaviour_t *) malloc(sizeof(Behaviour_t)); + + if (newbehaviour == NULL) return NULL; - + newbehaviour->priority = priority; newbehaviour->active=active; newbehaviour->next= NULL; @@ -528,11 +535,11 @@ 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; @@ -546,13 +553,13 @@ } else { /* Mit dem naechsten Eintrag vergleichen */ while(NULL != ptr->next) { - if (ptr->next->priority < behave->priority) + if (ptr->next->priority < behave->priority) break; - + /* Naechster Eintrag */ ptr = ptr->next; } - + temp = ptr->next; ptr->next = behave; behave->next = temp; @@ -564,7 +571,7 @@ /*! * @brief Behandelt die Tasten fuer die Verhaltensanezeige, die das jeweilige Verhalten aktivieren oder deaktivieren. * @author Timo Sandmann (mail@xxxxxxxxxxxxxxx) - * @date 14.02.2007 + * @date 14.02.2007 * @param data Zeiger auf ein Array mit Verhaltensdatensatzzeigern */ static void beh_disp_key_handler(Behaviour_t** data){ @@ -573,12 +580,12 @@ switch (RC5_Code){ case RC5_CODE_1: callee = data[0]; break; case RC5_CODE_2: callee = data[1]; break; - case RC5_CODE_3: callee = data[2]; break; - case RC5_CODE_4: callee = data[3]; break; - case RC5_CODE_5: callee = data[4]; break; - case RC5_CODE_6: callee = data[5]; break; - case RC5_CODE_7: callee = data[6]; break; - case RC5_CODE_8: callee = data[7]; break; + case RC5_CODE_3: callee = data[2]; break; + case RC5_CODE_4: callee = data[3]; break; + case RC5_CODE_5: callee = data[4]; break; + case RC5_CODE_6: callee = data[5]; break; + case RC5_CODE_7: callee = data[6]; break; + case RC5_CODE_8: callee = data[7]; break; } /* Verhaltensstatus toggeln */ if (callee != NULL){ @@ -586,29 +593,29 @@ callee->active ^= 1; } } - + /*! * @brief Zeigt Informationen ueber Verhalten an, 'A' fuer Verhalten aktiv, 'I' fuer Verhalten inaktiv. * @author Timo Sandmann (mail@xxxxxxxxxxxxxxx) - * @date 12.02.2007 - * Es werden zwei Spalten mit jeweils 4 Verhalten angezeigt. Gibt es mehr Verhalten in der Liste, kommt man + * @date 12.02.2007 + * Es werden zwei Spalten mit jeweils 4 Verhalten angezeigt. Gibt es mehr Verhalten in der Liste, kommt man * mit der Taste DOWN auf eine weitere Seite (die aber kein extra Screen ist). Mit der Taste UP geht's bei Bedarf * wieder zurueck. Vor den Prioritaeten steht eine Nummer von 1 bis 8, drueckt man die entsprechende Zifferntaste * auf der Fernbedienung, so wird das Verhalten aktiv oder inaktiv geschaltet, komplementaer zum aktuellen Status. - * Den Keyhandler dazu stellt beh_disp_key_handler() dar. + * Den Keyhandler dazu stellt beh_disp_key_handler() dar. */ - void behaviour_display(void){ + void behaviour_display(void){ static uint8 behaviour_page = 0; /*!< zuletzt angezeigte Verhaltensseite */ - if (RC5_Code == RC5_CODE_DOWN){ + if (RC5_Code == RC5_CODE_DOWN){ /* naechste Seite */ behaviour_page++; display_clear(); // Screen-Nr. wechselt nicht => Screen selbst loeschen - RC5_Code = 0; // Taste behandelt - } else if (RC5_Code == RC5_CODE_UP){ + RC5_Code = 0; // Taste behandelt + } else if (RC5_Code == RC5_CODE_UP){ /* vorherige Seite */ if (behaviour_page > 0) behaviour_page--; display_clear(); - RC5_Code = 0; + RC5_Code = 0; } Behaviour_t* behaviours[8] = {NULL}; /*!< speichert Zeiger auf die Verhalten fuer den Keyhandler zwischen */ uint8 i,j,k=0; @@ -628,7 +635,7 @@ } } char status[2] = "IA"; // I: inactive, A: active - /* max. 4 Zeilen mit jeweils 2 Verhalten (= 8 Verhalten) anzeigbar */ + /* max. 4 Zeilen mit jeweils 2 Verhalten (= 8 Verhalten) anzeigbar */ for (i=1; i<=20; i+=11){ // Spalten for (j=1; j<=4; j++){ // Zeilen while (ptr != NULL && ptr->priority > PRIO_VISIBLE_MAX) @@ -636,16 +643,16 @@ if (ptr == NULL || ptr->priority < PRIO_VISIBLE_MIN){ if (i==1 && j==1 && behaviour_page > 0) behaviour_page--; // keine unnoetige leere Seite anzeigen if (RC5_Code !=0) beh_disp_key_handler(behaviours); // Tasten auswerten - return; // fertig, da ptr == NULL oder Prioritaet bereits zu klein + return; // fertig, da ptr == NULL oder Prioritaet bereits zu klein } /* Ausgabe */ display_cursor(j, i); display_printf("%u: %3d=%c ", k+1, ptr->priority, status[ptr->active]); behaviours[k++] = ptr; // speichern fuer Tastenhandler ptr = ptr->next; - } - } + } + } if (RC5_Code !=0) beh_disp_key_handler(behaviours); // Tasten auswerten - } + } #endif // DISPLAY_BEHAVIOUR_AVAILABLE #endif // BEHAVIOUR_AVAILABLE Index: C:/temp/ct-bot/ct-Bot/bot-logic/behaviour_remotecall.c =================================================================== --- C:/temp/ct-bot/ct-Bot/bot-logic/behaviour_remotecall.c (revision 1224) +++ C:/temp/ct-bot/ct-Bot/bot-logic/behaviour_remotecall.c (working copy) @@ -1,24 +1,24 @@ /* * 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 + * 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 + * 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 + * 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.c * @brief ruft auf ein Kommando hin andere Verhalten auf und bestaetigt dann ihre Ausfuehrung * @author Benjamin Benz (bbe@xxxxxxxx) @@ -67,7 +67,7 @@ #endif -/*! +/*! * Hier muessen alle Boten-Funktionen rein, die Remote aufgerufen werden sollen * Diese stoÃ?en dann das zugehoerige Verhalten an * Ein Eintrag erfolgt so: @@ -76,14 +76,14 @@ * Alle Botenfunktionen muessen folgendem Schema entsprechen * void bot_xxx(Behaviour_t * caller, ...); * - * Erklaerung am Bsp: + * 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 @@ -93,7 +93,7 @@ #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 + #ifdef BEHAVIOUR_GOTOXY_AVAILABLE PREPARE_REMOTE_CALL(bot_gotoxy, 2, "float x, float y", 4, 4), #endif #ifdef BEHAVIOUR_SOLVE_MAZE_AVAILABLE @@ -131,11 +131,14 @@ PREPARE_REMOTE_CALL(bot_calibrate_pid,1,"int16 speed",2), #endif #ifdef BEHAVIOUR_CALIBRATE_SHARPS_AVAILABLE - PREPARE_REMOTE_CALL(bot_calibrate_sharps,0,"") - #endif + PREPARE_REMOTE_CALL(bot_calibrate_sharps,0,""), + #endif #ifdef BEHAVIOUR_TURN_TEST_AVAILABLE PREPARE_REMOTE_CALL(bot_turn_test,0,""), #endif + #ifdef BEHAVIOUR_FOLLOW_WALL_AVAILABLE + PREPARE_REMOTE_CALL(bot_follow_wall,1,""), + #endif }; #define STORED_CALLS (sizeof(calls)/sizeof(call_t)) /*!< Anzahl der Remote calls im Array */ @@ -147,7 +150,7 @@ */ 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)){ @@ -166,7 +169,7 @@ * @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@xxxxxxxxxxxxxxx) + * @author Timo Sandmann (mail@xxxxxxxxxxxxxxx) * @date 12.01.2007 */ static void remotecall_convert_params(uint8* dest, uint8 count, uint8* len, uint8* data){ @@ -174,25 +177,25 @@ /* 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 + 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"); @@ -211,13 +214,13 @@ // 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 - #ifdef DEBUG_REMOTE_CALLS + #ifdef DEBUG_REMOTE_CALLS LOG_DEBUG("call_id=%u",call_id); LOG_DEBUG("parameter_count=%u", parameter_count); #endif @@ -229,7 +232,7 @@ 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 und td initialisieren (s.u.) 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 */ @@ -240,7 +243,7 @@ :: "g" (tmp) : "memory" ); - } + } /* Parameter 1 (data) und Funktionsaufruf */ asm volatile( "pushl %0 # push data \n\t" @@ -254,16 +257,16 @@ 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: + * 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). + * 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]); @@ -283,20 +286,20 @@ ); func(data); // Die aufgerufene Botenfunktion starten #endif - + running_behaviour=REMOTE_CALL_RUNNING; return; } else { - LOG_DEBUG("Parameteranzahl unzulaessig!"); + LOG_DEBUG("Parameteranzahl unzulaessig!"); } break; - + case REMOTE_CALL_RUNNING: // Es lief ein Verhalten und ist nun zuende (sonst waeren wir nicht hier) { - // Antwort schicken + // Antwort schicken char * function_name; - + #ifdef PC function_name=(char*) &calls[function_id].name; #else @@ -304,34 +307,34 @@ char tmp[REMOTE_CALL_FUNCTION_NAME_LEN+1]; function_name=(char*)&tmp; - + uint8* from= (uint8*)& calls[function_id].name; - uint8 i; + uint8 i; for (i=0; i<REMOTE_CALL_FUNCTION_NAME_LEN+1; i++) - *function_name++ = (uint8) pgm_read_byte ( from++ ); + *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); LOG_DEBUG("Remote-call %s beendet (%d)",function_name,result); #endif - + // 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; - + } } @@ -353,27 +356,27 @@ // 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; + 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); + 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++ ); + parameter_length[i] = (uint8) pgm_read_byte ( from++ ); #endif - + LOG_DEBUG("func=%s param_count=%d Len= %u %u %u",func,parameter_count,parameter_length[0],parameter_length[1],parameter_length[2]); if (data != NULL){ LOG_DEBUG("data= %x %x %x",data[0],data[1],data[2]); } - + #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; } @@ -387,7 +390,7 @@ bot_remotecall(NULL, function_name, params); } -/*! +/*! * Listet alle verfuegbaren Remote-Calls auf und verschickt sie als einzelne Kommanods */ void remote_call_list(void){ @@ -397,9 +400,9 @@ uint8* from; #endif call_t* call; - + LOG_DEBUG("Liste %d remote calls",STORED_CALLS); - + int16 i; for (i=0; i< (STORED_CALLS); i++){ #ifdef MCU @@ -408,15 +411,15 @@ to= (uint8*)&call_storage; uint8 j; for (j=0; j< sizeof(call_t); j++){ - *to = (uint8) pgm_read_byte ( from++ ); + *to = (uint8) pgm_read_byte ( from++ ); to++; } call = &call_storage; #else call = (call_t*)&calls[i]; #endif - - #ifdef COMMAND_AVAILABLE + + #ifdef COMMAND_AVAILABLE // und uebertragen command_write_rawdata(CMD_REMOTE_CALL,SUB_REMOTE_CALL_ENTRY,&i,&i, sizeof(call_t),(uint8*)call); #endif Index: C:/temp/ct-bot/ct-Bot/include/bot-logic/available_behaviours.h =================================================================== --- C:/temp/ct-bot/ct-Bot/include/bot-logic/available_behaviours.h (revision 1224) +++ C:/temp/ct-bot/ct-Bot/include/bot-logic/available_behaviours.h (working copy) @@ -3,7 +3,7 @@ #ifdef BEHAVIOUR_AVAILABLE -/*! +/*! * @file available_behaviours.h * @brief globale Schalter fuer die einzelnen Verhalten */ @@ -13,7 +13,7 @@ #define BEHAVIOUR_AVOID_BORDER_AVAILABLE /*!< Abgruenden ausweichen vorhanden ?*/ #define BEHAVIOUR_AVOID_COL_AVAILABLE /*!< Hindernis ausweichen vorhanden ?*/ -//#define BEHAVIOUR_HANG_ON_AVAILABLE /*!< Erkennen des Haengenbleibens als Notfallverhalten ?*/ +#define BEHAVIOUR_HANG_ON_AVAILABLE /*!< Erkennen des Haengenbleibens als Notfallverhalten ?*/ //#define BEHAVIOUR_GOTO_AVAILABLE /*!< goto vorhanden ?*/ //#define BEHAVIOUR_GOTOXY_AVAILABLE /*!< gotoxy vorhanden ?*/ #define BEHAVIOUR_TURN_AVAILABLE /*!< turn vorhanden ?*/ @@ -30,6 +30,8 @@ #define BEHAVIOUR_SERVO_AVAILABLE /*!< Kontrollverhalten fuer die Servos */ +#define BEHAVIOUR_FOLLOW_WALL_AVAILABLE /*!< Follow Wall Explorer Verhalten */ + //#define BEHAVIOUR_OLYMPIC_AVAILABLE /*!< Olympiadenverhalten vorhanden ?*/ //#define BEHAVIOUR_CATCH_PILLAR_AVAILABLE /*!< Suche eine Dose und fange sie ein */ @@ -43,8 +45,8 @@ #define BEHAVIOUR_DELAY_AVAILABLE /*!< Delay-Routinen als Verhalten */ -/* Aufgrund einer ganzen reihe von Abhaengigkeiten sollte man beim Versuch Speicher - * zu sparen, zuerst mal bei den Hauptverhalten ausmisten, sonst kommen die +/* Aufgrund einer ganzen reihe von Abhaengigkeiten sollte man beim Versuch Speicher + * zu sparen, zuerst mal bei den Hauptverhalten ausmisten, sonst kommen die * Unterverhalten durch die Hintertuer wieder rein */ #ifndef MAP_AVAILABLE @@ -54,11 +56,11 @@ #ifdef BEHAVIOUR_GOTOXY_AVAILABLE #define BEHAVIOUR_TURN_AVAILABLE -#endif +#endif #ifdef BEHAVIOUR_AVOID_COL_AVAILABLE #define BEHAVIOUR_TURN_AVAILABLE -#endif +#endif #ifdef BEHAVIOUR_TURN_TEST_AVAILABLE #define BEHAVIOUR_TURN_AVAILABLE @@ -67,25 +69,25 @@ #ifdef BEHAVIOUR_FOLLOW_LINE_AVAILABLE #define BEHAVIOUR_DRIVE_DISTANCE_AVAILABLE #define BEHAVIOUR_TURN_AVAILABLE -#endif +#endif #ifdef BEHAVIOUR_OLYMPIC_AVAILABLE #define BEHAVIOUR_DRIVE_DISTANCE_AVAILABLE #define BEHAVIOUR_TURN_AVAILABLE -#endif +#endif #ifdef BEHAVIOUR_SIMPLE_AVAILABLE #define BEHAVIOUR_DRIVE_DISTANCE_AVAILABLE -#endif +#endif #ifdef BEHAVIOUR_SOLVE_MAZE_AVAILABLE #define BEHAVIOUR_DRIVE_DISTANCE_AVAILABLE #define BEHAVIOUR_DELAY_AVAILABLE -#endif +#endif #ifdef BEHAVIOUR_DRIVE_SQUARE_AVAILABLE #define BEHAVIOUR_GOTO_AVAILABLE -#endif +#endif #ifdef BEHAVIOUR_CATCH_PILLAR_AVAILABLE #define BEHAVIOUR_SERVO_AVAILABLE @@ -142,6 +144,8 @@ #include "bot-logic/behaviour_olympic.h" +#include "bot-logic/behaviour_follow_wall.h" + #include "bot-logic/behaviour_servo.h" #include "bot-logic/behaviour_catch_pillar.h" Index: C:/temp/ct-bot/ct-Bot/include/bot-logic/behaviour_olympic.h =================================================================== --- C:/temp/ct-bot/ct-Bot/include/bot-logic/behaviour_olympic.h (revision 1224) +++ C:/temp/ct-bot/ct-Bot/include/bot-logic/behaviour_olympic.h (working copy) @@ -28,6 +28,18 @@ #define BEHAVIOUR_OLYMPIC_H_ #include "bot-logic/bot-logik.h" + +/*! + * 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. + * Routine vorgezogen, damit sie auch anderen Verhalten zur Verfuegung steht wenn Olympic nicht benutzt wird + */ +int16 is_obstacle_ahead(int16 distance); + #ifdef BEHAVIOUR_OLYMPIC_AVAILABLE /*! Index: C:/temp/ct-bot/ct-Bot/include/bot-logic/behaviour_follow_wall.h =================================================================== --- C:/temp/ct-bot/ct-Bot/include/bot-logic/behaviour_follow_wall.h (revision 0) +++ C:/temp/ct-bot/ct-Bot/include/bot-logic/behaviour_follow_wall.h (revision 0) @@ -0,0 +1,52 @@ +/* + * 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.h + * @brief Wandfolger, der sich an Wand gewisse Zeit dreht; solange bis kein Hindernis mehr sichtbar ist + * + * @author Frank Menzel(Menzelfr@xxxxxxx) + * @date 03.11.06 +*/ + +#include "bot-logic/bot-logik.h" + +#ifndef BEHAVIOUR_FOLLOW_WALL_H_ +#define BEHAVIOUR_FOLLOW_WALL_H_ + +#ifdef BEHAVIOUR_FOLLOW_WALL_AVAILABLE + + +void border_follow_wall_handler(void); + +/* Faehrt vorwaerts bis zu einer Wand, von die er sich wegdreht + * @param data Verhaltensdatensatz + */ +void bot_follow_wall_behaviour(Behaviour_t *data); + +/*! Botenfunktion + * Faehrt vorwaerts bis zu einer Wand, von die er sich wegdreht + * @param check Abbruchfunktion; wenn diese True liefert wird das Verhalten beendet sonst endlos + * einfach 0 uebergeben, wenn keine definiert ist + * @param caller Verhaltensdatensatz + */ +void bot_follow_wall(Behaviour_t *caller,int8 (*check)(void)); + +#endif +#endif /*BEHAVIOUR_FOLLOW_WALL_H_*/ + Index: C:/temp/ct-bot/ct-Bot/ui/available_screens.h =================================================================== --- C:/temp/ct-bot/ct-Bot/ui/available_screens.h (revision 1224) +++ C:/temp/ct-bot/ct-Bot/ui/available_screens.h (working copy) @@ -35,7 +35,7 @@ #define SENSOR_DISPLAY_AVAILABLE /*!< zeigt die Sensordaten an */ //#define DISPLAY_REGELUNG_AVAILABLE /*!< Gibt Debug-Infos der Motorregelung aus */ -//#define DISPLAY_BEHAVIOUR_AVAILABLE /*!< zeigt Verhalten an */ +#define DISPLAY_BEHAVIOUR_AVAILABLE /*!< zeigt Verhalten an */ //#define MISC_DISPLAY_AVAILABLE /*!< aehm ja, der Rest irgendwie... */ #define DISPLAY_ODOMETRIC_INFO /*!< zeigt Positions- und Geschwindigkeitsdaten an */ #define DISPLAY_MMC_INFO /*!< Zeigt die Daten der MMC-Karte an */ Index: C:/temp/ct-bot/ct-Bot/.settings/org.eclipse.cdt.core.prefs =================================================================== --- C:/temp/ct-bot/ct-Bot/.settings/org.eclipse.cdt.core.prefs (revision 0) +++ C:/temp/ct-bot/ct-Bot/.settings/org.eclipse.cdt.core.prefs (revision 0) @@ -0,0 +1,3 @@ +#Thu Aug 23 21:04:37 CEST 2007 +eclipse.preferences.version=1 +indexerId=org.eclipse.cdt.core.fastIndexer