diff options
Diffstat (limited to 'source/Concept/Framework/modules/executor/navigator.c')
| -rwxr-xr-x | source/Concept/Framework/modules/executor/navigator.c | 264 |
1 files changed, 264 insertions, 0 deletions
diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c index ceeda60..47b1c69 100755 --- a/source/Concept/Framework/modules/executor/navigator.c +++ b/source/Concept/Framework/modules/executor/navigator.c @@ -1,3 +1,138 @@ +<<<<<<< .mine +#include "navigator.h" + +//----------------------------------------------------------------------------- +void Navigator::Stop() +{ + this->direction = 0; + this->targetAngle = 0; + //this->targetX = EMPTY_FLOAT; + //this->targetY = EMPTY_FLOAT; + this->robotSpeed = 0; + this->rotationSpeed = 0; + + /*for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++) + { + (parent->GetModule<Engine>(i))->SetSpeed(0); + (parent->GetModule<Engine>(i))->SetEnabled(true); + }*/ +} + +//----------------------------------------------------------------------------- +void Navigator::Rotate(float newTargetAngle,float newRotationSpeed) +{ + this->rotationSpeed = min(newRotationSpeed, 255.0f);; + this->direction = 0; + this->targetAngle = newTargetAngle; + //this->targetX = EMPTY_FLOAT; + //this->targetY = EMPTY_FLOAT; + this->robotSpeed = 0; + + /*for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++) + { + (parent->GetModule<Engine>(i))->SetSpeed(rotationSpeed); + (parent->GetModule<Engine>(i))->SetEnabled(true); + }*/ +} + +//----------------------------------------------------------------------------- +void Navigator::Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed) +{ + this->rotationSpeed = min(rotationSpeed, 255.0f); + this->direction = newDirection*PI/180; + this->targetAngle = newAngle; + this->targetX = EMPTY_FLOAT; + this->targetY = EMPTY_FLOAT; + this->robotSpeed = newSpeed; + + if(targetAngle == EMPTY_FLOAT) + { + rotationSpeed = 0; + } + else + { + rotationSpeed = fabs(rotationSpeed) * sign(PI - (targetAngle - (parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN))->GetOrientation())); + } + + CalculateEngines(); +} + +//----------------------------------------------------------------------------- +void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed) +{ + if(newX < 0 || newY < 0) return; + + Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(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; + } + } + + CalculateDirection(); +} + +//----------------------------------------------------------------------------- +void Navigator::Update() +{ + Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN); + + bool targetReached = false; + bool targetAngleReached = false; + + if(HasTarget() && distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < 1.0f) + { + targetX = EMPTY_FLOAT; + targetY = EMPTY_FLOAT; + direction = EMPTY_FLOAT; + robotSpeed = 0; + + targetReached = true; + } + + if(targetAngle != EMPTY_FLOAT && fabs(targetAngle - locationeer->GetOrientation()) < 0.1f) + { + targetAngle = EMPTY_FLOAT; + rotationSpeed = 0; + + targetAngleReached = true; + } + + if(targetReached && targetAngleReached) + { + Stop(); + } + else if(targetReached || targetAngleReached) + { + CalculateDirection(); + } + + if(!(correctionCountdown--)) + { + CalculateDirection(); + } +} + +//----------------------------------------------------------------------------- +void Navigator::CalculateDirection() +======= #include "navigator.h"
//-----------------------------------------------------------------------------
@@ -19,7 +154,135 @@ void Navigator::Stop() //-----------------------------------------------------------------------------
void Navigator::Rotate(float rotationSpeed)
+>>>>>>> .r199 {
+<<<<<<< .mine + Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN); + correctionCountdown = CYCLES_PER_CORRECTION; + + if(targetAngle == EMPTY_FLOAT && !HasTarget()) return; + + if(HasTarget()) + { + direction = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY); + } + + if(targetAngle != EMPTY_FLOAT) + { + rotationSpeed = fabs(rotationSpeed) * sign(PI - (targetAngle - locationeer->GetOrientation())); + } + + CalculateEngines(); +} + +//----------------------------------------------------------------------------- +void Navigator::CalculateEngines() +{ + 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; + + float maxSingleSpeed = max(max(fabs(vLeft), fabs(vBack)), fabs(vRight)); + + float calcSpeed = sqrt(vLeft * vLeft - vLeft * vRight + vRight * vRight + + vBack * (vBack + vLeft + vRight)); + + if(calcSpeed != 1.0f) + { + speedCorrection = 1.0f / calcSpeed; + } + + float maxOverallSpeed = robotSpeed * maxSingleSpeed * speedCorrection; + if(maxOverallSpeed > maxEngineSpeed) + { + robotSpeed = maxEngineSpeed / (maxSingleSpeed * 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)); + 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(); + } +} +======= this->rotationSpeed = min(rotationSpeed, 255.0f);;
this->direction = EMPTY_FLOAT;
this->targetAngle = EMPTY_FLOAT;
@@ -257,3 +520,4 @@ void Navigator::CalculateEngines() Stop();
}
}
+>>>>>>> .r199 |