Codework

source/Concept/Framework/default/Makefile

@@ -40,7 +40,7 @@ INCLUDES = -I"Y:\Concept\Framework\modules" -I"Y:\Concept\Framework\modules\exec
 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 
+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 explicitly added by the user
 LINKONLYOBJECTS = 
@@ -103,6 +103,15 @@ ball_tracker.o: ../modules/interpreter/ball_tracker.c
 navigator.o: ../modules/executor/navigator.c
 	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
 
+aktuator.o: ../modules/executor/aktuator.c
+	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
+
+wireless.o: ../modules/wireless.c
+	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
+
+logic.o: ../modules/logic/logic.c
+	$(CC) $(INCLUDES) $(CFLAGS) -c  $<
+
 ##Link
 $(TARGET): $(OBJECTS)
 	 $(CC) $(LDFLAGS) $(OBJECTS) $(LINKONLYOBJECTS) $(LIBDIRS) $(LIBS) -o $(TARGET)

source/Concept/Framework/defines.h

@@ -59,11 +59,19 @@
 #define SPEED_PER_PWM 1
 #define DISTANCE_PER_VALUE 1
 #define TICKS_PER_CM 205.0f
+//#define TICKS_PER_CM 90.0f
 
 #define PI 3.14159265358979323846f
 #define CYCLES_PER_CORRECTION 20
 #define EMPTY_FLOAT 81188.1484f
 
+#define WIRELESS_CODE "SPASS!"
+enum WirelessCommands
+{
+	WLESS_CMD_CONFIRM,
+	WLESS_CMD_RECIEVED_BALL,
+};
+
 //IO Module Names
 enum IOModuleNames
 {
@@ -94,9 +102,25 @@ enum IOModuleNames
 	IO_KICKER_MAIN = IO_KICKER_START,
 
 	IO_KICKER_END,
+
+	// Aktuator
+
+	IO_AKTUATOR_START = IO_KICKER_END,
+
+	IO_AKTUATOR_MAIN = IO_AKTUATOR_START,
+
+	IO_AKTUATOR_END,
+
+	// Wireless
+
+	IO_WIRELESS_START = IO_AKTUATOR_END,
+
+	IO_WIRELESS_MAIN = IO_WIRELESS_START,
+
+	IO_WIRELESS_END,
 	
 	//Sensors
-	IO_SENSOR_START = IO_KICKER_END,
+	IO_SENSOR_START = IO_WIRELESS_END,
 
 	IO_SENSOR_IR_0_DEG = IO_SENSOR_START,
 	IO_SENSOR_IR_30_DEG,
@@ -163,6 +187,14 @@ enum IOModuleNames
 
 	IO_NAVIGATOR_END,
 
+	//Logics
+
+	IO_LOGIC_START = IO_NAVIGATOR_END,
+
+	IO_LOGIC_MAIN,
+
+	IO_LOGIC_END,
+
 	//General
 	IO_END = IO_NAVIGATOR_END,
 };

source/Concept/Framework/main.c

@@ -39,6 +39,22 @@ int main()
 		newKicker = NULL;
 	}
 
+	//Init Aktuator
+	for(uint8 i = IO_AKTUATOR_START; i < IO_AKTUATOR_END; i++)
+	{
+		Aktuator* newAktuator = new Aktuator(i);
+		localRobot->AddModule(newAktuator);
+		newAktuator = NULL;
+	}
+
+	//Init Wireless
+	for(uint8 i = IO_WIRELESS_START; i < IO_WIRELESS_END; i++)
+	{
+		Wireless* newWireless = new Wireless(i);
+		localRobot->AddModule(newWireless);
+		newWireless = NULL;
+	}
+
 	//Init Sensors
 	for(uint8 i = IO_SENSOR_START; i < IO_SENSOR_END; i++)
 	{
@@ -130,6 +146,14 @@ int main()
 		newNavigator = NULL;
 	}
 
