Diff: position_controller.cpp

Revision:

2:2c5e4f521390

Parent:

1:aed2cca33061

Child:

3:65ae32169b33

--- a/position_controller.cpp	Fri Aug 03 03:57:44 2018 +0000
+++ b/position_controller.cpp	Mon Aug 20 13:54:33 2018 +0900
@@ -1,70 +1,78 @@
 #include "position_controller.h"
 
-PositionController::PositionController(int accDist, float maxV, int targetX, int targetY) :
-    accelerationDistance(accDist),
-    maxVelocity(maxV),
-    targetDistance(hypot(targetX, targetY)),
-    targetDistanceX(targetX),
-    targetDistanceY(targetY),
-    targetRadians(atan2(targetY, targetX))
+PositionController::PositionController(double accDistance_, double decDistance_, double initialVelocity_, double terminalVelocity_, float maxVelocity_, int targetX_, int targetY_) :
+    accDistance(accDistance_),
+    decDistance(decDistance_),
+    initialVelocity(initialVelocity_),
+    terminalVelocity(terminalVelocity_),
+    maxVelocity(maxVelocity_),
+    targetX(targetX_),
+    targetY(targetY_)
 {
-    if(targetDistance > accelerationDistance * 2.0) {
-        maxJerk = (maxVelocity / 2.0) / (pow(accelerationDistance / 2.0, 2));
+    target = hypot(targetX, targetY);
+    radians = atan2(targetY, targetX);
+    if(accDistance * fabs(maxVelocity - initialVelocity) + decDistance * fabs(maxVelocity - terminalVelocity) < target) {
+        enoughDistance = true;
     } else {
-        maxJerk = (maxVelocity * ((targetDistance/4.0)/(accelerationDistance/2.0))/2.0) / (pow(targetDistance / 4.0, 2));
+        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;
 }
 
-float PositionController::generateVelocity(int position)
-{
-    float vel;
-    if(targetDistance > accelerationDistance * 2.0) {
-        if(position >= 0 && position < accelerationDistance / 2.0) {
-            //acc acc
-            vel = pow(position, 2) * maxJerk;
-        } else if(position >= accelerationDistance / 2.0 && position < accelerationDistance) {
-            //acc dec
-            vel = pow(position - accelerationDistance, 2) * (-maxJerk) + maxVelocity;
-        } else if(position >= accelerationDistance && position < targetDistance - accelerationDistance) {
-            //max velocity
+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 >= targetDistance - accelerationDistance && position < targetDistance - accelerationDistance / 2.0) {
-            //dec acc
-            vel = pow(position - (targetDistance - accelerationDistance), 2) * (-maxJerk) + maxVelocity;
-        } else if(position >= targetDistance - accelerationDistance / 2.0){
-            //dec dec
-            vel = pow(position - targetDistance, 2) * maxJerk;
+        } 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 && position < targetDistance / 4.0) {
-            vel = pow(position, 2) * maxJerk;
-        } else if(position >= targetDistance / 4.0 && position < targetDistance / 2.0) {
-            //acc dec
-            vel = pow(position - targetDistance / 2.0, 2) * (-maxJerk) + maxVelocity * ((targetDistance/4.0)/(accelerationDistance/2.0));
-        } else if(position >= targetDistance / 2.0 && position < targetDistance * (3.0/4.0)) {
-            //dec acc
-            vel = pow(position - targetDistance / 2.0, 2) * (-maxJerk) + maxVelocity * ((targetDistance/4.0)/(accelerationDistance/2.0));
-        } else if(position >= targetDistance * (3.0 / 4.0)){
-            //dec dec
-            vel = pow(position - targetDistance, 2) * maxJerk;
+        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] = generateVelocity(((double)positionX / (double)targetDistanceX) * targetDistance) * cos(targetRadians);
-    velocity[1] = generateVelocity(((double)positionY / (double)targetDistanceY) * targetDistance) * sin(targetRadians);
+    velocity[0] = getVelocity(((double)positionX / (double)targetX)*target) * cos(radians);
+    velocity[1] = getVelocity(((double)positionY / (double)targetY)*target) * sin(radians);
 }
 
-float PositionController::getVelocityX()
+double PositionController::getVelocityX()
 {
     return velocity[0];
 }
 
-float PositionController::getVelocityY()
+double PositionController::getVelocityY()
 {
     return velocity[1];
 }
+