Alte Organisationsdaten gel?scht

SoccerTeam, Stuff from Magdeburg
2007-02-28 21:26:05 +00:00 · 2007-02-28 21:26:05 +00:00 · 2007-02-26 21:25:01 +00:00 · 2007-02-22 23:49:04 +00:00 · 2007-02-22 23:37:00 +00:00 · 2007-02-22 23:04:02 +00:00
37 changed files with 1697 additions and 516 deletions
--- a/Organisation/Gebühren.txt
+++ b/Organisation/Gebühren.txt
@ -1,63 +0,0 @@
-:::Teilnahmegebühren, Qualifikationstunier Magdeburg:::
-	Teilnehmerzahl:		12 Personen
-				7 davon im Rescue-Team,
-				5 davon im Soccer-Team
-
-	Teilnahmegebühr pro Person:		10€
-	Teilnahmegebühren für Rescue-Team:	70€
-	Teilnahmegebühren für Soccer-Team:	50€
-	Teilnahmegebühren Gesamt:		120€
-
-	Gebühren werden überwiesen an:
-		Empfänger: Manuela Kanneberg
-		Stadtsparkasse Magdeburg, Blz 8105 3272
-		Kontonummer 4000021422
-
-	Vollständig überweisen bis spätestens 7. Februar 2007.
-	Überweisung wurde getätigt von: Jakob Runge
-	Geld wird eingesammelt von: Jakob Runge
-
-
-
-:::Jugendherberge Magdeburg:::
-	Teilnehmerzahl:		10 Personen
-										(Jan Baumann und Roman Bierbach fahren nicht mit)
-				5 davon im Rescue-Team,
-				5 davon im Soccer-Team
-
-	Preis pro Person und Nacht:		19€
-	Übernachtungen:				2 Stück
-	Preis für Rescue-Team: 		190€
-	Preis für Soccer-Team:		190€
-	Preis, Gesamt: 						380€
-
-	Übernachtung erfolgt in Mehrbettzimmer, Frühstück inclusive
-
-	Geld wird eingesammelt und überwiesen von Jakob Runge
-
-
-
-:::Bahnfahrt-Hin:::
-	Teilnemherzahl:		10 Personen
-				5 davon im Rescue-Team
-				5 davon im Soccer-Team
-
-		Fahrtgebühren pro Person:  11,45€
-		Fahrtgebühren, Rescue-Team:57,25€
-		Fahrtgebühren, Soccer-Team:57,25€
-		Fahrtgebühren, Gesamt:		114,50€
-
-	Geld wurde ausgelegt von Marian Meyer
-	Geld wird eingesammelt von Jakob Runge, und weitergegeben an Marian Meyer
-
-
-
-:::Bahnfahrt, Rück:::
-	Teilnehmerzahl:		10 Personen
-				5 davon im Rescue-Team
-				5 davon im Soccer-Team
-
-	Fahrgebühren pro Person:	6,60€
-	Fahrtgebühren, Rescue-Team:33,00€
-	Fahrtgebühren, Soccer-Team:33,00€
-	Fahrtgebühren, Gesamt:		66,00€
--- a/Organisation/Kosten.ods
+++ b/Organisation/Kosten.ods
--- a/Organisation/Kosten.pdf
+++ b/Organisation/Kosten.pdf
--- a/Organisation/Magdeburg.txt
+++ b/Organisation/Magdeburg.txt
@ -1,65 +0,0 @@
-Schule:
-	Lauenburgische Gelehrtenschule Ratzeburg
-	23909 Ratzeburg
-	Schleswig-Holstein
-
-Chef(Anmeldung):
-	Jakob Runge
-	04544/1824
-
-Teams:
-	CompAG- Rescue
-	RoboRescue secondary
-
-	7 Mitglieder:
-		Annika Dzakowski
-		15 Jahre
-		Weiblich
-
-		Bengt Jaeschke
-		14 Jahre
-		Männlich
-
-		Jan Baumann
-		15 Jahre
-		Männlich
-
-		Lena Schiffer
-		14 Jahre
-		Weiblich
-
-		Matthias Schiffer
-		17 Jahre
-		Männlich
-
-		Roman Bierbach
-		15 Jahre
-		Männlich
-
-		Tobias Runge
-		14 Jahre
-		Männlich
-
-	CompAG - Soccer
-	Soccer 2vs2 secondary
-
-	5 Mitglieder:
-		Jakob Runge
-		18 Jahre
-		Männlich
-
-		Marian Meyer
-		17 Jahre
-		Männlich
-
-		Marius Jahrens
-		18 Jahre
-		Männlich
-
-		Phillipp Schütz
-		14 Jahre
-		Männlich
-
-		Steffanie Walter
-		15 Jahre
-		Weiblich
--- a/Organisation/Packliste.txt
+++ b/Organisation/Packliste.txt
@ -1,24 +0,0 @@
-Unbedingt mitnehmen:
-
-Zahnbürste
-Zahncreme
-Handtücher
-Waschlappen
-Duschzeug
-
-Kleidung für 3 Tage
-(Unterwäche, T-shirts, Hose, usw.)
-(Regen mit einplanen)
-
-Bettbezug
-Bettlaken
-
-Geld (min.50 Euro)
-
-Für weibliche Teilnehmer die Erlaubnis 
-das sie mit im Jungszimmer schlafen dürfen!!! (falls gebraucht)
-
-Techniksachen: Laptop, Handy, Camera, MP3 Player 
-
-"Freizeitbeschäftigungen" falls es Wartezeiten gibt
-
--- a/Organisation/TimeTable.txt
+++ b/Organisation/TimeTable.txt
@ -1,29 +0,0 @@
-:::Zeitplan des Soccerteams:::
-
-Teaminterne Ereignisse:
-
-
-Offizielle Ereignisse:
-
-	Anmeldeschluss, Robocup Junior:
-		31.01.07
-
-	Überweisungsfrist für Magdeburg
-		07.02.07
-
-	Qualifikationstunier in Magdeburg (Messegelände)
-		23.02.07 - 25.02.07
-		->Offizielles Qualifikationstunier für deutsche Meisterschaft
-			->Abfahrt RZ->Magdeburg:
-				Bahnhof RZ, spätestens um 10:15 Uhr anwesend sein !
-
-	Qualifikationstunier in Vöringen
-		02.03.07 - 04.03.07
-		->Offizielles Qualifikationstunier für deutsche Meisterschaft
-
-	Deutsche Meisterschaft in Hannover
-		April
-		->Offizielles Qualifikationstunier für WM
-
-	WM in Atlanta
-		1.07.07 - 10.07.07
--- a/Organisation/Todo.txt
+++ b/Organisation/Todo.txt
@ -1,57 +0,0 @@
-Hardware:
-	Getan:
-
-	Grundgerüst fertiggestellt
-	Hauptplatine ist montiert
-	Motorsteuerungen sind montiert
-	KeyLcd kist montiert
-	Kicker können montiert werden
-	Achsen-hüllsen sind gebohrt
-	Motoren können montiert werden
-	Grundstücke für Befestigung von Tastsensoren(Maschendraht)
-	Maussensoren eingebaut
-	Ultraschallsensoren befestigt
-	Motoren fertiggestellt
-	Ballsensoren fertiggestellt
-	Eigene Platinen fertiggestellt
-	Ballsensoren angeschlossen
-	Aussegungen für Dribbler
-
-	Todo:
-
-	Dribbler optimieren
-	Sensorhardware anschließen und testen
-	Verkabelung
-
-Software:
-	Getan:
-
-	Disskusion um Grundkonzept
-	weitere Ausarbeitungen am Design
-	Programmierumgebung eingerichtet
-	Platinen ausreichend konfigueriert(fuses gesetzt)
-	Code ins Framework integriert
-
-	Todo:
-
-	Navigation abschließen
-	TESTEN !
-	Sensorik fertigstellen
-	TESTEN TESTEN TESTEN
-	Logik !!!
-	
-
-Allgemein:
-	Getan:
-
-	Teilnahmegebühren(120€) überwiesen
-
-	Todo:
-		Anmeldung abschließen
-			->10€ Kosten von Teilnehmern an Jakob
-		Magdeburg organisieren
-			->Hin-/Rückfahrt
-			->Unterkunft
-			->Verpflegung
-			->Kosten
-			->Verkehrswege/Lage
--- a/Organisation/contacts.txt
+++ b/Organisation/contacts.txt
@ -1,27 +0,0 @@
-Members:
-
-	Marius Jahrens:
-		Telefon:	04541/802724
-		Handy:		0176/28835751
-		Email:		the-matrixman@web.de
-
-	Marian Meyer:
-		Telefon:	04541/883188
-		Handy:		0157/72542713
-		Email:		meyma@meymaris.de
-
-	Jakob Runge:
-		Telefon:	04544/1824 oder 04544/344
-		Handy:		Nicht vorhanden
-		Email:		sicarius@helix360.de
-
-	Phillip Schütz:
-		Telefon:	Nicht angeschlossen
-		Handy:		0174 955 4846
-		Email:		paladin_mof@hotmail.com
-		Geburtstag:	21.4.1992
-
-	Steffi Walter:
-		Telefon:	04541/859230
-		Handy:		01734681528
-		Email:		???
--- a/Sponsorenschreiben.doc
+++ b/Sponsorenschreiben.doc
--- a/Sponsorenschreiben.pdf
+++ b/Sponsorenschreiben.pdf
--- a/source/Concept/Framework/Concept.vcproj
+++ b/source/Concept/Framework/Concept.vcproj
@ -399,6 +399,78 @@
 					</File>
 				</Filter>
 			</Filter>
+			<Filter
+				Name="Logic"
+				Filter="">
+				<Filter
+					Name="Source Files"
+					Filter="">
+					<File
+						RelativePath=".\modules\logic\logic.c">
+					</File>
+				</Filter>
+				<Filter
+					Name="Header Files"
+					Filter="">
+					<File
+						RelativePath=".\modules\logic\logic.h">
+					</File>
+				</Filter>
+			</Filter>
+			<Filter
+				Name="Wireless"
+				Filter="">
+				<Filter
+					Name="Source Files"
+					Filter="">
+					<File
+						RelativePath=".\modules\wireless.c">
+					</File>
+				</Filter>
+				<Filter
+					Name="Header Files"
+					Filter="">
+					<File
+						RelativePath=".\modules\wireless.h">
+					</File>
+				</Filter>
+			</Filter>
+			<Filter
+				Name="Aktuator"
+				Filter="">
+				<Filter
+					Name="Source Files"
+					Filter="">
+					<File
+						RelativePath=".\modules\executor\aktuator.c">
+					</File>
+				</Filter>
+				<Filter
+					Name="Header Files"
+					Filter="">
+					<File
+						RelativePath=".\modules\executor\aktuator.h">
+					</File>
+				</Filter>
+			</Filter>
+			<Filter
+				Name="Command_Handler"
+				Filter="">
+				<Filter
+					Name="Source Files"
+					Filter="">
+					<File
+						RelativePath=".\modules\interpreter\command_handler.c">
+					</File>
+				</Filter>
+				<Filter
+					Name="Header Files"
+					Filter="">
+					<File
+						RelativePath=".\modules\interpreter\command_handler.h">
+					</File>
+				</Filter>
+			</Filter>
 		</Filter>
 		<Filter
 			Name="Hardware Interface"