+	//Init Logics
+	for(uint8 i = IO_LOGIC_START; i < IO_LOGIC_END; i++)
+	{
+		Logic* newLogic = new Logic(i);
+		localRobot->AddModule(newLogic);
+		newLogic = NULL;
+	}
+
 	sleep(1);
 	
 	//Debug code
@@ -146,8 +170,14 @@ int main()
 	uint32 i = 1;
 	Navigator* ourNavigator = localRobot->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
 	Position_Tracker* ourPosition_Tracker = localRobot->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
+	Ball_Tracker* ourBallTracker = localRobot->GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN);
+	Dribbler* ourDribbler = localRobot->GetModule<Dribbler>(IO_DRIBBLER_MAIN);
+	ourDribbler->SetSpeed(1);
+	ourDribbler->SetEnabled(true);
+	Aktuator* ourAktuator = localRobot->GetModule<Aktuator>(IO_AKTUATOR_MAIN);
 	
 	float rotation = 0;
+	float speed = 200;
 
 	//Mouse_Sensor* mouse_left = localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);
 	//Mouse_Sensor* mouse_right = localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);
@@ -157,19 +187,26 @@ int main()
 	while(true)
 	{
 		ourDisplay->Print(i++,1,1);
+
+		msleep(100);
 				
-		if(!(i % 200))
+		if(!(i % 1))
 		{
 			ourDisplay->Clear();
-			ourDisplay->PrintFloat(ourPosition_Tracker->GetPositionX(),1,2);
+			//ourDisplay->Print(ourPosition_Tracker->GetPositionX(),1,2);
-			ourDisplay->PrintFloat(ourPosition_Tracker->GetPositionY(),1,3);
+			//ourDisplay->Print(ourPosition_Tracker->GetPositionY(),1,3);
-			ourDisplay->PrintFloat(ourPosition_Tracker->GetOrientation() * 180.0f / PI,1,4);
+			//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);
+		distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_0_DEG);
 		value = distanceSensor->GetDistance();
-		ourDisplay->Print(value, 1, 4);
+		ourDisplay->Print(value, 1, 2);
 		ourDisplay->Print(";");
 		distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_90_DEG);
 		value = distanceSensor->GetDistance();
@@ -177,12 +214,12 @@ int main()
 		ourDisplay->Print(";");
 		distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_180_DEG);
 		value = distanceSensor->GetDistance();
-		ourDisplay->Print(value);
+		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(";");		*/
+		ourDisplay->Print(";");
 		
 		uint8 someInput = ourKeyboard->GetInput();
 		//ourDisplay->Print("Ready to accept...", 1, 2);
@@ -190,46 +227,45 @@ int main()
 		{
 			case 0:
 				ourNavigator->Stop();
+				return 0;
 				break;
 			case 1:
-				ourNavigator->Drive(225, rotation, 100, rotation);
+				ourNavigator->Drive(225, rotation, speed, rotation);
 				break;
 			case 2:
-				ourNavigator->Drive(180, rotation, 100, rotation);
+				ourNavigator->Drive(180, rotation, speed, rotation);
 				break;
 			case 3:
-				ourNavigator->Drive(135, rotation, 100, rotation);
+				ourNavigator->Drive(135, rotation, speed, rotation);
 				break;
 			case 4:
-				ourNavigator->Drive(270, rotation, 100, rotation);
+				ourNavigator->Drive(270, rotation, speed, rotation);
 				break;
 			case 5:
 				ourNavigator->Drive(0, rotation, 0, rotation);
 				break;
 			case 6:
-				ourNavigator->Drive(90, rotation, 100, rotation);
+				ourNavigator->Drive(90, rotation, speed, rotation);
 				break;
 			case 7:
-				ourNavigator->Drive(315, rotation, 100, rotation);
+				ourNavigator->Drive(315, rotation, speed, rotation);
 				break;
 			case 8:
-				ourNavigator->Drive(0, rotation, 100, rotation);
+				ourNavigator->Drive(0, rotation, speed, rotation);
 				break;
 			case 9:
-				ourNavigator->Drive(45, rotation, 100, rotation);
+				ourNavigator->Drive(45, rotation, speed, rotation);
 				break;
 			case 10:
-				rotation += 10;
+				ourPosition_Tracker->SetPosition(0,0,0); // Reset Position_Tracker
 				break;
 			case 12:
-				rotation -= 10;
+				ourPosition_Tracker->SetPosition(0,0,0); // Reset Position_Tracker
 				break;
 		}
 		
