+++ Additional Codework

source/Concept/Framework/default/Makefile

@@ -13,7 +13,7 @@ COMMON = -mmcu=$(MCU)
 
 ## Compile options common for all C compilation units.
 CFLAGS = $(COMMON)
-CFLAGS += -Wall -gdwarf-2     -DF_CPU=16000000ULUL  -O3 -fsigned-char
+CFLAGS += -Wall -gdwarf-2 -O3 -fsigned-char 
 CFLAGS += -MD -MP -MT $(*F).o -MF dep/$(@F).d 
 
 ## Assembly specific flags
@@ -33,8 +33,11 @@ HEX_EEPROM_FLAGS += --set-section-flags=.eeprom="alloc,load"
 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" 
+
 ## Objects that must be built in order to link
-OBJECTS = main.o sensor.o tools.o atmega128io.o display.o keyboard.o distance_sensor.o mouse_sensor.o engine.o dribbler.o io_module.o ir_sensor.o kicker.o led.o robot.o position_tracker.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 
 
 ## Objects explicitly added by the user
 LINKONLYOBJECTS = 
@@ -43,49 +46,58 @@ LINKONLYOBJECTS =
 all: $(TARGET) RoboCode.hex RoboCode.eep size
 
 ## Compile
-main.o: ../main.cpp
+io_module.o: ../modules/io_module.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
 
-tools.o: ../tools.cpp
+atmega128io.o: ../atmega128io.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
 
-io_module.o: ../io_module.cpp
+main.o: ../main.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
 
-robot.o: ../robot.cpp
+robot.o: ../robot.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
 
-sensor.o: ../sensor.cpp
+tools.o: ../tools.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
 
-engine.o: ../engine.cpp
+distance_sensor.o: ../modules/input/distance_sensor.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
 
-led.o: ../led.cpp
+ir_sensor.o: ../modules/input/ir_sensor.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
 
-display.o: ../display.cpp
+keyboard.o: ../modules/input/keyboard.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
 
-keyboard.o: ../keyboard.cpp
+mouse_sensor.o: ../modules/input/mouse_sensor.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
 
-ir_sensor.o: ../ir_sensor.cpp
+sensor.o: ../modules/input/sensor.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
 
-distance_sensor.o: ../distance_sensor.cpp
+position_tracker.o: ../modules/interpreter/position_tracker.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
 
-mouse_sensor.o: ../mouse_sensor.cpp
+display.o: ../modules/output/display.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
 
-kicker.o: ../kicker.cpp
+dribbler.o: ../modules/output/dribbler.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
 
-dribbler.o: ../dribbler.cpp
+engine.o: ../modules/output/engine.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
 
-atmega128io.o: ../atmega128io.cpp
+kicker.o: ../modules/output/kicker.c
+	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
+
+led.o: ../modules/output/led.c
+	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
+
+ball_tracker.o: ../modules/interpreter/ball_tracker.c
+	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
+
+navigator.o: ../modules/executor/navigator.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
 
 ##Link

source/Concept/Framework/defines.h

@@ -58,6 +58,7 @@
 //Constants
 #define SPEED_PER_PWM 1
 #define DISTANCE_PER_VALUE 1
+#define PI 3.14159265f
 
 //IO Module Names
 enum IOModuleNames
@@ -142,8 +143,24 @@ enum IOModuleNames
 
 	IO_POSITION_TRACKER_END,
 
+	//Position Tracker
+
+	IO_BALL_TRACKER_START = IO_POSITION_TRACKER_END,
+
+	IO_BALL_TRACKER_MAIN,
+
+	IO_BALL_TRACKER_END,
+
+	//Navigators
+
+	IO_NAVIGATOR_START = IO_BALL_TRACKER_END,
+
+	IO_NAVIGATOR_MAIN,
+
+	IO_NAVIGATOR_END,
+
 	//General
-	IO_END = IO_POSITION_TRACKER_END,
+	IO_END = IO_NAVIGATOR_END,
 };
 
 #endif

source/Concept/Framework/main.c

@@ -102,11 +102,38 @@ int main()
 		newKeyboard = NULL;
 	}
 
-	Keyboard* ourKeyboard = localRobot->GetModule<Keyboard>(IO_KEYBOARD_MAIN);
+	//Init Position Tracker
+	for(uint8 i = IO_POSITION_TRACKER_START; i < IO_POSITION_TRACKER_END; i++)
+	{
+		Position_Tracker* newPositionTracker = new Position_Tracker(i);
+		localRobot->AddModule(newPositionTracker);
+		newPositionTracker = NULL;
+	}
+
+	//Init Ball Tracker
+	for(uint8 i = IO_BALL_TRACKER_START; i < IO_BALL_TRACKER_END; i++)
+	{
+		Ball_Tracker* newBallTracker = new Ball_Tracker(i);
+		localRobot->AddModule(newBallTracker);
+		newBallTracker = NULL;
+	}
+
+	//Init Navigators
+	for(uint8 i = IO_NAVIGATOR_START; i < IO_NAVIGATOR_END; i++)
+	{
+		Navigator* newNavigator = new Navigator(i);
+		localRobot->AddModule(newNavigator);
+		newNavigator = NULL;
+	}
+
+	
+
 	IR_Sensor* ourSensor = NULL;
 	uint16 value = 0;
+	int8 value2 = 0;
 	Display* ourDisplay = localRobot->GetModule<Display>(IO_DISPLAY_MAIN);
 	uint32 i = 1;
+	Ball_Tracker* ourBallTracker = localRobot->GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN);
 
 	//Run
 	while(true)
@@ -114,7 +141,6 @@ int main()
 		msleep(500);
 		ourDisplay->Clear();
 		ourDisplay->Print(i++);
-		ourDisplay->NewLine();
 		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_0_DEG);
 		value = ourSensor->GetIRIntensity();
 		ourDisplay->Print(value, 1, 2);
@@ -147,9 +173,14 @@ int main()
 		value = ourSensor->GetIRIntensity();
 		ourDisplay->Print(value);
 		ourDisplay->Print(";");
