Diff: position_controller.cpp

Revision:

4:4cc455e372c2

Parent:

3:65ae32169b33

--- a/position_controller.cpp	Sat Sep 15 05:05:27 2018 +0000
+++ b/position_controller.cpp	Tue Jun 25 08:28:21 2019 +0000
@@ -1,6 +1,6 @@
 #include "position_controller.h"
 
-PositionController::PositionController(double accDistance_, double decDistance_, double initialVelocity_, double terminalVelocity_, float maxVelocity_) :
+PositionController::PositionController(float accDistance_, float decDistance_, float initialVelocity_, float terminalVelocity_, float maxVelocity_) :
     accDistance(accDistance_),
     decDistance(decDistance_),
     initialVelocity(initialVelocity_),
@@ -13,8 +13,8 @@
 {
     targetX = targetX_;
     targetY = targetY_;
-    target = hypot((double)targetX, (double)targetY);
-    radians = atan2((double)targetY, (double)targetX);
+    target = hypot((float)targetX, (float)targetY);
+    radians = atan2((float)targetY, (float)targetX);
     if(accDistance * fabs(maxVelocity - initialVelocity) + decDistance * fabs(maxVelocity - terminalVelocity) < target) {
         enoughDistance = true;
     } else {
@@ -25,16 +25,16 @@
     }
 }
 
-double PositionController::generateSineWave(double x, double initV, double termV, double start, double length)
+float PositionController::generateSineWave(float x, float initV, float termV, float start, float length)
 {
-    return ((termV - initV) * sin(M_PI * ((2.0 * x - 2.0 * start - length)/(2.0 * length))) + termV + initV)/2.0;
+    return ((termV - initV) * sin(F_PI * ((2.0f * x - 2.0f * start - length)/(2.0f * length))) + termV + initV)/2.0f;
 }
 
-double PositionController::getVelocity(int position)
+float PositionController::getVelocity(int position)
 {
-    double vel = 0;
+    float vel = 0;
     if(enoughDistance) {
-        if(position < 0) {
+        if(position < 0.0f) {
             vel = initialVelocity;
         } else if(position < accDistance * fabs(maxVelocity - initialVelocity)) {
             vel = generateSineWave(position, initialVelocity, maxVelocity, 0, accDistance * fabs(maxVelocity - initialVelocity));
@@ -42,7 +42,7 @@
             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) {
+        } else if(position < 2.0f * target) {
             vel = -generateSineWave(position, -terminalVelocity, maxVelocity, target, target);
         } else {
             vel = -maxVelocity;
@@ -65,16 +65,16 @@
 
 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);
+    velocity[0] = getVelocity(((float)positionX / (float)targetX)*target) * cos(radians);
+    velocity[1] = getVelocity(((float)positionY / (float)targetY)*target) * sin(radians);
 }
 
-double PositionController::getVelocityX()
+float PositionController::getVelocityX()
 {
     return velocity[0];
 }
 
-double PositionController::getVelocityY()
+float PositionController::getVelocityY()
 {
     return velocity[1];
 }