--- a/source/Concept/Framework/RoboCode.aps
+++ b/source/Concept/Framework/RoboCode.aps
--- a/source/Concept/Framework/atmega128io.c
+++ b/source/Concept/Framework/atmega128io.c
@ -581,5 +581,173 @@ void uart1_puts_p(const char *progmem_s )

 }/* uart1_puts_p */

+/////////////////////////////////////////////////////////////////////
+/////////////////////////////////////////////////////////////////////
+/////////////////////////////////////////////////////////////////////
+/////////////////////////////////////////////////////////////////////
+// TWI-Driver
+
+
+/*!
+ * TWI Bus initialsieren
+ * @return Resultat der Aktion
+ */
+
+int8 Init_TWI(void){
+	TWAR = OWN_ADR;							/*!< Eigenen Slave Adresse setzen */
+	TWBR = 64;                    			/*!< Setze Baudrate auf 100 KHz  */
+											/*!< 16 MHz xtal */
+	TWCR = (1<<TWEN);						/*!< TWI-Interface einschalten */
+
+    return 1;
+}    
+
+/*!
+ * TWI Buss schliesen
+ * @return Resultat der Aktion
+ */
+ 
+int8 Close_TWI(void){
+	TWCR = (0<<TWEN);						/*!< TWI-Interface ausschalten */
+
+    return 0;
+}    
+
+/*!
+ * Warte auf TWI interrupt
+ */
+ 
+void Wait_TWI_int(void){
+	while (!(TWCR & (1<<TWINT)))
+	    ; 
+}    
+
+/*!
+ * Sende Start Sequence
+ * @return Resultat der Aktion
+ */
+
+uint8 Send_start(void){
+	TWCR = ((1<<TWINT)+(1<<TWSTA)+(1<<TWEN)); 		/*!< Sende START */
+	
+	Wait_TWI_int();									/*!< Warte auf TWI interrupt */
+
+    if((TWSR != START)&&(TWSR != REP_START))		/*!< Ist der Status ein Anderer als Start (0x08) oder wiederholter Start (0x10) */
+		return TWSR;								/*!< -> error  und Rueckgabe TWSR. */
+	return SUCCESS;									/*!< wenn OK Rueckgabe SUCCESS */
+}							
+
+/*!
+ * Sende Stop Sequence
+ */
+ 
+void Send_stop(void){
+	TWCR = ((1<<TWEN)+(1<<TWINT)+(1<<TWSTO));
+}        
+    
+/*!
+ * Hier wird der eigentliche TWI-Treiber angesprochen
+ * @param *data_pack Container mit den Daten fuer den Treiber
+ * @return Resultat der Aktion
+ */
+uint8 Send_to_TWI(tx_type *data_pack){
+	uint8 state,i,j;
+
+	state = SUCCESS;
+	
+	for(i=0;(data_pack[i].slave_adr != OWN_ADR)&&(state == SUCCESS);i++)	{
+		state = Send_start();
+		if (state == SUCCESS)				
+			state = Send_adr(data_pack[i].slave_adr);
+		
+		/*!
+		 * Abhaengig von W/R senden oder empfangen
+		 */
+		if(!(data_pack[i].slave_adr & R)) {
+			if (state == SUCCESS){
+				/*!
+				 * Wenn W bis alle Daten gesendet sind
+				 */
+				for(j=0;((j<data_pack[i].size)&&(state == SUCCESS));j++)
+					state = Send_byte(data_pack[i].data_ptr[j]);
+			}
+		}	
+		else{
+			if (state == MRX_ADR_NACK)	{
+				state = Send_start();
+			}
+			
+			if (state == SUCCESS){
+				/*!
+				 * Wenn R bis alle Daten empfangen sind
+				 */
+				for(j=0;((j<data_pack[i].size)&&(state == SUCCESS));j++){
+					/*!
+					 * Wenn wir keine Daten mehr erwarten NACK senden
+					 */
+					if(j == data_pack[i].size-1)
+						state = Get_byte(data_pack[i].data_ptr++,0);
+					else
+						state = Get_byte(data_pack[i].data_ptr++,1);
+				}
+			}
+		}					
+	  	Send_stop();
+	}
+ 	Close_TWI();
+  	  
+	return state;
+}
+
+/*!
+ * Sende ein Byte
+ * @param data das zu uebertragende Byte
+ */
+ 
+uint8 Send_byte(uint8 data){
+	Wait_TWI_int();
+	TWDR = data;
+ 	TWCR = ((1<<TWINT)+(1<<TWEN));
+	Wait_TWI_int();
+	if(TWSR != MTX_DATA_ACK)
+		return TWSR;																
+	return SUCCESS;
+}	
+
+/*!
+ * Sende Slave Adresse
+ * @param adr die gewuenschte Adresse
+ * @return Resultat der Aktion
+ */
+ 
+uint8 Send_adr(uint8 adr){
+	Wait_TWI_int();
+	TWDR = adr;
+	TWCR = ((1<<TWINT)+(1<<TWEN));
+	Wait_TWI_int();
+	if((TWSR != MTX_ADR_ACK)&&(TWSR != MRX_ADR_ACK))
+		return TWSR;
+	return SUCCESS;
+}	
+
+/*!
+ * Empfange ein Byte
+ * @param *rx_ptr Container fuer die Daten
+ * @param last_byte Flag ob noch Daten erwartet werden
+ * @return Resultat der Aktion
+ */
+ 
+uint8 Get_byte(uint8 *rx_ptr,uint8 last_byte){
+	Wait_TWI_int();
+	if(last_byte)
+		TWCR = ((1<<TWINT)+(1<<TWEA)+(1<<TWEN));
+	else
+		TWCR = ((1<<TWINT)+(1<<TWEN)); 			
+	Wait_TWI_int();
+	*rx_ptr = TWDR;
+ 	if(((TWSR == MRX_DATA_NACK)&&(last_byte == 0))||(TWSR == MRX_DATA_ACK))
+		return SUCCESS;	  
+	return TWSR;
+}

 #endif
--- a/source/Concept/Framework/atmega128io.h
+++ b/source/Concept/Framework/atmega128io.h
@ -6,6 +6,8 @@
 #include <avr/io.h>
 #include <avr/interrupt.h>

+#include "defines.h"
+
 /************************************************************************
 Title:    Interrupt UART library with receive/transmit circular buffers
 Author:   Peter Fleury <pfleury@gmx.ch>   http://jump.to/fleury
@ -180,4 +182,97 @@ extern void uart1_puts_p(const char *s );

 /**@}*/

+/////////////////////////////////////////////////////
+/////////////////////////////////////////////////////
+// TWI-Driver
+
+	/*!
+		Struktur Definition 
+		tx_type ist eine Datenstruktur um den TWI Treiber anzusprechen 
+		und behinhaltet folgende Informationen:
+		Slave Adresse + Datenrichtung
+		Anzahl der zu uebertragendenden Bytes (Senden oder Empfangen)
+		Pointer auf den Sende- oder Empfangspuffer
+	*/
+	typedef struct
+	{
+		uint8 slave_adr;				/*!< Slave Adresse and W/R byte */
+		uint8 size;						/*!< Anzahl der Bytes, die gesendet oder empfagen werden sollen */
+		uint8 *data_ptr;				/*!< Pointer zum Sende und Empfangs Puffer */
+	}tx_type;	
+	
+/*!
+ * Hier wird der eigentliche TWI-Treiber angesprochen
+ * @param *data_pack Container mit den Daten fuer den Treiber
+ * @return Resultat der Aktion
+ */
+ 	extern uint8 Send_to_TWI(tx_type *data_pack);	
+/*!
+ * Sende ein Byte
+ * @param data das zu uebertragende Byte
+ */
+	extern uint8 Send_byte(uint8 data);
+/*!
+ * Empfange ein Byte
+ * @param *rx_ptr Container f<EFBFBD>r die Daten
+ * @param last_byte Flag ob noch Daten erwartet werden
+ * @return Resultat der Aktion
+ */
+	extern uint8 Get_byte(uint8 *rx_ptr, uint8 last_byte);
+/*!
+ * Sende Start Sequence
+ * @return Resultat der Aktion
+ */
+	extern uint8 Send_start(void);
+/*!
+ * Sende Slave Adresse
+ * @param adr die gewuenschte Adresse
+ * @return Resultat der Aktion
+ */
+	extern uint8 Send_adr(uint8 adr);
+/*!
+ * Sende Stop Sequence
+ */
+	extern void Send_stop(void);
+/*!
+ * Warte auf TWI interrupt
+ */
+	extern void Wait_TWI_int(void);
+
+/*!
+ * TWI Bus initialsieren
+ * @return Resultat der Aktion
+ */
+	extern int8 Init_TWI(void);
+/*!
+ * TWI Bus schliesen
+ * @return Resultat der Aktion
+ */
+	extern int8 Close_TWI(void);
+	
+	#define W 				0			/*!< Daten Transfer Richtung Schreiben */
+	#define R 				1			/*!< Daten Transfer Richtung Lesen */
+	#define OWN_ADR 		60  		/*!< Die eigene Slave Adresse */
+	#define SUCCESS 		0xFF		/*!< Status Code alles OK */
+	
+	/*!
+	TWI Stautus Register Definitionen
+	*/
+
+	/*!< Genereller Master Statuscode */
+	#define START		0x08					/*!< START wurde uebertragen */	
+	#define	REP_START	0x10					/*!< Wiederholter START wurde uebertragen */
+	/*!< Master Sender Statuscode	 */										
+	#define	MTX_ADR_ACK		0x18				/*!< SLA+W wurde uebertragen und ACK empfangen */
+	#define	MTX_ADR_NACK	0x20				/*!< SLA+W wurde uebertragen und NACK empfangen */
+	#define	MTX_DATA_ACK	0x28				/*!< Datenbyte wurde uebertragen und ACK empfangen */			
+	#define	MTX_DATA_NACK	0x30				/*!< Datenbyte wurde uebertragen und NACK empfangen */		
+	#define	MTX_ARB_LOST	0x38				/*!< Schlichtung verloren in SLA+W oder Datenbytes */
+	/*!< Master Empfaenger Statuscode  */
+	#define	MRX_ARB_LOST	0x38				/*!< Schlichtung verloren in SLA+R oder NACK bit */
+	#define	MRX_ADR_ACK		0x40				/*!< SLA+R wurde uebertragen und ACK empfangen */
+	#define	MRX_ADR_NACK	0x48				/*!< SLA+R wurde uebertragen und NACK empfangen */	
+	#define	MRX_DATA_ACK	0x50				/*!< Datenbyte wurde empfangen und ACK gesendet */
+	#define	MRX_DATA_NACK	0x58				/*!< Datenbyte wurde empfangen und NACK gesendet */
+
 #endif
--- a/source/Concept/Framework/default/Makefile
+++ b/source/Concept/Framework/default/Makefile
@ -34,13 +34,13 @@ HEX_EEPROM_FLAGS += --change-section-lma .eeprom=0


 ## Include Directories
-INCLUDES = -I"Y:\Concept\Framework\modules" -I"Y:\Concept\Framework\modules\executor" -I"Y:\Concept\Framework\modules\input" -I"Y:\Concept\Framework\modules\interpreter" -I"Y:\Concept\Framework\modules\logic" -I"Y:\Concept\Framework\modules\output" 
+INCLUDES = -I"Z:\rc2007-soccer\source\Concept\Framework\modules" -I"Z:\rc2007-soccer\source\Concept\Framework\modules\executor" -I"Z:\rc2007-soccer\source\Concept\Framework\modules\input" -I"Z:\rc2007-soccer\source\Concept\Framework\modules\interpreter" -I"Z:\rc2007-soccer\source\Concept\Framework\modules\logic" -I"Z:\rc2007-soccer\source\Concept\Framework\modules\output" 

 ## Libraries
 LIBS = -lm 

 ## Objects that must be built in order to link
