From 655dd7522fb60e2ef0e68437337178184109f347 Mon Sep 17 00:00:00 2001 From: sicarius Date: Thu, 22 Feb 2007 13:12:03 +0000 Subject: Codework --- .../Framework/modules/interpreter/ball_tracker.c | 7 ++++++ .../Framework/modules/interpreter/ball_tracker.h | 10 +++++++++ .../modules/interpreter/position_tracker.c | 13 ++++++++--- .../modules/interpreter/position_tracker.h | 26 ++++++++++++++-------- 4 files changed, 44 insertions(+), 12 deletions(-) (limited to 'source/Concept/Framework/modules/interpreter') diff --git a/source/Concept/Framework/modules/interpreter/ball_tracker.c b/source/Concept/Framework/modules/interpreter/ball_tracker.c index 1701121..2d85b96 100755 --- a/source/Concept/Framework/modules/interpreter/ball_tracker.c +++ b/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 { diff --git a/source/Concept/Framework/modules/interpreter/ball_tracker.h b/source/Concept/Framework/modules/interpreter/ball_tracker.h index bea4a19..c62f05e 100755 --- a/source/Concept/Framework/modules/interpreter/ball_tracker.h +++ b/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 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(IO_SENSOR_MOUSE_LEFT); Mouse_Sensor* mouseRight = parent->GetModule(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(IO_DISPLAY_MAIN))->Print(" ", 5, 1); - //(parent->GetModule(IO_DISPLAY_MAIN))->Print(absoluteDiffX, 5, 1); + //(parent->GetModule(IO_DISPLAY_MAIN))->Print(robotMovementY + movementLeftY + mouseLeft->GetPositionY() + (mouseLeft->GetPositionX() * sin(orientationDiff)), 5, 1); //(parent->GetModule(IO_DISPLAY_MAIN))->Print(absoluteDiffY, 12, 1); this->positionX += absoluteDiffX; diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.h b/source/Concept/Framework/modules/interpreter/position_tracker.h index e93ef0c..47d0740 100755 --- a/source/Concept/Framework/modules/interpreter/position_tracker.h +++ b/source/Concept/Framework/modules/interpreter/position_tracker.h @@ -32,19 +32,27 @@ protected: public: void Update(); - float GetPositionX() - { - return positionX; + // Sets the current position; x and y in mm + void SetPosition(int newPositionX, int newPositionY, float newOrientation) { + positionX = newPositionX*(TICKS_PER_CM/10.0f); + positionY = newPositionY*(TICKS_PER_CM/10.0f); + orientation = easyAngle(newOrientation); } - float GetPositionY() - { - return positionY; + // returns x-koordinate in mm + int GetPositionX() { + return (int)((positionX*10.0f)/TICKS_PER_CM); } - float GetOrientation() - { - return orientation; + // returns y-koordinate in mm + int GetPositionY() { + return (int)((positionY*10.0f)/TICKS_PER_CM); + } + + // returns orientation + float GetOrientation() { + return 0.0f; //tmp!!!!!!!!!! + //return orientation; } }; -- cgit v1.2.3