diff options
Diffstat (limited to 'source/Concept/Framework/modules/interpreter')
4 files changed, 44 insertions, 12 deletions
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<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;
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;
}
};
|