Codework
This commit is contained in:
parent
9525458918
commit
655dd7522f
14 changed files with 212 additions and 166 deletions
|
@ -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
|
||||
{
|
||||
|
|
|
@ -30,6 +30,16 @@ public:
|
|||
{
|
||||
return direction;
|
||||
}
|
||||
|
||||
bool KnowsBallDirection()
|
||||
{
|
||||
return direction != EMPTY_FLOAT;
|
||||
}
|
||||
|
||||
bool RobotHasBall()
|
||||
{
|
||||
//fill me!
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
};
|
||||
|
||||
|
|
Reference in a new issue