1 files changed, 10 insertions, 3 deletions

diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.c b/source/Concept/Framework/modules/interpreter/position_tracker.c
index 63a7f5f..a64ab60 100755
--- a/source/Concept/Framework/modules/interpreter/position_tracker.c
+++ b/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;