1 files changed, 16 insertions, 0 deletions
diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.c b/source/Concept/Framework/modules/interpreter/position_tracker.c
index f6d67ac..fab8085 100755
--- a/source/Concept/Framework/modules/interpreter/position_tracker.c
+++ b/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));