From da9860f9a414d8ab403496ac74a51afc5ad3e48b Mon Sep 17 00:00:00 2001 From: sicarius Date: Sun, 18 Feb 2007 17:57:03 +0000 Subject: +++ Additional Codework --- .../Concept/Framework/modules/executor/navigator.c | 223 ++++++++++++++------- 1 file changed, 148 insertions(+), 75 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 459d4c1..691b1d0 100755 --- a/source/Concept/Framework/modules/executor/navigator.c +++ b/source/Concept/Framework/modules/executor/navigator.c @@ -7,7 +7,7 @@ void Navigator::Stop() this->targetAngle = -1.0f; this->targetX = -1.0f; this->targetY = -1.0f; - this->speed = 0; + this->robotSpeed = 0; this->rotationSpeed = 0; for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++) @@ -20,12 +20,12 @@ void Navigator::Stop() //----------------------------------------------------------------------------- void Navigator::Rotate(float rotationSpeed) { - this->rotationSpeed = rotationSpeed; + this->rotationSpeed = min(rotationSpeed, 255.0f);; this->direction = -1.0f; this->targetAngle = -1.0f; this->targetX = -1.0f; this->targetY = -1.0f; - this->speed = 0; + this->robotSpeed = 0; for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++) { @@ -34,6 +34,66 @@ void Navigator::Rotate(float rotationSpeed) } } +//----------------------------------------------------------------------------- +void Navigator::Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed) +{ + this->rotationSpeed = min(rotationSpeed, 255.0f); + this->direction = newDirection; + this->targetAngle = newAngle; + this->targetX = -1.0f; + this->targetY = -1.0f; + this->robotSpeed = newSpeed; + + if(targetAngle - (parent->GetModule(IO_POSITION_TRACKER_MAIN))->GetOrientation() > PI) + { + if(rotationSpeed > 0) + { + rotationSpeed = -rotationSpeed; + } + } + else + { + if(rotationSpeed < 0) + { + rotationSpeed = -rotationSpeed; + } + } + + Update(); +} + +//----------------------------------------------------------------------------- +void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed) +{ + if(newX < 0 || newY < 0) return; + + Position_Tracker* locationeer = parent->GetModule(IO_POSITION_TRACKER_MAIN); + + this->rotationSpeed = min(rotationSpeed, 255.0f); + this->targetX = newX; + this->targetY = newY; + this->direction = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);; + this->targetAngle = newAngle; + this->robotSpeed = newSpeed; + + if(targetAngle - locationeer->GetOrientation() > PI) + { + if(rotationSpeed > 0) + { + rotationSpeed = -rotationSpeed; + } + } + else + { + if(rotationSpeed < 0) + { + rotationSpeed = -rotationSpeed; + } + } + + Update(); +} + //----------------------------------------------------------------------------- void Navigator::Update() { @@ -103,97 +163,110 @@ void Navigator::Update() //Calculate directional/rotational engine speed if(hasDistanceToDrive) { + float relativeDirection = this->direction - locationeer->GetOrientation(); - - float maxRobotSpeed = 255.0f * sqrt(2) / 2.0f; - - if(speed > maxRobotSpeed) + if(targetX >= 0 && targetY >= 0) { - speed = maxRobotSpeed; + float directionToTarget = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY); + relativeDirection = directionToTarget - locationeer->GetOrientation(); } - maxMotorSpeed = speed / (sqrt(2) / 2); + float xDrive = cos(relativeDirection); + float yDrive = sin(relativeDirection); + float vLeft = (-yDrive + sqrt(3) * xDrive) / 2.0f; + float vBack = yDrive; + float vRight = (-yDrive - sqrt(3) * xDrive)/2; + float speedCorrection = 1.0f; - relAngel = direction - orientation - - robotSpeed = sin(45) * maxMotorSpeed - maxMotorSpeed = robotSpeed / sin(45) + float maxEngineSpeed = 255.0f; + float minEngineSpeed = -255.0f; - if(relAngel > 45) - { - sin(relAngel) * (speed / sin(45)) * sin(relAngel + 45); + float maxSingleSpeed = max(max(vLeft, vBack), vRight); - back = speed / sin(relAngel); - } - else + float calcSpeed = sqrt(vLeft * vLeft - vLeft * vRight + vRight * vRight + + vBack * (vBack + vLeft + vRight)); + + if(calcSpeed != 1.0f) { - + speedCorrection = 1.0f / calcSpeed; } - left = - back = sin(relAngel) * speed - - - direction = 0: - orientation = 0: - left = speed - right = -speed - back = 0 + float maxOverallSpeed = robotSpeed * maxSingleSpeed * speedCorrection; + if(maxOverallSpeed > maxEngineSpeed) + { + robotSpeed = maxEngineSpeed / (maxSingleSpeed * speedCorrection); + } + - direction = 0: - orientation = 90: - left = speed - right = speed - back = (sinVcos)(45) * speed + vLeft = vLeft * this->robotSpeed * speedCorrection; + vBack = vBack * this->robotSpeed * speedCorrection; + vRight = vRight * this->robotSpeed * speedCorrection; - direction = 0: - orientation = 45: - left = speed - right = 0 - back = -speed + maxSingleSpeed = max(max(vLeft, vBack), vRight); + float minSingleSpeed = min(min(vLeft, vBack), vRight); - direction = 0: - orientation = 180: - left = -speed - right = speed - back = 0 + if(hasOrientationToAdjust) + { + 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(!hasOrientationToAdjust) + else if(hasOrientationToAdjust) { - Stop(); - } + 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(); } } - -// Aktualieren ohne Parameter -/*void Navigator::Update() { - // Richtung in x und y-Kompontente zerlegen - double y = cos((double)direction*0.01745); // richtung ist winkel - double x = sin((double)direction*0.01745); - - // Abweichung der Ausrichtung ermitteln(als winkel) - int w = sensor.GetAusrichtung() - ausrichtung; - - // Stärke der einzelnen Motoren berechnen - double v0 = (-x+sqrt(3)*y)/2; - double v1 = x; - double v2 = (-x-sqrt(3)*y)/2; - - // Ausgerechnete Stärke an die Motoren übergeben - board.motor(0,(int)((double)v0*speed +w)); - board.motor(1,(int)((double)v1*speed +w)); - board.motor(2,(int)((double)v2*speed +w)); -} - -// Aktualieren mit allen Parametern -void Navigator::Drive(float newDirection, float newAngle, float newSpeed) { - SetDirection(newDirection); - SetAngle(newAngle); - SetSpeed(newSpeed); - Update(); // Und anwenden -}*/ -- cgit v1.2.3