-OBJECTS = io_module.o atmega128io.o main.o robot.o tools.o distance_sensor.o ir_sensor.o keyboard.o mouse_sensor.o sensor.o position_tracker.o display.o dribbler.o engine.o kicker.o led.o ball_tracker.o navigator.o aktuator.o wireless.o logic.o 
+OBJECTS = io_module.o atmega128io.o main.o robot.o tools.o distance_sensor.o ir_sensor.o keyboard.o mouse_sensor.o sensor.o position_tracker.o display.o dribbler.o engine.o kicker.o led.o ball_tracker.o navigator.o aktuator.o wireless.o logic.o command_handler.o obstacle_tracker.o 

 ## Objects explicitly added by the user
 LINKONLYOBJECTS = 
@ -112,6 +112,12 @@ wireless.o: ../modules/wireless.c
 logic.o: ../modules/logic/logic.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<

+command_handler.o: ../modules/interpreter/command_handler.c
+	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
+
+obstacle_tracker.o: ../modules/interpreter/obstacle_tracker.c
+	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
+
 ##Link
 $(TARGET): $(OBJECTS)
 	 $(CC) $(LDFLAGS) $(OBJECTS) $(LINKONLYOBJECTS) $(LIBDIRS) $(LIBS) -o $(TARGET)
--- a/source/Concept/Framework/defines.h
+++ b/source/Concept/Framework/defines.h
@ -60,11 +60,49 @@
 #define DISTANCE_PER_VALUE 1
 #define TICKS_PER_CM 205.0f
 //#define TICKS_PER_CM 90.0f
+#define ULTRASONIC_PER_CM 2.0f
+
+#define WIRELESS_ACTIVE false

 #define PI 3.14159265358979323846f
 #define CYCLES_PER_CORRECTION 20
 #define EMPTY_FLOAT 81188.1484f

+#define BALL_HELD_INTENSITY 900
+
+#define KEEPER_LEFT_ANGLE 20.0f * PI / 180.0f
+#define KEEPER_RIGHT_ANGLE (2 * PI) - (20.0f * PI / 180.0f)
+
+#define COMMAND_BUFFER_SIZE 20
+
+#define DEFAULT_SPEED 200
+#define DEFAULT_ROTATION_SPEED 200
+
+#define AREA_BOUNDS_X 20000.0f
+#define AREA_BOUNDS_Y 5000.0f
+
+#define WALL_DISTANCE_TOLERANCE 30.0f
+#define ENEMY_ROBOT_RADIUS 40.0f
+
+#define HOME_LOC_X -500.0f
+#define HOME_LOC_Y 0
+#define ENEMY_LOC_X 500.0f
+#define ENEMY_LOC_Y 0
+#define DEFENCE_L_LOC_X -500.0f
+#define DEFENCE_L_LOC_Y 70.0f
+#define DEFENCE_R_LOC_X -500.0f
+#define DEFENCE_R_LOC_Y -70.0f
+
+#define DISTANCE_TOLERANCE 1.0f
+#define ORIENTATION_TOLERANCE 0.1f
+
+#define MAX_OBSTACLE_COUNT 20
+
+#define OBSTACLE_DECAY 100
+
+#define BALL_DIRECTION_FRONT_L 45.0f * PI / 180.0f
+#define BALL_DIRECTION_FRONT_R 315.0f * PI / 180.0f
+
 #define WIRELESS_CODE "SPASS!"
 enum WirelessCommands
 {
@ -163,17 +201,33 @@ enum IOModuleNames

 	IO_KEYBOARD_END,

+	//Command Handler
+
+	IO_COMMAND_HANDLER_START = IO_KEYBOARD_END,
+
+	IO_COMMAND_HANDLER_MAIN = IO_COMMAND_HANDLER_START,
+
+	IO_COMMAND_HANDLER_END,
+
 	//Position Tracker

-	IO_POSITION_TRACKER_START = IO_KEYBOARD_END,
+	IO_POSITION_TRACKER_START = IO_COMMAND_HANDLER_END,

 	IO_POSITION_TRACKER_MAIN,

 	IO_POSITION_TRACKER_END,

-	//Position Tracker
+	//Obstacle Tracker

-	IO_BALL_TRACKER_START = IO_POSITION_TRACKER_END,
+	IO_OBSTACLE_TRACKER_START = IO_POSITION_TRACKER_END,
+
+	IO_OBSTACLE_TRACKER_MAIN,
+
+	IO_OBSTACLE_TRACKER_END,
+
+	//Ball Tracker
+
+	IO_BALL_TRACKER_START = IO_OBSTACLE_TRACKER_END,

 	IO_BALL_TRACKER_MAIN,

--- a/source/Concept/Framework/main.c
+++ b/source/Concept/Framework/main.c
@ -122,6 +122,22 @@ int main()
 		newKeyboard = NULL;
 	}

+	//Init Command Handler
+	for(uint8 i = IO_COMMAND_HANDLER_START; i < IO_COMMAND_HANDLER_END; i++)
+	{
+		Command_Handler* newCommandHandler = new Command_Handler(i);
+		localRobot->AddModule(newCommandHandler);
+		newCommandHandler = NULL;
+	}
+
+	//Init Obstacle Tracker
+	for(uint8 i = IO_OBSTACLE_TRACKER_START; i < IO_OBSTACLE_TRACKER_END; i++)
+	{
+		Obstacle_Tracker* newObstacleTracker = new Obstacle_Tracker(i);
+		localRobot->AddModule(newObstacleTracker);
+		newObstacleTracker = NULL;
+	}
+
 	//Init Position Tracker
 	for(uint8 i = IO_POSITION_TRACKER_START; i < IO_POSITION_TRACKER_END; i++)
 	{
@ -159,6 +175,8 @@ int main()
 	//Debug code
 	Display* ourDisplay = localRobot->GetModule<Display>(IO_DISPLAY_MAIN);
 	ourDisplay->Clear();
+	ourDisplay->Print("Roboter fertig gestartet",1,1);
+	sleep(1);
 	

 	IR_Sensor* ourSensor = NULL;
@ -166,47 +184,68 @@ int main()
 	uint16 value = 0;
 	int8 value2 = 0;
 	
-	Keyboard* ourKeyboard = localRobot->GetModule<Keyboard>(IO_KEYBOARD_MAIN);
+	Command_Handler* ourCommandHandler = localRobot->GetModule<Command_Handler>(IO_COMMAND_HANDLER_MAIN);
 	uint32 i = 1;
 	Navigator* ourNavigator = localRobot->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
-	Position_Tracker* ourPosition_Tracker = localRobot->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
+	Position_Tracker* ourPositionTracker = localRobot->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
 	Ball_Tracker* ourBallTracker = localRobot->GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN);
 	Dribbler* ourDribbler = localRobot->GetModule<Dribbler>(IO_DRIBBLER_MAIN);
-	ourDribbler->SetSpeed(1);
-	ourDribbler->SetEnabled(true);
+	//ourDribbler->SetSpeed(1);
+	//ourDribbler->SetEnabled(true);
 	Aktuator* ourAktuator = localRobot->GetModule<Aktuator>(IO_AKTUATOR_MAIN);
+	Logic* ourLogic = localRobot->GetModule<Logic>(IO_LOGIC_MAIN);	
+	Wireless* ourWireless = localRobot->GetModule<Wireless>(IO_WIRELESS_MAIN);

-	float rotation = 0;
-	float speed = 200;
+	//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_BACK))->SetSpeed(200);

-	//Mouse_Sensor* mouse_left = localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);
-	//Mouse_Sensor* mouse_right = localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);
+	//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetSpeed(200);
+	//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetEnabled(true);

