serbo4soku

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

Revision:
10:7a340c52e270
Parent:
9:905f93247688
--- a/ToePosi/ToePosi.cpp	Mon Feb 11 01:56:49 2019 +0000
+++ b/ToePosi/ToePosi.cpp	Mon Feb 11 04:07:51 2019 +0000
@@ -11,19 +11,18 @@
 float ToePosi::GetY_m()
 {
     float phase = GetPhase();
-    float y_m;
-    if (phase < M_PI) //最初の半周期は空中
-        y_m = sin(phase) * height_m_;
-    else //後の半周期は着地。直線移動
-        y_m = 0;
+    float y_m = offsety_m_;
+    if (phase < M_PI) //空中にいるとき(最初の半周期)のみy座標は変化
+        y_m += sin(phase) * height_m_;
     return y_m;
 }
 
-ToePosi::ToePosi(float stride_m, float height_m)
+ToePosi::ToePosi(float stride_m, float height_m, float offsety_m)
 {
     phase_ = 0;
     stride_m_ = stride_m;
     height_m_ = height_m;
+    offsety_m_ = offsety_m;
 }
 float ToePosi::GetPhase()
 {
@@ -36,8 +35,5 @@
         phase -= 2 * M_PI;
     while (phase < 0)
         phase += 2 * M_PI;
-
     phase_ = phase;
 }
-
-