Diff: Walk/OverCome/OverCome.cpp

Revision:

43:2ed84f3558c1

Parent:

35:b4e1b8f25cd7

Child:

49:198030e84936

diff -r 982064594ba6 -r 2ed84f3558c1 Walk/OverCome/OverCome.cpp
--- a/Walk/OverCome/OverCome.cpp	Tue Mar 05 09:08:40 2019 +0000
+++ b/Walk/OverCome/OverCome.cpp	Wed Mar 06 12:13:46 2019 +0000
@@ -1,8 +1,9 @@
 #include "OverCome.h"
 #include "change_walk.h"
+#include "math.h"
 OverCome::OverCome(float start_x_m[4], float start_y_m[4],
                    float d_x_m, float goal_y_m[4], float height_m[4], float gravity_dist[4],
-                   OneLeg legs[4]) :walk(legs)
+                    OneLeg legs[4], float raise_offset_x_m[4]) : walk(legs)
 {
     for (int i = 0; i < 4; i++)
     {
@@ -11,11 +12,13 @@
         goal_y_m_[i] = goal_y_m[i];   //目標地点までのy
         gravity_dist_[i] = gravity_dist[i];
         height_m_[i] = height_m[i];
+        raise_offset_x_m_[i] = raise_offset_x_m[i];
     }
     d_x_m_ = d_x_m; //目標地点までのx
 
     d_time_ = 0.5; //各動きの時間
     d_time_slow_ = 0.5;
+    
     next_point_ = 0; //次のparamのindex
     GetLine();
     for (int i = 0; i < 4; i++)
@@ -29,7 +32,9 @@
         legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_;
         legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m;
         legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m;
+        legs_[i][next_point_].is_point_to_point = 0; //直線軌道を維持
     }
+    legs_[legnum][next_point_].x_m += raise_offset_x_m_[legnum];
     legs_[legnum][next_point_].y_m -= height_m_[legnum];
     next_point_++;
 }
@@ -38,10 +43,11 @@
     for (int i = 0; i < 4; i++)
     {
         legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_;
-        legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m;
+        legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m ;
         legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m;
+        legs_[i][next_point_].is_point_to_point = 0; //直線軌道を維持
     }
-    legs_[legnum][next_point_].x_m += d_x_m_;
+    legs_[legnum][next_point_].x_m += d_x_m_ - raise_offset_x_m_[legnum];
     next_point_++;
 }
 void OverCome::Land(int legnum)
@@ -51,6 +57,7 @@
         legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_slow_;
         legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m;
         legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m;
+        legs_[i][next_point_].is_point_to_point = 0; //ゆっくり着地するために直線軌道を維持
     }
     legs_[legnum][next_point_].y_m = goal_y_m_[legnum]; //着地
     next_point_++;
@@ -64,11 +71,14 @@
 //重心移動、param direct:動く正負。+-1を入れる
 void OverCome::GravityMove(int legnum)
 {
+    if (fabs(gravity_dist_[legnum]) < 0.01)
+        return;
     for (int i = 0; i < 4; i++)
     {
         legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_;
         legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m - gravity_dist_[legnum];
         legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m;
+        legs_[i][next_point_].is_point_to_point = 0; //ゆっくり移動するために直線軌道を維持
     }
     ++next_point_;
 }
@@ -89,6 +99,7 @@
         legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_;
         legs_[i][next_point_].x_m = start_x_m_[i];
         legs_[i][next_point_].y_m = goal_y_m_[i];
+        legs_[i][next_point_].is_point_to_point = 0; //ゆっくり移動するために直線軌道を維持
     }
     ++next_point_;
 }