+	//sleep(2);
+
+	//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT))->SetSpeed(200);
+	//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT))->SetEnabled(true);
+
+	//sleep(2);
+
+	
+
+	//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetSpeed(200);
+	//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetEnabled(true);
+
+	/*(localRobot->GetModule<Dribbler>(IO_DRIBBLER_MAIN))->SetSpeed(200);
+	(localRobot->GetModule<Dribbler>(IO_DRIBBLER_MAIN))->SetEnabled(true);*/
+
+	/*(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT))->SetSpeed(80);
+	(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT))->SetEnabled(true);*/
+
+	(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_BACK))->SetSpeed(255);
+	(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_BACK))->SetEnabled(true);
+
+	(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetSpeed(255);
+	(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetEnabled(true);
+
+	sleep(2);

 	//Run
 	while(true)
 	{
-		ourDisplay->Print(i++,1,1);
+		i++;
 		
-		msleep(100);
-				
-		if(!(i % 1))
+		/*if(!(i % 20))
 		{
 			ourDisplay->Clear();
-			//ourDisplay->Print(ourPosition_Tracker->GetPositionX(),1,2);
-			//ourDisplay->Print(ourPosition_Tracker->GetPositionY(),1,3);
-			//ourDisplay->Print(ourPosition_Tracker->GetOrientation() * 180.0f / PI,1,4);
+			msleep(10);
+			ourCommandHandler->PrintCommand();
 		}
 		
-		if(!(i % 50))
+		localRobot->Update();
+
+		if(ourCommandHandler->displayDistanceSensors && !(i % 20))
 		{
-			ourAktuator->Kick();
-		}
-
-		
 			distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_0_DEG);
 			value = distanceSensor->GetDistance();
-		ourDisplay->Print(value, 1, 2);
+			ourDisplay->Print(value, 1, 4);
 			ourDisplay->Print(";");
 			distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_90_DEG);
 			value = distanceSensor->GetDistance();
@ -214,72 +253,50 @@ int main()
 			ourDisplay->Print(";");
 			distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_180_DEG);
 			value = distanceSensor->GetDistance();
-		ourDisplay->Print(value, 1, 3);
+			ourDisplay->Print(value);
 			ourDisplay->Print(";");
 			distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_270_DEG);
 			value = distanceSensor->GetDistance();
 			ourDisplay->Print(value);
 			ourDisplay->Print(";");
-		
-		uint8 someInput = ourKeyboard->GetInput();
-		//ourDisplay->Print("Ready to accept...", 1, 2);
-		switch(someInput)
-		{
-			case 0:
-				ourNavigator->Stop();
-				return 0;
-				break;
-			case 1:
-				ourNavigator->Drive(225, rotation, speed, rotation);
-				break;
-			case 2:
-				ourNavigator->Drive(180, rotation, speed, rotation);
-				break;
-			case 3:
-				ourNavigator->Drive(135, rotation, speed, rotation);
-				break;
-			case 4:
-				ourNavigator->Drive(270, rotation, speed, rotation);
-				break;
-			case 5:
-				ourNavigator->Drive(0, rotation, 0, rotation);
-				break;
-			case 6:
-				ourNavigator->Drive(90, rotation, speed, rotation);
-				break;
-			case 7:
-				ourNavigator->Drive(315, rotation, speed, rotation);
-				break;
-			case 8:
-				ourNavigator->Drive(0, rotation, speed, rotation);
-				break;
-			case 9:
-				ourNavigator->Drive(45, rotation, speed, rotation);
-				break;
-			case 10:
-				ourPosition_Tracker->SetPosition(0,0,0); // Reset Position_Tracker
-				break;
-			case 12:
-				ourPosition_Tracker->SetPosition(0,0,0); // Reset Position_Tracker
-				break;
 		}

-		//ourDisplay->Clear();
-		/*ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_0_DEG);
+		if(ourCommandHandler->ticksPerCmOffset && !(i % 20))
+		{
+			ourDisplay->Print(ourCommandHandler->ticksPerCmOffset * 10.0f, 10, 1);
+		}
+
+		if(ourCommandHandler->displayPositionTracker && !(i % 20))
+		{
+			ourDisplay->Print(ourPositionTracker->GetPositionX(), 1, 2);
+			ourDisplay->Print(ourPositionTracker->GetPositionY(), 1, 3);
+			ourDisplay->Print(ourPositionTracker->GetOrientation(), 1, 4);
+		}
+
+		if(ourCommandHandler->displayPositionTracker && !(i % 20))
+		{
+			ourDisplay->Print(ourPositionTracker->GetPositionX(), 1, 2);
+			ourDisplay->Print(ourPositionTracker->GetPositionY(), 1, 3);
+			ourDisplay->Print(ourPositionTracker->GetOrientation() * 180.0f / PI, 1, 4);
+		}
+
+		if(ourCommandHandler->displayBallTracker && !(i % 20))
+		{
+			ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_0_DEG);
 			value = ourSensor->GetIRIntensity();
 			ourDisplay->Print(value, 1, 2);
 			ourDisplay->Print(";");
 			ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_30_DEG);
 			value = ourSensor->GetIRIntensity();
-		ourDisplay->Print(value);
+			ourDisplay->Print(value, 6, 2);
 			ourDisplay->Print(";");
 			ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_60_DEG);
 			value = ourSensor->GetIRIntensity();
-		ourDisplay->Print(value);
+			ourDisplay->Print(value, 11, 2);
 			ourDisplay->Print(";");
 			ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_100_DEG);
 			value = ourSensor->GetIRIntensity();
-		ourDisplay->Print(value);
+			ourDisplay->Print(value, 16, 2);
 			ourDisplay->Print(";");
 			ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_180_DEG);
 			value = ourSensor->GetIRIntensity();
@ -287,22 +304,43 @@ int main()
 			ourDisplay->Print(";");
 			ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_260_DEG);
 			value = ourSensor->GetIRIntensity();
-		ourDisplay->Print(value);
+			ourDisplay->Print(value, 6, 3);
 			ourDisplay->Print(";");
 			ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_300_DEG);
 			value = ourSensor->GetIRIntensity();
-		ourDisplay->Print(value);
+			ourDisplay->Print(value, 11, 3);
 			ourDisplay->Print(";");
 			ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_330_DEG);
 			value = ourSensor->GetIRIntensity();
-		ourDisplay->Print(value);
-		ourDisplay->Print(";");*/
+			ourDisplay->Print(value, 16, 3);
+			ourDisplay->Print(";");
+			if(ourBallTracker->KnowsBallDirection())
+			{
+				ourDisplay->Print(ourBallTracker->GetBallDirection() * 180.0f / PI, 1, 4);
+			}
+			else
+			{
+				ourDisplay->Print("Ball not found", 1, 4);
+			}

-		localRobot->Update();
+			if(ourBallTracker->HasBall()) {
+				ourDisplay->Print("Has Ball", 5, 4);
+			}
+		}

-		//ourDisplay->Print(ourBallTracker->GetBallDirection() * 180.0f / PI, 1, 4);
+		//if(!(i % 20))
+		//{
+		//	ourDisplay->Print(ourWireless->Recieve(), 10, 1);
+		//}

-		//ourNavigator->Drive(ourBallTracker->GetBallDirection() * 180.0f / PI, 0, 255, 0);
+		if(ourBallTracker->KnowsBallDirection())
+		{
+			ourNavigator->Drive(ourBallTracker->GetBallDirection() * 180.0f / PI, 0, 255, 0);
+		}
+		else
+		{
+			ourNavigator->Stop();
+		}*/
 	}

 	//Cleanup
--- a/source/Concept/Framework/modules/executor/aktuator.c
+++ b/source/Concept/Framework/modules/executor/aktuator.c
@ -0,0 +1,11 @@
+#include "aktuator.h"
+
+//-----------------------------------------------------------------------------
+void Aktuator::Kick() 
+{
+	(parent->GetModule<Kicker>(IO_KICKER_MAIN))->SetEnabled(true);//aktivate kicker
+	(parent->GetModule<Dribbler>(IO_DRIBBLER_MAIN))->SetSpeed(-1);//aktivate dribbler reverse
+	msleep(10);//wait 100us
+	(parent->GetModule<Kicker>(IO_KICKER_MAIN))->SetEnabled(false);//deaktivate kicker
+	(parent->GetModule<Dribbler>(IO_DRIBBLER_MAIN))->SetSpeed(1);//deaktivate dribbler reverse			
+}
--- a/source/Concept/Framework/modules/executor/aktuator.h
+++ b/source/Concept/Framework/modules/executor/aktuator.h
@ -0,0 +1,27 @@
+#ifndef _AKTUATOR_H
+#define _AKTUATOR_H
+
+#include "../../stdafx.h"
+
+class Aktuator : public IO_Module
+{
+public:
+	Aktuator()
+	{
+		this->parent = NULL;
+		this->moduleId = 0;
+	}
+
+	Aktuator(uint32 aktuatorId)
+	{
+		this->parent = NULL;
+		this->moduleId = aktuatorId;
+	}
+
+protected:
+
+public:
+	void Kick();
+};
+
+#endif
--- a/source/Concept/Framework/modules/executor/navigator.c
+++ b/source/Concept/Framework/modules/executor/navigator.c
@ -59,14 +59,14 @@ void Navigator::Drive(float newDirection, float newAngle, float newSpeed, float
 //-----------------------------------------------------------------------------
 void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed)
 {
-	if(newX < 0 || newY < 0) return;
+	if(newX == EMPTY_FLOAT || newY == EMPTY_FLOAT) return;

 	Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);

 	this->rotationSpeed = min(rotationSpeed, 255.0f);
 	this->targetX = newX;
 	this->targetY = newY;
-	this->direction = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);;
+	this->direction = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);
 	this->targetAngle = newAngle * PI / 180.0f;
 	this->robotSpeed = newSpeed;

@ -89,39 +89,93 @@ void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed,
 }

 //-----------------------------------------------------------------------------
-void Navigator::Update()
+void Navigator::RotateTo(float newAngle, float rotationSpeed) 
+{
+	Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
+
+	this->targetX = EMPTY_FLOAT;
+	this->targetY = EMPTY_FLOAT;
+	this->direction = EMPTY_FLOAT;
+	this->rotationSpeed = min(rotationSpeed, 255.0f);	
+	this->targetAngle = newAngle * PI / 180.0f;
+	this->robotSpeed = 0;
+
+	if(targetAngle - locationeer->GetOrientation() > PI)
+	{
+		if(rotationSpeed > 0)
+		{
+			rotationSpeed = -rotationSpeed;
+		}
+	}
+	else
+	{
+		if(rotationSpeed < 0)
+		{
+			rotationSpeed = -rotationSpeed;
+		}
+	}
+
+	CalculateDirection();
+}
+
+//-----------------------------------------------------------------------------
+bool Navigator::TargetReached() 
 {
 	Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);

 	bool targetReached = false;
-	bool targetAngleReached = false;

-	if(HasTarget() && distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < 1.0f)
+	if(!HasTarget() || (distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < DISTANCE_TOLERANCE))
 	{
-		targetX = EMPTY_FLOAT;
-		targetY = EMPTY_FLOAT;
-		direction = EMPTY_FLOAT;
-		robotSpeed = 0;
-
 		targetReached = true;
 	}

-	if(targetAngle != EMPTY_FLOAT && fabs(targetAngle - locationeer->GetOrientation()) < 0.1f)
-	{
-		targetAngle = EMPTY_FLOAT;
-		rotationSpeed = 0;
+	return targetReached;
+}

+//-----------------------------------------------------------------------------
+bool Navigator::AngleReached()
+{
+	Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
+
+	bool targetAngleReached = false;
+
+	if(!HasTargetAngle() || (fabs(targetAngle - locationeer->GetOrientation()) < 0.1f))
+	{
 		targetAngleReached = true;
 	}

-	if(targetReached && targetAngleReached)
+	return targetAngleReached;
+}
+
+//-----------------------------------------------------------------------------
+void Navigator::Update()
+{
+	if(this->direction == EMPTY_FLOAT || (HasTarget() && TargetReached()))
+	{
+		if(this->rotationSpeed == 0 || (HasTargetAngle() && AngleReached()))
 		{
 			Stop();
 		}
-	else if(targetReached || targetAngleReached)
+		else if(!AngleReached())
 		{
+			RotateTo(this->targetAngle, this->rotationSpeed);
+		}
+		else
+		{
+			Rotate(this->rotationSpeed);
+		}
+	}
+	else
+	{
+		if(this->rotationSpeed == 0 || (HasTargetAngle() && AngleReached()))
+		{
+			this->rotationSpeed = 0;
+			this->targetAngle = EMPTY_FLOAT;
+
 			CalculateDirection();
 		}
+	}

 	if(!(correctionCountdown--))
 	{
@ -176,12 +230,10 @@ void Navigator::CalculateEngines()
 	vBack = ((float)vBack * (float)((float)this->robotSpeed / (float)speedCorrection)) + this->rotationSpeed;
 	vRight = ((float)vRight * (float)((float)this->robotSpeed / (float)speedCorrection)) + this->rotationSpeed;

-	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vLeft,10,2);
+	(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vBack,10,2);
 	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vBack,10,3);
 	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vRight,10,4);

-
-
 	// Transfer the values to the engines
 	Engine* curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT);
 	curEngine->SetSpeed(vLeft);
--- a/source/Concept/Framework/modules/executor/navigator.h
+++ b/source/Concept/Framework/modules/executor/navigator.h
@ -49,6 +49,7 @@ public:
 	void Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed);

 	void DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed);
+	void RotateTo(float newAngle,float roationSpeed);

 	void Rotate(float rotationSpeed);

@ -65,6 +66,34 @@ public:
 	{
 		return (targetX != EMPTY_FLOAT && targetY != EMPTY_FLOAT);
 	}
+
+	float GetTargetX()
+	{
+		return targetX;
+	}
+
+	float GetTargetY()
+	{
+		return targetY;
+	}
+
+	float GetDirection()
+	{
+		return direction;
+	}
+
+	bool HasTargetAngle()
+	{
+		return (targetAngle != EMPTY_FLOAT);
+	}
+
+	bool TargetReached();
+	bool AngleReached();
+
+	bool IsMoving()
+	{
+		return (direction != EMPTY_FLOAT || rotationSpeed != 0);
+	}
 };

 #endif
--- a/source/Concept/Framework/modules/input/distance_sensor.c
+++ b/source/Concept/Framework/modules/input/distance_sensor.c
@ -1,41 +1,159 @@
 #include "distance_sensor.h"

 //-----------------------------------------------------------------------------
