Diff: Walk/Walk.cpp

Revision:

11:e81425872740

Child:

12:2ac37fe6c3bb

diff -r 7a340c52e270 -r e81425872740 Walk/Walk.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Walk/Walk.cpp	Mon Feb 11 12:56:00 2019 +0000
@@ -0,0 +1,66 @@
+#include "Walk.h"
+#include "math.h"
+const float M_PI = 3.141592;
+Orbit::Orbit(float stridetime_s, float risetime_s, float stride_m, float height_m, float ground_m)
+{
+    stridetime_s_ = stridetime_s;
+    risetime_s_ = risetime_s;
+    stride_m_ = stride_m;
+    height_m_ = height_m;
+    ground_m_ = ground_m;
+}
+//着地中の動き.直線軌道.等速
+OneLeg Orbit::StrideLine_(OneLeg leg, float phasetime_s)
+{
+    float x_m = -stride_m_ * phasetime_s / stridetime_s_ + stride_m_ * 0.5;
+    float y_m = ground_m_;
+    leg.SetXY_m(x_m, y_m);
+    return leg;
+}
+//空中の動き.楕円軌道
+OneLeg Orbit::RiseEllipse_(OneLeg leg, float phasetime_s)
+{
+    float rad = M_PI * phasetime_s / risetime_s_ + M_PI;
+    float x_m = stride_m_ * 0.5 * cos(rad);
+    float y_m = height_m_ * sin(rad) + ground_m_;
+    leg.SetXY_m(x_m, y_m);
+    return leg;
+}
+//楕円軌道
+OneLeg Orbit::OrbitEllipse_(OneLeg leg, float phasetime_s)
+{
+    if (phasetime_s < stridetime_s_)
+        return StrideLine_(leg, phasetime_s);
+    else
+        return RiseEllipse_(leg, phasetime_s);
+}
+//将来的には引数などで軌道の種類を変えて出力できるようにしたい。
+OneLeg Orbit::GetOrbit(OneLeg leg, float phasetime_s)
+{
+    return OrbitEllipse_(leg, phasetime_s);
+}
+float Orbit::GetOneWalkTime()
+{
+    return stridetime_s_ + risetime_s_;
+}
+
+Walk::Walk(Orbit orbit[4], float offsettime_s[4], float cycletime_s)
+{
+    for (int i = 0; i < 4; i++)
+    {
+        orbit_[i] = orbit[i];
+        offsettime_s_[i] = offsettime_s[i];
+    }
+    cycletime_s_ = cycletime_s;
+}
+void Walk::Cal4LegsPosi(OneLeg (&leg)[4])
+{
+    phasetime_s_ += cycletime_s_;
+    for (int i = 0; i < 4; i++)
+    {
+        float phasetime_s = phasetime_s_ + offsettime_s_[i];
+        while (phasetime_s > orbit_[i].GetOneWalkTime())
+            phasetime_s -= orbit_[i].GetOneWalkTime();
+        leg[i] = orbit_[i].GetOrbit(leg[i], phasetime_s);
+    }
+}