Alte Organisationsdaten gel?scht

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€

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

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
-

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

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

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:		???

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"

source/Concept/Framework/atmega128io.c

@@ -580,6 +580,174 @@ void uart1_puts_p(const char *progmem_s )
       uart1_putc(c);
 
 }/* 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

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
@@ -178,6 +180,99 @@ extern void uart1_puts_p(const char *s );
 /** @brief  Macro to automatically put a string constant into program memory */
 #define uart1_puts_P(__s)       uart1_puts_p(PSTR(__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

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)

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,
 

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,143 +184,163 @@ 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->SetSpeed(1);
-	ourDribbler->SetEnabled(true);
+	//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);
+
+	//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_BACK))->SetSpeed(200);
+
+	//(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);
+
 	
-	float rotation = 0;
-	float speed = 200;
 
-	//Mouse_Sensor* mouse_left = localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);
+	//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetSpeed(200);
-	//Mouse_Sensor* mouse_right = localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);
+	//(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 % 20))
-				
-		if(!(i % 1))
 		{
 			ourDisplay->Clear();
-			//ourDisplay->Print(ourPosition_Tracker->GetPositionX(),1,2);
+			msleep(10);
-			//ourDisplay->Print(ourPosition_Tracker->GetPositionY(),1,3);
+			ourCommandHandler->PrintCommand();
-			//ourDisplay->Print(ourPosition_Tracker->GetOrientation() * 180.0f / PI,1,4);
 		}
-
-		if(!(i % 50))
-		{
-			ourAktuator->Kick();
-		}
-
-		
-		distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_0_DEG);
-		value = distanceSensor->GetDistance();
-		ourDisplay->Print(value, 1, 2);
-		ourDisplay->Print(";");
-		distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_90_DEG);
-		value = distanceSensor->GetDistance();
-		ourDisplay->Print(value);
-		ourDisplay->Print(";");
-		distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_180_DEG);
-		value = distanceSensor->GetDistance();
-		ourDisplay->Print(value, 1, 3);
-		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);
-		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(";");
-		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_60_DEG);
-		value = ourSensor->GetIRIntensity();
-		ourDisplay->Print(value);
-		ourDisplay->Print(";");
-		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_100_DEG);
-		value = ourSensor->GetIRIntensity();
-		ourDisplay->Print(value);
-		ourDisplay->Print(";");
-		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_180_DEG);
-		value = ourSensor->GetIRIntensity();
-		ourDisplay->Print(value, 1, 3);
-		ourDisplay->Print(";");
-		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_260_DEG);
-		value = ourSensor->GetIRIntensity();
-		ourDisplay->Print(value);
-		ourDisplay->Print(";");
-		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_300_DEG);
-		value = ourSensor->GetIRIntensity();
-		ourDisplay->Print(value);
-		ourDisplay->Print(";");
-		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_330_DEG);
-		value = ourSensor->GetIRIntensity();
-		ourDisplay->Print(value);
-		ourDisplay->Print(";");*/
 		
 		localRobot->Update();
 
-		//ourDisplay->Print(ourBallTracker->GetBallDirection() * 180.0f / PI, 1, 4);
+		if(ourCommandHandler->displayDistanceSensors && !(i % 20))
+		{
+			distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_0_DEG);
+			value = distanceSensor->GetDistance();
+			ourDisplay->Print(value, 1, 4);
+			ourDisplay->Print(";");
+			distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_90_DEG);
+			value = distanceSensor->GetDistance();
+			ourDisplay->Print(value);
+			ourDisplay->Print(";");
+			distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_180_DEG);
+			value = distanceSensor->GetDistance();
+			ourDisplay->Print(value);
+			ourDisplay->Print(";");
+			distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_270_DEG);
+			value = distanceSensor->GetDistance();
+			ourDisplay->Print(value);
+			ourDisplay->Print(";");
+		}
 
