c't

c't-Projekte - Mailinglisten


[Voriger (Datum)] [Nächster (Datum)] [Voriger (Thread)] [Nächster (Thread)]
[Nach Datum][Nach Thread]

[ct-bot] neues Stackverhalten zum Merken relevanter Wegepositionen

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="&quot;${ProjDirPath}&quot;"/>
 <listOptionValue builtIn="false" value="&quot;${ProjDirPath}/include&quot;"/>
-<listOptionValue builtIn="false" value="&quot;C:\Programme\MinGW\include&quot;"/>
-<listOptionValue builtIn="false" value="&quot;C:\Programme\pthreads\pthreads.2&quot;"/>
+<listOptionValue builtIn="false" value="&quot;C:\MinGW\include&quot;"/>
+<listOptionValue builtIn="false" value="&quot;C:\pthreads\pthreads.2&quot;"/>
 </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="&quot;C:\Programme\pthreads\Pre-built.2\lib&quot;"/>
-<listOptionValue builtIn="false" value="&quot;C:\Programme\MinGW\lib&quot;"/>
+<listOptionValue builtIn="false" value="&quot;C:\pthreads\Pre-built.2\lib&quot;"/>
+<listOptionValue builtIn="false" value="&quot;C:\MinGW\lib&quot;"/>
 </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 */