From 22433e52a7b0f2e5c548edb915ea11a38a8077ad Mon Sep 17 00:00:00 2001 From: sicarius Date: Tue, 20 Feb 2007 21:01:02 +0000 Subject: +++ Codework, Navigator&Positiontracker done (buggy?) --- .../modules/interpreter/position_tracker.c | 62 +++++++++++++++++++++- 1 file changed, 61 insertions(+), 1 deletion(-) (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 262069a..c8cd52d 100755 --- a/source/Concept/Framework/modules/interpreter/position_tracker.c +++ b/source/Concept/Framework/modules/interpreter/position_tracker.c @@ -3,5 +3,65 @@ //----------------------------------------------------------------------------- void Position_Tracker::Update() { - //insert code here + Mouse_Sensor* mouseLeft = parent->GetModule(IO_SENSOR_MOUSE_LEFT); + Mouse_Sensor* mouseRight = parent->GetModule(IO_SENSOR_MOUSE_RIGHT); + + int8 leftX = mouseLeft->GetXMovement(); + int8 leftY = mouseLeft->GetYMovement(); + float distanceLeft = sqrt(leftX * leftX + leftY * leftY); + float angleLeft = easyAngle(atan2(leftY, leftX) + (225.0f * PI / 180.0f)); + + float movementLeftX = cos(angleLeft) * distanceLeft; + float movementLeftY = sin(angleLeft) * distanceLeft; + + if(!leftX && !leftY) + { + movementLeftX = 0; + movementLeftY = 0; + } + + int8 rightX = mouseRight->GetXMovement(); + int8 rightY = mouseRight->GetYMovement(); + float distanceRight = sqrt(rightX * rightX + rightY * rightY); + float angleRight = easyAngle(atan2(rightY, rightX) - (45.0f * PI / 180.0f)); + + float movementRightX = cos(angleRight) * distanceRight; + float movementRightY = sin(angleRight) * distanceRight; + + if(!rightX && !rightY) + { + movementRightX = 0; + movementRightY = 0; + } + + float movementDifferenceX = movementRightX - movementLeftX; + float movementDifferenceY = (movementRightY + mouseRight->GetPositionY()) - (movementLeftY + mouseLeft->GetPositionY()); + + float robotMovementX = movementDifferenceX / 2.0f; + float robotMovementY = movementDifferenceY / 2.0f; + robotMovementX += movementLeftX; + robotMovementY += movementLeftY + mouseLeft->GetPositionY(); + float robotDistance = sqrt(robotMovementX * robotMovementX + robotMovementY * robotMovementY); + + float orientationDiff = atan2(movementDifferenceY, movementDifferenceX) - (PI / 2.0f); + + float absoluteDiffX = cos(this->orientation + (orientationDiff / 2.0f)) * robotDistance * sign(robotMovementX); + float absoluteDiffY = sin(this->orientation + (orientationDiff / 2.0f)) * robotDistance * sign(robotMovementY); + + if(!robotMovementX && !robotMovementY) + { + absoluteDiffX = 0; + absoluteDiffY = 0; + } + + //(parent->GetModule(IO_DISPLAY_MAIN))->Print(" ", 5, 1); + //(parent->GetModule(IO_DISPLAY_MAIN))->Print(absoluteDiffX, 5, 1); + //(parent->GetModule(IO_DISPLAY_MAIN))->Print(absoluteDiffY, 12, 1); + + this->positionX += absoluteDiffX; + this->positionY += absoluteDiffY; + + this->orientation += orientationDiff; + + this->orientation = easyAngle(this->orientation); } -- cgit v1.2.3