Diff: Walk/orbit/freeline/freeline.cpp

Revision:

41:38d79b6513c0

Parent:

35:b4e1b8f25cd7

Child:

42:982064594ba6

--- a/Walk/orbit/freeline/freeline.cpp	Mon Mar 04 23:44:07 2019 +0000
+++ b/Walk/orbit/freeline/freeline.cpp	Tue Mar 05 01:45:03 2019 +0000
@@ -20,10 +20,20 @@
     float d_phase = phasetime_s - lineparams_[next_point - 1].time_s;
     float d_time = lineparams_[next_point].time_s - lineparams_[next_point - 1].time_s;
     float phase_rate = d_phase / d_time;
-    x_m = lineparams_[next_point - 1].x_m +
-          (lineparams_[next_point].x_m - lineparams_[next_point - 1].x_m) * phase_rate;
-    y_m = lineparams_[next_point - 1].y_m +
-          (lineparams_[next_point].y_m - lineparams_[next_point - 1].y_m) * phase_rate;
+    
+    if(lineparams_[next_point - 1].line_state == 1)
+    {
+        x_m = lineparams_[next_point].x_m;
+        y_m = lineparams_[next_point].y_m;
+    }
+    else
+    {
+        x_m = lineparams_[next_point - 1].x_m +
+              (lineparams_[next_point].x_m - lineparams_[next_point - 1].x_m) * phase_rate;
+        y_m = lineparams_[next_point - 1].y_m +
+              (lineparams_[next_point].y_m - lineparams_[next_point - 1].y_m) * phase_rate;
+    }
+    
     return leg.SetXY_m(x_m, y_m);
 }
 float FreeLines::GetOneWalkTime() //足一周の時間