Diff: main.cpp

Revision:

2:27f16e37a176

Parent:

1:e79de7ffd211

Child:

3:e6d345973797

--- a/main.cpp	Wed Dec 04 10:35:14 2019 +0000
+++ b/main.cpp	Wed Dec 04 13:30:46 2019 +0000
@@ -31,13 +31,13 @@
 double quat2psi(double qw, double qx, double qy, double qz);
 double getShortRotation(double ang);
 
-const int N = 4;
-double Pathx[N] = {0, 2, 1, 4};
-double Pathy[N] = {0, 3, 0 ,1};
-double R = 0.16;
+const int N = 40;
+double Pathx[N] = {7.3000, 7.2500, 7.2000, 7.1500, 7.1000, 7.0500, 7.0000, 6.9500, 6.9000, 6.8500, 6.8000, 6.7500, 6.7000, 6.6500, 6.6000, 6.5500, 6.5000, 6.4500, 6.4000, 6.3500, 6.3000, 6.2500, 6.2000, 6.1500, 6.1000, 6.0500, 6.0000, 5.9500, 5.9000, 5.8500, 5.8000, 5.7500, 5.7000, 5.6500, 5.6000, 5.5500, 5.5000, 5.4500, 5.4000, 5.3500};
+double Pathy[N] = {6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9500, 6.9000, 6.8500, 6.8000, 6.7500, 6.7000, 6.6500, 6.6000, 6.5500, 6.5000, 6.4500, 6.4000, 6.3500, 6.3000, 6.2500};
+double R = 0.116959064327485;
 Target_Planer target_planer(Pathx, Pathy, (uint16_t)N, R);
-double RobotPosx = 10.1;
-double RobotPosy = 10.1;
+double RobotPosx = 4.35;
+double RobotPosy = 6.19;
 double TargetPosx = -999.0;
 double TargetPosy = -999.0;
 double TargetAng  = -999.0;
@@ -58,6 +58,8 @@
         
         dt = timer.read();
         timer.reset();
+        
+        target_planer.initialize(RobotPosx, RobotPosy);
                 
         /*
         double phiR = quat2psi(double qw, double qx, double qy, double qz); // transform actual robot orientation (quaternioni) into turning angle
@@ -70,14 +72,14 @@
         // double qy = 0.177899336159591;
         // double qz = 0.001629344754243;
         
-        if(i < 4) {
+        if(i < 2) {
             // 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; %.6f;\r\n", i, TargetPosx, TargetPosy, TargetAng, dt);
+            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);
         }