diff options
Diffstat (limited to 'source/Concept/Framework/modules/executor/navigator.c')
| -rwxr-xr-x | source/Concept/Framework/modules/executor/navigator.c | 149 |
1 files changed, 30 insertions, 119 deletions
diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c index 5dbf8bf..928a5ff 100755 --- a/source/Concept/Framework/modules/executor/navigator.c +++ b/source/Concept/Framework/modules/executor/navigator.c @@ -54,30 +54,6 @@ void Navigator::Drive(float newDirection, float newAngle, float newSpeed, float }
CalculateEngines();
-
- //dings,,,
-
- /*float xDrive = cos(direction + PI / 6.0f);
- float yDrive = sin(direction + PI / 6.0f);
-
- float vLeft = xDrive;
- float vBack = (-xDrive + sqrt(3) * yDrive) / 2.0f;
- float vRight = (-xDrive - sqrt(3) * yDrive) / 2.0f;
-
- vLeft = vLeft * this->robotSpeed + rotationSpeed;
- vBack = vBack * this->robotSpeed + rotationSpeed;
- vRight = vRight * this->robotSpeed + rotationSpeed;
-
- Engine* curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT);
- curEngine->SetSpeed(vLeft);
- curEngine->SetEnabled(true);
- curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_BACK);
- curEngine->SetSpeed(vBack);
- curEngine->SetEnabled(true);
- curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT);
- curEngine->SetSpeed(vRight);
- curEngine->SetEnabled(true);
- curEngine = NULL;*/
} //-----------------------------------------------------------------------------
@@ -178,108 +154,43 @@ void Navigator::CalculateDirection() //-----------------------------------------------------------------------------
void Navigator::CalculateEngines()
{
+ // Use the position_tracker
Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
-
- if(direction != EMPTY_FLOAT)
- {
- float relativeDirection = this->direction - locationeer->GetOrientation();
-
- float xDrive = cos(relativeDirection + PI / 6.0f);
- float yDrive = sin(relativeDirection + PI / 6.0f);
-
- 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;
+ // get the relative position of the robot
+ float relativeDirection = this->direction - locationeer->GetOrientation();
- float maxSingleSpeed = max(max(fabs(vLeft), fabs(vBack)), fabs(vRight));
+ // calculate x and y-koordinates
+ float xDrive = cos(relativeDirection + PI / 6.0f);
+ float yDrive = sin(relativeDirection + PI / 6.0f);
- float calcSpeed = 1.0f;//sqrt(vLeft * vLeft - vLeft * vRight + vRight * vRight +
- // vBack * (vBack + vLeft + vRight));
+ // calculate relative speed of engines
+ float vLeft = xDrive;
+ float vBack = (-xDrive + sqrt(3) * yDrive) / 2.0f;
+ float vRight = (-xDrive - sqrt(3) * yDrive) / 2.0f;
- if(calcSpeed != 1.0f)
- {
- speedCorrection = 1.0f / calcSpeed;
- }
+ // Get the maximal value
+ float speedCorrection = (float)max(max(fabs(vLeft),fabs(vBack)),fabs(vRight));
- float maxOverallSpeed = robotSpeed * maxSingleSpeed * speedCorrection;
- if(maxOverallSpeed > maxEngineSpeed)
- {
- robotSpeed = maxEngineSpeed / (maxSingleSpeed * speedCorrection);
- }*/
-
+ // calculate the correct speeds of the engines
+ vLeft = ((float)vLeft * (float)((float)this->robotSpeed / (float)speedCorrection)) + this->rotationSpeed;
+ vBack = ((float)vBack * (float)((float)this->robotSpeed / (float)speedCorrection)) + this->rotationSpeed;
+ vRight = ((float)vRight * (float)((float)this->robotSpeed / (float)speedCorrection)) + this->rotationSpeed;
- vLeft = vLeft * this->robotSpeed;// * speedCorrection;
- vBack = vBack * this->robotSpeed;// * speedCorrection;
- vRight = vRight * this->robotSpeed;// * speedCorrection;
+ //(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vLeft,10,2);
+ //(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vBack,10,3);
+ //(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vRight,10,4);
- /*maxSingleSpeed = max(max(fabs(vLeft), fabs(vBack)), fabs(vRight));
- float minSingleSpeed = min(min(vLeft, vBack), vRight);
- if(rotationSpeed)
- {
- if(this->rotationSpeed > 0)
- {
- if(maxEngineSpeed - maxSingleSpeed < this->rotationSpeed)
- {
- vLeft += maxEngineSpeed - maxSingleSpeed;
- vBack += maxEngineSpeed - maxSingleSpeed;
- vRight += maxEngineSpeed - maxSingleSpeed;
- }
- else
- {
- vLeft += this->rotationSpeed;
- vBack += this->rotationSpeed;
- vRight += this->rotationSpeed;
- }
- }
- else
- {
- if((minEngineSpeed - minSingleSpeed) < this->rotationSpeed)
- {
- vLeft -= minEngineSpeed - minSingleSpeed;
- vBack -= minEngineSpeed - minSingleSpeed;
- vRight -= minEngineSpeed - minSingleSpeed;
- }
- else
- {
- vLeft -= this->rotationSpeed;
- vBack -= this->rotationSpeed;
- vRight -= this->rotationSpeed;
- }
- }
- }*/
- Engine* curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT);
- curEngine->SetSpeed(vLeft);
- curEngine->SetEnabled(true);
- curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_BACK);
- curEngine->SetSpeed(vBack);
- curEngine->SetEnabled(true);
- curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT);
- curEngine->SetSpeed(vRight);
- curEngine->SetEnabled(true);
- curEngine = NULL;
- }
- else if(rotationSpeed)
- {
- Engine* curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT);
- curEngine->SetSpeed(this->rotationSpeed);
- curEngine->SetEnabled(true);
- curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_BACK);
- curEngine->SetSpeed(this->rotationSpeed);
- curEngine->SetEnabled(true);
- curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT);
- curEngine->SetSpeed(this->rotationSpeed);
- curEngine->SetEnabled(true);
- curEngine = NULL;
- }
- else
- {
- Stop();
- }
+ // Transfer the values to the engines
+ Engine* curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT);
+ curEngine->SetSpeed(vLeft);
+ curEngine->SetEnabled(true);
+ curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_BACK);
+ curEngine->SetSpeed(vBack);
+ curEngine->SetEnabled(true);
+ curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT);
+ curEngine->SetSpeed(vRight);
+ curEngine->SetEnabled(true);
+ curEngine = NULL;
} |