diff options
Diffstat (limited to 'source/Concept/Framework/modules/executor/navigator.c')
| -rwxr-xr-x | source/Concept/Framework/modules/executor/navigator.c | 159 |
1 files changed, 73 insertions, 86 deletions
diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c index 691b1d0..ceeda60 100755 --- a/source/Concept/Framework/modules/executor/navigator.c +++ b/source/Concept/Framework/modules/executor/navigator.c @@ -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;
+ targetReached = true;
}
- else if(targetX >= 0 && targetY >= 0)
+
+ if(targetAngle != EMPTY_FLOAT && fabs(targetAngle - locationeer->GetOrientation()) < 0.1f)
{
- if(distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < 1.0f)
- {
- targetX = -1.0f;
- targetY = -1.0f;
+ targetAngle = EMPTY_FLOAT;
+ rotationSpeed = 0;
- hasDistanceToDrive = false;
- }
- else
- {
- hasDistanceToDrive = true;
- }
+ targetAngleReached = true;
}
- else
+
+ if(targetReached && targetAngleReached)
{
- if(direction >= 0)
- {
- hasDistanceToDrive = true;
- }
- else
- {
- hasDistanceToDrive = false;
- }
+ Stop();
+ }
+ else if(targetReached || targetAngleReached)
+ {
+ CalculateDirection();
}
- //Check for orientation to adjust
- if(targetAngle >= 0)
+ if(!(correctionCountdown--))
{
- if(fabs(targetAngle - locationeer->GetOrientation()) < 0.1f)
- {
- hasOrientationToAdjust = false;
- }
- else
- {
- hasOrientationToAdjust = true;
- }
+ CalculateDirection();
}
- else
+}
+
+//-----------------------------------------------------------------------------
+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())
{
- if(rotationSpeed != 0)
- {
- hasOrientationToAdjust = true;
- }
- else
- {
- hasOrientationToAdjust = false;
- }
+ direction = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);
}
- //Calculate directional/rotational engine speed
- if(hasDistanceToDrive)
+ if(targetAngle != EMPTY_FLOAT)
{
- float relativeDirection = this->direction - locationeer->GetOrientation();
+ rotationSpeed = fabs(rotationSpeed) * sign(PI - (targetAngle - locationeer->GetOrientation()));
+ }
- if(targetX >= 0 && targetY >= 0)
- {
- float directionToTarget = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);
- relativeDirection = directionToTarget - locationeer->GetOrientation();
- }
+ CalculateEngines();
+}
+
+//-----------------------------------------------------------------------------
+void Navigator::CalculateEngines()
+{
+ if(direction != EMPTY_FLOAT)
+ {
+ float relativeDirection = this->direction - locationeer->GetOrientation();
- float xDrive = cos(relativeDirection);
- float yDrive = sin(relativeDirection);
+ float xDrive = cos(relativeDirection + PI / 6.0f);
+ float yDrive = sin(relativeDirection + PI / 6.0f);
- 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);
|