-		//ourNavigator->Drive(ourBallTracker->GetBallDirection() * 180.0f / PI, 0, 255, 0);
+		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, 6, 2);
+			ourDisplay->Print(";");
+			ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_60_DEG);
+			value = ourSensor->GetIRIntensity();
+			ourDisplay->Print(value, 11, 2);
+			ourDisplay->Print(";");
+			ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_100_DEG);
+			value = ourSensor->GetIRIntensity();
+			ourDisplay->Print(value, 16, 2);
+			ourDisplay->Print(";");
+			ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_180_DEG);
+			value = ourSensor->GetIRIntensity();
+			ourDisplay->Print(value, 1, 3);
+			ourDisplay->Print(";");
+			ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_260_DEG);
+			value = ourSensor->GetIRIntensity();
+			ourDisplay->Print(value, 6, 3);
+			ourDisplay->Print(";");
+			ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_300_DEG);
+			value = ourSensor->GetIRIntensity();
+			ourDisplay->Print(value, 11, 3);
+			ourDisplay->Print(";");
+			ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_330_DEG);
+			value = ourSensor->GetIRIntensity();
+			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);
+			}
+
+			if(ourBallTracker->HasBall()) {
+				ourDisplay->Print("Has Ball", 5, 4);
+			}
+		}
+
+		//if(!(i % 20))
+		//{
+		//	ourDisplay->Print(ourWireless->Recieve(), 10, 1);
+		//}
+
+		if(ourBallTracker->KnowsBallDirection())
+		{
+			ourNavigator->Drive(ourBallTracker->GetBallDirection() * 180.0f / PI, 0, 255, 0);
+		}
+		else
+		{
+			ourNavigator->Stop();
+		}*/
 	}
 
 	//Cleanup

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			
+}

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

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;
 
@@ -86,41 +86,95 @@ void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed,
 	}
 
 	CalculateDirection();
-}
+}
-
+
 //-----------------------------------------------------------------------------
-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)
+	return targetReached;
-	{
+}
-		targetAngle = EMPTY_FLOAT;
-		rotationSpeed = 0;
 
+//-----------------------------------------------------------------------------
+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()))
 	{
-		Stop();
+		if(this->rotationSpeed == 0 || (HasTargetAngle() && AngleReached()))
+		{
+			Stop();
+		}
+		else if(!AngleReached())
+		{
+			RotateTo(this->targetAngle, this->rotationSpeed);
+		}
+		else
+		{
+			Rotate(this->rotationSpeed);
+		}
 	}
-	else if(targetReached || targetAngleReached)
+	else
 	{
-		CalculateDirection();
+		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);

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

source/Concept/Framework/modules/input/distance_sensor.c

@@ -1,41 +1,159 @@
-#include "distance_sensor.h"
+#include "distance_sensor.h"
+
+//-----------------------------------------------------------------------------
+void Distance_Sensor::SetSignalFactor(uint8 factor)
+{
+  if(factor > 16)
+	{
+	 	factor = 16;
+	}
+
+	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;
+}
 
 //-----------------------------------------------------------------------------
-float Distance_Sensor::GetDistance()
+uint16 Distance_Sensor::GetDistance()
 {
-	uint32 result = 0;
+	return srf10_get_measure();
-
-	//(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)) 
-	{
-		result++;
-		asm volatile("nop");
-		asm volatile("nop");
-		asm volatile("nop");
-		asm volatile("nop");
-		asm volatile("nop");
-		asm volatile("nop");
-		asm volatile("nop");
-	}
-
-	return (float(result) * DISTANCE_PER_VALUE);
 }

source/Concept/Framework/modules/input/distance_sensor.h

@@ -2,7 +2,21 @@
 #define _DISTANCE_SENSOR_H
 
 #include "../../stdafx.h"
