c't Projekte - c't-Bot und c't-Sim -
Mailinglisten
[Voriger (Datum)]
[Nächster (Datum)]
[Voriger (Thread)]
[Nächster (Thread)]
[Nach Datum][Nach Thread]
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 */