diff options
Diffstat (limited to 'source/Concept/Framework/modules/executor/navigator.c')
| -rwxr-xr-x | source/Concept/Framework/modules/executor/navigator.c | 44 |
1 files changed, 35 insertions, 9 deletions
diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c index a69c533..5dbf8bf 100755 --- a/source/Concept/Framework/modules/executor/navigator.c +++ b/source/Concept/Framework/modules/executor/navigator.c @@ -54,6 +54,30 @@ 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;*/
} //-----------------------------------------------------------------------------
@@ -154,6 +178,8 @@ void Navigator::CalculateDirection() //-----------------------------------------------------------------------------
void Navigator::CalculateEngines()
{
+ Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
+
if(direction != EMPTY_FLOAT)
{
float relativeDirection = this->direction - locationeer->GetOrientation();
@@ -165,15 +191,15 @@ void Navigator::CalculateEngines() float vBack = (-xDrive + sqrt(3) * yDrive) / 2.0f;
float vRight = (-xDrive - sqrt(3) * yDrive) / 2.0f;
- float speedCorrection = 1.0f;
+ /*float speedCorrection = 1.0f;
float maxEngineSpeed = 255.0f;
float minEngineSpeed = -255.0f;
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 = 1.0f;//sqrt(vLeft * vLeft - vLeft * vRight + vRight * vRight +
+ // vBack * (vBack + vLeft + vRight));
if(calcSpeed != 1.0f)
{
@@ -184,14 +210,14 @@ void Navigator::CalculateEngines() if(maxOverallSpeed > maxEngineSpeed)
{
robotSpeed = maxEngineSpeed / (maxSingleSpeed * speedCorrection);
- }
+ }*/
- vLeft = vLeft * this->robotSpeed * speedCorrection;
- vBack = vBack * this->robotSpeed * speedCorrection;
- vRight = vRight * this->robotSpeed * speedCorrection;
+ vLeft = vLeft * this->robotSpeed;// * speedCorrection;
+ vBack = vBack * this->robotSpeed;// * speedCorrection;
+ vRight = vRight * this->robotSpeed;// * speedCorrection;
- maxSingleSpeed = max(max(fabs(vLeft), fabs(vBack)), fabs(vRight));
+ /*maxSingleSpeed = max(max(fabs(vLeft), fabs(vBack)), fabs(vRight));
float minSingleSpeed = min(min(vLeft, vBack), vRight);
if(rotationSpeed)
@@ -226,7 +252,7 @@ void Navigator::CalculateEngines() vRight -= this->rotationSpeed;
}
}
- }
+ }*/
Engine* curEngine = parent->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT);
curEngine->SetSpeed(vLeft);
|