Diff: main.cpp

Revision:

1:e79de7ffd211

Parent:

0:937360eb9f8c

Child:

2:27f16e37a176

diff -r 937360eb9f8c -r e79de7ffd211 main.cpp
--- a/main.cpp	Wed Dec 04 10:24:29 2019 +0000
+++ b/main.cpp	Wed Dec 04 10:35:14 2019 +0000
@@ -38,8 +38,9 @@
 Target_Planer target_planer(Pathx, Pathy, (uint16_t)N, R);
 double RobotPosx = 10.1;
 double RobotPosy = 10.1;
-double TargetPosx = 37;
-double TargetPosy = 23;
+double TargetPosx = -999.0;
+double TargetPosy = -999.0;
+double TargetAng  = -999.0;
 
 int main()
 {
@@ -72,10 +73,12 @@
         if(i < 4) {
             // 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; %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; %.6f;\r\n", i, TargetPosx, TargetPosy, TargetAng, dt);
+            target_planer.updateAndReturnTarget(RobotPosx, RobotPosy, &TargetPosx, &TargetPosy, &TargetAng);
         }
         
         i++;