From 655dd7522fb60e2ef0e68437337178184109f347 Mon Sep 17 00:00:00 2001 From: sicarius Date: Thu, 22 Feb 2007 13:12:03 +0000 Subject: Codework --- .../Concept/Framework/modules/executor/navigator.c | 149 +++++---------------- 1 file changed, 30 insertions(+), 119 deletions(-) (limited to 'source/Concept/Framework/modules/executor') 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(IO_ENGINE_DRIVE_LEFT); - curEngine->SetSpeed(vLeft); - curEngine->SetEnabled(true); - curEngine = parent->GetModule(IO_ENGINE_DRIVE_BACK); - curEngine->SetSpeed(vBack); - curEngine->SetEnabled(true); - curEngine = parent->GetModule(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(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(IO_DISPLAY_MAIN))->Print(vLeft,10,2); + //(parent->GetModule(IO_DISPLAY_MAIN))->Print(vBack,10,3); + //(parent->GetModule(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(IO_ENGINE_DRIVE_LEFT); - curEngine->SetSpeed(vLeft); - curEngine->SetEnabled(true); - curEngine = parent->GetModule(IO_ENGINE_DRIVE_BACK); - curEngine->SetSpeed(vBack); - curEngine->SetEnabled(true); - curEngine = parent->GetModule(IO_ENGINE_DRIVE_RIGHT); - curEngine->SetSpeed(vRight); - curEngine->SetEnabled(true); - curEngine = NULL; - } - else if(rotationSpeed) - { - Engine* curEngine = parent->GetModule(IO_ENGINE_DRIVE_LEFT); - curEngine->SetSpeed(this->rotationSpeed); - curEngine->SetEnabled(true); - curEngine = parent->GetModule(IO_ENGINE_DRIVE_BACK); - curEngine->SetSpeed(this->rotationSpeed); - curEngine->SetEnabled(true); - curEngine = parent->GetModule(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(IO_ENGINE_DRIVE_LEFT); + curEngine->SetSpeed(vLeft); + curEngine->SetEnabled(true); + curEngine = parent->GetModule(IO_ENGINE_DRIVE_BACK); + curEngine->SetSpeed(vBack); + curEngine->SetEnabled(true); + curEngine = parent->GetModule(IO_ENGINE_DRIVE_RIGHT); + curEngine->SetSpeed(vRight); + curEngine->SetEnabled(true); + curEngine = NULL; } -- cgit v1.2.3