Diff: main.cpp

Revision:

3:e6d345973797

Parent:

2:27f16e37a176

Child:

5:4c03a1a0ad98

diff -r 27f16e37a176 -r e6d345973797 main.cpp
--- a/main.cpp	Wed Dec 04 13:30:46 2019 +0000
+++ b/main.cpp	Thu Dec 05 09:02:14 2019 +0000
@@ -1,7 +1,7 @@
 #include "mbed.h"
 #include "PT1_Controller.h"
 #include "Target_Planer.h"
-#include <vector>
+#include "Motion.h"
  
 using namespace  std;
 
@@ -42,6 +42,14 @@
 double TargetPosy = -999.0;
 double TargetAng  = -999.0;
 
+double pos0   = 179.0*pi/180.0;
+float  vel0   = 0.0f*(float)pi/180.0f;
+float  velMax = 90.0f*(float)pi/180.0f;
+float  accMax = 60.0f*(float)pi/180.0f;
+Motion motion_planer(pos0, vel0, velMax, accMax);
+double targetPos = 0*pi/180.0;
+float  targetVel = 0*(float)pi/180.0f;
+
 int main()
 {
     pc.baud(2000000);
@@ -72,15 +80,18 @@
         // double qy = 0.177899336159591;
         // double qz = 0.001629344754243;
         
-        if(i < 2) {
+        if(i < 200) {
             // pc.printf("%i; %.12f; %.12f; %0.12f; %.6f;\r\n", i, quat2psi(qw, qx, qy, qz), qx*qy + qw*qz, 0.5 - (qy*qy + qz*qz), dt);
             // pc.printf("%i; %i; %.6f;\r\n", i, target_planer.returnPathLength(), dt);
             // pc.printf("%i; %.12f; %.12f; %i; %i; %.6f;\r\n", i, target_planer.returnPathxAtIndex(i), target_planer.returnPathyAtIndex(i), target_planer.returnPathLength(), target_planer.returnClosestPointOnPath(RobotPosx, RobotPosy), dt);
             // pc.printf("%i; %.12f; %.12f;;\r\n", i, TargetPosx, TargetPosy);
             // target_planer.update(RobotPosx, RobotPosx);
             // target_planer.readTargetPos(&TargetPosx, &TargetPosy);
-            pc.printf("%i; %.12f; %.12f; %.12f; %i; %i; %.6f;\r\n", i, TargetPosx, TargetPosy, TargetAng, target_planer.return_i_min_(), target_planer.returnClosestPointOnPath(RobotPosx, RobotPosy), dt);
-            target_planer.updateAndReturnTarget(RobotPosx, RobotPosy, &TargetPosx, &TargetPosy, &TargetAng);
+            // pc.printf("%i; %.12f; %.12f; %.12f; %i; %i; %.6f;\r\n", i, TargetPosx, TargetPosy, TargetAng, target_planer.return_i_min_(), target_planer.returnClosestPointOnPath(RobotPosx, RobotPosy), dt);
+            // target_planer.updateAndReturnTarget(RobotPosx, RobotPosy, &TargetPosx, &TargetPosy, &TargetAng);
+            
+            pc.printf("%i; %.6f; %.6f; %.6f; %.6f; %.6f; %.6f;\r\n", i, motion_planer.getTimeToPosition(targetPos), motion_planer.getDistanceToStop(), motion_planer.getPosition(), motion_planer.getVelocity(), motion_planer.getAcceleration(), dt);
+            motion_planer.incrementToPosition(targetPos, Ts);
         }
         
         i++;