-#include "sensor.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
 {
@@ -10,11 +24,8 @@ public:
 	Distance_Sensor()
 	{
 		this->parent = NULL;
-		this->moduleId = 0;
+		this->moduleId = 0;
-		this->hardwarePort = NULL;
+		this->slaveAddr = 0; // Set the I2C Slave-Adress
-		this->hardwareDDR = NULL;
-		this->hardwarePin = NULL;
-		this->pin = 0;
 	}
 
 	Distance_Sensor(uint32 sensorId)
@@ -24,48 +35,59 @@ public:
 
 		switch(sensorId)
 		{
-			case IO_SENSOR_DISTANCE_0_DEG:
+			case IO_SENSOR_DISTANCE_0_DEG:
-				this->hardwarePort = &PORTC;
+				this->slaveAddr = 0xE0; // Set the I2C Slave-Adress
-				this->hardwareDDR = &DDRC;
-				this->hardwarePin = &PINC;
-				this->pin = (1 << 0);
 				break;
-			case IO_SENSOR_DISTANCE_90_DEG:
+			case IO_SENSOR_DISTANCE_90_DEG:
-				this->hardwarePort = &PORTC;
+				this->slaveAddr = 0xE2;
-				this->hardwareDDR = &DDRC;
-				this->hardwarePin = &PINC;
-				this->pin = (1 << 1);
 				break;
-			case IO_SENSOR_DISTANCE_180_DEG:
+			case IO_SENSOR_DISTANCE_180_DEG:
-				this->hardwarePort = &PORTC;
+				this->slaveAddr = 0xE4;
-				this->hardwareDDR = &DDRC;
-				this->hardwarePin = &PINC;
-				this->pin = (1 << 2);
 				break;
-			case IO_SENSOR_DISTANCE_270_DEG:
+			case IO_SENSOR_DISTANCE_270_DEG:
-				this->hardwarePort = &PORTC;
+				this->slaveAddr = 0xE6;
-				this->hardwareDDR = &DDRC;
-				this->hardwarePin = &PINC;
-				this->pin = (1 << 3);
 				break;
-			default:
+			default:
-				this->hardwarePort = NULL;
+				this->slaveAddr = 0;
-				this->hardwareDDR = NULL;
-				this->hardwarePin = NULL;
-				this->pin = 0;
 				break;
 		}
+
+		//SetRange(100);
+		SetRange(2000);
+		SetSignalFactor(0x06);
 	}
 
 protected:
-	//Hardware
+	//Hardware
-	volatile uint8* hardwarePort;
+	uint8 slaveAddr;
-	volatile uint8* hardwareDDR;
+
-	volatile uint8* hardwarePin;
+	void SetSignalFactor(uint8 factor);
-	uint8 pin;
+
+	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

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;

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

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
 	{

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);
 	}
 };
 

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);
+	}
+}

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

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 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 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);
 
 	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);*/
 }

source/Concept/Framework/modules/interpreter/position_tracker.h

@@ -40,19 +40,21 @@ public:
 	}
 
 	// returns x-koordinate in mm
-	int GetPositionX() 	{
+	int16 GetPositionX() 
-		return (int)((positionX*10.0f)/TICKS_PER_CM);
+	{
+		return (int16)((positionX*10.0f)/TICKS_PER_CM);
 	}
 
 	// returns y-koordinate in mm
-	int GetPositionY() 	{
+	int16 GetPositionY() 	
-		return (int)((positionY*10.0f)/TICKS_PER_CM);
+	{
+		return (int16)((positionY*10.0f)/TICKS_PER_CM);
 	}
 
 	// returns orientation
-	float GetOrientation()	{
+	float GetOrientation()
-		return 0.0f; //tmp!!!!!!!!!!
+	{
-		//return orientation;
+		return orientation;
 	}
 };
 

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);
+		}
+	}
+}

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

source/Concept/Framework/modules/wireless.c

@@ -0,0 +1 @@
+#include "wireless.h"

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

source/Concept/Framework/robot.c

@@ -42,7 +42,10 @@ Robot::Robot()
 	TCCR3B =  (1<<ICNC3) | (1<<CS31); // set clock/prescaler 1/8 -> enable counter
 
 	//Activate interrupt
-	sei();
+	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();
 }
 
 //-----------------------------------------------------------------------------

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"