-		ourDisplay->Print(ourKeyboard->GetInput(), 1, 4);
+		value2 = (localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT))->GetXMovement();
+		ourDisplay->Print(value2, 10, 4);
+		value2 = (localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT))->GetYMovement();
+		ourDisplay->Print(value2, 15, 4);
 
 		localRobot->Update();
+
+		ourDisplay->PrintFloat(ourBallTracker->GetBallDirection(), 1, 4);
 	}
 
 	//Cleanup

source/Concept/Framework/modules/executor/navigator.c

@@ -0,0 +1,199 @@
+#include "navigator.h"
+
+//-----------------------------------------------------------------------------
+void Navigator::Stop()
+{	
+	this->direction = -1.0f;
+	this->targetAngle = -1.0f;
+	this->targetX = -1.0f;
+	this->targetY = -1.0f;
+	this->speed = 0;
+	this->rotationSpeed = 0;
+
+	for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++)
+	{
+		(parent->GetModule<Engine>(i))->SetSpeed(0);
+		(parent->GetModule<Engine>(i))->SetEnabled(true);
+	}
+}
+
+//-----------------------------------------------------------------------------
+void Navigator::Rotate(float rotationSpeed)
+{
+	this->rotationSpeed = rotationSpeed;
+	this->direction = -1.0f;
+	this->targetAngle = -1.0f;
+	this->targetX = -1.0f;
+	this->targetY = -1.0f;
+	this->speed = 0;
+
+	for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++)
+	{
+		(parent->GetModule<Engine>(i))->SetSpeed(rotationSpeed);
+		(parent->GetModule<Engine>(i))->SetEnabled(true);
+	}
+}
+
+//-----------------------------------------------------------------------------
+void Navigator::Update()
+{
+	Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
+
+	bool hasDistanceToDrive = true;
+	bool hasOrientationToAdjust = true;
+
+	//Check for distance to drive
+	if((targetX >= 0) != (targetY >= 0))
+	{
+		targetX = -1.0f;
+		targetY = -1.0f;
+
+		hasDistanceToDrive = false;
+	}
+	else if(targetX >= 0 && targetY >= 0)
+	{
+		if(distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < 1.0f)
+		{
+			targetX = -1.0f;
+			targetY = -1.0f;
+
+			hasDistanceToDrive = false;
+		}
+		else
+		{
+			hasDistanceToDrive = true;
+		}
+	}
+	else
+	{
+		if(direction >= 0)
+		{
+			hasDistanceToDrive = true;
+		}
+		else
+		{
+			hasDistanceToDrive = false;
+		}
+	}
+
+	//Check for orientation to adjust
+	if(targetAngle >= 0)
+	{
+		if(fabs(targetAngle - locationeer->GetOrientation()) < 0.1f)
+		{
+			hasOrientationToAdjust = false;
+		}
+		else
+		{
+			hasOrientationToAdjust = true;
+		}
+	}
+	else
+	{
+		if(rotationSpeed != 0)
+		{
+			hasOrientationToAdjust = true;
+		}
+		else
+		{
+			hasOrientationToAdjust = false;
+		}
+	}
+
+	//Calculate directional/rotational engine speed
+	if(hasDistanceToDrive)
+	{
+
+
+		float maxRobotSpeed = 255.0f * sqrt(2) / 2.0f;
+
+		if(speed > maxRobotSpeed)
+		{
+			speed = maxRobotSpeed;
+		}
+
+		maxMotorSpeed = speed / (sqrt(2) / 2);
+
+
+
+		relAngel = direction - orientation
+		
+		robotSpeed = sin(45) * maxMotorSpeed
+		maxMotorSpeed = robotSpeed / sin(45)
+
+		if(relAngel > 45)
+		{
+			sin(relAngel) * (speed / sin(45)) * sin(relAngel + 45);
+
+			back = speed / sin(relAngel);
+		}
+		else
+		{
+			
+		}
+
+		left = 
+		back = sin(relAngel) * speed
+
+
+		direction = 0:
+		orientation = 0:
+		left = speed
+		right = -speed
+		back = 0
+
+		direction = 0:
+		orientation = 90:
+		left = speed
+		right = speed
+		back = (sinVcos)(45) * speed
+
+		direction = 0:
+		orientation = 45:
+		left = speed
+		right = 0
+		back = -speed
+
+		direction = 0:
+		orientation = 180:
+		left = -speed
+		right = speed
+		back = 0
+
+	}
+	else if(!hasOrientationToAdjust)
+	{
+		Stop();
+	}
+	else
+	{
+	}
+}
+
+// Aktualieren ohne Parameter
+/*void Navigator::Update() {	
+	// Richtung in x und y-Kompontente zerlegen
+	double y = cos((double)direction*0.01745); // richtung ist winkel
+	double x = sin((double)direction*0.01745);
+	
+	// Abweichung der Ausrichtung ermitteln(als winkel)
+	int w = sensor.GetAusrichtung() - ausrichtung;
+
+	// Stärke der einzelnen Motoren berechnen	
+	double v0 = (-x+sqrt(3)*y)/2;
+	double v1 = x;
+	double v2 = (-x-sqrt(3)*y)/2;
+
+	// Ausgerechnete Stärke an die Motoren übergeben
+	board.motor(0,(int)((double)v0*speed +w));
+	board.motor(1,(int)((double)v1*speed +w));
+	board.motor(2,(int)((double)v2*speed +w));
+}
+
+// Aktualieren mit allen Parametern
+void Navigator::Drive(float newDirection, float newAngle, float newSpeed) {
+	SetDirection(newDirection);
+	SetAngle(newAngle);
+	SetSpeed(newSpeed);
+	Update(); // Und anwenden
+}*/

source/Concept/Framework/modules/executor/navigator.h

