Diff: position_controller.cpp

Revision:

1:aed2cca33061

Child:

2:2c5e4f521390

diff -r fe7037fdc1b5 -r aed2cca33061 position_controller.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/position_controller.cpp	Fri Aug 03 03:57:44 2018 +0000
@@ -0,0 +1,70 @@
+#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))
+{
+    if(targetDistance > accelerationDistance * 2.0) {
+        maxJerk = (maxVelocity / 2.0) / (pow(accelerationDistance / 2.0, 2));
+    } else {
+        maxJerk = (maxVelocity * ((targetDistance/4.0)/(accelerationDistance/2.0))/2.0) / (pow(targetDistance / 4.0, 2));
+    }
+}
+
+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
+            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 >= 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;
+        }
+    }
+
+    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);
+}
+
+float PositionController::getVelocityX()
+{
+    return velocity[0];
+}
+
+float PositionController::getVelocityY()
+{
+    return velocity[1];
+}