Diff: lib/S-ShapeModel/position_controller.cpp

Revision:

0:76663617eca3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lib/S-ShapeModel/position_controller.cpp	Fri Sep 13 02:15:30 2019 +0000
@@ -0,0 +1,81 @@
+#include "position_controller.h"
+
+PositionController::PositionController(double accDistance_, double decDistance_, double initialVelocity_, double terminalVelocity_, float maxVelocity_) :
+    accDistance(accDistance_),
+    decDistance(decDistance_),
+    initialVelocity(initialVelocity_),
+    terminalVelocity(terminalVelocity_),
+    maxVelocity(maxVelocity_)
+{
+}
+
+void PositionController::targetXY(int targetX_, int targetY_)
+{
+    targetX = targetX_;
+    targetY = targetY_;
+    target = hypot((double)targetX, (double)targetY);
+    radians = atan2((double)targetY, (double)targetX);
+    if(accDistance * fabs(maxVelocity - initialVelocity) + decDistance * fabs(maxVelocity - terminalVelocity) < target) {
+        enoughDistance = true;
+    } else {
+        enoughDistance = false;
+        maxVelocity = initialVelocity + (maxVelocity - initialVelocity) * (target/(accDistance+decDistance));
+        accTrue = accDistance * (target/(accDistance+decDistance));
+        decTrue = decDistance * (target/(accDistance+decDistance));
+    }
+}
+
+double PositionController::generateSineWave(double x, double initV, double termV, double start, double length)
+{
+    return ((termV - initV) * sin(M_PI * ((2.0 * x - 2.0 * start - length)/(2.0 * length))) + termV + initV)/2.0;
+}
+
+double PositionController::getVelocity(int position)
+{
+    double vel = 0;
+    if(enoughDistance) {
+        if(position < 0) {
+            vel = initialVelocity;
+        } else if(position < accDistance * fabs(maxVelocity - initialVelocity)) {
+            vel = generateSineWave(position, initialVelocity, maxVelocity, 0, accDistance * fabs(maxVelocity - initialVelocity));
+        } else if(position < target - (decDistance * fabs(maxVelocity - terminalVelocity))) {
+            vel = maxVelocity;
+        } else if(position < target) {
+            vel = -generateSineWave(position, -maxVelocity, -terminalVelocity, target - decDistance * fabs(maxVelocity - terminalVelocity), decDistance * fabs(maxVelocity - terminalVelocity));
+        } else if(position < 2 * target) {
+            vel = -generateSineWave(position, -terminalVelocity, maxVelocity, target, target);
+        } else {
+            vel = -maxVelocity;
+        }
+    } else {
+        if(position < 0) {
+            vel = initialVelocity;
+        } else if(position < accTrue) {
+            vel = generateSineWave(position, initialVelocity, maxVelocity, 0, accTrue);
+        } else if(position < target) {
+            vel = -generateSineWave(position, -maxVelocity, -terminalVelocity, target - decTrue, decTrue);
+        } else if(position < 2 * target) {
+            vel = -generateSineWave(position, -terminalVelocity, maxVelocity, target, target);
+        } else {
+            vel = -maxVelocity;
+        }
+    }
+    return vel;
+}
+
+void PositionController::compute(int positionX, int positionY)
+{
+    velocity[0] = getVelocity(((double)positionX / (double)targetX)*target) * cos(radians);
+    velocity[1] = getVelocity(((double)positionY / (double)targetY)*target) * sin(radians);
+}
+
+double PositionController::getVelocityX()
+{
+    return velocity[0];
+}
+
+double PositionController::getVelocityY()
+{
+    return velocity[1];
+}
+