-		/*
 		//ourDisplay->Clear();
-		ourDisplay->Print(i++,1,1);
+		/*ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_0_DEG);
-		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_0_DEG);
 		value = ourSensor->GetIRIntensity();
 		ourDisplay->Print(value, 1, 2);
 		ourDisplay->Print(";");
@@ -260,13 +296,13 @@ int main()
 		ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_330_DEG);
 		value = ourSensor->GetIRIntensity();
 		ourDisplay->Print(value);
-		ourDisplay->Print(";");
+		ourDisplay->Print(";");*/
 		
-		msleep(500);
-		*/
 		localRobot->Update();
 
-		//ourDisplay->PrintFloat(ourBallTracker->GetBallDirection(), 1, 4);
+		//ourDisplay->Print(ourBallTracker->GetBallDirection() * 180.0f / PI, 1, 4);
+
+		//ourNavigator->Drive(ourBallTracker->GetBallDirection() * 180.0f / PI, 0, 255, 0);
 	}
 
 	//Cleanup

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

@@ -54,30 +54,6 @@ void Navigator::Drive(float newDirection, float newAngle, float newSpeed, float
 	}
 
 	CalculateEngines();
-
-	//dings,,,
-
-	/*float xDrive = cos(direction + PI / 6.0f);
-	float yDrive = sin(direction + PI / 6.0f);
-
-	float vLeft = xDrive;
-	float vBack = (-xDrive + sqrt(3) * yDrive) / 2.0f;
-	float vRight = (-xDrive - sqrt(3) * yDrive) / 2.0f;
-
-	vLeft = vLeft * this->robotSpeed + rotationSpeed;
-	vBack = vBack * this->robotSpeed + rotationSpeed;
-	vRight = vRight * this->robotSpeed + rotationSpeed;
-
-	Engine* curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT);
-	curEngine->SetSpeed(vLeft);
-	curEngine->SetEnabled(true);
-	curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_BACK);
-	curEngine->SetSpeed(vBack);
-	curEngine->SetEnabled(true);
-	curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT);
-	curEngine->SetSpeed(vRight);
-	curEngine->SetEnabled(true);
-	curEngine = NULL;*/
 }
 
 //-----------------------------------------------------------------------------
