SoccerTeam, Stuff from Magdeburg

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,6 +60,9 @@
 #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
@@ -67,8 +70,39 @@
 
 #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
 {
@@ -183,9 +217,17 @@ enum IOModuleNames
 
 	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

@@ -130,6 +130,14 @@ int main()
 		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++)
 	{
@@ -179,33 +187,62 @@ int 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);
 
-	float rotation = 0;
+	//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_BACK))->SetSpeed(200);
-	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);
 
-	ourDisplay->Clear();
+	//sleep(2);
+
+	//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT))->SetSpeed(200);
+	//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT))->SetEnabled(true);
+
+	//sleep(2);
+
+	
+
+	//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetSpeed(200);
+	//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetEnabled(true);
+
+	/*(localRobot->GetModule<Dribbler>(IO_DRIBBLER_MAIN))->SetSpeed(200);
+	(localRobot->GetModule<Dribbler>(IO_DRIBBLER_MAIN))->SetEnabled(true);*/
+
+	/*(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT))->SetSpeed(80);
+	(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT))->SetEnabled(true);*/
+
+	(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_BACK))->SetSpeed(255);
+	(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_BACK))->SetEnabled(true);
+
+	(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetSpeed(255);
+	(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetEnabled(true);
+
+	sleep(2);
 
 	//Run
 	while(true)
 	{
-		ourDisplay->Print(i++,1,1);
+		i++;
 		
-		//msleep(50);
+		/*if(!(i % 20))
+		{
+			ourDisplay->Clear();
+			msleep(10);
+			ourCommandHandler->PrintCommand();
+		}
+		
+		localRobot->Update();
 
 		if(ourCommandHandler->displayDistanceSensors && !(i % 20))
 		{
-			ourDisplay->Clear();
-
 			distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_0_DEG);
 			value = distanceSensor->GetDistance();
 			ourDisplay->Print(value, 1, 4);
@@ -224,68 +261,42 @@ int main()
 			ourDisplay->Print(";");
 		}
 
-		/*uint8 someInput = ourKeyboard->GetInput();
+		if(ourCommandHandler->ticksPerCmOffset && !(i % 20))
-		//ourDisplay->Print("Ready to accept...", 1, 2);
-		switch(someInput)
 		{
-			case 0:
+			ourDisplay->Print(ourCommandHandler->ticksPerCmOffset * 10.0f, 10, 1);
-				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 11:
-				ourLogic->SetKeeper(true); // Reset Position_Tracker
-				break;
-		}*/
 
-		//ourDisplay->Clear();
+		if(ourCommandHandler->displayPositionTracker && !(i % 20))
-		if(!(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))
 		{
-			/*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(value, 6, 2);
 			ourDisplay->Print(";");
 			ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_60_DEG);
 			value = ourSensor->GetIRIntensity();
-			ourDisplay->Print(value);
+			ourDisplay->Print(value, 11, 2);
 			ourDisplay->Print(";");
 			ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_100_DEG);
 			value = ourSensor->GetIRIntensity();
-			ourDisplay->Print(value);
+			ourDisplay->Print(value, 16, 2);
 			ourDisplay->Print(";");
 			ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_180_DEG);
 			value = ourSensor->GetIRIntensity();
@@ -293,15 +304,15 @@ int main()
 			ourDisplay->Print(";");
 			ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_260_DEG);
 			value = ourSensor->GetIRIntensity();
-			ourDisplay->Print(value);
+			ourDisplay->Print(value, 6, 3);
 			ourDisplay->Print(";");
 			ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_300_DEG);
 			value = ourSensor->GetIRIntensity();
-			ourDisplay->Print(value);
+			ourDisplay->Print(value, 11, 3);
 			ourDisplay->Print(";");
 			ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_330_DEG);
 			value = ourSensor->GetIRIntensity();
-			ourDisplay->Print(value);
+			ourDisplay->Print(value, 16, 3);
 			ourDisplay->Print(";");
 			if(ourBallTracker->KnowsBallDirection())
 			{
@@ -309,15 +320,27 @@ int main()
 			}
 			else
 			{
-				ourDisplay->Print("Not found oder so", 1, 4);
+				ourDisplay->Print("Ball not found", 1, 4);
-			}*/
 			}
 
-		localRobot->Update();
+			if(ourBallTracker->HasBall()) {
+				ourDisplay->Print("Has Ball", 5, 4);
+			}
+		}
 
-		//ourDisplay->Print(ourBallTracker->GetBallDirection() * 180.0f / PI, 1, 4);
+		//if(!(i % 20))
+		//{
+		//	ourDisplay->Print(ourWireless->Recieve(), 10, 1);
+		//}
 
-		//ourNavigator->Drive(ourBallTracker->GetBallDirection() * 180.0f / PI, 0, 255, 0);
+		if(ourBallTracker->KnowsBallDirection())
+		{
+			ourNavigator->Drive(ourBallTracker->GetBallDirection() * 180.0f / PI, 0, 255, 0);
+		}
+		else
+		{
+			ourNavigator->Stop();
+		}*/
 	}
 
 	//Cleanup

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

@@ -59,7 +59,7 @@ 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);
 
