diff options
Diffstat (limited to 'source/Concept/Framework/modules/executor')
| -rwxr-xr-x | source/Concept/Framework/modules/executor/navigator.c | 199 | ||||
| -rwxr-xr-x | source/Concept/Framework/modules/executor/navigator.h | 53 |
2 files changed, 252 insertions, 0 deletions
diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c new file mode 100755 index 0000000..459d4c1 --- /dev/null +++ b/source/Concept/Framework/modules/executor/navigator.c @@ -0,0 +1,199 @@ +#include "navigator.h"
+
+//-----------------------------------------------------------------------------
+void Navigator::Stop()
+{
+ this->direction = -1.0f;
+ this->targetAngle = -1.0f;
+ this->targetX = -1.0f;
+ this->targetY = -1.0f;
+ this->speed = 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 rotationSpeed)
+{
+ this->rotationSpeed = rotationSpeed;
+ this->direction = -1.0f;
+ this->targetAngle = -1.0f;
+ this->targetX = -1.0f;
+ this->targetY = -1.0f;
+ this->speed = 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::Update()
+{
+ Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
+
+ bool hasDistanceToDrive = true;
+ bool hasOrientationToAdjust = true;
+
+ //Check for distance to drive
+ if((targetX >= 0) != (targetY >= 0))
+ {
+ targetX = -1.0f;
+ targetY = -1.0f;
+
+ hasDistanceToDrive = false;
+ }
+ else if(targetX >= 0 && targetY >= 0)
+ {
+ if(distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < 1.0f)
+ {
+ targetX = -1.0f;
+ targetY = -1.0f;
+
+ hasDistanceToDrive = false;
+ }
+ else
+ {
+ hasDistanceToDrive = true;
+ }
+ }
+ else
+ {
+ if(direction >= 0)
+ {
+ hasDistanceToDrive = true;
+ }
+ else
+ {
+ hasDistanceToDrive = false;
+ }
+ }
+
+ //Check for orientation to adjust
+ if(targetAngle >= 0)
+ {
+ if(fabs(targetAngle - locationeer->GetOrientation()) < 0.1f)
+ {
+ hasOrientationToAdjust = false;
+ }
+ else
+ {
+ hasOrientationToAdjust = true;
+ }
+ }
+ else
+ {
+ if(rotationSpeed != 0)
+ {
+ hasOrientationToAdjust = true;
+ }
+ else
+ {
+ hasOrientationToAdjust = false;
+ }
+ }
+
+ //Calculate directional/rotational engine speed
+ if(hasDistanceToDrive)
+ {
+
+
+ float maxRobotSpeed = 255.0f * sqrt(2) / 2.0f;
+
+ if(speed > maxRobotSpeed)
+ {
+ speed = maxRobotSpeed;
+ }
+
+ maxMotorSpeed = speed / (sqrt(2) / 2);
+
+
+
+ relAngel = direction - orientation
+
+ robotSpeed = sin(45) * maxMotorSpeed
+ maxMotorSpeed = robotSpeed / sin(45)
+
+ if(relAngel > 45)
+ {
+ sin(relAngel) * (speed / sin(45)) * sin(relAngel + 45);
+
+ back = speed / sin(relAngel);
+ }
+ else
+ {
+
+ }
+
+ left =
+ back = sin(relAngel) * speed
+
+
+ direction = 0:
+ orientation = 0:
+ left = speed
+ right = -speed
+ back = 0
+
+ direction = 0:
+ orientation = 90:
+ left = speed
+ right = speed
+ back = (sinVcos)(45) * speed
+
+ direction = 0:
+ orientation = 45:
+ left = speed
+ right = 0
+ back = -speed
+
+ direction = 0:
+ orientation = 180:
+ left = -speed
+ right = speed
+ back = 0
+
+ }
+ else if(!hasOrientationToAdjust)
+ {
+ Stop();
+ }
+ else
+ {
+ }
+}
+
+// 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
+}*/
diff --git a/source/Concept/Framework/modules/executor/navigator.h b/source/Concept/Framework/modules/executor/navigator.h new file mode 100755 index 0000000..ed8b3c8 --- /dev/null +++ b/source/Concept/Framework/modules/executor/navigator.h @@ -0,0 +1,53 @@ +#ifndef _NAVIGATOR_H
+#define _NAVIGATOR_H
+
+#include "../../stdafx.h"
+
+class Navigator : public IO_Module
+{
+public:
+ Navigator()
+ {
+ this->parent = NULL;
+ this->moduleId = 0;
+ this->direction = -1.0f;
+ this->targetAngle = -1.0f;
+ this->targetX = -1.0f;
+ this->targetY = -1.0f;
+ this->speed = 0;
+ this->rotationSpeed = 0;
+ }
+
+ Navigator(uint32 navigatorId)
+ {
+ this->parent = NULL;
+ this->moduleId = navigatorId;
+ this->direction = -1.0f;
+ this->targetAngle = -1.0f;
+ this->targetX = -1.0f;
+ this->targetY = -1.0f;
+ this->speed = 0;
+ this->rotationSpeed = 0;
+ }
+
+protected:
+ float direction;
+ float targetAngle;
+ float targetX;
+ float targetY;
+ float speed;
+ 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);
+
+ void Rotate(float rotationSpeed);
+};
+
+#endif |