@@ -178,108 +154,43 @@ void Navigator::CalculateDirection()
 //-----------------------------------------------------------------------------
 void Navigator::CalculateEngines()
 {
+	// Use the position_tracker
 	Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
-	
+	// get the relative position of the robot
-	if(direction != EMPTY_FLOAT)
+	float relativeDirection = this->direction - locationeer->GetOrientation();
-	{
-		float relativeDirection = this->direction - locationeer->GetOrientation();
 
-		float xDrive = cos(relativeDirection + PI / 6.0f);
+	// calculate x and y-koordinates
-		float yDrive = sin(relativeDirection + PI / 6.0f);
+	float xDrive = cos(relativeDirection + PI / 6.0f);
+	float yDrive = sin(relativeDirection + PI / 6.0f);
 
-		float vLeft = xDrive;
+	// calculate relative speed of engines
-		float vBack = (-xDrive + sqrt(3) * yDrive) / 2.0f;
+	float vLeft = xDrive;
-		float vRight = (-xDrive - sqrt(3) * yDrive) / 2.0f;
+	float vBack = (-xDrive + sqrt(3) * yDrive) / 2.0f;
+	float vRight = (-xDrive - sqrt(3) * yDrive) / 2.0f;
 
-		/*float speedCorrection = 1.0f;
+	// Get the maximal value
+	float speedCorrection = (float)max(max(fabs(vLeft),fabs(vBack)),fabs(vRight));
 
-		float maxEngineSpeed = 255.0f;
+	// calculate the correct speeds of the engines
-		float minEngineSpeed = -255.0f;
+	vLeft = ((float)vLeft * (float)((float)this->robotSpeed / (float)speedCorrection)) + this->rotationSpeed;
+	vBack = ((float)vBack * (float)((float)this->robotSpeed / (float)speedCorrection)) + this->rotationSpeed;
+	vRight = ((float)vRight * (float)((float)this->robotSpeed / (float)speedCorrection)) + this->rotationSpeed;
 
-		float maxSingleSpeed = max(max(fabs(vLeft), fabs(vBack)), fabs(vRight));
+	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vLeft,10,2);
+	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vBack,10,3);
+	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vRight,10,4);
 
-		float calcSpeed = 1.0f;//sqrt(vLeft * vLeft - vLeft * vRight + vRight * vRight + 
-		//									vBack * (vBack + vLeft + vRight));
 
-		if(calcSpeed != 1.0f)
-		{
-			speedCorrection = 1.0f / calcSpeed;
-		}
 
-		float maxOverallSpeed = robotSpeed * maxSingleSpeed * speedCorrection;
+	// Transfer the values to the engines
-		if(maxOverallSpeed > maxEngineSpeed)
+	Engine* curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT);
-		{
+	curEngine->SetSpeed(vLeft);
-			robotSpeed = maxEngineSpeed / (maxSingleSpeed * speedCorrection);
+	curEngine->SetEnabled(true);
-		}*/
+	curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_BACK);
-		
+	curEngine->SetSpeed(vBack);
-
+	curEngine->SetEnabled(true);
-		vLeft = vLeft * this->robotSpeed;// * speedCorrection;
+	curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT);
-		vBack = vBack * this->robotSpeed;// * speedCorrection;
+	curEngine->SetSpeed(vRight);
-		vRight = vRight * this->robotSpeed;// * speedCorrection;
+	curEngine->SetEnabled(true);
-
+	curEngine = NULL;
-		/*maxSingleSpeed = max(max(fabs(vLeft), fabs(vBack)), fabs(vRight));
-		float minSingleSpeed = min(min(vLeft, vBack), vRight);
-
-		if(rotationSpeed)
-		{
-			if(this->rotationSpeed > 0)
-			{
-				if(maxEngineSpeed - maxSingleSpeed < this->rotationSpeed)
-				{
-					vLeft += maxEngineSpeed - maxSingleSpeed;
-					vBack += maxEngineSpeed - maxSingleSpeed;
-					vRight += maxEngineSpeed - maxSingleSpeed;
-				}
-				else
-				{
-					vLeft += this->rotationSpeed;
-					vBack += this->rotationSpeed;
-					vRight += this->rotationSpeed;
-				}
-			}
-			else
-			{
-				if((minEngineSpeed - minSingleSpeed) < this->rotationSpeed)
-				{
-					vLeft -= minEngineSpeed - minSingleSpeed;
-					vBack -= minEngineSpeed - minSingleSpeed;
-					vRight -= minEngineSpeed - minSingleSpeed;
-				}
-				else
-				{
-					vLeft -= this->rotationSpeed;
-					vBack -= this->rotationSpeed;
-					vRight -= this->rotationSpeed;
-				}
-			}
-		}*/
-
-		Engine* curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT);
-		curEngine->SetSpeed(vLeft);
-		curEngine->SetEnabled(true);
-		curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_BACK);
-		curEngine->SetSpeed(vBack);
-		curEngine->SetEnabled(true);
-		curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT);
-		curEngine->SetSpeed(vRight);
-		curEngine->SetEnabled(true);
-		curEngine = NULL;
-	}
-	else if(rotationSpeed)
-	{
-		Engine* curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT);
-		curEngine->SetSpeed(this->rotationSpeed);
-		curEngine->SetEnabled(true);
-		curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_BACK);
-		curEngine->SetSpeed(this->rotationSpeed);
-		curEngine->SetEnabled(true);
-		curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT);
-		curEngine->SetSpeed(this->rotationSpeed);
-		curEngine->SetEnabled(true);
-		curEngine = NULL;
-	}	
-	else
-	{
-		Stop();
-	}
 }

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