@@ -125,7 +125,7 @@ bool Navigator::TargetReached()
 
 	bool targetReached = false;
 
-	if(!HasTarget() || (distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < 1.0f))
+	if(!HasTarget() || (distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < DISTANCE_TOLERANCE))
 	{
 		targetReached = true;
 	}
@@ -230,7 +230,7 @@ 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);
 

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

@@ -67,6 +67,21 @@ 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);

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;
@@ -26,15 +28,12 @@ void Ball_Tracker::Update()
 		{
 			if(intensity[i] > BALL_HELD_INTENSITY) // Ball derzeit sehr nah dran
 			{
-				ballHeld = true;
+				if(ballHeldCounter < 10) ballHeldCounter++;
-				ballHeldCounter = 100;
 			}
-			else if(ballHeldCounter > 0) // Oder vor kurzem erst sehr nah dran
+			else if(ballHeldCounter > 0)
 			{
-				ballHeld = true;
 				ballHeldCounter--;		
 			}
-			else ballHeld = false; // ansonsten hat er den Ball nicht
 		}
 	}
 
@@ -132,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

@@ -12,7 +12,6 @@ public:
 		this->moduleId = 0;
 		this->direction = EMPTY_FLOAT;
 		this->ballHeldCounter = 0;
-		this->ballHeld = false;
 	}
 
 	Ball_Tracker(uint32 trackerId)
@@ -21,13 +20,11 @@ public:
 		this->moduleId = trackerId;
 		this->direction = EMPTY_FLOAT;
 		this->ballHeldCounter = 0;
-		this->ballHeld = false;
 	}
 
 protected:
 	float direction;
 	uint8 ballHeldCounter;
-	bool ballHeld;
 
 public:
 	void Update();
@@ -42,9 +39,9 @@ public:
 		return (direction != EMPTY_FLOAT);
 	}
 
-	bool RobotHasBall()
+	bool HasBall()
 	{
-		return ballHeld;
+		return (ballHeldCounter >= 3);
 	}
 };
 

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

@@ -3,6 +3,7 @@
 //-----------------------------------------------------------------------------
 void Command_Handler::Update()
 {
+	Display* ourDisplay = parent->GetModule<Display>(IO_DISPLAY_MAIN);
 	Keyboard* ourKeyboard = parent->GetModule<Keyboard>(IO_KEYBOARD_MAIN);
 
 	uint8 curInput = ourKeyboard->GetInput();
@@ -13,11 +14,20 @@ void Command_Handler::Update()
 		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;
 			}
@@ -28,6 +38,7 @@ void Command_Handler::Update()
 			{
 				this->buffer[this->currentCommandLength] = curInput;
 				this->currentCommandLength++;
+				ourDisplay->Print(curInput, this->currentCommandLength, 1);
 			}
 		}
 