-float Distance_Sensor::GetDistance()
+void Distance_Sensor::SetSignalFactor(uint8 factor)
 {
-	uint32 result = 0;
-
-	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("Gen Pulse; Pin:",1,4);
-	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print((int)(*hardwarePin & pin));
-	
-	msleep(500);
-
-	//Generate pulse
-	*hardwareDDR |= pin;//Set pin output
-	*hardwarePort |= pin;//Activate port
-	usleep(10);//Wait for 10ľs
-	*hardwareDDR &= ~pin;//Set pin input
-	*hardwarePort &= ~pin;//Deactivate port
-
-	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("Wait response; Pin:",1,4);	
-	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print((int)(*hardwarePin & pin));
-	
-	uint16 i;
-	//Wait for response
-	for(i = 0; (!(*hardwarePin & pin)) && (i < 1000); i++) { asm volatile("nop"); }
-	
-	//Calculate duration of response
-	while((*hardwarePin & pin)&&(result < 300000)) 
+  if(factor > 16)
 	{
-		result++;
-		asm volatile("nop");
-		asm volatile("nop");
-		asm volatile("nop");
-		asm volatile("nop");
-		asm volatile("nop");
-		asm volatile("nop");
-		asm volatile("nop");
+	 	factor = 16;
 	}

-	return (float(result) * DISTANCE_PER_VALUE);
+	uint8 temp[2];
+	uint8 state;
+	tx_type tx_frame[2];
+	
+	state = SUCCESS;
+
+	tx_frame[0].slave_adr = this->slaveAddr + W;
+	tx_frame[0].size = 2;
+	tx_frame[0].data_ptr = temp;
+	tx_frame[0].data_ptr[0] = 1;
+	tx_frame[0].data_ptr[1] = factor;
+
+	tx_frame[1].slave_adr = OWN_ADR;
+
+	state = Send_to_TWI(tx_frame);
+}
+
+//-----------------------------------------------------------------------------
+void Distance_Sensor::SetSlaveAddress(uint8 newSlaveAddress)
+{
+	uint8 temp[2];
+	uint8 state;
+	tx_type tx_frame[2];
+	
+	state = SUCCESS;
+
+	tx_frame[0].slave_adr = this->slaveAddr + W;
+	tx_frame[0].size = 2;
+	tx_frame[0].data_ptr = temp;
+	tx_frame[0].data_ptr[0] = 0;		
+	tx_frame[0].data_ptr[1] = 160;		
+	tx_frame[1].slave_adr = OWN_ADR;
+	state = Send_to_TWI(tx_frame);
+
+	msleep(60);
+
+	tx_frame[0].data_ptr[1] = 170;	
+	state = Send_to_TWI(tx_frame);
+
+	msleep(60);
+
+	tx_frame[0].data_ptr[1] = 165;	
+	state = Send_to_TWI(tx_frame);
+
+	msleep(60);
+
+	tx_frame[0].data_ptr[1] = newSlaveAddress;
+	state = Send_to_TWI(tx_frame);
+}
+
+//-----------------------------------------------------------------------------
+void Distance_Sensor::SetRange(unsigned int millimeters){
+	uint8 temp[2];
+	uint8 state;
+	tx_type tx_frame[2];
+	
+	state = SUCCESS;
+
+	millimeters = (millimeters/43); 
+
+	tx_frame[0].slave_adr = this->slaveAddr+W;
+	tx_frame[0].size = 2;
+	tx_frame[0].data_ptr = temp;
+	tx_frame[0].data_ptr[0] = 2;
+	tx_frame[0].data_ptr[1] = millimeters;
+
+	tx_frame[1].slave_adr = OWN_ADR;
+
+	state = Send_to_TWI(tx_frame);
+}
+
+/*!
+ * Messung ausloesen
+ * @param metric_unit 0x50 in Zoll, 0x51 in cm, 0x52 ms
+ * @return Resultat der Aktion
+ */
+
+uint8 Distance_Sensor::srf10_ping(uint8 metric_unit){
+	uint8 temp[2];
+	uint8 state;
+	tx_type tx_frame[2];
+	
+	state = SUCCESS;
+
+	tx_frame[0].slave_adr = this->slaveAddr+W;
+	tx_frame[0].size = 2;
+	tx_frame[0].data_ptr = temp;
+	tx_frame[0].data_ptr[0] = SRF10_COMMAND;
+	tx_frame[0].data_ptr[1] = metric_unit;
+
+	tx_frame[1].slave_adr = OWN_ADR;
+
+	state = Send_to_TWI(tx_frame);
+	
+	return state;
+}
+
+//-----------------------------------------------------------------------------
+uint8 Distance_Sensor::ReadRegister(uint8 registerToRead){
+	uint8 temp;
+	uint8 value;
+	uint8 state;
+	tx_type tx_frame[3];
+
+	state = SUCCESS;
+	value = 0;
+
+	tx_frame[0].slave_adr = this->slaveAddr+W;
+	tx_frame[0].size = 1;
+	tx_frame[0].data_ptr = &temp;
+	tx_frame[0].data_ptr[0] = registerToRead;
+
+	tx_frame[1].slave_adr = this->slaveAddr+R;
+	tx_frame[1].size = 1;
+	tx_frame[1].data_ptr = &value;
+
+	tx_frame[2].slave_adr = OWN_ADR;
+
+	state = Send_to_TWI(tx_frame);
+	
+	return value;
+}
+
+/*!
+ * Messung starten Ergebniss aufbereiten und zurueckgeben
+ * @return Messergebniss
+ */
+
+uint16 Distance_Sensor::srf10_get_measure(){
+	char hib;
+	char lob;
+	char state;
+
+	state = SUCCESS;
+
+	state = srf10_ping(SRF10_CENTIMETERS);
+	msleep(10);									//Optimierungs Potential
+	lob=ReadRegister(SRF10_LOB);
+	msleep(10);									//Optimierungs Potential
+	hib=ReadRegister(SRF10_HIB);
+	
+	return (hib*256)+lob;
+}
+
+//-----------------------------------------------------------------------------
+uint16 Distance_Sensor::GetDistance()
+{
+	return srf10_get_measure();
 }
--- a/source/Concept/Framework/modules/input/distance_sensor.h
+++ b/source/Concept/Framework/modules/input/distance_sensor.h
@ -4,6 +4,20 @@
 #include "../../stdafx.h"
 #include "sensor.h"

+#define SRF10_MIN_GAIN        0      /*!< setze Verstaerung auf 40 */
+#define SRF10_MAX_GAIN        16     /*!< setze Verstaerung auf 700 */
+#define SRF10_MIN_RANGE       0      /*!< Minimale Reichweite 43mm */
+#define SRF10_MAX_RANGE       5977   /*!< Maximale Reichweite 5977mm */
+
+#define SRF10_INCHES          0x50	 /*!< Messung in INCHES */
+#define SRF10_CENTIMETERS     0x51	 /*!< Messung in CM */
+#define SRF10_MICROSECONDS    0x52	 /*!< Messung in Millisekunden */
+
+#define SRF10_COMMAND         0		 /*!< W=Befehls-Register R=Firmware*/
+#define SRF10_LIGHT           1		 /*!< W=Verstaerkungsfaktor R=Nicht benutzt */
+#define SRF10_HIB             2		 /*!< W=Reichweite R=Messung High-Byte */
+#define SRF10_LOB             3		 /*!< W=Nicht benutzt R=Messung Low-Byte */
+
 class Distance_Sensor : public Sensor
 {
 public:
@ -11,10 +25,7 @@ public:
 	{
 		this->parent = NULL;
 		this->moduleId = 0;
-		this->hardwarePort = NULL;
-		this->hardwareDDR = NULL;
-		this->hardwarePin = NULL;
-		this->pin = 0;
+		this->slaveAddr = 0; // Set the I2C Slave-Adress
 	}

 	Distance_Sensor(uint32 sensorId)
@ -25,47 +36,58 @@ public:
 		switch(sensorId)
 		{
 			case IO_SENSOR_DISTANCE_0_DEG:
-				this->hardwarePort = &PORTC;
-				this->hardwareDDR = &DDRC;
-				this->hardwarePin = &PINC;
-				this->pin = (1 << 0);
+				this->slaveAddr = 0xE0; // Set the I2C Slave-Adress
 				break;
 			case IO_SENSOR_DISTANCE_90_DEG:
-				this->hardwarePort = &PORTC;
-				this->hardwareDDR = &DDRC;
-				this->hardwarePin = &PINC;
-				this->pin = (1 << 1);
+				this->slaveAddr = 0xE2;
 				break;
 			case IO_SENSOR_DISTANCE_180_DEG:
-				this->hardwarePort = &PORTC;
-				this->hardwareDDR = &DDRC;
-				this->hardwarePin = &PINC;
-				this->pin = (1 << 2);
+				this->slaveAddr = 0xE4;
 				break;
 			case IO_SENSOR_DISTANCE_270_DEG:
-				this->hardwarePort = &PORTC;
-				this->hardwareDDR = &DDRC;
-				this->hardwarePin = &PINC;
-				this->pin = (1 << 3);
+				this->slaveAddr = 0xE6;
 				break;
 			default:
-				this->hardwarePort = NULL;
-				this->hardwareDDR = NULL;
-				this->hardwarePin = NULL;
-				this->pin = 0;
+				this->slaveAddr = 0;
 				break;
 		}
+
+		//SetRange(100);
+		SetRange(2000);
+		SetSignalFactor(0x06);
 	}

 protected:
 	//Hardware
-	volatile uint8* hardwarePort;
-	volatile uint8* hardwareDDR;
-	volatile uint8* hardwarePin;
-	uint8 pin;
+	uint8 slaveAddr;
+
+	void SetSignalFactor(uint8 factor);
+
+	void SetRange(uint16 millimeters);
+
+/*!
+ * Messung ausloesen
+ * @param metric_unit 0x50 in Zoll, 0x51 in cm, 0x52 ms
+ * @return Resultat der Aktion
+ */
+ uint8	srf10_ping(uint8 metric_unit);
+
+/*!
+ * Register auslesen
+ * @param srf10_register welches Register soll ausgelsen werden
+ * @return Inhalt des Registers
+ */
+ uint8	ReadRegister(uint8 registerToRead);
+
+/*!
+ * Messung starten Ergebniss aufbereiten und zurueckgeben
+ * @return Messergebniss
+ */
+ uint16	srf10_get_measure(void);

 public:
-	float GetDistance();
+	uint16 GetDistance();
+	void SetSlaveAddress(uint8 newSlaveAddress);
 };

 #endif
--- a/source/Concept/Framework/modules/input/ir_sensor.h
+++ b/source/Concept/Framework/modules/input/ir_sensor.h
@ -34,11 +34,11 @@ public:
 				break;
 			case IO_SENSOR_IR_100_DEG:
 				this->channel = 3;
-				this->intensityCorrection = 40;
+				this->intensityCorrection = 80;
 				break;
 			case IO_SENSOR_IR_180_DEG:
 				this->channel = 4;
-				this->intensityCorrection = 50;
+				this->intensityCorrection = 70;
 				break;
 			case IO_SENSOR_IR_260_DEG:
 				this->channel = 5;
--- a/source/Concept/Framework/modules/input/mouse_sensor.h
+++ b/source/Concept/Framework/modules/input/mouse_sensor.h
@ -214,6 +214,16 @@ public:
 	{
 		return positionY;
 	}
+
+	void AdjustPositionX(float newPosX)
+	{
+		this->positionX = newPosX;
+	}
+
+	void AdjustPositionY(float newPosY)
+	{
+		this->positionY = newPosY;
+	}
 };

 #endif