@@ -0,0 +1,53 @@
+#ifndef _NAVIGATOR_H
+#define _NAVIGATOR_H
+
+#include "../../stdafx.h"
+
+class Navigator : public IO_Module
+{
+public:
+	Navigator()
+	{
+		this->parent = NULL;
+		this->moduleId = 0;
+		this->direction = -1.0f;
+		this->targetAngle = -1.0f;
+		this->targetX = -1.0f;
+		this->targetY = -1.0f;
+		this->speed = 0;
+		this->rotationSpeed = 0;
+	}
+
+	Navigator(uint32 navigatorId)
+	{
+		this->parent = NULL;
+		this->moduleId = navigatorId;
+		this->direction = -1.0f;
+		this->targetAngle = -1.0f;
+		this->targetX = -1.0f;
+		this->targetY = -1.0f;
+		this->speed = 0;
+		this->rotationSpeed = 0;
+	}
+
+protected:
+	float direction; 
+	float targetAngle;
+	float targetX;
+	float targetY;
+	float speed;
+	float rotationSpeed;
+
+public:
+	void Update();	
+
+	void Stop();
+
+	void Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed);
+
+	void DriveTo(float newX, float newY, float newAngle);
+
+	void Rotate(float rotationSpeed);
+};
+
+#endif

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

@@ -9,11 +9,15 @@ float Distance_Sensor::GetDistance()
 	*hardwareDDR |= pin;//Set pin output
 	*hardwarePort |= pin;//Activate port
 	usleep(10);//Wait for 10µs
-	*hardwarePort &= ~pin;//Deactivate port
+	//*hardwarePort &= ~pin;//Deactivate port
 	*hardwareDDR &= ~pin;//Set pin input
 
