This commit is contained in:
sicarius 2007-02-22 13:12:03 +00:00
parent 9525458918
commit 655dd7522f
14 changed files with 212 additions and 166 deletions

View file

@ -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
{

View file

@ -30,6 +30,16 @@ public:
{
return direction;
}
bool KnowsBallDirection()
{
return direction != EMPTY_FLOAT;
}
bool RobotHasBall()
{
//fill me!
}
};
#endif

View file

@ -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;

View file

@ -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;
}
};