@@ -5,21 +5,36 @@ float Distance_Sensor::GetDistance()
 {
 	uint32 result = 0;
 
+	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("Gen Pulse; Pin:",1,4);
+	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print((int)(*hardwarePin & pin));
+	
+	msleep(500);
+
 	//Generate pulse
 	*hardwareDDR |= pin;//Set pin output
 	*hardwarePort |= pin;//Activate port
 	usleep(10);//Wait for 10µs
-	*hardwarePort &= ~pin;//Deactivate port
 	*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(uint16 i = 0; (!(*hardwarePin & pin)) && (i < 1000); i++) { asm volatile("nop"); }
+	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/ir_sensor.c

@@ -5,5 +5,5 @@ uint16 IR_Sensor::GetIRIntensity()
 {
 	if(!parent) return 0;
 
-	return parent->GetADCValue(channel);
+	return min(parent->GetADCValue(channel) + this->intensityCorrection, 1023);
 }

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

@@ -12,12 +12,14 @@ public:
 	{
 		this->parent = NULL;
 		this->moduleId = 0;
+		this->intensityCorrection = 0;
 	}
 
 	IR_Sensor(uint32 sensorId)
 	{
 		this->parent = NULL;
 		this->moduleId = sensorId;
+		this->intensityCorrection = 0;
 
 		switch(sensorId)
 		{
@@ -32,12 +34,15 @@ public:
 				break;
 			case IO_SENSOR_IR_100_DEG:
 				this->channel = 3;
+				this->intensityCorrection = 40;
 				break;
 			case IO_SENSOR_IR_180_DEG:
 				this->channel = 4;
+				this->intensityCorrection = 50;
 				break;
 			case IO_SENSOR_IR_260_DEG:
 				this->channel = 5;
+				this->intensityCorrection = 70;
 				break;
 			case IO_SENSOR_IR_300_DEG:
 				this->channel = 6;
@@ -55,6 +60,8 @@ protected:
 	//Hardware
 	uint8 channel;
 
+	uint8 intensityCorrection;
+
 public:
 	uint16 GetIRIntensity();
 };

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

@@ -109,9 +109,16 @@ void Ball_Tracker::Update()
 					break;
 			}
 
+			if(fabs(mainDirection - secondDirection) > PI)
+			{
+				min(mainDirection, secondDirection) += 2.0f * PI;
+			}
+
 			direction = (intensity[greatestIntensity] * mainDirection + 
 									intensity[secondIntensity] * secondDirection) / 
 									(intensity[greatestIntensity] + intensity[secondIntensity]);
+
+			direction = easyAngle(direction);
 		}
 		else
 		{

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

@@ -30,6 +30,16 @@ public:
 	{
 		return direction;
 	}
+
+	bool KnowsBallDirection()
+	{
+		return direction != EMPTY_FLOAT;
+	}
+
+	bool RobotHasBall()
+	{
+		//fill me!
+	}
 };
 
 #endif

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