+	(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("pre 1", 4, 1);
+
 	//Wait for response
-	while(!(*hardwarePin & pin));
+	while(!(PINC & pin)){asm volatile("nop");}
+
+	(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("pre 2", 4, 1);
 	
 	//Calculate duration of response
 	while(*hardwarePin & pin) 
@@ -22,5 +26,7 @@ float Distance_Sensor::GetDistance()
 		asm volatile("nop");
 	}
 
+	(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("pre 3", 4, 1);
+
 	return (float(result) * DISTANCE_PER_VALUE);
 }

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

@@ -0,0 +1,71 @@
+#ifndef _DISTANCE_SENSOR_H
+#define _DISTANCE_SENSOR_H
+
+#include "../../stdafx.h"
+#include "sensor.h"
+
+class Distance_Sensor : public Sensor
+{
+public:
+	Distance_Sensor()
+	{
+		this->parent = NULL;
+		this->moduleId = 0;
+		this->hardwarePort = NULL;
+		this->hardwareDDR = NULL;
+		this->hardwarePin = NULL;
+		this->pin = 0;
+	}
+
+	Distance_Sensor(uint32 sensorId)
+	{
+		this->parent = NULL;
+		this->moduleId = sensorId;
+
+		switch(sensorId)
+		{
+			case IO_SENSOR_DISTANCE_0_DEG:
+				this->hardwarePort = &PORTC;
+				this->hardwareDDR = &DDRC;
+				this->hardwarePin = &PINC;
+				this->pin = (1 << 0);
+				break;
+			case IO_SENSOR_DISTANCE_90_DEG:
+				this->hardwarePort = &PORTC;
+				this->hardwareDDR = &DDRC;
+				this->hardwarePin = &PINC;
+				this->pin = (1 << 1);
+				break;
+			case IO_SENSOR_DISTANCE_180_DEG:
+				this->hardwarePort = &PORTC;
+				this->hardwareDDR = &DDRC;
+				this->hardwarePin = &PINC;
+				this->pin = (1 << 2);
+				break;
+			case IO_SENSOR_DISTANCE_270_DEG:
+				this->hardwarePort = &PORTC;
+				this->hardwareDDR = &DDRC;
+				this->hardwarePin = &PINC;
+				this->pin = (1 << 3);
+				break;
+			default:
+				this->hardwarePort = NULL;
+				this->hardwareDDR = NULL;
+				this->hardwarePin = NULL;
+				this->pin = 0;
+				break;
+		}
+	}
+
+protected:
+	//Hardware
+	volatile uint8* hardwarePort;
+	volatile uint8* hardwareDDR;
+	volatile uint8* hardwarePin;
+	uint8 pin;
+
+public:
+	float GetDistance();
+};
+
+#endif

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

@@ -0,0 +1,62 @@
+#ifndef _IR_SENSOR_H
+#define _IR_SENSOR_H
+
+#include "../../defines.h"
+#include "../../robot.h"
+#include "sensor.h"
+
+class IR_Sensor : public Sensor
+{
+public:
+	IR_Sensor()
+	{
+		this->parent = NULL;
+		this->moduleId = 0;
+	}
+
+	IR_Sensor(uint32 sensorId)
+	{
+		this->parent = NULL;
+		this->moduleId = sensorId;
+
+		switch(sensorId)
+		{
+			case IO_SENSOR_IR_0_DEG:
+				this->channel = 0;
+				break;
+			case IO_SENSOR_IR_30_DEG:
+				this->channel = 1;
+				break;
+			case IO_SENSOR_IR_60_DEG:
+				this->channel = 2;
+				break;
+			case IO_SENSOR_IR_100_DEG:
+				this->channel = 3;
+				break;
+			case IO_SENSOR_IR_180_DEG:
+				this->channel = 4;
+				break;
+			case IO_SENSOR_IR_260_DEG:
+				this->channel = 5;
+				break;
+			case IO_SENSOR_IR_300_DEG:
+				this->channel = 6;
+				break;
+			case IO_SENSOR_IR_330_DEG:
+				this->channel = 7;
+				break;
+			default:
+				this->channel = 8;
+				break;
+		}
+	}
+
+protected:
+	//Hardware
+	uint8 channel;
+
+public:
+	uint16 GetIRIntensity();
+};
+
+#endif

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

@@ -0,0 +1,87 @@
+#ifndef _KEYBOARD_H
+#define _KEYBOARD_H
+
+#include "../../stdafx.h"
+
+class Keyboard : public IO_Module
+{
+public:
+	Keyboard()
+	{
+		this->parent = NULL;
+		this->moduleId = 0;
+		this->commandSetting = 0;
+		this->settingClearBuffer = 0;
+	}
+
+	Keyboard(uint32 keyboardId)
+	{
+		this->parent = NULL;
+		this->moduleId = keyboardId;
+		
+		switch(keyboardId)
+		{
+			case IO_KEYBOARD_MAIN:
+				this->commandSetting = 27;
+				this->settingClearBuffer = 123;
+				break;
+			default:
+				this->commandSetting = 0;
+				this->settingClearBuffer = 0;
+				break;
+		}
+	}
+
+protected:
+	//Commands
+	uint8 commandSetting;
+	//Settings
+	uint8 settingClearBuffer;
+
+	void SendCommand(uint8 newCommand)
+	{
+		switch(moduleId)
+		{
+			case IO_KEYBOARD_MAIN:
+				uart1_putc(newCommand);
+				break;
+			default:
+				break;
+		}
+	}
+
+public:
+	uint8 GetInput()
+	{
+		uint16 input = uart1_getc();
+
+		if(input == 0x100)//no data
+		{
+			return 0xEE;//empty
+		}
+		else if(input >= '0' && input <= '9')
+		{
+			return (uint8)(input - '0');
+		}
+		else if(input == '*')
+		{
+			return 10;
+		}
+		else if(input == '#')
+		{
+			return 11;
+		}
+		else
+		{
+			return 0xFF;//unknown
+		}
+	}
+
+	void ClearKeyBuffer()
+	{
+		SendCommand(commandSetting);
+		SendCommand(settingClearBuffer);
+	}
+};
+
+#endif

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

@@ -0,0 +1,199 @@
+#ifndef _MOUSE_SENSOR_H
+#define _MOUSE_SENSOR_H
+
+#include "../../stdafx.h"
+#include "sensor.h"
+
+class Mouse_Sensor : public Sensor
+{
+public:
+	Mouse_Sensor()
+	{
+		this->parent = NULL;
+		this->moduleId = 0;
+		this->hardwarePort = NULL;
+		this->hardwareDDR = NULL;
+		this->hardwarePin = NULL;
+		this->pinSDA = 0;
+		this->pinSCK = 0;
+		this->registerConfig = 0;
+		this->registerPixelData = 0;
+		this->registerSqual = 0;
+		this->registerDeltaX = 0;
+		this->registerDeltaY = 0;
+		this->configReset = 0;
+		this->configAwake = 0;
+		this->newImage = false;
+	}
+
+	Mouse_Sensor(uint32 sensorId)
+	{
+		this->parent = NULL;
+		this->moduleId = sensorId;
+		this->newImage = false;
+
+		switch(sensorId)
+		{
+			case IO_SENSOR_MOUSE_LEFT:
+				this->hardwarePort = &PORTC;
+				this->hardwareDDR = &DDRC;
+				this->hardwarePin = &PINC;
+				this->pinSDA = (1 << 4);
+				this->pinSCK = (1 << 6);
+				this->registerConfig = 0x00;
+				this->registerPixelData = 0x08;
+				this->registerSqual = 0x4;
+				this->registerDeltaX = 0x3;
+				this->registerDeltaY = 0x2;
+				this->configReset = 0x80;
+				this->configAwake = 0x01;
+				break;
+			case IO_SENSOR_MOUSE_RIGHT:
+				this->hardwarePort = &PORTC;
+				this->hardwareDDR = &DDRC;
+				this->hardwarePin = &PINC;
+				this->pinSDA = (1 << 5);
+				this->pinSCK = (1 << 7);
+				this->registerConfig = 0x00;
+				this->registerPixelData = 0x08;
+				this->registerSqual = 0x4;
+				this->registerDeltaX = 0x3;
+				this->registerDeltaY = 0x2;
+				this->configReset = 0x80;
+				this->configAwake = 0x01;
+				break;
+			default:
+				this->hardwarePort = NULL;
+				this->hardwareDDR = NULL;
+				this->hardwarePin = NULL;
+				this->pinSDA = 0;
+				this->pinSCK = 0;
+				this->registerConfig = 0;
+				this->registerPixelData = 0;
+				this->registerSqual = 0;
+				this->registerDeltaX = 0;
+				this->registerDeltaY = 0;
+				this->configReset = 0;
+				this->configAwake = 0;
+				break;
+		}
+
+		*hardwareDDR  |= pinSCK;
+		*hardwarePort &= ~pinSCK;
+	
+		Write(registerConfig, configReset);
+		Write(registerConfig, configAwake);
+	}
+
+protected:
+	//Hardware
+	volatile uint8* hardwarePort;
+	volatile uint8* hardwareDDR;
+	volatile uint8* hardwarePin;
+	uint8 pinSDA;
+	uint8 pinSCK;
+	bool newImage;
+	//Registers and Settings
+	uint8 registerConfig;
+	uint8 registerPixelData;
+	uint8 registerSqual;
+	uint8 registerDeltaX;
+	uint8 registerDeltaY;
+	uint8 configReset;
+	uint8 configAwake;
+
+public:
+	void WriteByte(uint8 newByte)
+	{
+		*hardwareDDR |= pinSDA;//Set SDA output
+	
+		for(uint8 i = 0; i < 8; i++)
+		{
+			*hardwarePort &= ~pinSCK;//prepare SCK
+		
+			//write data
+			*hardwarePort = (*hardwarePort & (~(*hardwarePin))) | 
+				((newByte >> 7) * pinSDA);
+
+			newByte = newByte << 1;//prepare next byte
+			asm volatile("nop");
+		
+			*hardwarePort |= pinSCK;
+		}
+	}
+
+	void Write(int8 adr, uint8 data)
+	{	
+		WriteByte(adr | 0x80);
+		WriteByte(data);
+		usleep(100);
+	}
+
+	uint8 ReadByte()
+	{
+		uint8 data=0;
+
+		*hardwareDDR  &= ~pinSDA;//Set SDA input
+
+		for(uint8 i = 0; i < 8; i++)
+		{
+			*hardwarePort &= ~pinSCK;//Prepare data
+			data = data << 1;
+
+			asm volatile("nop");
+			*hardwarePort |= pinSCK;//Prepare for reading
+
+			data |= (*hardwarePin & pinSDA) / pinSDA;
+		}
+
+		return data;
+	}
+
+	uint8 Read(uint8 adr)
+	{
+		WriteByte(adr);
+		usleep(100);
+	
+		return ReadByte();
+	}
+
+	void ImagePrepare()
+	{
+		Write(registerConfig, configAwake);
+		Write(registerPixelData, 0x00);
+
+		newImage = true;
+	}
+
+	uint8 ImageRead()
+	{
+		uint8 pixel = Read(registerPixelData);
+		if(newImage)
+		{
+			while (!(pixel & 0x80))//0x80 indicates first pixel
+			{
+				pixel=Read(registerPixelData);
+			}
+			newImage = false;
+		}
+	
+		return pixel;
+	}
+
+	uint8 GetSqual()
+	{
+		return Read(registerSqual);
+	}
+
+	int8 GetXMovement()
+	{
+		return (int8)(Read(registerDeltaX));
+	}
+
+	int8 GetYMovement()
+	{
+		return (int8)(Read(registerDeltaY));
+	}
+};
+
+#endif

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

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

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

@@ -0,0 +1,27 @@
+#ifndef _SENSOR_H
+#define _SENSOR_H
+
+#include "../../defines.h"
+#include "../io_module.h"
+
+class Sensor : public IO_Module
+{
+public:
+	Sensor()
+	{
+		this->parent = NULL;
+		this->moduleId = 0;
+	}
+
+	Sensor(uint32 sensorId)
+	{
+		this->parent = NULL;
+		this->moduleId = sensorId;
+	}
+
+protected:
+
+public:
+};
+
+#endif

source/Concept/Framework/modules/interpreter/ball_tracker.c

@@ -0,0 +1,125 @@
+#include "ball_tracker.h"
+
+//-----------------------------------------------------------------------------
+void Ball_Tracker::Update()
+{
+	uint8 sensorCount = (IO_SENSOR_IR_330_DEG - IO_SENSOR_IR_0_DEG) + 1;
+	uint16 intensity[sensorCount];
+	uint8 greatestIntensity = 0;
+	for(uint8 i = 0; i < sensorCount; i++)
+	{
+		IR_Sensor* currentSensor = parent->GetModule<IR_Sensor>(i + IO_SENSOR_IR_0_DEG);
+
+		intensity[i] = 1023 - currentSensor->GetIRIntensity();
+
+		if(intensity[i] < 24)
+		{
+			intensity[i] = 0;
+		}
+		
+		if(intensity[i] > intensity[greatestIntensity])
+		{
+			greatestIntensity = i;
+		}
+	}
+
+	if(intensity[greatestIntensity])
+	{
+		uint8 secondIntensity = 0xFF;
+		uint8 leftSensor = (greatestIntensity + 1) % sensorCount;
+		uint8 rightSensor = (greatestIntensity + sensorCount - 1) % sensorCount;
+
+		if(intensity[leftSensor])
+		{
+			secondIntensity = leftSensor;
+		}
+
+		if(intensity[rightSensor] > intensity[leftSensor])
+		{
+			secondIntensity = rightSensor;
+		}
+
+		float mainDirection;
+		
+		switch(greatestIntensity + IO_SENSOR_IR_0_DEG)
+		{
+			case IO_SENSOR_IR_0_DEG:
+				mainDirection = 0;
+				break;
+			case IO_SENSOR_IR_30_DEG:
+				mainDirection = 1.0f * PI / 6.0f;
+				break;
+			case IO_SENSOR_IR_60_DEG:
+				mainDirection = 1.0f * PI / 3.0f;
+				break;
+			case IO_SENSOR_IR_100_DEG:
+				mainDirection = 5.0f * PI / 9.0f;
+				break;
+			case IO_SENSOR_IR_180_DEG:
+				mainDirection = PI;
+				break;
+			case IO_SENSOR_IR_260_DEG:
+				mainDirection = 13.0f * PI / 9.0f;
+				break;
+			case IO_SENSOR_IR_300_DEG:
+				mainDirection = 15.0f * PI / 9.0f;
+				break;
+			case IO_SENSOR_IR_330_DEG:
+				mainDirection = 33.0f * PI / 18.0f;
+				break;
+			default:
+				mainDirection = -1.0f;
+				return;
+				break;
+		}
+
+		if(secondIntensity != 0xFF)
+		{
+			float secondDirection;
+
+			switch(secondIntensity + IO_SENSOR_IR_0_DEG)
+			{
+				case IO_SENSOR_IR_0_DEG:
+					secondDirection = 0;
+					break;
+				case IO_SENSOR_IR_30_DEG:
+					secondDirection = 1.0f * PI / 6.0f;
+					break;
+				case IO_SENSOR_IR_60_DEG:
+					secondDirection = 1.0f * PI / 3.0f;
+					break;
+				case IO_SENSOR_IR_100_DEG:
+					secondDirection = 5.0f * PI / 9.0f;
+					break;
+				case IO_SENSOR_IR_180_DEG:
+					secondDirection = PI;
+					break;
+				case IO_SENSOR_IR_260_DEG:
+					secondDirection = 13.0f * PI / 9.0f;
+					break;
+				case IO_SENSOR_IR_300_DEG:
+					secondDirection = 15.0f * PI / 9.0f;
+					break;
+				case IO_SENSOR_IR_330_DEG:
+					secondDirection = 33.0f * PI / 18.0f;
+					break;
+				default:
+					secondDirection = -1.0f;
+					return;
+					break;
+			}
+
+			direction = (intensity[greatestIntensity] * mainDirection + 
+									intensity[secondIntensity] * secondDirection) / 
+									(intensity[greatestIntensity] + intensity[secondIntensity]);
+		}
+		else
+		{
+			direction = mainDirection;
+		}
+	}
+	else
+	{
+		direction = -1.0f;
+	}
+}

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

@@ -0,0 +1,35 @@
+#ifndef _BALL_TRACKER_H
+#define _BALL_TRACKER_H
+
+#include "../../stdafx.h"
+
+class Ball_Tracker : public IO_Module
+{
+public:
+	Ball_Tracker()
+	{
+		this->parent = NULL;
+		this->moduleId = 0;
+		this->direction = -1.0f;
+	}
+
+	Ball_Tracker(uint32 trackerId)
+	{
+		this->parent = NULL;
+		this->moduleId = trackerId;
+		this->direction = -1.0f;
+	}
+
+protected:
+	float direction;
+
+public:
+	void Update();
+
+	float GetBallDirection()
+	{
+		return direction;
+	}
+};
+
+#endif

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

@@ -0,0 +1,45 @@
+#ifndef _POSITION_TRACKER_H
+#define _POSITION_TRACKER_H
+
+#include "../../stdafx.h"
+
+class Position_Tracker : public IO_Module
+{
+public:
+	Position_Tracker()
+	{
+		this->parent = NULL;
+		this->moduleId = 0;
+	}
+
+	Position_Tracker(uint32 trackerId)
+	{
+		this->parent = NULL;
+		this->moduleId = trackerId;
+	}
+
+protected:
+	float positionX;
+	float positionY;
+	float orientation;
+
+public:
+	void Update();
+
+	float GetPositionX()
+	{
+		return positionX;
+	}
+
+	float GetPositionY()
+	{
+		return positionY;
+	}
+
+	float GetOrientation()
+	{
+		return orientation;
+	}
+};
+
+#endif

source/Concept/Framework/modules/io_module.h

@@ -0,0 +1,45 @@
+#ifndef _MODULE_H
+#define _MODULE_H
+
+#include "../defines.h"
+#include "../tools.h"
+
+class Robot;
+
+class IO_Module
+{
+public:
+	IO_Module()
+	{
+		this->parent = NULL; 
+		this->moduleId = 0;
+	}
+
+	IO_Module(uint32 moduleId)
+	{
+		this->parent = NULL;
+		this->moduleId = moduleId;
+	}
+
+protected:
+	Robot* parent;
+	uint32 moduleId;
+
+public:
+	Robot* GetParent()
+	{
+		return parent;
+	}
+
+	void SetParent(Robot* newParent)
+	{
+		parent = newParent;
+	}
+
+	uint32 GetId()
+	{
+		return moduleId;
+	}
+};
+
+#endif

source/Concept/Framework/modules/output/display.h

@@ -0,0 +1,198 @@
+#ifndef _DISPLAY_H
+#define _DISPLAY_H
+
+#include "../../stdafx.h"
+
+class Display : public IO_Module
+{
+public:
+	Display()
+	{
+		this->parent = NULL;
+		this->moduleId = 0;
+		this->cursorVisible = false;
+		this->illuminationEnabled = true;
+		this->commandClear = 0;
+		this->commandReturnCursor = 0;
+		this->commandNewLine = 0;
+		this->commandSetting = 0;
+		this->settingCursorVisible = 0;
+		this->settingIllumination = 0;
+		this->settingCursorPosition = 0;
+	}
+
+	Display(uint32 displayId)
+	{
+		this->parent = NULL;
+		this->moduleId = displayId;
+		this->cursorVisible = false;
+		this->illuminationEnabled = true;
+		
+		switch(displayId)
+		{
+			case IO_DISPLAY_MAIN:
+				this->commandClear = 12;
+				this->commandReturnCursor = 13;
+				this->commandNewLine = 10;
+				this->commandSetting = 27;
+				this->settingCursorVisible = 67;
+				this->settingIllumination = 76;
+				this->settingCursorPosition = 79;
+				msleep(100);
+				uart1_init(103);//9600 BAUD at 16MHz Atmel
+				msleep(100);
+				break;
+			default:
+				this->commandClear = 0;
+				this->commandReturnCursor = 0;
+				this->commandNewLine = 0;
+				this->commandSetting = 0;
+				this->settingCursorVisible = 0;
+				this->settingIllumination = 0;
+				this->settingCursorPosition = 0;
+				break;
+		}
+	}
+
+protected:
+	bool cursorVisible;
+	bool illuminationEnabled;
+	//Commands
+	uint8 commandClear;
+	uint8 commandReturnCursor;
+	uint8 commandNewLine;
+	uint8 commandSetting;
+	//Settings
+	uint8 settingCursorVisible;
+	uint8 settingIllumination;
+	uint8 settingCursorPosition;
+
+	void SendCommand(uint8 newCommand)
+	{
+		switch(moduleId)
+		{
+			case IO_DISPLAY_MAIN:
+				uart1_putc(newCommand);
+				break;
+			default:
+				break;
+		}
+	}
+
+public:
+	void Print(char* newString)
+	{
+		switch(moduleId)
+		{
+			case IO_DISPLAY_MAIN:
+				uart1_puts(newString);
+				break;
+			default:
+				break;
+		}
+	}
+
+	void Print(int32 newInteger)
+	{
+		char buffer[12];
+		ltoa(newInteger, buffer, 10);
+		Print(buffer);
+	}
+
+	void PrintFloat(float newFloat)
+	{
+		Print((int32)(newFloat));
+		Print(".");
+		Print(abs((uint32)(newFloat - float((int32)(newFloat)) * 100000)));
+	}
+
+	void Print(char* newString, uint8 xPos, uint8 yPos)
+	{
+		SetCursorPosition(xPos, yPos);
+		Print(newString);
+	}
+
+	void Print(int32 newInteger, uint8 xPos, uint8 yPos)
+	{
+		SetCursorPosition(xPos, yPos);
+		Print(newInteger);
+	}
+
+	void PrintFloat(float newFloat, uint8 xPos, uint8 yPos)
+	{
+		SetCursorPosition(xPos, yPos);
+		PrintFloat(newFloat);
+	}
+
+	void Clear()
+	{
+		SendCommand(commandClear);
+	}
+
+	void ReturnCursor()
+	{
+		SendCommand(commandReturnCursor);
+	}
+
+	void NewLine()
+	{
+		SendCommand(commandNewLine);
+	}
+
+	bool GetCursorVisible()
+	{
+		return cursorVisible;
+	}
+
+	void SetCursorVisible(bool newStatus)
+	{
+		cursorVisible = newStatus;
+
+		SendCommand(commandSetting);
+		SendCommand(settingCursorVisible);
+
+		if(cursorVisible)
+		{
+			SendCommand(1);
+		}
+		else
+		{
+			SendCommand(0);
+		}
+	}
+
+	bool GetLightingEnabled()
+	{
+		return illuminationEnabled;
+	}
+
+	void SetLightingEnabled(bool newStatus)
+	{
+		illuminationEnabled = newStatus;
+
+		SendCommand(commandSetting);
+		SendCommand(settingIllumination);
+
+		if(illuminationEnabled)
+		{
+			SendCommand(1);
+		}
+		else
+		{
+			SendCommand(0);
+		}
+	}
+
+	void SetCursorPosition(uint8 newX, uint8 newY)
+	{
+		if(!newX || newX > 20) return;
+		if(!newY || newY > 4) return;
+
+		SendCommand(commandSetting);
+		SendCommand(settingCursorPosition);
+		SendCommand(newX);
+		SendCommand(newY);
+	}
+};
+
+#endif

source/Concept/Framework/modules/output/dribbler.h

@@ -0,0 +1,118 @@
+#ifndef _DRIBBLER_H
+#define _DRIBBLER_H
+
+#include "../../stdafx.h"
+
+class Dribbler : public IO_Module
+{
+public:
+	Dribbler()
+	{
+		this->enabled = false;
+		this->curSpeed = 0;
+		this->parent = NULL;
+		this->moduleId = 0;
+		this->hardwarePort = NULL;
+		this->portPower = NULL;
+		this->pinForward = 0;
+		this->pinReverse = 0;
+		this->pinPower = 0;
+	}
+
+	Dribbler(uint32 dribblerId)
+	{
+		this->enabled = false;
+		this->curSpeed = 1.0f;
+		this->parent = NULL;
+		this->moduleId = dribblerId;
+		
+		switch(dribblerId)
+		{
+			case IO_DRIBBLER_MAIN:
+				this->hardwarePort = &PORTD;
+				this->portPower = &PORTA;
+				this->pinForward = (1 << 6);
+				this->pinReverse = (1 << 7);
+				this->pinPower = (1 << 5);
+				break;
+			default:
+				this->hardwarePort = NULL;
+				this->portPower = NULL;
+				this->pinForward = 0;
+				this->pinReverse = 0;
+				this->pinPower = 0;
+				break;
+		}
+
+		UpdateDirection();
+	}
+
+protected:
+	bool enabled;
+	float curSpeed;
+
+	//Hardware
+	volatile uint8* hardwarePort;
+	volatile uint8* portPower;
+	uint8 pinForward;
+	uint8 pinReverse;
+	uint8 pinPower;
+
+	void UpdateDirection()
+	{
+		if(enabled)
+		{
+			if(curSpeed > 0)
+			{
+				*hardwarePort |= pinForward;
+				*hardwarePort &= ~pinReverse;
+			}
+			else if(curSpeed < 0)
+			{
+				*hardwarePort |= pinReverse;
+				*hardwarePort &= ~pinForward;
+			}
+			else
+			{
+				*hardwarePort |= pinForward;
+				*hardwarePort |= pinReverse;
+			}
+
+			*portPower |= pinPower;
+		}
+		else
+		{
+			*hardwarePort &= ~pinForward;
+			*hardwarePort &= ~pinReverse;
+
+			*portPower &= ~pinPower;
+		}
+	}
+
+public:
+	float GetSpeed()
+	{
+		return curSpeed;
+	}
+
+	void SetSpeed(float newSpeed)
+	{
+		curSpeed = newSpeed;
+
+		UpdateDirection();
+	}
+
+	bool GetEnabled()
+	{
+		return enabled;
+	}
+
+	void SetEnabled(bool newStatus)
+	{
+		enabled = newStatus;
+
+		UpdateDirection();
+	}
+};
+
+#endif

source/Concept/Framework/modules/output/engine.h

@@ -0,0 +1,124 @@
+#ifndef _ENGINE_H
+#define _ENGINE_H
+
+#include "../../stdafx.h"
+
+class Engine : public IO_Module
+{
+public:
+	Engine()
+	{
+		this->enabled = false;
+		this->curSpeed = 0;
+		this->parent = NULL;
+		this->moduleId = 0;
+		this->hardwarePort = NULL;
+		this->pwmSpeed = NULL;
+		this->pinForward = 0;
+		this->pinReverse = 0;
+	}
+
+	Engine(uint32 engineId)
+	{
+		this->enabled = false;
+		this->curSpeed = 0;
+		this->parent = NULL;
+		this->moduleId = engineId;
+		
+		switch(engineId)
+		{
+			case IO_ENGINE_DRIVE_LEFT:
+				this->hardwarePort = &PORTB;
+				this->pwmSpeed = &OCR1A;
+				this->pinForward = (1 << 0);
+				this->pinReverse = (1 << 1);
+				break;
+			case IO_ENGINE_DRIVE_BACK:
+				this->hardwarePort = &PORTB;
+				this->pwmSpeed = &OCR1B;
+				this->pinForward = (1 << 2);
+				this->pinReverse = (1 << 3);
+				break;
+			case IO_ENGINE_DRIVE_RIGHT:
+				this->hardwarePort = &PORTD;
+				this->pwmSpeed = &OCR3A;
+				this->pinForward = (1 << 5);
+				this->pinReverse = (1 << 4);
+				break;
+			default:
+				this->hardwarePort = NULL;
+				this->pwmSpeed = NULL;
+				this->pinForward = 0;
+				this->pinReverse = 0;
+				break;
+		}
+
+		*this->pwmSpeed = 0;
+	}
+
+protected:
+	bool enabled;
+	float curSpeed;
+
+	//Hardware
+	volatile uint8* hardwarePort;
+	volatile uint16* pwmSpeed;
+	uint8 pinForward;
+	uint8 pinReverse;
+
+	void UpdateDirection()
+	{
+		if(enabled)
+		{
+			if(curSpeed > 0)
+			{
+				*hardwarePort |= pinForward;
+				*hardwarePort &= ~pinReverse;
+			}
+			else if(curSpeed < 0)
+			{
+				*hardwarePort |= pinReverse;
+				*hardwarePort &= ~pinForward;
+			}
+			else
+			{
+				*hardwarePort |= pinForward;
+				*hardwarePort |= pinReverse;
+			}
+		}
+		else
+		{
+			*hardwarePort &= ~pinForward;
+			*hardwarePort &= ~pinReverse;
+		}
+	}
+
+public:
+	float GetSpeed()
+	{
+		return curSpeed;
+	}
+
+	void SetSpeed(float newSpeed)
+	{
+		curSpeed = newSpeed;
+
+		*pwmSpeed = (abs((int16)(newSpeed / SPEED_PER_PWM)));
+
+		UpdateDirection();
+	}
+
+	bool GetEnabled()
+	{
+		return enabled;
+	}
+
+	void SetEnabled(bool newStatus)
+	{
+		enabled = newStatus;
+
+		UpdateDirection();
+	}
+};
+
+#endif

source/Concept/Framework/modules/output/kicker.h

@@ -0,0 +1,84 @@
+#ifndef _KICKER_H
+#define _KICKER_H
+
+#include "../../stdafx.h"
+
+class Kicker : public IO_Module
+{
+public:
+	Kicker()
+	{
+		this->enabled = false;
+		this->parent = NULL;
+		this->moduleId = 0;
+		this->portPower = NULL;
+		this->portForward = NULL;
+		this->portReverse = NULL;
+		this->pinPower = 0;
+		this->pinForward = 0;
+		this->pinReverse = 0;
+	}
+
+	Kicker(uint32 kickerId)
+	{
+		this->enabled = false;
+		this->parent = NULL;
+		this->moduleId = kickerId;
+
+		switch(kickerId)
+		{
+			case IO_KICKER_MAIN:
+				this->portPower = &PORTG;
+				this->portForward = &PORTA;
+				this->portReverse = &PORTE;
+				this->pinPower = (1 << 3);
+				this->pinForward = (1 << 2);
+				this->pinReverse = (1 << 6);
+				break;
+			default:
+				this->portPower = NULL;
+				this->portForward = NULL;
+				this->portReverse = NULL;
+				this->pinPower = 0;
+				this->pinForward = 0;
+				this->pinReverse = 0;
+				break;
+		}
+
+		*this->portForward |= this->pinForward;
+		*this->portReverse &= ~this->pinReverse;
+	}
+
+protected:
+	bool enabled;
+
+	//Hardware
+	volatile uint8* portPower;
+	volatile uint8* portForward;
+	volatile uint8* portReverse;
+	uint8 pinPower;
+	uint8 pinForward;
+	uint8 pinReverse;
+
+public:
+	bool GetEnabled()
+	{
+		return enabled;
+	}
+
+	void SetEnabled(bool newStatus)
+	{
+		enabled = newStatus;
+
+		if(enabled)
+		{
+			*portPower |= pinPower;
+		}
+		else
+		{
+			*portPower &= ~pinPower;
+		}
+	}
+};
+
+#endif

source/Concept/Framework/modules/output/led.h

@@ -0,0 +1,65 @@
+#ifndef _LED_H
+#define _LED_H
+
+#include "../../stdafx.h"
+
+class Led : public IO_Module
+{
+public:
+	Led()
+	{
+		this->enabled = false;
+		this->parent = NULL;
+		this->moduleId = 0;
+		this->hardwarePort = NULL;
+		this->pinPower = 0;
+	}
+
+	Led(uint32 ledId)
+	{
+		this->enabled = false;
+		this->parent = NULL;
+		this->moduleId = ledId;
+
+		switch(ledId)
+		{
+			case IO_LED_MAIN:
+				this->hardwarePort = &PORTB;
+				this->pinPower = (1 << 1);
+				break;
+			default:
+				this->hardwarePort = NULL;
+				this->pinPower = 0;
+				break;
+		}
+	}
+
+protected:
+	bool enabled;
+
+	//Hardware
+	volatile uint8* hardwarePort;
+	uint8 pinPower;
+
+public:
+	bool GetEnabled()
+	{
+		return enabled;
+	}
+
+	void SetEnabled(bool newStatus)
+	{
+		enabled = newStatus;
+
+		if(enabled)
+		{
+			*hardwarePort &= ~pinPower;
+		}
+		else
+		{
+			*hardwarePort |= pinPower;
+		}
+	}
+};
+
+#endif

source/Concept/Framework/robot.c

@@ -45,7 +45,7 @@ Robot::Robot()
 	sei();
 
 	//Interface
-	for(uint8 i = 0; i < IO_END; i++)
+	for(uint8 i = IO_START; i < IO_END; i++)
 	{
 		modules[i] = NULL;
 	}
@@ -85,6 +85,12 @@ bool Robot::RemoveModule(IO_Module* oldModule)
 //-----------------------------------------------------------------------------
 void Robot::Update()
 {
+	GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN)->Update();
+
+	GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN)->Update();
+
+	GetModule<Navigator>(IO_NAVIGATOR_MAIN)->Update();
+
 	//insert code here
 }
 

source/Concept/Framework/sensor.cpp

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

source/Concept/Framework/stdafx.h

@@ -19,4 +19,6 @@
 #include "ir_sensor.h"
 #include "mouse_sensor.h"
 #include "position_tracker.h"
+#include "ball_tracker.h"
+#include "navigator.h"
 #include "robot.h"

source/Concept/Framework/tools.h

@@ -2,6 +2,7 @@
 #define _TOOLS_H
 
 #include <stdlib.h>
+#include <math.h>
 
 #ifndef new
 	void* operator new(size_t sz);
@@ -36,4 +37,9 @@ inline void usleep(int usec)
   }
 };
 
+inline float distance2d(float x1, float y1, float x2, float y2)
+{
+	return sqrt(((x1 - x2) * (x1 - x2)) + ((y1 - y2) * (y1 - y2)));
+}
+
 #endif