Diff: position_controller.cpp

Revision:

3:65ae32169b33

Parent:

2:2c5e4f521390

Child:

4:4cc455e372c2

--- a/position_controller.cpp	Mon Aug 20 13:54:33 2018 +0900
+++ b/position_controller.cpp	Sat Sep 15 05:05:27 2018 +0000
@@ -1,16 +1,20 @@
 #include "position_controller.h"
 
-PositionController::PositionController(double accDistance_, double decDistance_, double initialVelocity_, double terminalVelocity_, float maxVelocity_, int targetX_, int targetY_) :
+PositionController::PositionController(double accDistance_, double decDistance_, double initialVelocity_, double terminalVelocity_, float maxVelocity_) :
     accDistance(accDistance_),
     decDistance(decDistance_),
     initialVelocity(initialVelocity_),
     terminalVelocity(terminalVelocity_),
-    maxVelocity(maxVelocity_),
-    targetX(targetX_),
-    targetY(targetY_)
+    maxVelocity(maxVelocity_)
 {
-    target = hypot(targetX, targetY);
-    radians = atan2(targetY, targetX);
+}
+
+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 {
@@ -19,7 +23,6 @@
         accTrue = accDistance * (target/(accDistance+decDistance));
         decTrue = decDistance * (target/(accDistance+decDistance));
     }
-
 }
 
 double PositionController::generateSineWave(double x, double initV, double termV, double start, double length)
@@ -28,7 +31,7 @@
 }
 
 double PositionController::getVelocity(int position)
-{   
+{
     double vel = 0;
     if(enoughDistance) {
         if(position < 0) {
@@ -39,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 * target) {
             vel = -generateSineWave(position, -terminalVelocity, maxVelocity, target, target);
         } else {
             vel = -maxVelocity;
@@ -48,10 +51,10 @@
         if(position < 0) {
             vel = initialVelocity;
         } else if(position < accTrue) {
-            vel = generateSineWave(position, initialVelocity, maxVelocity, 0, 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){
+        } else if(position < 2 * target) {
             vel = -generateSineWave(position, -terminalVelocity, maxVelocity, target, target);
         } else {
             vel = -maxVelocity;