From 56d9bdd39ed36c36e9a61411b86c76d5228b2133 Mon Sep 17 00:00:00 2001
From: sicarius <devnull@localhost>
Date: Sun, 11 Feb 2007 18:32:03 +0000
Subject: Added lot's of code-files used during work

---
 source/ct-Bot/bot-logic/behaviour_goto.c | 141 +++++++++++++++++++++++++++++++
 1 file changed, 141 insertions(+)
 create mode 100644 source/ct-Bot/bot-logic/behaviour_goto.c

(limited to 'source/ct-Bot/bot-logic/behaviour_goto.c')

diff --git a/source/ct-Bot/bot-logic/behaviour_goto.c b/source/ct-Bot/bot-logic/behaviour_goto.c
new file mode 100644
index 0000000..9b655a4
--- /dev/null
+++ b/source/ct-Bot/bot-logic/behaviour_goto.c
@@ -0,0 +1,141 @@
+/*
+ * 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_goto.c
+ * @brief 	Bot faehrt einen punkt an
+ * 
+ * @author 	Benjamin Benz (bbe@heise.de)
+ * @date 	03.11.06
+*/
+
+
+
+#include "bot-logic/bot-logik.h"
+
+#ifdef BEHAVIOUR_GOTO_AVAILABLE
+#include <stdlib.h>
+
+
+#define MOT_GOTO_MAX  	 4 			/*!< Richtungsaenderungen, bis goto erreicht sein muss */
+#define GOTO_REACHED	 2			/*!< Wenn Encoder-Distanz <= GOTO_REACHED dann stop */
+#define GOTO_SLOW		 4			/*!< Wenn Encoder-Distanz < GOTO_SLOW dann langsame Fahrt */
+#define GOTO_NORMAL	10			/*!< Wenn Encoder-Distanz < GOTO_NORMAL dann normale Fahrt */
+#define GOTO_FAST		40			/*!< Wenn Encoder-Distanz < GOTO_FAST dann schnelle Fahrt, sonst maximale Fahrt */
+
+
+int16 mot_l_goto=0;		/*!< Speichert wie weit der linke Motor drehen soll */
+int16 mot_r_goto=0;		/*!< Speichert wie weit der rechte Motor drehen soll */
+
+
+/*!
+ * Kuemmert sich intern um die Ausfuehrung der goto-Kommandos,
+ * @param *data der Verhaltensdatensatz
+ * @see bot_goto()
+ */
+void bot_goto_behaviour(Behaviour_t *data){
+	static int16 mot_goto_l=0;	/*!< Muss der linke Motor noch drehen?  */
+	static int16 mot_goto_r=0;	/*!< Muss der rechte Motor noch drehen?  */
+
+  	int16 diff_l;	/* Restdistanz links */
+	int16 diff_r; /* Restdistanz rechts */
+
+	/* Sind beide Zaehler Null und ist die Funktion active 
+	 * -- sonst waeren wir nicht hier -- 
+	 * so ist dies der erste Aufruf ==> initialisieren */	
+	if (( mot_goto_l ==0) && ( mot_goto_r ==0)){
+		/* Zaehler einstellen */
+		if (mot_l_goto !=0) 
+			mot_goto_l= MOT_GOTO_MAX; 
+		if (mot_r_goto !=0) 
+			mot_goto_r=MOT_GOTO_MAX;
+
+		/* Encoder zuruecksetzen */
+		sensEncL=0;
+		sensEncR=0;			
+	}		
+	
+	/* Motor L hat noch keine MOT_GOTO_MAX Nulldurchgaenge gehabt */
+	if (mot_goto_l >0){
+		diff_l = sensEncL - mot_l_goto;		/* Restdistanz links */
+
+		if (abs(diff_l) <= GOTO_REACHED){				/* 2 Encoderstaende Genauigkeit reicht */
+			speedWishLeft = BOT_SPEED_STOP;	/* Stop */
+			mot_goto_l--;					/* wie Nulldurchgang behandeln */
+		}else if (abs(diff_l) < GOTO_SLOW)
+			speedWishLeft= BOT_SPEED_SLOW;
+		else if (abs(diff_l) < GOTO_NORMAL)
+			speedWishLeft= BOT_SPEED_NORMAL;
+		else if (abs(diff_l) < GOTO_FAST)
+			speedWishLeft= BOT_SPEED_FAST;
+		else speedWishLeft= BOT_SPEED_MAX;
+
+		// Richtung	
+		if (diff_l>0) {		// Wenn uebersteuert,
+			speedWishLeft= -speedWishLeft;	//Richtung umkehren
+		}
+		
+		// Wenn neue Richtung ungleich alter Richtung
+		if (((speedWishLeft<0)&& (direction.left == DIRECTION_FORWARD))|| ( (speedWishLeft>0) && (direction.left == DIRECTION_BACKWARD) ) ) 
+			mot_goto_l--;		// Nulldurchgang merken
+	} 
+	
+	// Motor R hat noch keine MOT_GOTO_MAX Nulldurchgaenge gehabt
+	if (mot_goto_r >0){
+		diff_r = sensEncR - mot_r_goto;	/* Restdistanz rechts */
+
+		if (abs(diff_r) <= GOTO_REACHED){			// 2 Encoderstaende Genauigkeit reicht
+			speedWishRight = BOT_SPEED_STOP;	//Stop
+			mot_goto_r--;			// wie Nulldurchgang behandeln
+		}else if (abs(diff_r) < GOTO_SLOW)
+			speedWishRight= BOT_SPEED_SLOW;
+		else if (abs(diff_r) < GOTO_NORMAL)
+			speedWishRight= BOT_SPEED_NORMAL;
+		else if (abs(diff_r) < GOTO_FAST)
+			speedWishRight= BOT_SPEED_FAST;
+		else speedWishRight= BOT_SPEED_MAX;
+
+		// Richtung	
+		if (diff_r>0) {		// Wenn uebersteurt,
+			speedWishRight= -speedWishRight;	//Richtung umkehren
+		}
+
+		// Wenn neue Richtung ungleich alter Richtung
+		if (((speedWishRight<0)&& (direction.right == DIRECTION_FORWARD))|| ( (speedWishRight>0) && (direction.right == DIRECTION_BACKWARD) ) ) 
+			mot_goto_r--;		// Nulldurchgang merken
+	} 
+	
+	/* Sind wir fertig, dann Regel deaktivieren */
+	if ((mot_goto_l == 0) && (mot_goto_r == 0)){
+		return_from_behaviour(data);
+	}
+}
+
+/*!
+ * Drehe die Raeder um die gegebene Zahl an Encoder-Schritten weiter
+ * @param left Schritte links
+ * @param right Schritte rechts
+ */
+void bot_goto(Behaviour_t * caller, int16 left, int16 right){
+	// Zielwerte speichern
+	mot_l_goto=left; 
+	mot_r_goto=right;
+
+	switch_to_behaviour(caller,bot_goto_behaviour,OVERRIDE);	
+}
+#endif
-- 
cgit v1.2.3