From f61eb900879fe941460a284ff9e4681b0958b944 Mon Sep 17 00:00:00 2001 From: sicarius Date: Mon, 26 Feb 2007 21:25:01 +0000 Subject: SoccerTeam, Stuff from Magdeburg --- .../Framework/modules/interpreter/position_tracker.c | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) (limited to 'source/Concept/Framework/modules/interpreter/position_tracker.c') 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(IO_COMMAND_HANDLER_MAIN); + Display* ourDisplay = parent->GetModule(IO_DISPLAY_MAIN); + // We want to use the mouse-sensors Mouse_Sensor* mouseLeft = parent->GetModule(IO_SENSOR_MOUSE_LEFT); Mouse_Sensor* mouseRight = parent->GetModule(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)); -- cgit v1.2.3