summaryrefslogtreecommitdiffstats
path: root/source/Concept/Framework/modules/executor
diff options
context:
space:
mode:
Diffstat (limited to 'source/Concept/Framework/modules/executor')
-rwxr-xr-xsource/Concept/Framework/modules/executor/navigator.c264
-rwxr-xr-xsource/Concept/Framework/modules/executor/navigator.h73
2 files changed, 337 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
diff --git a/source/Concept/Framework/modules/executor/navigator.h b/source/Concept/Framework/modules/executor/navigator.h
index 56e7b8f..848c9fa 100755
--- a/source/Concept/Framework/modules/executor/navigator.h
+++ b/source/Concept/Framework/modules/executor/navigator.h
@@ -1,3 +1,75 @@
+<<<<<<< .mine
+#ifndef _NAVIGATOR_H
+#define _NAVIGATOR_H
+
+//#include <math.h>
+#include "../../stdafx.h"
+
+class Navigator : public IO_Module
+{
+public:
+ Navigator()
+ {
+ this->parent = NULL;
+ this->moduleId = 0;
+ 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;
+ }
+
+ Navigator(uint32 navigatorId)
+ {
+ this->parent = NULL;
+ this->moduleId = navigatorId;
+ 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;
+ float targetY;
+ float robotSpeed;
+ float rotationSpeed;
+
+public:
+ void Update();
+
+ void Stop();
+
+ void Drive(float newDirection, float newAngle, float newSpeed, float rotationSpeed);
+
+ void DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed);
+
+ void Rotate(float rotationSpeed);
+
+ void SetSpeed(float newSpeed)
+ {
+ this->robotSpeed = newSpeed;
+ }
+
+ void CalculateDirection();
+
+ void CalculateEngines();
+
+ bool HasTarget()
+ {
+ return (targetX != EMPTY_FLOAT && targetY != EMPTY_FLOAT);
+ }
+};
+
+=======
#ifndef _NAVIGATOR_H
#define _NAVIGATOR_H
@@ -68,4 +140,5 @@ public:
}
};
+>>>>>>> .r199
#endif
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-22 09:16:08 +0100