@@ -3,9 +3,11 @@
 //-----------------------------------------------------------------------------
 void Position_Tracker::Update()
 {
+	// 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);
 
+	// Generate a vector for the left mouse
 	int8 leftX = mouseLeft->GetXMovement();
 	int8 leftY = mouseLeft->GetYMovement();
 	float distanceLeft = sqrt(leftX * leftX + leftY * leftY);
@@ -14,12 +16,14 @@ void Position_Tracker::Update()
 	float movementLeftX = cos(angleLeft) * distanceLeft;
 	float movementLeftY = sin(angleLeft) * distanceLeft;
 
+	// AVR calculates a little bit strange ;)
 	if(!leftX && !leftY)
 	{
 		movementLeftX = 0;
 		movementLeftY = 0;
 	}
 
+	// Generate a vector for the right mouse
 	int8 rightX = mouseRight->GetXMovement();
 	int8 rightY = mouseRight->GetYMovement();
 	float distanceRight = sqrt(rightX * rightX + rightY * rightY);
@@ -28,21 +32,24 @@ void Position_Tracker::Update()
 	float movementRightX = cos(angleRight) * distanceRight;
 	float movementRightY = sin(angleRight) * distanceRight;
 
+	// AVR calculates a little bit strange ;)
 	if(!rightX && !rightY)
 	{
 		movementRightX = 0;
 		movementRightY = 0;
 	}
 
-	float movementDifferenceX = (movementRightX + mouseRight->GetPositionX()) - (movementLeftX + mouseLeft->GetPositionX());
+	// Generate vector from P:left to P:right
+	float movementDifferenceX = movementRightX - movementLeftX;
 	float movementDifferenceY = (movementRightY + mouseRight->GetPositionY()) - (movementLeftY + mouseLeft->GetPositionY());
 
+	// Calculate the difference of orientation
 	float orientationDiff = atan2(movementDifferenceY, movementDifferenceX) - (PI / 2.0f);
 
 	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));
+	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;
@@ -58,7 +65,7 @@ void Position_Tracker::Update()
 	}
 
 	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print("            ", 5, 1);
-	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(absoluteDiffX, 5, 1);
+	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(robotMovementY + movementLeftY + mouseLeft->GetPositionY() + (mouseLeft->GetPositionX() * sin(orientationDiff)), 5, 1);
 	//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(absoluteDiffY, 12, 1);
 
 	this->positionX += absoluteDiffX;

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

@@ -32,19 +32,27 @@ protected:
 public:
 	void Update();
 
-	float GetPositionX()
+	// Sets the current position; x and y in mm
-	{
+	void SetPosition(int newPositionX, int newPositionY, float newOrientation) {
-		return positionX;
+		positionX = newPositionX*(TICKS_PER_CM/10.0f);
+		positionY = newPositionY*(TICKS_PER_CM/10.0f);
+		orientation = easyAngle(newOrientation);
 	}
 
-	float GetPositionY()
+	// returns x-koordinate in mm
-	{
+	int GetPositionX() 	{
-		return positionY;
+		return (int)((positionX*10.0f)/TICKS_PER_CM);
 	}
 
-	float GetOrientation()
+	// returns y-koordinate in mm
-	{
+	int GetPositionY() 	{
-		return orientation;
+		return (int)((positionY*10.0f)/TICKS_PER_CM);
+	}
+
+	// returns orientation
+	float GetOrientation()	{
+		return 0.0f; //tmp!!!!!!!!!!
+		//return orientation;
 	}
 };
 

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

@@ -66,19 +66,20 @@ protected:
 			{
 				*hardwarePort |= pinForward;
 				*hardwarePort &= ~pinReverse;
+				*portPower |= pinPower;
 			}
 			else if(curSpeed < 0)
 			{
 				*hardwarePort |= pinReverse;
 				*hardwarePort &= ~pinForward;
+				*portPower |= pinPower;
 			}
 			else
 			{
 				*hardwarePort |= pinForward;
 				*hardwarePort |= pinReverse;
-			}
+				*portPower &= ~pinPower;
-
+			}			
-			*portPower |= pinPower;
 		}
 		else
 		{

source/Concept/Framework/stdafx.h

@@ -14,6 +14,8 @@
 #include "engine.h"
 #include "dribbler.h"
 #include "kicker.h"
+#include "aktuator.h"
+#include "wireless.h"
 #include "led.h"
 #include "distance_sensor.h"
 #include "ir_sensor.h"
@@ -21,4 +23,5 @@
 #include "position_tracker.h"
 #include "ball_tracker.h"
 #include "navigator.h"
-#include "robot.h"
+#include "logic.h"
+#include "robot.h"