Revision 3:65ae32169b33, committed 2018-09-15

Comitter:

tanabe2000

Date:

Sat Sep 15 05:05:27 2018 +0000

Parent:

2:2c5e4f521390

Commit message:

targetmode????

Changed in this revision

--- 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;

--- a/position_controller.h	Mon Aug 20 13:54:33 2018 +0900
+++ b/position_controller.h	Sat Sep 15 05:05:27 2018 +0000
@@ -2,12 +2,14 @@
 #define POSITION_CONTROLLER_H
 
 #include "mbed.h"
+#define M_PI 3.141592653589793238462643383219502884
 
 class PositionController {
     public :
-        PositionController(double accDistance_, double decDistance_, double initialVelocity_, double terminalVelocity_, float maxVelocity_, int targetX_, int targetY_);
+        PositionController(double accDistance_, double decDistance_, double initialVelocity_, double terminalVelocity_, float maxVelocity_);
 
         void compute(int positionX, int positionY);
+        void targetXY(int targetX_, int targetY_);
         double getVelocityX();
         double getVelocityY();
     private :