diff options
Diffstat (limited to 'source/Concept/Framework/modules/executor/navigator.c')
| -rwxr-xr-x | source/Concept/Framework/modules/executor/navigator.c | 199 |
1 files changed, 199 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
+}*/
|