summaryrefslogtreecommitdiffstats
path: root/source/Concept/Framework/modules/executor/navigator.c
diff options
context:
space:
mode:
Diffstat (limited to 'source/Concept/Framework/modules/executor/navigator.c')
-rwxr-xr-xsource/Concept/Framework/modules/executor/navigator.c298
1 files changed, 17 insertions, 281 deletions
diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c
index 47b1c69..a69c533 100755
--- a/source/Concept/Framework/modules/executor/navigator.c
+++ b/source/Concept/Framework/modules/executor/navigator.c
@@ -1,138 +1,3 @@
-<<<<<<< .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"
//-----------------------------------------------------------------------------
@@ -150,140 +15,12 @@ void Navigator::Stop()
(parent->GetModule<Engine>(i))->SetSpeed(0);
(parent->GetModule<Engine>(i))->SetEnabled(true);
}
-}
-
-//-----------------------------------------------------------------------------
-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()
+//-----------------------------------------------------------------------------
+void Navigator::Rotate(float rotationSpeed)
{
- 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->rotationSpeed = min(fabs(rotationSpeed), 255.0f) * sign(rotationSpeed);
this->direction = EMPTY_FLOAT;
this->targetAngle = EMPTY_FLOAT;
this->targetX = EMPTY_FLOAT;
@@ -295,14 +32,14 @@ void Navigator::CalculateEngines()
(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;
- this->targetAngle = newAngle;
+ this->direction = newDirection * PI / 180.0f;
+ this->targetAngle = newAngle * PI / 180.0f;
this->targetX = EMPTY_FLOAT;
this->targetY = EMPTY_FLOAT;
this->robotSpeed = newSpeed;
@@ -317,8 +54,8 @@ void Navigator::Drive(float newDirection, float newAngle, float newSpeed, float
}
CalculateEngines();
-}
-
+}
+
//-----------------------------------------------------------------------------
void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed)
{
@@ -330,7 +67,7 @@ void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed,
this->targetX = newX;
this->targetY = newY;
this->direction = direction2d(locationeer->GetPositionX(), locationeer->GetPositionY(), targetX, targetY);;
- this->targetAngle = newAngle;
+ this->targetAngle = newAngle * PI / 180.0f;
this->robotSpeed = newSpeed;
if(targetAngle - locationeer->GetOrientation() > PI)
@@ -349,8 +86,8 @@ void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed,
}
CalculateDirection();
-}
-
+}
+
//-----------------------------------------------------------------------------
void Navigator::Update()
{
@@ -390,8 +127,8 @@ void Navigator::Update()
{
CalculateDirection();
}
-}
-
+}
+
//-----------------------------------------------------------------------------
void Navigator::CalculateDirection()
{
@@ -412,8 +149,8 @@ void Navigator::CalculateDirection()
}
CalculateEngines();
-}
-
+}
+
//-----------------------------------------------------------------------------
void Navigator::CalculateEngines()
{
@@ -519,5 +256,4 @@ void Navigator::CalculateEngines()
{
Stop();
}
-}
->>>>>>> .r199
+}
generated by cgit v1.2.3 (git 2.25.1) at 2024-04-29 03:51:18 +0200