Absender: Frank Menzel
Datum: Di, 18.03.2008 20:30:31
Hallo, bevor ich über Ostern nun in den Urlaub fahre, anbei noch ein kleines Stackverhalten. Dieses legt nach Aktivierung (via dem Stack-Screen) während des weiteren Rumfahrens relevante Wegepositionen automatisch auf den Stack. Nach Start des Stack-Fahrverhaltens werden diese dann ab der aktuellen Position abgefahren, d.h. der bot fährt seinen Weg rückwärts ab. Schöne Ostern ! Gruß, Frank Menzel
Index: C:/eclipse/workspace/ct-Bot/bot-logic/behaviour_drive_stack.c =================================================================== --- C:/eclipse/workspace/ct-Bot/bot-logic/behaviour_drive_stack.c (revision 1395) +++ C:/eclipse/workspace/ct-Bot/bot-logic/behaviour_drive_stack.c (working copy) @@ -33,6 +33,7 @@ #include "rc5.h" #include "rc5-codes.h" #include "pos_stack.h" +#include "math_utils.h" #include <stdlib.h> static uint8_t drivestack_state = 0; /*!< Status des drive_stack-Verhaltens */ @@ -57,6 +58,7 @@ default: pos_stack_clear(); // sicherheitshalber bereinigen und auf Null + deactivateBehaviour(bot_goto_pos_behaviour); //komischerweise fuhr bot hier weiter, daher deaktivieren return_from_behaviour(data); break; } @@ -69,6 +71,7 @@ void bot_drive_stack(Behaviour_t * caller) { switch_to_behaviour(caller,bot_drive_stack_behaviour,OVERRIDE); drivestack_state = 0; + deactivateBehaviour(bot_put_stack_waypositions_behaviour); } /*! @@ -96,7 +99,82 @@ } } + +static uint8_t waypos_state = 0; /*!< Status des drive_stack-Push-Verhaltens */ + +static int16 last_xpos=0; /*!< letzte gemerkte x-Position */ +static int16 last_ypos=0; /*!< letzte gemerkte y-Position */ +static int16 last_heading=0; /*!< letzte gemerkte Botausrichtung */ + +#define DIST_FOR_PUSH 14400 /*!< Quadrat des Abstandes zum letzten Punkt, ab dem gepush wird */ +#define DIST_FOR_PUSH_TURN 3600 /*!< Quadrat des Abstandes nach erreichen eines Drehwinkels zum letzten Punkt */ +#define ANGLE_FOR_PUSH 20 /*!< nach Erreichen dieses Drehwinkels wird ebenfalls gepusht */ + /*! + * Hilfsroutine zum Speichern der aktuellen Botposition in die Zwischenvariablen + */ +void set_pos_to_last(void) { + last_xpos=x_pos; + last_ypos=y_pos; + last_heading=heading; +} + + +/*! + * Verhalten um sich entlang des Fahrweges relevante Koordinaten auf dem Stack zu merken; Verhalten ist nach Aktivierung via Botenroutine + * ein Endlosverhalten und sammelt bis zur Deaktivierung die Wegepunkte; deaktiviert wird es via Notaus oder direkt mit Befehl zum Zurueckfahren und + * damit Start des Stack-Fahrverhaltens + * @param *caller Der Verhaltensdatensatz des Aufrufers + */ +void bot_put_stack_waypositions_behaviour(Behaviour_t * data) { + switch (waypos_state) { + case 0: + // Botpos wird in den Stack geschrieben zum Startzeitpunkt des Verhaltens (Stack noch leer) oder + // wenn die Entfernung zur zuletzt gemerkten Koordinate gewissen Abstand ueberschreitet oder + // wenn Entfernung noch nicht erreicht ist aber ein gewisser Drehwinkel erreicht wurde + + // Abstand zur letzten Position ueberschritten + if (get_dist(last_xpos,last_ypos,x_pos,y_pos)>DIST_FOR_PUSH) { + // kein Push notwendig bei gerader Fahrt voraus zum Sparen des Stack-Spiehcerplatzes + if ((int16) heading != last_heading) + bot_push_pos(x_pos,y_pos); + + set_pos_to_last(); // Position jedenfalls merken + break; + } + + // bei Drehwinkelaenderung und Uberschreitung einer gewissen Groesse mit geringer Abstandsentfernung zum letzten Punkt kommt er in den Stack + if (( last_heading != (int16) heading) && (360-abs(calc_diff_from_angles(last_heading)))>ANGLE_FOR_PUSH && get_dist(last_xpos,last_ypos,x_pos,y_pos)>DIST_FOR_PUSH_TURN) { + set_pos_to_last(); + bot_push_pos(x_pos,y_pos); + } + + + break; + + default: + // kommt eigentlich nie hierher, da es solange aktiv ist bis Deaktivierung eintritt; das kann Notaus sein oder das Stackfahrverhalten + // zum Zurueck-Abfahren der Stackpunkte + return_from_behaviour(data); + break; + } +} + +/*! + * Botenfunktion: Verhalten um sich entlang des Fahrweges relevante Koordinaten auf dem Stack zu merken + * @param *caller Der Verhaltensdatensatz des Aufrufers + */ +void bot_put_stack_waypositions(Behaviour_t * caller) { + pos_stack_clear(); + set_pos_to_last(); // aktuelle Botposition wird zum ersten Stackeintrag und merken der Position + bot_push_pos(last_xpos,last_ypos); + switch_to_behaviour(caller,bot_put_stack_waypositions_behaviour,OVERRIDE); + drivestack_state = 0; + last_xpos=0;last_ypos=0;last_heading=0; +} + + +/*! * Keyhandler zur Verwendung via Fernbedienung auf dem Display zum Stackanfahren */ #ifdef DISPLAY_DRIVE_STACK_AVAILABLE @@ -113,6 +191,18 @@ RC5_Code = 0; bot_drive_stack(NULL); break; + + case RC5_CODE_5: + /* Verhalten zum Speichern relevanter Wegepopsitionen zum Spaeteren Zurueckfahren */ + RC5_Code = 0; + bot_put_stack_waypositions(NULL); + break; + + case RC5_CODE_8: + /* Loeschen des Positionsstacks */ + RC5_Code = 0; + pos_stack_clear(); + break; } // switch } // Ende Keyhandler @@ -126,8 +216,10 @@ display_printf("Bot-Pos %5d %5d", (int16_t)x_pos, (int16_t)y_pos); display_cursor(2,1); display_printf("Stack %5d %5d", posx, posy); + display_cursor(3,1); + display_printf("Save/Goto/Del: 3/4/8"); display_cursor(4,1); - display_printf("Pos Save/Goto: 3/4"); + display_printf("Start WayPushPos: 5"); drivestack_disp_key_handler(); // aufrufen des Key-Handlers } Index: C:/eclipse/workspace/ct-Bot/bot-logic/bot-logik.c =================================================================== --- C:/eclipse/workspace/ct-Bot/bot-logic/bot-logik.c (revision 1395) +++ C:/eclipse/workspace/ct-Bot/bot-logic/bot-logik.c (working copy) @@ -247,6 +247,7 @@ #endif #ifdef BEHAVIOUR_DRIVE_STACK_AVAILABLE + insert_behaviour_to_list(&behaviour, new_behaviour(34,bot_put_stack_waypositions_behaviour, INACTIVE)); insert_behaviour_to_list(&behaviour, new_behaviour(33, bot_drive_stack_behaviour, INACTIVE)); #endif Index: C:/eclipse/workspace/ct-Bot/include/bot-logic/behaviour_drive_stack.h =================================================================== --- C:/eclipse/workspace/ct-Bot/include/bot-logic/behaviour_drive_stack.h (revision 1395) +++ C:/eclipse/workspace/ct-Bot/include/bot-logic/behaviour_drive_stack.h (working copy) @@ -60,6 +60,20 @@ void bot_push_pos(int16_t pos_x, int16_t pos_y); /*! + * Verhalten um sich entlang des Fahrweges relevante Koordinaten auf dem Stack zu merken; Verhalten ist nach Aktivierung via Botenroutine + * ein Endlosverhalten und sammelt bis zur Deaktivierung die Wegepunkte; deaktiviert wird es via Notaus oder direkt mit Befehl zum Zurueckfahren und + * damit Start des Stack-Fahrverhaltens + * @param *caller Der Verhaltensdatensatz des Aufrufers + */ +void bot_put_stack_waypositions_behaviour(Behaviour_t * data); + +/*! + * Botenfunktion: Verhalten um sich entlang des Fahrweges relevante Koordinaten auf dem Stack zu merken + * @param *caller Der Verhaltensdatensatz des Aufrufers + */ +void bot_put_stack_waypositions(Behaviour_t * caller); + +/*! * Display zum Setzen und Anfahren der Stackpunkte */ void drive_stack_display(void); Index: C:/eclipse/workspace/ct-Bot/include/bot-logic/available_behaviours.h =================================================================== --- C:/eclipse/workspace/ct-Bot/include/bot-logic/available_behaviours.h (revision 1395) +++ C:/eclipse/workspace/ct-Bot/include/bot-logic/available_behaviours.h (working copy) @@ -33,7 +33,7 @@ #define BEHAVIOUR_SERVO_AVAILABLE /*!< Kontrollverhalten fuer die Servos */ -//#define BEHAVIOUR_DRIVE_STACK_AVAILABLE /*!< Abfahren der auf dem Stack gesicherten Koordinaten */ +#define BEHAVIOUR_DRIVE_STACK_AVAILABLE /*!< Abfahren der auf dem Stack gesicherten Koordinaten */ //#define BEHAVIOUR_OLYMPIC_AVAILABLE /*!< Olympiadenverhalten vorhanden? */ Index: C:/eclipse/workspace/ct-Bot/include/math_utils.h =================================================================== --- C:/eclipse/workspace/ct-Bot/include/math_utils.h (revision 1395) +++ C:/eclipse/workspace/ct-Bot/include/math_utils.h (working copy) @@ -75,4 +75,20 @@ return (z & 0x80000000) ? -1 : 1; } +/* berechnet Winkeldifferenz zwischen aktueller Botausrichtung und einem Zielwinkel + * param dest_head Zielwinkel + * @return berechnete Winkeldifferenz + */ +int16 calc_diff_from_angles(int16 dest_head); + +/*! + * Ermittlung des Quadrat-Abstandes zwischen 2 Koordinaten + * @param x1 x-Koordinate des ersten Punktes + * @param y1 y-Koordinate des ersten Punktes + * @param x2 Map-Koordinate des Zielpunktes + * @param y2 Map-Koordinate des Zielpunktes + * @return liefert Quadrat-Abstand zwischen den Map-Punkten + */ +int16 get_dist(int16 x1, int16 y1, int16 x2, int16 y2); + #endif /*MATH_UTILS_H_*/ Index: C:/eclipse/workspace/ct-Bot/include/pos_stack.h =================================================================== --- C:/eclipse/workspace/ct-Bot/include/pos_stack.h (revision 1395) +++ C:/eclipse/workspace/ct-Bot/include/pos_stack.h (working copy) @@ -39,7 +39,7 @@ /*! ab hier der statische Stack mittels Array */ #ifdef ARRAY_POINT_STACK -#define POS_STACK_SIZE 20 // Stackgroesse ist beim Array begrenzt +#define POS_STACK_SIZE 30 // Stackgroesse ist beim Array begrenzt /*! Datenstruktur eines Koordinatenpunktes */ typedef struct { Index: C:/eclipse/workspace/ct-Bot/math_utils.c =================================================================== --- C:/eclipse/workspace/ct-Bot/math_utils.c (revision 1395) +++ C:/eclipse/workspace/ct-Bot/math_utils.c (working copy) @@ -26,6 +26,7 @@ #include "ct-Bot.h" #include "sensor.h" #include <math.h> +#include <stdlib.h> /*! * Berechnung einer Winkeldifferenz zwischen dem aktuellen Standpunkt und einem anderen Ort @@ -49,3 +50,34 @@ return toTurn; } + +/* berechnet Winkeldifferenz zwischen aktueller Botausrichtung und einem Zielwinkel + * param dest_head Zielwinkel + * @return berechnete Winkeldifferenz + */ +int16 calc_diff_from_angles(int16 dest_head) { + int16_t to_turn = (int16_t)(dest_head - (int16_t)heading) % 360; + if (to_turn > 180) to_turn = 360 - to_turn; + else if (to_turn < -180) to_turn += 360; + to_turn=360 - abs(to_turn); + + return to_turn; +} + +/*! + * Ermittlung des Quadrat-Abstandes zwischen 2 Koordinaten + * @param x1 x-Koordinate des ersten Punktes + * @param y1 y-Koordinate des ersten Punktes + * @param x2 Map-Koordinate des Zielpunktes + * @param y2 Map-Koordinate des Zielpunktes + * @return liefert Quadrat-Abstand zwischen den Map-Punkten + */ +int16 get_dist(int16 x1, int16 y1, int16 x2, int16 y2){ + int16 xt=x2 - x1; + int16 yt=y2 - y1; + + /* Abstandsermittlung nach dem guten alten Pythagoras ohne Ziehen der Wurzel */ + return (xt*xt+yt*yt); +} + + Index: C:/eclipse/workspace/ct-Bot/Changelog.txt =================================================================== --- C:/eclipse/workspace/ct-Bot/Changelog.txt (revision 1395) +++ C:/eclipse/workspace/ct-Bot/Changelog.txt (working copy) @@ -1,5 +1,7 @@ CHANGELOG fuer c't-Bot ====================== +2008-03-18 Frank Menzel [Menzelfr@xxxxxxx]: Verhalten um sich entlang des Fahrweges relevante Koordinaten auf dem Stack zu merken + 2008-03-16 Benjamin Benz [bbe@xxxxxxxx]: Rohcode für die automatische Adressvergabe eingefügt 2008-03-16 Timo Sandmann [mail@xxxxxxxxxxxxxxx]: Bugfix fuer command.c Index: C:/eclipse/workspace/ct-Bot/.cdtbuild =================================================================== --- C:/eclipse/workspace/ct-Bot/.cdtbuild (revision 1395) +++ C:/eclipse/workspace/ct-Bot/.cdtbuild (working copy) @@ -10,8 +10,8 @@ <option id="gnu.c.compiler.option.include.paths.387475860" superClass="gnu.c.compiler.option.include.paths" valueType="includePath"> <listOptionValue builtIn="false" value=""${ProjDirPath}""/> <listOptionValue builtIn="false" value=""${ProjDirPath}/include""/> -<listOptionValue builtIn="false" value=""C:\Programme\MinGW\include""/> -<listOptionValue builtIn="false" value=""C:\Programme\pthreads\pthreads.2""/> +<listOptionValue builtIn="false" value=""C:\MinGW\include""/> +<listOptionValue builtIn="false" value=""C:\pthreads\pthreads.2""/> </option> <option id="gnu.c.compiler.option.preprocessor.def.symbols.1823712582" superClass="gnu.c.compiler.option.preprocessor.def.symbols" valueType="definedSymbols"> <listOptionValue builtIn="false" value="PC"/> @@ -27,8 +27,8 @@ <listOptionValue builtIn="false" value="pthreadGC2"/> </option> <option id="gnu.c.link.option.paths.142594843" superClass="gnu.c.link.option.paths" valueType="stringList"> -<listOptionValue builtIn="false" value=""C:\Programme\pthreads\Pre-built.2\lib""/> -<listOptionValue builtIn="false" value=""C:\Programme\MinGW\lib""/> +<listOptionValue builtIn="false" value=""C:\pthreads\Pre-built.2\lib""/> +<listOptionValue builtIn="false" value=""C:\MinGW\lib""/> </option> <option id="gnu.c.link.option.noshared.1812776571" superClass="gnu.c.link.option.noshared" value="true" valueType="boolean"/> </tool> Index: C:/eclipse/workspace/ct-Bot/.settings/org.eclipse.cdt.core.prefs =================================================================== --- C:/eclipse/workspace/ct-Bot/.settings/org.eclipse.cdt.core.prefs (revision 0) +++ C:/eclipse/workspace/ct-Bot/.settings/org.eclipse.cdt.core.prefs (revision 0) @@ -0,0 +1,3 @@ +#Tue Mar 18 19:50:26 CET 2008 +eclipse.preferences.version=1 +indexerId=org.eclipse.cdt.core.fastIndexer Index: C:/eclipse/workspace/ct-Bot/.settings/org.eclipse.cdt.managedbuilder.core.prefs =================================================================== --- C:/eclipse/workspace/ct-Bot/.settings/org.eclipse.cdt.managedbuilder.core.prefs (revision 1395) +++ C:/eclipse/workspace/ct-Bot/.settings/org.eclipse.cdt.managedbuilder.core.prefs (working copy) @@ -1,13 +1,15 @@ -#Thu Mar 29 15:23:55 CEST 2007 -cdt.managedbuild.config.gnu.exe.debug.1197043799/internalBuilder/enabled=false -cdt.managedbuild.config.gnu.exe.debug.1197043799/internalBuilder/ignoreErr=true -eclipse.preferences.version=1 -environment/buildEnvironmentInclude/cdt.managedbuild.config.gnu.exe.debug.1077176217=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment>\n<variable name\="CPATH" operation\="remove"/>\n<variable name\="C_INCLUDE_PATH" operation\="remove"/>\n</environment>\n -environment/buildEnvironmentInclude/cdt.managedbuild.config.gnu.exe.debug.1197043799=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment>\n<variable name\="CPATH" operation\="remove"/>\n<variable name\="C_INCLUDE_PATH" operation\="remove"/>\n</environment>\n -environment/buildEnvironmentInclude/cdt.managedbuild.config.gnu.exe.debug.1197043799.2016949464=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment>\n<variable name\="CPATH" operation\="remove"/>\n<variable name\="C_INCLUDE_PATH" operation\="remove"/>\n</environment>\n -environment/buildEnvironmentLibrary/cdt.managedbuild.config.gnu.exe.debug.1077176217=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment>\n<variable name\="LIBRARY_PATH" operation\="remove"/>\n</environment>\n -environment/buildEnvironmentLibrary/cdt.managedbuild.config.gnu.exe.debug.1197043799=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment>\n<variable name\="LIBRARY_PATH" operation\="remove"/>\n</environment>\n -environment/buildEnvironmentLibrary/cdt.managedbuild.config.gnu.exe.debug.1197043799.2016949464=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment>\n<variable name\="LIBRARY_PATH" operation\="remove"/>\n</environment>\n -environment/project=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment/>\n -environment/project/cdt.managedbuild.config.gnu.exe.debug.1077176217=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment/>\n -environment/project/cdt.managedbuild.config.gnu.exe.debug.1197043799=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment>\n<variable delimiter\="\:" name\="PATH" operation\="append" value\="/usr/local/avr/bin"/>\n</environment>\n +#Tue Mar 18 19:51:16 CET 2008 +cdt.managedbuild.config.gnu.exe.debug.1077176217/internalBuilder/enabled=false +cdt.managedbuild.config.gnu.exe.debug.1077176217/internalBuilder/ignoreErr=true +cdt.managedbuild.config.gnu.exe.debug.1197043799/internalBuilder/enabled=false +cdt.managedbuild.config.gnu.exe.debug.1197043799/internalBuilder/ignoreErr=true +eclipse.preferences.version=1 +environment/buildEnvironmentInclude/cdt.managedbuild.config.gnu.exe.debug.1077176217=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment>\n<variable name\="CPATH" operation\="remove"/>\n<variable name\="C_INCLUDE_PATH" operation\="remove"/>\n</environment>\n +environment/buildEnvironmentInclude/cdt.managedbuild.config.gnu.exe.debug.1197043799=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment>\n<variable name\="CPATH" operation\="remove"/>\n<variable name\="C_INCLUDE_PATH" operation\="remove"/>\n</environment>\n +environment/buildEnvironmentInclude/cdt.managedbuild.config.gnu.exe.debug.1197043799.2016949464=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment>\n<variable name\="CPATH" operation\="remove"/>\n<variable name\="C_INCLUDE_PATH" operation\="remove"/>\n</environment>\n +environment/buildEnvironmentLibrary/cdt.managedbuild.config.gnu.exe.debug.1077176217=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment>\n<variable name\="LIBRARY_PATH" operation\="remove"/>\n</environment>\n +environment/buildEnvironmentLibrary/cdt.managedbuild.config.gnu.exe.debug.1197043799=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment>\n<variable name\="LIBRARY_PATH" operation\="remove"/>\n</environment>\n +environment/buildEnvironmentLibrary/cdt.managedbuild.config.gnu.exe.debug.1197043799.2016949464=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment>\n<variable name\="LIBRARY_PATH" operation\="remove"/>\n</environment>\n +environment/project=<?xml version\="1.0" encoding\="UTF-8"?>\r\n<environment/>\r\n +environment/project/cdt.managedbuild.config.gnu.exe.debug.1077176217=<?xml version\="1.0" encoding\="UTF-8"?>\r\n<environment/>\r\n +environment/project/cdt.managedbuild.config.gnu.exe.debug.1197043799=<?xml version\="1.0" encoding\="UTF-8"?>\n<environment>\n<variable delimiter\="\:" name\="PATH" operation\="append" value\="/usr/local/avr/bin"/>\n</environment>\n Index: C:/eclipse/workspace/ct-Bot/ct-Bot.h =================================================================== --- C:/eclipse/workspace/ct-Bot/ct-Bot.h (revision 1395) +++ C:/eclipse/workspace/ct-Bot/ct-Bot.h (working copy) @@ -51,7 +51,7 @@ #define MEASURE_MOUSE_AVAILABLE /*!< Geschwindigkeiten werden aus den Maussensordaten berechnet */ //#define MEASURE_COUPLED_AVAILABLE /*!< Geschwindigkeiten werden aus Maus- und Encoderwerten ermittelt und gekoppelt */ -//#define POS_STACK_AVAILABLE /*!< Positionsstack vorhanden */ +#define POS_STACK_AVAILABLE /*!< Positionsstack vorhanden */ //#define WELCOME_AVAILABLE /*!< kleiner Willkommensgruss */