From 22433e52a7b0f2e5c548edb915ea11a38a8077ad Mon Sep 17 00:00:00 2001 From: sicarius Date: Tue, 20 Feb 2007 21:01:02 +0000 Subject: +++ Codework, Navigator&Positiontracker done (buggy?) --- .../Concept/Framework/modules/executor/navigator.c | 44 +++++++++++++++++----- 1 file changed, 35 insertions(+), 9 deletions(-) (limited to 'source/Concept/Framework/modules/executor/navigator.c') 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(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;*/ } //----------------------------------------------------------------------------- @@ -154,6 +178,8 @@ void Navigator::CalculateDirection() //----------------------------------------------------------------------------- void Navigator::CalculateEngines() { + Position_Tracker* locationeer = parent->GetModule(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(IO_ENGINE_DRIVE_LEFT); curEngine->SetSpeed(vLeft); -- cgit v1.2.3