--- a/source/Concept/Framework/modules/interpreter/ball_tracker.c
+++ b/source/Concept/Framework/modules/interpreter/ball_tracker.c
@ -3,6 +3,8 @@
 //-----------------------------------------------------------------------------
 void Ball_Tracker::Update()
 {
+	Position_Tracker* ourPositionTracker = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
+
 	uint8 sensorCount = (IO_SENSOR_IR_330_DEG - IO_SENSOR_IR_0_DEG) + 1;
 	uint16 intensity[sensorCount];
 	uint8 greatestIntensity = 0;
@ -21,6 +23,18 @@ void Ball_Tracker::Update()
 		{
 			greatestIntensity = i;
 		}
+
+		if(i == 0)
+		{
+			if(intensity[i] > BALL_HELD_INTENSITY) // Ball derzeit sehr nah dran
+			{
+				if(ballHeldCounter < 10) ballHeldCounter++;
+			}
+			else if(ballHeldCounter > 0)
+			{
+				ballHeldCounter--;		
+			}
+		}
 	}

 	if(intensity[greatestIntensity])
@ -117,13 +131,15 @@ void Ball_Tracker::Update()
 			direction = (intensity[greatestIntensity] * mainDirection + 
 									intensity[secondIntensity] * secondDirection) / 
 									(intensity[greatestIntensity] + intensity[secondIntensity]);
-
-			direction = easyAngle(direction);
 		}
 		else
 		{
 			direction = mainDirection;
 		}
+
+		direction += ourPositionTracker->GetOrientation();
+
+		direction = easyAngle(direction);
 	}
 	else
 	{
--- a/source/Concept/Framework/modules/interpreter/ball_tracker.h
+++ b/source/Concept/Framework/modules/interpreter/ball_tracker.h
@ -11,6 +11,7 @@ public:
 		this->parent = NULL;
 		this->moduleId = 0;
 		this->direction = EMPTY_FLOAT;
+		this->ballHeldCounter = 0;
 	}

 	Ball_Tracker(uint32 trackerId)
@ -18,10 +19,12 @@ public:
 		this->parent = NULL;
 		this->moduleId = trackerId;
 		this->direction = EMPTY_FLOAT;
+		this->ballHeldCounter = 0;
 	}

 protected:
 	float direction;
+	uint8 ballHeldCounter;

 public:
 	void Update();
@ -33,12 +36,12 @@ public:

 	bool KnowsBallDirection()
 	{
-		return direction != EMPTY_FLOAT;
+		return (direction != EMPTY_FLOAT);
 	}

-	bool RobotHasBall()
+	bool HasBall()
 	{
-		//fill me!
+		return (ballHeldCounter >= 3);
 	}
 };

--- a/source/Concept/Framework/modules/interpreter/command_handler.c
+++ b/source/Concept/Framework/modules/interpreter/command_handler.c
@ -0,0 +1,211 @@
+#include "command_handler.h"
+
+//-----------------------------------------------------------------------------
+void Command_Handler::Update()
+{
+	Display* ourDisplay = parent->GetModule<Display>(IO_DISPLAY_MAIN);
+	Keyboard* ourKeyboard = parent->GetModule<Keyboard>(IO_KEYBOARD_MAIN);
+
+	uint8 curInput = ourKeyboard->GetInput();
+
+
+	while(curInput != 0xEE && curInput != 0xFF)
+	{
+		if(curInput == 10)
+		{
+			ExecuteCommand();
+
+			for(uint8 i = 0; i < this->currentCommandLength; i++)
+			{
+				this->buffer[i] = NULL;
+				ourDisplay->Print(" ", i + 1, 1);
+			}
+
+			this->currentCommandLength = 0;
+		}
+		else if(curInput == 11)
+		{
+			if(this->currentCommandLength > 0)
+			{
+				ourDisplay->Print(" ", this->currentCommandLength, 1);
+				this->currentCommandLength--;
+				this->buffer[currentCommandLength] = NULL;
+			}
+		}
+		else if(curInput >= 0 && curInput <= 9)
+		{
+			if(this->currentCommandLength < COMMAND_BUFFER_SIZE)
+			{
+				this->buffer[this->currentCommandLength] = curInput;
+				this->currentCommandLength++;
+				ourDisplay->Print(curInput, this->currentCommandLength, 1);
+			}
+		}
+
+		curInput = ourKeyboard->GetInput();
+	}
+}
+
+//-----------------------------------------------------------------------------
+void Command_Handler::ExecuteCommand()
+{
+	if(this->buffer[0] == 1)
+	{
+		if(this->buffer[1] == 1)
+		{
+			this->displayDistanceSensors = true;
+			this->displayPositionTracker = false;
+			this->displayBallTracker = false;
+			this->displayMouseSensors = false;
+		}
+		else if(this->buffer[1] == 2)
+		{
+			this->displayDistanceSensors = false;
+			this->displayPositionTracker = true;
+			this->displayBallTracker = false;
+			this->displayMouseSensors = false;
+		}
+		else if(this->buffer[1] == 3)
+		{
+			this->displayDistanceSensors = false;
+			this->displayPositionTracker = false;
+			this->displayBallTracker = true;
+			this->displayMouseSensors = false;
+		}
+		else if(this->buffer[1] == 4)
+		{
+			this->displayDistanceSensors = false;
+			this->displayPositionTracker = false;
+			this->displayBallTracker = false;
+			this->displayMouseSensors = true;
+		}
+		else if(this->buffer[1] == 0)
+		{
+			this->displayDistanceSensors = false;
+			this->displayPositionTracker = false;
+			this->displayBallTracker = false;
+			this->displayMouseSensors = false;
+		}
+	}
+	else if(this->buffer[0] == 2)
+	{
+		Logic* ourLogic = parent->GetModule<Logic>(IO_LOGIC_MAIN);	
+		if(this->buffer[1] == 1)
+		{
+			ourLogic->SetKeeper(true);
+		}
+		else if(this->buffer[1] == 2)
+		{
+			ourLogic->SetKeeper(false);
+		}
+	}
+	else if(this->buffer[0] == 3)
+	{
+		if(this->buffer[1] == 1)
+		{
+			Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
+			ourNavigator->RotateTo(180, 200);
+		}
+		else if(this->buffer[1] == 2)
+		{
+			Aktuator* ourAktuator = parent->GetModule<Aktuator>(IO_AKTUATOR_MAIN);
+			ourAktuator->Kick();
+		}
+		else if(this->buffer[1] == 3)
+		{
+			Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
+			ourNavigator->RotateTo(float(buffer[2]) * 100.0f + float(buffer[3]) * 10.0f + float(buffer[4]), 200);
+		}
+		else if(this->buffer[1] == 4)
+		{
+			Mouse_Sensor* ourLeftMouse = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);
+			Mouse_Sensor* ourRightMouse = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);
+
+			this->ticksPerCmOffset += 2.5f;
+			ourLeftMouse->AdjustPositionX(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
+			ourLeftMouse->AdjustPositionY(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
+			ourRightMouse->AdjustPositionX(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
+			ourRightMouse->AdjustPositionY(3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
+		}
+		else if(this->buffer[1] == 5)
+		{
+			Mouse_Sensor* ourLeftMouse = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);
+			Mouse_Sensor* ourRightMouse = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);
+
+			this->ticksPerCmOffset -= 2.5f;
+			ourLeftMouse->AdjustPositionX(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
+			ourLeftMouse->AdjustPositionY(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
+			ourRightMouse->AdjustPositionX(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
+			ourRightMouse->AdjustPositionY(3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
+		}
+	}
+	else if(this->buffer[0] == 4)
+	{
+		Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
+		int16 speed = 200;
+		switch(this->buffer[1])
+		{
+			case 1:
+				ourNavigator->Drive(225, 0, speed, 0);
+				break;
+			case 2:
+				ourNavigator->Drive(180, 0, speed, 0);
+				break;
+			case 3:
+				ourNavigator->Drive(135, 0, speed, 0);
+				break;
+			case 4:
+				ourNavigator->Drive(270, 0, speed, 0);
+				break;
+			case 5:
+				ourNavigator->Rotate(DEFAULT_ROTATION_SPEED);
+				break;
+			case 6:
+				ourNavigator->Drive(90, 0, speed, 0);
+				break;
+			case 7:
+				ourNavigator->Drive(315, 0, speed, 0);
+				break;
+			case 8:
+				ourNavigator->Drive(0, 0, speed, 0);
+				break;
+			case 9:
+				ourNavigator->Drive(45, 0, speed, 0);
+				break;
+			case 0:
+				ourNavigator->Stop();
+				break;
+		}
+	}
+	else if(this->buffer[0] == 5)
+	{
+		Position_Tracker* ourPositionTracker = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
+		if(this->buffer[1] == 1)
+		{
+			ourPositionTracker->SetPosition(float(), ourPositionTracker->GetPositionY(), ourPositionTracker->GetOrientation());
+		}
+		else if(this->buffer[1] == 0)
+		{
+			ourPositionTracker->SetPosition(0, 0, 0);
+		}
+	}
+	else if(this->buffer[0] == 6)
+	{
+		if(this->buffer[1] == 1)
+		{
+			Wireless* ourWireless = parent->GetModule<Wireless>(IO_WIRELESS_MAIN);
+			ourWireless->Send("Heyho!");
+		}
+	}
+}
+
+//-----------------------------------------------------------------------------
+void Command_Handler::PrintCommand()
+{
+	Display* ourDisplay = parent->GetModule<Display>(IO_DISPLAY_MAIN);
+	
+	for(uint8 i = 0; i < currentCommandLength; i++)
+	{
+		ourDisplay->Print(this->buffer[i], i + 1, 1);
+	}
+}
--- a/source/Concept/Framework/modules/interpreter/command_handler.h
+++ b/source/Concept/Framework/modules/interpreter/command_handler.h
@ -0,0 +1,63 @@
+#ifndef _COMMAND_HANDLER_H
+#define _COMMAND_HANDLER_H
+
+#include "../../stdafx.h"
+
+class Command_Handler : public IO_Module
+{
+public:
+	Command_Handler()
+	{
+		this->parent = NULL;
+		this->moduleId = 0;
+		this->currentCommandLength = 0;
+		this->displayDistanceSensors = false;
+		this->displayPositionTracker = false;
+		this->displayBallTracker = false;
+		this->displayMouseSensors = false;
+		this->ticksPerCmOffset = 0;
+
+		for(uint8 i = 0; i < COMMAND_BUFFER_SIZE; i++)
+		{
+			buffer[i] = 0;
+		}
+	}
+
+	Command_Handler(uint32 commandHandlerId)
+	{
+		this->parent = NULL;
+		this->moduleId = commandHandlerId;
+		this->currentCommandLength = 0;
+		this->displayDistanceSensors = false;
+		this->displayPositionTracker = false;
+		this->displayBallTracker = false;
+		this->displayMouseSensors = false;
+		this->ticksPerCmOffset = 0;
+
+		for(uint8 i = 0; i < COMMAND_BUFFER_SIZE; i++)
+		{
+			buffer[i] = 0;
+		}
+	}
+
+protected:
+	uint8 currentCommandLength;
+	uint8 buffer[COMMAND_BUFFER_SIZE];
+
+	void ExecuteCommand();
+
+public:
+	void Update();
+
+	void PrintCommand();
+
+	//Command variables
+	bool displayDistanceSensors;
+	bool displayPositionTracker;
+	bool displayBallTracker;
+	bool displayMouseSensors;
+
+	float ticksPerCmOffset;
+};
+
+#endif
--- a/source/Concept/Framework/modules/interpreter/position_tracker.c
+++ b/source/Concept/Framework/modules/interpreter/position_tracker.c
@ -3,6 +3,9 @@
 //-----------------------------------------------------------------------------
 void Position_Tracker::Update()
 {
+	Command_Handler* ourCommandHandler = parent->GetModule<Command_Handler>(IO_COMMAND_HANDLER_MAIN);
+	Display* ourDisplay = parent->GetModule<Display>(IO_DISPLAY_MAIN);
+
 	// We want to use the mouse-sensors
 	Mouse_Sensor* mouseLeft = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);
 	Mouse_Sensor* mouseRight = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);
@ -10,6 +13,10 @@ void Position_Tracker::Update()
 	// Generate a vector for the left mouse
 	int8 leftX = mouseLeft->GetXMovement();
 	int8 leftY = mouseLeft->GetYMovement();
+	// Generate a vector for the right mouse
+	int8 rightX = mouseRight->GetXMovement();
+	int8 rightY = mouseRight->GetYMovement();
+
 	float distanceLeft = sqrt(leftX * leftX + leftY * leftY);
 	float angleLeft = easyAngle(atan2(leftY, leftX) + (225.0f * PI / 180.0f));

@ -23,9 +30,6 @@ void Position_Tracker::Update()
 		movementLeftY = 0;
 	}

-	// Generate a vector for the right mouse
-	int8 rightX = mouseRight->GetXMovement();
-	int8 rightY = mouseRight->GetYMovement();
 	float distanceRight = sqrt(rightX * rightX + rightY * rightY);
 	float angleRight = easyAngle(atan2(rightY, rightX) - (45.0f * PI / 180.0f));

@ -39,6 +43,14 @@ void Position_Tracker::Update()
 		movementRightY = 0;
 	}

+	if(ourCommandHandler->displayMouseSensors)
+	{
+		ourDisplay->Print(movementLeftX, 1, 2);
+		ourDisplay->Print(movementLeftY, 10, 2);
+		ourDisplay->Print(movementRightX, 1, 3);
+		ourDisplay->Print(movementRightY, 10, 3);
+	}
+
 	// Generate vector from P:left to P:right
 	float movementDifferenceX = movementRightX - movementLeftX;
 	float movementDifferenceY = (movementRightY + mouseRight->GetPositionY()) - (movementLeftY + mouseLeft->GetPositionY());
@ -46,17 +58,18 @@ void Position_Tracker::Update()
 	// Calculate the difference of orientation
 	float orientationDiff = atan2(movementDifferenceY, movementDifferenceX) - (PI / 2.0f);

+	if(ourCommandHandler->displayMouseSensors)
+	{
+		ourDisplay->Print(orientationDiff * 180.0f / PI, 1, 4);
+	}
+
 	float robotMovementX = movementDifferenceX / 2.0f;
 	float robotMovementY = movementDifferenceY / 2.0f;
 	robotMovementX += movementLeftX + mouseLeft->GetPositionX() + (-mouseLeft->GetPositionX() * cos(orientationDiff));
 	robotMovementY += movementLeftY + mouseLeft->GetPositionY() + (mouseLeft->GetPositionX() * sin(orientationDiff));
-	//float robotDistance = sqrt(robotMovementX * robotMovementX + robotMovementY * robotMovementY);

-	float absoluteDiffX = cos(this->orientation + (orientationDiff / 2.0f)) * robotMovementX - sin(this->orientation + (orientationDiff / 2.0f)) * robotMovementY;
-	float absoluteDiffY	= sin(this->orientation + (orientationDiff / 2.0f) + PI / 2.0f) * robotMovementY + cos(this->orientation + (orientationDiff / 2.0f) - PI / 2.0f) * robotMovementX;
-
-	//float absoluteDiffX = cos(this->orientation + (orientationDiff / 2.0f)) * robotDistance * sign(robotMovementX);
-	//float absoluteDiffY	= sin(this->orientation + (orientationDiff / 2.0f)) * robotDistance * sign(robotMovementY);
+	float absoluteDiffX = cos(this->orientation + (orientationDiff / 2.0f)) * robotMovementX + sin(this->orientation + (orientationDiff / 2.0f)) * robotMovementY;
+	float absoluteDiffY	= sin(this->orientation + (orientationDiff / 2.0f) + PI / 2.0f) * robotMovementY - cos(this->orientation + (orientationDiff / 2.0f) - PI / 2.0f) * robotMovementX;

 	if(!robotMovementX && !robotMovementY)
 	{
@ -74,4 +87,10 @@ void Position_Tracker::Update()
 	this->orientation += orientationDiff;

 	this->orientation = easyAngle(this->orientation);
+
+	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("            ", 5, 1);
+	/*(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Clear();
+	(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(this->orientation * 180.0f / PI, 5, 1);
+	(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(this->positionX, 1, 2);
+	(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(this->positionY, 1, 3);*/
 }
--- a/source/Concept/Framework/modules/interpreter/position_tracker.h
+++ b/source/Concept/Framework/modules/interpreter/position_tracker.h
@ -40,19 +40,21 @@ public:
 	}

 	// returns x-koordinate in mm