@@ -38,8 +49,163 @@ void Command_Handler::Update()
 //-----------------------------------------------------------------------------
 void Command_Handler::ExecuteCommand()
 {
-	if(this->buffer[0] == 5)
+	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

@@ -12,6 +12,10 @@ public:
 		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++)
 		{
@@ -25,6 +29,10 @@ public:
 		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++)
 		{
@@ -41,8 +49,15 @@ protected:
 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);
@@ -40,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());
@@ -47,6 +58,11 @@ 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));

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

@@ -40,18 +40,20 @@ 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;
 	}
 };

source/Concept/Framework/modules/logic/logic.c

@@ -3,16 +3,51 @@
 //-----------------------------------------------------------------------------
 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()
 {
-	Wireless* ourWireless = parent->GetModule<Wireless>(IO_WIRELESS_MAIN);
 	
-	//ourWireless->Send(WIRELESS_CODE);
+}
-	//ourWireless->Send();
+
+//-----------------------------------------------------------------------------
+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();
 }
 
 //-----------------------------------------------------------------------------
@@ -20,21 +55,217 @@ 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);
 
-	// is Keeper?
+	if(!this->HasBall() && ourBallTracker->HasBall())
-	if(isKeeper) {
+	{
-		// turn around al little bit...
+		this->hasBall = true;
-		if((status == STATUS_KEEPER_TURN_LEFT && !ourNavigator->IsMoving()) ||
+		this->OnBallOwned();
-			 (status != STATUS_KEEPER_TURN_LEFT && status != STATUS_KEEPER_TURN_RIGHT)) {
-			status = STATUS_KEEPER_TURN_RIGHT;
-			ourNavigator->RotateTo(315,200);
 	}
-		else if(status == STATUS_KEEPER_TURN_RIGHT && !ourNavigator->IsMoving()) {
+	else if(this->HasBall() && !ourBallTracker->HasBall())
-			status = STATUS_KEEPER_TURN_LEFT;
+	{
-			ourNavigator->RotateTo(45, 200);
+		this->hasBall = false;
+		this->OnBallLost();
 	}
-	}
-	else { // is Player?
 
+	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

@@ -12,6 +12,8 @@ public:
 		this->moduleId = 0;
 		this->isKeeper = false;
 		this->status = 0;
+		this->hasBall = false;
+		this->avoidsObstacle = false;
 	}
 
 	Logic(uint32 logicId)
@@ -20,16 +22,29 @@ public:
 		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();
@@ -51,7 +66,8 @@ public:
 		return !isKeeper;
 	}
 
-	void SetKeeper(bool newStatus) {
+	void SetKeeper(bool newStatus)
+	{
 		if(!this->isKeeper && newStatus)
 		{
 			this->OnBecomeKeeper();
@@ -65,8 +81,11 @@ public:
 
 	bool HasBall()
 	{
-		//fill me!
+		return this->hasBall;
 	}
+
+	void UpdateKeeperMovement();
+	void UpdateAttackerMovement();
 };
 
 #endif

source/Concept/Framework/robot.c

@@ -90,15 +90,15 @@ void Robot::Update()
 {
 	GetModule<Command_Handler>(IO_COMMAND_HANDLER_MAIN)->Update();
 
-	GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN)->Update();
-
 	GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN)->Update();
 
-	GetModule<Logic>(IO_LOGIC_MAIN)->Update();
+	GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN)->Update();
+
+	GetModule<Obstacle_Tracker>(IO_OBSTACLE_TRACKER_MAIN)->Update();
+
+	//GetModule<Logic>(IO_LOGIC_MAIN)->Update();
 
 	GetModule<Navigator>(IO_NAVIGATOR_MAIN)->Update();
-
-	//insert code here
 }
 
 //-----------------------------------------------------------------------------

source/Concept/Framework/stdafx.h

@@ -22,6 +22,7 @@
 #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"