neocturne/rc2007-soccer
Archived
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

+++ improved navigator

Browse source
This commit is contained in:
masterm 2007-02-19 17:15:02 +00:00
parent c3ce4e7069
commit 354ef5f0d0
7 changed files with 254 additions and 126 deletions
Download patch file Download diff file Expand all files Collapse all files

173
source/Concept/Framework/modules/executor/navigator.c
View file

@ -3,10 +3,10 @@
//-----------------------------------------------------------------------------
void Navigator::Stop()
{
this->direction = -1.0f;
this->targetAngle = -1.0f;
this->targetX = -1.0f;
this->targetY = -1.0f;
this->direction = EMPTY_FLOAT;
this->targetAngle = EMPTY_FLOAT;
this->targetX = EMPTY_FLOAT;
this->targetY = EMPTY_FLOAT;
this->robotSpeed = 0;
this->rotationSpeed = 0;
@ -21,10 +21,10 @@ void Navigator::Stop()
void Navigator::Rotate(float rotationSpeed)
{
this->rotationSpeed = min(rotationSpeed, 255.0f);;
this->direction = -1.0f;
this->targetAngle = -1.0f;
this->targetX = -1.0f;
this->targetY = -1.0f;
this->direction = EMPTY_FLOAT;
this->targetAngle = EMPTY_FLOAT;
this->targetX = EMPTY_FLOAT;
this->targetY = EMPTY_FLOAT;
this->robotSpeed = 0;
for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++)
@ -40,26 +40,20 @@ void Navigator::Drive(float newDirection, float newAngle, float newSpeed, float
this->rotationSpeed = min(rotationSpeed, 255.0f);
this->direction = newDirection;
this->targetAngle = newAngle;
this->targetX = -1.0f;
this->targetY = -1.0f;
this->targetX = EMPTY_FLOAT;
this->targetY = EMPTY_FLOAT;
this->robotSpeed = newSpeed;
if(targetAngle - (parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN))->GetOrientation() > PI)
if(targetAngle == EMPTY_FLOAT)
{
if(rotationSpeed > 0)
{
rotationSpeed = -rotationSpeed;
}
rotationSpeed = 0;
}
else
{
if(rotationSpeed < 0)
{
rotationSpeed = -rotationSpeed;
}
rotationSpeed = fabs(rotationSpeed) * sign(PI - (targetAngle - (parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN))->GetOrientation()));
}
Update();
CalculateEngines();
}
//-----------------------------------------------------------------------------
@ -91,7 +85,7 @@ void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed,
}
}
Update();
CalculateDirection();
}
//-----------------------------------------------------------------------------
@ -99,94 +93,87 @@ void Navigator::Update()
{
Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
bool hasDistanceToDrive = true;
bool hasOrientationToAdjust = true;
bool targetReached = false;
bool targetAngleReached = false;
//Check for distance to drive
if((targetX >= 0) != (targetY >= 0))
if(HasTarget() && distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < 1.0f)
{
targetX = -1.0f;
targetY = -1.0f;
targetX = EMPTY_FLOAT;
targetY = EMPTY_FLOAT;
direction = EMPTY_FLOAT;
robotSpeed = 0;
hasDistanceToDrive = false;
}
else if(targetX >= 0 && targetY >= 0)
{
if(distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < 1.0f)
{
targetX = -1.0f;
targetY = -1.0f;
hasDistanceToDrive = false;
}
else
{
hasDistanceToDrive = true;
}
}
else
{
if(direction >= 0)
{
hasDistanceToDrive = true;
}
else
{
hasDistanceToDrive = false;
}
targetReached = true;
}
//Check for orientation to adjust
if(targetAngle >= 0)
if(targetAngle != EMPTY_FLOAT && fabs(targetAngle - locationeer->GetOrientation()) < 0.1f)
{
if(fabs(targetAngle - locationeer->GetOrientation()) < 0.1f)
{
hasOrientationToAdjust = false;
}
else
{
hasOrientationToAdjust = true;
}
}
else
{
if(rotationSpeed != 0)
{
hasOrientationToAdjust = true;
}
else
{
hasOrientationToAdjust = false;
}
targetAngle = EMPTY_FLOAT;
rotationSpeed = 0;
targetAngleReached = true;
}
//Calculate directional/rotational engine speed
if(hasDistanceToDrive)
if(targetReached && targetAngleReached)
{
Stop();
}
else if(targetReached || targetAngleReached)
{
CalculateDirection();
}
if(!(correctionCountdown--))
{
CalculateDirection();
}
}
//-----------------------------------------------------------------------------
void Navigator::CalculateDirection()
{
correctionCountdown = CYCLES_PER_CORRECTION;
if(targetAngle == EMPTY_FLOAT && !HasTarget()) return;
Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
if(HasTarget())
{
direction = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);
}
if(targetAngle != EMPTY_FLOAT)
{
rotationSpeed = fabs(rotationSpeed) * sign(PI - (targetAngle - locationeer->GetOrientation()));
}
CalculateEngines();
}
//-----------------------------------------------------------------------------
void Navigator::CalculateEngines()
{
if(direction != EMPTY_FLOAT)
{
float relativeDirection = this->direction - locationeer->GetOrientation();
if(targetX >= 0 && targetY >= 0)
{
float directionToTarget = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);
relativeDirection = directionToTarget - locationeer->GetOrientation();
}
float xDrive = cos(relativeDirection + PI / 6.0f);
float yDrive = sin(relativeDirection + PI / 6.0f);
float xDrive = cos(relativeDirection);
float yDrive = sin(relativeDirection);
float vLeft = (-yDrive + sqrt(3) * xDrive) / 2.0f;
float vBack = yDrive;
float vRight = (-yDrive - sqrt(3) * xDrive)/2;
float vLeft = xDrive;
float vBack = (-xDrive + sqrt(3) * yDrive) / 2.0f;
float vRight = (-xDrive - sqrt(3) * yDrive) / 2.0f;
float speedCorrection = 1.0f;
float maxEngineSpeed = 255.0f;
float minEngineSpeed = -255.0f;
float maxSingleSpeed = max(max(vLeft, vBack), vRight);
float maxSingleSpeed = max(max(fabs(vLeft), fabs(vBack)), fabs(vRight));
float calcSpeed = sqrt(vLeft * vLeft - vLeft * vRight + vRight * vRight +
vBack * (vBack + vLeft + vRight));
/*float calcSpeed = sqrt(vLeft * vLeft - vLeft * vRight + vRight * vRight +
vBack * (vBack + vLeft + vRight));*/
if(calcSpeed != 1.0f)
{
@ -204,10 +191,10 @@ void Navigator::Update()
vBack = vBack * this->robotSpeed * speedCorrection;
vRight = vRight * this->robotSpeed * speedCorrection;
maxSingleSpeed = max(max(vLeft, vBack), vRight);
maxSingleSpeed = max(max(fabs(vLeft), fabs(vBack)), fabs(vRight));
float minSingleSpeed = min(min(vLeft, vBack), vRight);
if(hasOrientationToAdjust)
if(rotationSpeed)
{
if(this->rotationSpeed > 0)
{
@ -252,7 +239,7 @@ void Navigator::Update()
curEngine->SetEnabled(true);
curEngine = NULL;
}
else if(hasOrientationToAdjust)
else if(rotationSpeed)
{
Engine* curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT);
curEngine->SetSpeed(this->rotationSpeed);

28
source/Concept/Framework/modules/executor/navigator.h
View file

@ -11,10 +11,11 @@ public:
{
this->parent = NULL;
this->moduleId = 0;
this->direction = -1.0f;
this->targetAngle = -1.0f;
this->targetX = -1.0f;
this->targetY = -1.0f;
this->correctionCountdown = CYCLES_PER_CORRECTION;
this->direction = EMPTY_FLOAT;
this->targetAngle = EMPTY_FLOAT;
this->targetX = EMPTY_FLOAT;
this->targetY = EMPTY_FLOAT;
this->robotSpeed = 0;
this->rotationSpeed = 0;
}
@ -23,15 +24,17 @@ public:
{
this->parent = NULL;
this->moduleId = navigatorId;
this->direction = -1.0f;
this->targetAngle = -1.0f;
this->targetX = -1.0f;
this->targetY = -1.0f;
this->correctionCountdown = CYCLES_PER_CORRECTION;
this->direction = EMPTY_FLOAT;
this->targetAngle = EMPTY_FLOAT;
this->targetX = EMPTY_FLOAT;
this->targetY = EMPTY_FLOAT;
this->robotSpeed = 0;
this->rotationSpeed = 0;
}
protected:
uint16 correctionCountdown;
float direction;
float targetAngle;
float targetX;
@ -54,6 +57,15 @@ public:
{
this->robotSpeed = newSpeed;
}
void CalculateDirection();
void CalculateEngines();
bool HasTarget()
{
return (targetX != EMPTY_FLOAT && targetY != EMPTY_FLOAT);
}
};
#endif