-	int GetPositionX() 	{
-		return (int)((positionX*10.0f)/TICKS_PER_CM);
+	int16 GetPositionX() 
+	{
+		return (int16)((positionX*10.0f)/TICKS_PER_CM);
 	}

 	// returns y-koordinate in mm
-	int GetPositionY() 	{
-		return (int)((positionY*10.0f)/TICKS_PER_CM);
+	int16 GetPositionY() 	
+	{
+		return (int16)((positionY*10.0f)/TICKS_PER_CM);
 	}

 	// returns orientation
-	float GetOrientation()	{
-		return 0.0f; //tmp!!!!!!!!!!
-		//return orientation;
+	float GetOrientation()
+	{
+		return orientation;
 	}
 };

--- a/source/Concept/Framework/modules/logic/logic.c
+++ b/source/Concept/Framework/modules/logic/logic.c
@ -0,0 +1,271 @@
+#include "logic.h"
+
+//-----------------------------------------------------------------------------
+void Logic::OnBallOwned()
+{
+	bool wasKeeper = this->IsKeeper();
+
+	this->SetKeeper(false);
+
+	if(!WIRELESS_ACTIVE)
+	{
+		Aktuator* ourAktuator = parent->GetModule<Aktuator>(IO_AKTUATOR_MAIN);
+
+		ourAktuator->Kick();
+		this->SetKeeper(wasKeeper);
+	}
+}
+
+//-----------------------------------------------------------------------------
+void Logic::OnBallLost()
+{
+	
+}
+
+//-----------------------------------------------------------------------------
+void Logic::OnBecomeKeeper()
+{
+	Position_Tracker* ourPositionTracker = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
+	Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
+	
+	if(distance2d(HOME_LOC_X, HOME_LOC_Y, ourPositionTracker->GetPositionX(), ourPositionTracker->GetPositionY()) > DISTANCE_TOLERANCE)
+	{
+		status = STATUS_KEEPER_RETURN_HOME;
+
+		ourNavigator->DriveTo(HOME_LOC_X, HOME_LOC_Y, ourPositionTracker->GetOrientation(), DEFAULT_SPEED, 0);
+	}
+	else
+	{
+		UpdateKeeperMovement();
+	}
+}
+
+//-----------------------------------------------------------------------------
+void Logic::OnBecomeAttacker()
+{
+	 Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
+
+	 ourNavigator->Stop();
+
+	 UpdateAttackerMovement();
+}
+
+//-----------------------------------------------------------------------------
+void Logic::Update()
+{
+	// We want to use a navigator
+	Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
+	Ball_Tracker* ourBallTracker = parent->GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN);
+	Display* ourDisplay = parent->GetModule<Display>(IO_DISPLAY_MAIN);
+	Position_Tracker* ourPositionTracker = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
+	Obstacle_Tracker* ourObstacleTracker = parent->GetModule<Obstacle_Tracker>(IO_OBSTACLE_TRACKER_MAIN);
+
+	if(!this->HasBall() && ourBallTracker->HasBall())
+	{
+		this->hasBall = true;
+		this->OnBallOwned();
+	}
+	else if(this->HasBall() && !ourBallTracker->HasBall())
+	{
+		this->hasBall = false;
+		this->OnBallLost();
+	}
+
+	switch(status)
+	{
+		case STATUS_ATTACKER_SEARCHING_AT_HOME:
+		case STATUS_ATTACKER_SEARCHING_AT_ENEMY:
+			if(ourBallTracker->KnowsBallDirection())
+			{
+				ourNavigator->Stop();
+			}
+			break;
+		case STATUS_KEEPER_HUNT_BALL:
+			if(ourBallTracker->KnowsBallDirection())
+			{
+				ourNavigator->Drive(ourBallTracker->GetBallDirection(), EMPTY_FLOAT, DEFAULT_SPEED, 0);
+			}
+			else
+			{
+				ourNavigator->Stop();
+			}
+			break;
+	}
+
+	if(!ourNavigator->IsMoving())
+	{
+		if(this->IsKeeper())
+		{
+			UpdateKeeperMovement();
+		}
+		else
+		{
+			UpdateAttackerMovement();
+		}
+	}
+	
+	if(ourNavigator->IsMoving())
+	{
+		if(ourObstacleTracker->IsObstacleOnPath())
+		{
+			avoidsObstacle = true;
+			for(uint8 i = 0; i < 5; i++)
+			{
+				if(ourObstacleTracker->IsObstacleOnPath(easyAngle(ourNavigator->GetDirection() - ((i - 2) * PI / 4.0f))))
+				{
+				}
+				ourNavigator->GetDirection();
+			}
+		}
+	}
+}
+
+//-----------------------------------------------------------------------------
+void Logic::UpdateKeeperMovement()
+{
+	Ball_Tracker* ourBallTracker = parent->GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN);
+	Position_Tracker* ourPositionTracker = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
+	Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
+
+	if(ourBallTracker->KnowsBallDirection())
+	{
+		if(ourBallTracker->GetBallDirection() > KEEPER_LEFT_ANGLE && 
+			ourBallTracker->GetBallDirection() <= PI)
+		{
+			if(distance2d(DEFENCE_L_LOC_X, DEFENCE_L_LOC_Y, ourPositionTracker->GetPositionX(), ourPositionTracker->GetPositionY()) < DISTANCE_TOLERANCE)
+			{
+				float angleDifference = fabs(ourBallTracker->GetBallDirection() - ourPositionTracker->GetOrientation());
+				if(angleDifference > PI)
+				{
+					angleDifference = (2 * PI) - angleDifference;
+				}
+
+				if(angleDifference > ORIENTATION_TOLERANCE)
+				{
+					status = STATUS_KEEPER_TURN_TO_BALL;
+					ourNavigator->RotateTo(ourBallTracker->GetBallDirection(), DEFAULT_ROTATION_SPEED);
+				}
+				else
+				{
+					status = STATUS_KEEPER_HUNT_BALL;
+					ourNavigator->Drive(ourBallTracker->GetBallDirection(), EMPTY_FLOAT, DEFAULT_SPEED, 0);
+				}
+			}
+			else
+			{
+				status = STATUS_KEEPER_DRIVE_TO_DEFEND;
+				ourNavigator->DriveTo(DEFENCE_L_LOC_X, DEFENCE_L_LOC_Y, EMPTY_FLOAT, DEFAULT_SPEED, 0);
+			}
+		}
+		else if(ourBallTracker->GetBallDirection() < KEEPER_RIGHT_ANGLE && 
+			ourBallTracker->GetBallDirection() >= PI)
+		{
+			if(distance2d(DEFENCE_R_LOC_X, DEFENCE_R_LOC_Y, ourPositionTracker->GetPositionX(), ourPositionTracker->GetPositionY()) < DISTANCE_TOLERANCE)
+			{
+				float angleDifference = fabs(ourBallTracker->GetBallDirection() - ourPositionTracker->GetOrientation());
+				if(angleDifference > PI)
+				{
+					angleDifference = (2 * PI) - angleDifference;
+				}
+
+				if(angleDifference > ORIENTATION_TOLERANCE)
+				{
+					status = STATUS_KEEPER_TURN_TO_BALL;
+					ourNavigator->RotateTo(ourBallTracker->GetBallDirection(), DEFAULT_ROTATION_SPEED);
+				}
+				else
+				{
+					status = STATUS_KEEPER_HUNT_BALL;
+					ourNavigator->Drive(ourBallTracker->GetBallDirection(), EMPTY_FLOAT, DEFAULT_SPEED, 0);
+				}
+			}
+			else
+			{
+				status = STATUS_KEEPER_DRIVE_TO_DEFEND;
+				ourNavigator->DriveTo(DEFENCE_R_LOC_X, DEFENCE_R_LOC_Y, EMPTY_FLOAT, DEFAULT_SPEED, 0);
+			}
+		}
+		else
+		{
+			if(distance2d(HOME_LOC_X, HOME_LOC_Y, ourPositionTracker->GetPositionX(), ourPositionTracker->GetPositionY()) < DISTANCE_TOLERANCE)
+			{
+				float angleDifference = fabs(ourBallTracker->GetBallDirection() - ourPositionTracker->GetOrientation());
+				if(angleDifference > PI)
+				{
+					angleDifference = (2 * PI) - angleDifference;
+				}
+
+				if(angleDifference > ORIENTATION_TOLERANCE)
+				{
+					status = STATUS_KEEPER_TURN_TO_BALL;
+					ourNavigator->RotateTo(ourBallTracker->GetBallDirection(), DEFAULT_ROTATION_SPEED);
+				}
+				else
+				{
+					status = STATUS_KEEPER_HUNT_BALL;
+					ourNavigator->Drive(ourBallTracker->GetBallDirection(), EMPTY_FLOAT, DEFAULT_SPEED, 0);
+				}
+			}
+			else
+			{
+				status = STATUS_KEEPER_DRIVE_TO_DEFEND;
+				ourNavigator->DriveTo(HOME_LOC_X, HOME_LOC_Y, EMPTY_FLOAT, DEFAULT_SPEED, 0);
+			}
+		}
+	}
+	else
+	{
+		if(status == STATUS_KEEPER_TURN_LEFT)
+		{
+			status = STATUS_KEEPER_TURN_RIGHT;
+			ourNavigator->RotateTo(315, DEFAULT_ROTATION_SPEED);
+		}
+		else
+		{
+			status = STATUS_KEEPER_TURN_LEFT;
+			ourNavigator->RotateTo(45, DEFAULT_ROTATION_SPEED);
+		}
+	}
+}
+
+//-----------------------------------------------------------------------------
+void Logic::UpdateAttackerMovement()
+{
+	Ball_Tracker* ourBallTracker = parent->GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN);
+	Position_Tracker* ourPositionTracker = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
+	Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
+
+	if(ourBallTracker->KnowsBallDirection())
+	{
+		float angleDifference = fabs(ourBallTracker->GetBallDirection() - ourPositionTracker->GetOrientation());
+
+		if(angleDifference > PI)
+		{
+			angleDifference = (2 * PI) - angleDifference;
+		}
+
+		if(angleDifference > ORIENTATION_TOLERANCE)
+		{
+			status = STATUS_ATTACKER_TURN_TO_BALL;
+			ourNavigator->RotateTo(ourBallTracker->GetBallDirection(), DEFAULT_ROTATION_SPEED);
+		}
+		else
+		{
+			status = STATUS_ATTACKER_DRIVE_TO_BALL;
+			ourNavigator->Drive(ourBallTracker->GetBallDirection(), EMPTY_FLOAT, DEFAULT_SPEED, 0);
+		}
+	}
+	else
+	{
+		if(ourPositionTracker->GetPositionX() > 0)
+		{
+			status = STATUS_ATTACKER_SEARCHING_AT_HOME;
+			ourNavigator->DriveTo(HOME_LOC_X, HOME_LOC_Y, EMPTY_FLOAT, DEFAULT_SPEED, 0);
+		}
+		else
+		{
+			status = STATUS_ATTACKER_SEARCHING_AT_ENEMY;
+			ourNavigator->DriveTo(ENEMY_LOC_X, ENEMY_LOC_Y, EMPTY_FLOAT, DEFAULT_SPEED, 0);
+		}
+	}
+}
--- a/source/Concept/Framework/modules/logic/logic.h
+++ b/source/Concept/Framework/modules/logic/logic.h
@ -0,0 +1,91 @@
+#ifndef _LOGIC_H
+#define _LOGIC_H
+
+#include "../../stdafx.h"
+
+class Logic : public IO_Module
+{
+public:
+	Logic()
+	{
+		this->parent = NULL;
+		this->moduleId = 0;
+		this->isKeeper = false;
+		this->status = 0;
+		this->hasBall = false;
+		this->avoidsObstacle = false;
+	}
+
+	Logic(uint32 logicId)
+	{
+		this->parent = NULL;
+		this->moduleId = logicId;
+		this->isKeeper = false;
+		this->status = 0;
+		this->hasBall = false;
+		this->avoidsObstacle = false;
+	}
+
+protected:
+	bool isKeeper;
+	uint8 status;
+	bool hasBall;
+	bool avoidsObstacle;
+
+	enum LogicalStatus
+	{
+		STATUS_KEEPER_TURN_RIGHT,
+		STATUS_KEEPER_TURN_LEFT,
+		STATUS_KEEPER_RETURN_HOME,
+		STATUS_KEEPER_HUNT_BALL,
+		STATUS_KEEPER_TURN_TO_BALL,
+		STATUS_KEEPER_DRIVE_TO_DEFEND,
+
+		STATUS_ATTACKER_TURN_TO_BALL,
+		STATUS_ATTACKER_DRIVE_TO_BALL,
+		STATUS_ATTACKER_SEARCHING_AT_HOME,
+		STATUS_ATTACKER_SEARCHING_AT_ENEMY,
+	};
+
+	void OnBallOwned();
+	void OnBallLost();
+
+	void OnBecomeKeeper();
+	void OnBecomeAttacker();
+
+public:
+	void Update();
+
+	bool IsKeeper()
+	{
+		return isKeeper;
+	}
+
+	bool IsAttacker()
+	{
+		return !isKeeper;
+	}
+
+	void SetKeeper(bool newStatus)
+	{
+		if(!this->isKeeper && newStatus)
+		{
+			this->OnBecomeKeeper();
+		}
+		else if(this->isKeeper && !newStatus)
+		{
+			this->OnBecomeAttacker();
+		}
+		this->isKeeper = newStatus;
+	}
+
+	bool HasBall()
+	{
+		return this->hasBall;
+	}
+
+	void UpdateKeeperMovement();
+	void UpdateAttackerMovement();
+};
+
+#endif
--- a/source/Concept/Framework/modules/wireless.c
+++ b/source/Concept/Framework/modules/wireless.c
@ -0,0 +1 @@
+#include "wireless.h"
--- a/source/Concept/Framework/modules/wireless.h
+++ b/source/Concept/Framework/modules/wireless.h
@ -0,0 +1,58 @@
+#ifndef _WIRELESS_H
+#define _WIRELESS_H
+
+#include "../../stdafx.h"
+
+class Wireless : public IO_Module
+{
+public:
+	Wireless()
+	{
+		this->parent = NULL;
+		this->moduleId = 0;
+	}
+
+	Wireless(uint32 wirelessId)
+	{
+		this->parent = NULL;
+		this->moduleId = wirelessId;
+
+		switch(wirelessId)
+		{
+			case IO_WIRELESS_MAIN:
+				uart_init(51); // 19200 Baud at 16MHz Atmel
+				break;
+			default:
+				break;
+		}
+	}
+
+protected:
+	uint8 transmitPower;
+public:
+	void SetTransmitPower(uint8 newTransmitPower)
+	{
+		this->transmitPower = newTransmitPower;
+		
+		char buffer[12];
+		ltoa(this->transmitPower-1, buffer, 10);
+		uart_puts(buffer);
+	}
+
+	void Send(char* message)
+	{
+		uart_puts(message);
+	}
+
+	void Send(uint8 message)
+	{
+		uart_putc(message);
+	}
+
+	int16 Recieve()
+	{
+		return uart_getc();
+	}
+};
+
+#endif
--- a/source/Concept/Framework/robot.c
+++ b/source/Concept/Framework/robot.c
@ -44,6 +44,9 @@ Robot::Robot()
 	//Activate interrupt
 	sei();

