Diff: Walk/Walk.cpp

Revision:

22:787a764271dd

Parent:

21:61971fc18b90

Child:

23:97f34a7dc63c

diff -r 61971fc18b90 -r 787a764271dd Walk/Walk.cpp
--- a/Walk/Walk.cpp	Sat Feb 16 05:00:19 2019 +0000
+++ b/Walk/Walk.cpp	Sun Feb 17 14:20:06 2019 +0000
@@ -49,9 +49,12 @@
 OneLeg Orbit::RiseEllipse2_(OneLeg leg, float phasetime_s)
 {
     ///////////x,yを計算.注：計算は正しくないので直す必要がある。
-    float rad = M_PI * (phasetime_s - stridetime_s_) / risetime_s_ + M_PI;
-    float x_m = stride_m_ * 0.5 * cos(rad) + ellipse_center_x_m_;
-    float y_m = height_m_ * sin(rad) + ellipse_center_y_m_;
+    float theta1=acosf(stride_m_/ellipse_long_m_*0.5);                      //stride の始まりの位置の角度（数学基準）
+    float theta2=acosf(-stride_m_/ellipse_long_m_*0.5);                      //stride の終わりの位置の角度（数学基準）
+    float omega=(2*M_PI-theta2+theta1)/risetime_s_;                         //楕円軌道における角速度、一定の値とする（適当）
+    float rad=(phasetime_s-stridetime_s_)*omega+theta2;                     //角度の基準はtheta1とする（適当） 
+    float x_m = ellipse_long_m_* cos(rad) + ellipse_center_x_m_;
+    float y_m =ellipse_short_m_ * sin(rad) + ellipse_center_y_m_;
     ///////////
     //x,yを代入
     leg.SetXY_m(x_m, y_m);