From 354ef5f0d0ad5487288e8d25fc9b1fcecf730eba Mon Sep 17 00:00:00 2001 From: masterm Date: Mon, 19 Feb 2007 17:15:02 +0000 Subject: +++ improved navigator --- .../Concept/Framework/modules/executor/navigator.c | 159 ++++++++++----------- .../Concept/Framework/modules/executor/navigator.h | 28 ++-- .../Framework/modules/interpreter/ball_tracker.c | 6 +- .../Framework/modules/interpreter/ball_tracker.h | 4 +- 4 files changed, 98 insertions(+), 99 deletions(-) (limited to 'source/Concept/Framework/modules') diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c index 691b1d0..ceeda60 100755 --- a/source/Concept/Framework/modules/executor/navigator.c +++ b/source/Concept/Framework/modules/executor/navigator.c @@ -3,10 +3,10 @@ //----------------------------------------------------------------------------- void Navigator::Stop() { - this->direction = -1.0f; - this->targetAngle = -1.0f; - this->targetX = -1.0f; - this->targetY = -1.0f; + this->direction = EMPTY_FLOAT; + this->targetAngle = EMPTY_FLOAT; + this->targetX = EMPTY_FLOAT; + this->targetY = EMPTY_FLOAT; this->robotSpeed = 0; this->rotationSpeed = 0; @@ -21,10 +21,10 @@ void Navigator::Stop() void Navigator::Rotate(float rotationSpeed) { this->rotationSpeed = min(rotationSpeed, 255.0f);; - this->direction = -1.0f; - this->targetAngle = -1.0f; - this->targetX = -1.0f; - this->targetY = -1.0f; + this->direction = EMPTY_FLOAT; + this->targetAngle = EMPTY_FLOAT; + this->targetX = EMPTY_FLOAT; + this->targetY = EMPTY_FLOAT; this->robotSpeed = 0; for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++) @@ -40,26 +40,20 @@ void Navigator::Drive(float newDirection, float newAngle, float newSpeed, float this->rotationSpeed = min(rotationSpeed, 255.0f); this->direction = newDirection; this->targetAngle = newAngle; - this->targetX = -1.0f; - this->targetY = -1.0f; + this->targetX = EMPTY_FLOAT; + this->targetY = EMPTY_FLOAT; this->robotSpeed = newSpeed; - if(targetAngle - (parent->GetModule(IO_POSITION_TRACKER_MAIN))->GetOrientation() > PI) + if(targetAngle == EMPTY_FLOAT) { - if(rotationSpeed > 0) - { - rotationSpeed = -rotationSpeed; - } + rotationSpeed = 0; } else { - if(rotationSpeed < 0) - { - rotationSpeed = -rotationSpeed; - } + rotationSpeed = fabs(rotationSpeed) * sign(PI - (targetAngle - (parent->GetModule(IO_POSITION_TRACKER_MAIN))->GetOrientation())); } - Update(); + CalculateEngines(); } //----------------------------------------------------------------------------- @@ -91,7 +85,7 @@ void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed, } } - Update(); + CalculateDirection(); } //----------------------------------------------------------------------------- @@ -99,94 +93,87 @@ void Navigator::Update() { Position_Tracker* locationeer = parent->GetModule(IO_POSITION_TRACKER_MAIN); - bool hasDistanceToDrive = true; - bool hasOrientationToAdjust = true; + bool targetReached = false; + bool targetAngleReached = false; - //Check for distance to drive - if((targetX >= 0) != (targetY >= 0)) + if(HasTarget() && distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < 1.0f) { - targetX = -1.0f; - targetY = -1.0f; + targetX = EMPTY_FLOAT; + targetY = EMPTY_FLOAT; + direction = EMPTY_FLOAT; + robotSpeed = 0; - hasDistanceToDrive = false; + targetReached = true; } - else if(targetX >= 0 && targetY >= 0) + + if(targetAngle != EMPTY_FLOAT && fabs(targetAngle - locationeer->GetOrientation()) < 0.1f) { - if(distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < 1.0f) - { - targetX = -1.0f; - targetY = -1.0f; + targetAngle = EMPTY_FLOAT; + rotationSpeed = 0; - hasDistanceToDrive = false; - } - else - { - hasDistanceToDrive = true; - } + targetAngleReached = true; } - else + + if(targetReached && targetAngleReached) { - if(direction >= 0) - { - hasDistanceToDrive = true; - } - else - { - hasDistanceToDrive = false; - } + Stop(); + } + else if(targetReached || targetAngleReached) + { + CalculateDirection(); } - //Check for orientation to adjust - if(targetAngle >= 0) + if(!(correctionCountdown--)) { - if(fabs(targetAngle - locationeer->GetOrientation()) < 0.1f) - { - hasOrientationToAdjust = false; - } - else - { - hasOrientationToAdjust = true; - } + CalculateDirection(); } - else +} + +//----------------------------------------------------------------------------- +void Navigator::CalculateDirection() +{ + correctionCountdown = CYCLES_PER_CORRECTION; + + if(targetAngle == EMPTY_FLOAT && !HasTarget()) return; + + Position_Tracker* locationeer = parent->GetModule(IO_POSITION_TRACKER_MAIN); + + if(HasTarget()) { - if(rotationSpeed != 0) - { - hasOrientationToAdjust = true; - } - else - { - hasOrientationToAdjust = false; - } + direction = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY); } - //Calculate directional/rotational engine speed - if(hasDistanceToDrive) + if(targetAngle != EMPTY_FLOAT) { - float relativeDirection = this->direction - locationeer->GetOrientation(); + rotationSpeed = fabs(rotationSpeed) * sign(PI - (targetAngle - locationeer->GetOrientation())); + } - if(targetX >= 0 && targetY >= 0) - { - float directionToTarget = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY); - relativeDirection = directionToTarget - locationeer->GetOrientation(); - } + CalculateEngines(); +} + +//----------------------------------------------------------------------------- +void Navigator::CalculateEngines() +{ + if(direction != EMPTY_FLOAT) + { + float relativeDirection = this->direction - locationeer->GetOrientation(); - float xDrive = cos(relativeDirection); - float yDrive = sin(relativeDirection); + float xDrive = cos(relativeDirection + PI / 6.0f); + float yDrive = sin(relativeDirection + PI / 6.0f); - float vLeft = (-yDrive + sqrt(3) * xDrive) / 2.0f; - float vBack = yDrive; - float vRight = (-yDrive - sqrt(3) * xDrive)/2; + 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(vLeft, vBack), vRight); + 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 = sqrt(vLeft * vLeft - vLeft * vRight + vRight * vRight + + vBack * (vBack + vLeft + vRight));*/ if(calcSpeed != 1.0f) { @@ -204,10 +191,10 @@ void Navigator::Update() vBack = vBack * this->robotSpeed * speedCorrection; vRight = vRight * this->robotSpeed * speedCorrection; - maxSingleSpeed = max(max(vLeft, vBack), vRight); + maxSingleSpeed = max(max(fabs(vLeft), fabs(vBack)), fabs(vRight)); float minSingleSpeed = min(min(vLeft, vBack), vRight); - if(hasOrientationToAdjust) + if(rotationSpeed) { if(this->rotationSpeed > 0) { @@ -252,7 +239,7 @@ void Navigator::Update() curEngine->SetEnabled(true); curEngine = NULL; } - else if(hasOrientationToAdjust) + else if(rotationSpeed) { Engine* curEngine = parent->GetModule(IO_ENGINE_DRIVE_LEFT); curEngine->SetSpeed(this->rotationSpeed); diff --git a/source/Concept/Framework/modules/executor/navigator.h b/source/Concept/Framework/modules/executor/navigator.h index 8da52f6..56e7b8f 100755 --- a/source/Concept/Framework/modules/executor/navigator.h +++ b/source/Concept/Framework/modules/executor/navigator.h @@ -11,10 +11,11 @@ public: { this->parent = NULL; this->moduleId = 0; - this->direction = -1.0f; - this->targetAngle = -1.0f; - this->targetX = -1.0f; - this->targetY = -1.0f; + this->correctionCountdown = CYCLES_PER_CORRECTION; + this->direction = EMPTY_FLOAT; + this->targetAngle = EMPTY_FLOAT; + this->targetX = EMPTY_FLOAT; + this->targetY = EMPTY_FLOAT; this->robotSpeed = 0; this->rotationSpeed = 0; } @@ -23,15 +24,17 @@ public: { this->parent = NULL; this->moduleId = navigatorId; - this->direction = -1.0f; - this->targetAngle = -1.0f; - this->targetX = -1.0f; - this->targetY = -1.0f; + this->correctionCountdown = CYCLES_PER_CORRECTION; + this->direction = EMPTY_FLOAT; + this->targetAngle = EMPTY_FLOAT; + this->targetX = EMPTY_FLOAT; + this->targetY = EMPTY_FLOAT; this->robotSpeed = 0; this->rotationSpeed = 0; } protected: + uint16 correctionCountdown; float direction; float targetAngle; float targetX; @@ -54,6 +57,15 @@ public: { this->robotSpeed = newSpeed; } + + void CalculateDirection(); + + void CalculateEngines(); + + bool HasTarget() + { + return (targetX != EMPTY_FLOAT && targetY != EMPTY_FLOAT); + } }; #endif diff --git a/source/Concept/Framework/modules/interpreter/ball_tracker.c b/source/Concept/Framework/modules/interpreter/ball_tracker.c index 6679d3e..1701121 100755 --- a/source/Concept/Framework/modules/interpreter/ball_tracker.c +++ b/source/Concept/Framework/modules/interpreter/ball_tracker.c @@ -68,7 +68,7 @@ void Ball_Tracker::Update() mainDirection = 33.0f * PI / 18.0f; break; default: - mainDirection = -1.0f; + mainDirection = EMPTY_FLOAT; return; break; } @@ -104,7 +104,7 @@ void Ball_Tracker::Update() secondDirection = 33.0f * PI / 18.0f; break; default: - secondDirection = -1.0f; + secondDirection = EMPTY_FLOAT; return; break; } @@ -120,6 +120,6 @@ void Ball_Tracker::Update() } else { - direction = -1.0f; + direction = EMPTY_FLOAT; } } diff --git a/source/Concept/Framework/modules/interpreter/ball_tracker.h b/source/Concept/Framework/modules/interpreter/ball_tracker.h index ed8801f..bea4a19 100755 --- a/source/Concept/Framework/modules/interpreter/ball_tracker.h +++ b/source/Concept/Framework/modules/interpreter/ball_tracker.h @@ -10,14 +10,14 @@ public: { this->parent = NULL; this->moduleId = 0; - this->direction = -1.0f; + this->direction = EMPTY_FLOAT; } Ball_Tracker(uint32 trackerId) { this->parent = NULL; this->moduleId = trackerId; - this->direction = -1.0f; + this->direction = EMPTY_FLOAT; } protected: -- cgit v1.2.3