+	// Initialiate TWI
+	Init_TWI();
+
 	//Interface
 	for(uint8 i = IO_START; i < IO_END; i++)
 	{
@ -85,13 +88,17 @@ bool Robot::RemoveModule(IO_Module* oldModule)
 //-----------------------------------------------------------------------------
 void Robot::Update()
 {
-	GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN)->Update();
+	GetModule<Command_Handler>(IO_COMMAND_HANDLER_MAIN)->Update();

 	GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN)->Update();

-	GetModule<Navigator>(IO_NAVIGATOR_MAIN)->Update();
+	GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN)->Update();

-	//insert code here
+	GetModule<Obstacle_Tracker>(IO_OBSTACLE_TRACKER_MAIN)->Update();
+
+	//GetModule<Logic>(IO_LOGIC_MAIN)->Update();
+
+	GetModule<Navigator>(IO_NAVIGATOR_MAIN)->Update();
 }

 //-----------------------------------------------------------------------------
--- a/source/Concept/Framework/stdafx.h
+++ b/source/Concept/Framework/stdafx.h
@ -20,7 +20,9 @@
 #include "distance_sensor.h"
 #include "ir_sensor.h"
 #include "mouse_sensor.h"
+#include "command_handler.h"
 #include "position_tracker.h"
+#include "obstacle_tracker.h"
 #include "ball_tracker.h"
 #include "navigator.h"
 #include "logic.h"
Author	SHA1	Message	Date
sicarius	e1eb964ba7	Alte Organisationsdaten gel?scht	2007-02-28 21:26:05 +00:00
sicarius	ddbcf3789f	Alte Organisationsdaten gel?scht	2007-02-28 21:26:05 +00:00
sicarius	f61eb90087	SoccerTeam, Stuff from Magdeburg	2007-02-26 21:25:01 +00:00
masterm	343397ecf6	small corection	2007-02-22 23:49:04 +00:00
masterm	b18e19bee5	+++ implemented command handler	2007-02-22 23:37:00 +00:00
masterm	e51f1c2038	++++ fixed navigator logics	2007-02-22 23:04:02 +00:00
sicarius	d8e83400c8	Codestuff	2007-02-22 21:22:02 +00:00
sicarius	f544ab7822	The Last Day ?	2007-02-22 20:59:02 +00:00
meyma	dc8b1cde31	Additional Codework on Distance_Sensor	2007-02-22 14:27:02 +00:00
meyma	d7ac8f546a	We have a SRF10, not SRF05	2007-02-22 14:08:01 +00:00