Diff: Walk/Walk.cpp

Revision:

27:79b4b932a6dd

Parent:

26:24ae5a4f5b1f

Child:

29:7d8b8011a88d

diff -r 24ae5a4f5b1f -r 79b4b932a6dd Walk/Walk.cpp
--- a/Walk/Walk.cpp	Fri Feb 22 05:20:28 2019 +0000
+++ b/Walk/Walk.cpp	Wed Feb 27 01:24:00 2019 +0000
@@ -1,126 +1,254 @@
-#include "Walk.h"
+#define _USE_MATH_DEFINES
 #include "math.h"
-#include "pi.h"
+#include <stdio.h>
+#include "stdlib.h"
+#include "Walk.h"
+ #include "pi.h"
 //Orbitは足毎の軌道をあらわす。
-
-void Orbit::SetStraightParam(float stridetime_s, float risetime_s,
-                             float stride_m, float height_m, float ground_m,
-                             float ellipse_center_x_m, float ellipse_center_y_m)
+const float kGravity = 9.8;
+void EllipseOrbit::SetParam(float stridetime_s, float risetime_s,
+                            float stride_m, float height_m, float ground_m,
+                            float ellipsecenter_x_m, float ellipsecenter_y_m)
 {
     stridetime_s_ = stridetime_s;
     risetime_s_ = risetime_s;
     stride_m_ = stride_m;
     height_m_ = height_m;
     ground_m_ = ground_m;
-    ellipse_center_x_m_ = ellipse_center_x_m;
-    ellipse_center_y_m_ = ellipse_center_y_m;
-}
-
-void Orbit::SetStandParam(float ground_m)
-{
-    stridetime_s_ = 1;
-    risetime_s_ = 0;
-    stride_m_ = 0;
-    height_m_ = 0;
-    ground_m_ = ground_m;
-    ellipse_center_x_m_ = 0;
-    ellipse_center_y_m_ = ground_m;
-    
-    ellipse_long_m_=stride_m*0.5/cosf(asinf(ground_m_-ellipse_center_y_m_)); 
-    ellipse_short_m_=2*(height_m_-grounf_m_+ellipse_center_y_m_);
+    ellipsecenter_x_m_ = ellipsecenter_x_m;
+    ellipsecenter_y_m_ = ellipsecenter_y_m;
 }
 
 //着地中の動き.直線軌道.等速
-OneLeg Orbit::StrideLine_(OneLeg leg, float phasetime_s)
+int EllipseOrbit::StrideLine_(OneLeg &leg, float phasetime_s)
 {
-    float x_m = -stride_m_ * phasetime_s / stridetime_s_ + stride_m_ * 0.5 + ellipse_center_x_m_;
-    float y_m = ellipse_center_y_m_;
-    leg.SetXY_m(x_m, y_m);
-    return leg;
+    float x_m = -stride_m_ * phasetime_s / stridetime_s_ + stride_m_ * 0.5 + ellipsecenter_x_m_;
+    float y_m = ellipsecenter_y_m_;
+    return leg.SetXY_m(x_m, y_m);
 }
-
-//着地中の動き.直線軌道.加速
-OneLeg Orbit::StrideLineAccel_(OneLeg leg, float phasetime_s)
+int EllipseOrbit::StrideLineAccel_(OneLeg &leg, float phasetime_s)
 {
-     ///////////x,yを計算.注：計算は正しくないので直す必要がある。
-    float s0=stride_m_*0.5+ellipse_center_x_m_;
-    float s1=-stride_m_*0.5+ellipse_center_x_m_;
-    
-    float gravity=9.8;
-    float g_h=sqrtf(gravity/ground_m_);
-    float t=phasetime_s/stridetime_s_;
-    float denominator=expf(g_h)-expf(-g_h);//分母
-    
-    float x_m = -(s0*expf(-g_h)-s1)*expf(g_h*t)/denominator + (s0*expf(g_h)-s1)*expf(-g_h*t)/denominator;
+    ///////////x,yを計算
+    float s0 = stride_m_ * 0.5 + ellipsecenter_x_m_;
+    float s1 = -stride_m_ * 0.5 + ellipsecenter_x_m_;
+    float g_h = sqrtf(kGravity / ground_m_);
+    float t = phasetime_s / stridetime_s_;
+    float denominator = expf(g_h) - expf(-g_h); //分母
+
+    float x_m = -(s0 * expf(-g_h) - s1) * expf(g_h * t) / denominator + (s0 * expf(g_h) - s1) * expf(-g_h * t) / denominator;
     float y_m = ground_m_;
-        ///////////
     //x,yを代入
-    leg.SetXY_m(x_m, y_m);
-    return leg;
+    return leg.SetXY_m(x_m, y_m);
 }
-
 //空中の動き.半分にきれいに切れる楕円軌道
-OneLeg Orbit::RiseEllipse_(OneLeg leg, float phasetime_s)
+int EllipseOrbit::RiseEllipse_(OneLeg &leg, float phasetime_s)
 {
     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_;
-    leg.SetXY_m(x_m, y_m);
-    return leg;
+    float x_m = stride_m_ * 0.5 * cosf(rad) + ellipsecenter_x_m_;
+    float y_m = height_m_ * sinf(rad) + ellipsecenter_y_m_;
+    return leg.SetXY_m(x_m, y_m);
 }
-
 //空中の動き.完全に半分には切れない楕円軌道
-OneLeg Orbit::RiseEllipse2_(OneLeg leg, float phasetime_s)
+int EllipseOrbit::RiseEllipse2_(OneLeg &leg, float phasetime_s)
 {
-    ///////////x,yを計算.注：計算は正しくないので直す必要がある。
-    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を計算
+    float ellipselong_m = stride_m_ * 0.5 / cosf(asinf(ground_m_ - ellipsecenter_y_m_));
+    float ellipseshort_m = 2 * (height_m_ - ground_m_ + ellipsecenter_y_m_);
+    float theta1 = acosf(stride_m_ / ellipselong_m * 0.5);      //stride の始まりの位置の角度（数学基準）
+    float theta2 = acosf(-stride_m_ / ellipselong_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 = ellipselong_m * cos(rad) + ellipsecenter_x_m_;
+    float y_m = ellipseshort_m * sin(rad) + ellipsecenter_y_m_;
     //x,yを代入
-    leg.SetXY_m(x_m, y_m);
-    return leg;
+    return leg.SetXY_m(x_m, y_m);
 }
 //楕円軌道
-OneLeg Orbit::OrbitEllipse_(OneLeg leg, float phasetime_s)
+int EllipseOrbit::GetOrbit(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)
+float EllipseOrbit::GetOneWalkTime()
+{
+    return stridetime_s_ + risetime_s_;
+}
+void EllipseOrbit::ChangeOneParam(EllipseParams param, float val)
+{
+    switch (param)
+    {
+    case STRIDE_M:
+        stride_m_ = val;
+        break;
+    case HEIGHT_M:
+        height_m_ = val;
+        break;
+    case GROUND_M:
+        ground_m_ = val;
+        break;
+    case ELLIPSE_CENTER_X_M:
+        ellipsecenter_x_m_ = val;
+        break;
+    case ELLIPSE_CENTER_Y_M:
+        ellipsecenter_y_m_ = val;
+        break;
+    }
+}
+void FreeLineOrbit::SetFreeLinesParam(LineParam lineparams[], int point_num)
+{
+    point_num_ = point_num;
+    if (point_num_ == 1)
+    {
+        printf("error:point_num = 1. You should put >=2 to make line.");
+        while (1)
+            ;
+    }
+    for (int i = 0; i < point_num_; i++)
+        lineparams_[i] = lineparams[i];
+}
+int FreeLineOrbit::GetOrbit(OneLeg &leg, float phasetime_s)
 {
-    return OrbitEllipse_(leg, phasetime_s);
+    ;
+    //現在の最終到達pointを決定
+    int arrived_point;
+    float sum_time = 0;
+    for (arrived_point = 0; arrived_point < point_num_ - 2; arrived_point++) //arrived_point = point_num-1(これが配列の最大index)の状態で終わらないように-2している
+    {
+        sum_time += lineparams_[arrived_point].time_s;
+        if (phasetime_s < sum_time)
+            break;
+    }
+    //x,yを計算
+    float x_m, y_m;
+    x_m = lineparams_[arrived_point].x_m +
+          (lineparams_[arrived_point + 1].x_m - lineparams_[arrived_point].x_m) * phasetime_s / lineparams_[arrived_point].time_s;
+    y_m = lineparams_[arrived_point].y_m +
+          (lineparams_[arrived_point + 1].y_m - lineparams_[arrived_point].y_m) * phasetime_s / lineparams_[arrived_point].time_s;
+    return leg.SetXY_m(x_m, y_m);
+}
+float FreeLineOrbit::GetOneWalkTime() //足一周の時間
+{
+    float sum_time = 0;
+    for (int i = 0; i < point_num_; i++)
+        sum_time += lineparams_[i].time_s;
+    return sum_time;
+}
+void FreeLineOrbit::SetStandParam(float x_m, float y_m, float time_s)
+{
+    point_num_ = 1;
+    lineparams_[0].x_m = x_m;
+    lineparams_[0].y_m = y_m;
+    lineparams_[0].time_s = time_s;
+    lineparams_[1].x_m = x_m;
+    lineparams_[1].y_m = y_m;
+    lineparams_[1].time_s = 0;
+}
+
+Orbit::Orbit(OrbitPattern pattern)
+{
+    pattern_ = pattern;
 }
 float Orbit::GetOneWalkTime()
 {
-    return stridetime_s_ + risetime_s_;
+    float time_s;
+    switch (pattern_)
+    {
+    case FREELINES:
+        time_s = FreeLineOrbit::GetOneWalkTime();
+        break;
+
+    default:
+        time_s = EllipseOrbit::GetOneWalkTime();
+        break;
+    }
+    return time_s;
+}
+int Orbit::GetOrbit(OneLeg &leg, float phasetime_s)
+{
+    int ret;
+    switch (pattern_)
+    {
+    case FREELINES:
+        ret = FreeLineOrbit::GetOrbit(leg, phasetime_s);
+        break;
+    default:
+        ret = EllipseOrbit::GetOrbit(leg, phasetime_s);
+        break;
+    }
+    return ret;
 }
 
-Walk::Walk(Orbit orbit[4], float offsettime_s[4], float cycletime_s)
+Walk::Walk()
+{
+    for (int i = 0; i < 4; i++)
+        offset_multi_[i] = 0;
+}
+float Walk::calctime_s_;
+void Walk::Cal4LegsPosi(OneLeg leg[4])
 {
     for (int i = 0; i < 4; i++)
     {
-        orbit_[i] = orbit[i];
-        offsettime_s_[i] = offsettime_s[i];
+        float one_walk_time = orbit_[i].GetOneWalkTime();
+        phasetime_s_[i] += calctime_s_;
+        while (phasetime_s_[i] > one_walk_time)
+            phasetime_s_[i] -= one_walk_time;
+        orbit_[i].GetOrbit(leg[i], phasetime_s_[i]);
     }
-    cycletime_s_ = cycletime_s;
 }
-void Walk::Cal4LegsPosi(OneLeg (&leg)[4])
+float Walk::GetOneWalkTime()
+{
+    return orbit_[0].GetOneWalkTime(); //4足全て同じ時間のはずなので一例としてorbit_[0]のものを返している.
+};
+void Walk::SetOffset(float offset_multi0, float offset_multi1, float offset_multi2, float offset_multi3)
 {
-    phasetime_s_ += cycletime_s_;
-    while (phasetime_s_ > orbit_[0].GetOneWalkTime())
-        phasetime_s_ -= orbit_[0].GetOneWalkTime();
+    offset_multi_[0] = offset_multi0;
+    offset_multi_[1] = offset_multi1;
+    offset_multi_[2] = offset_multi2;
+    offset_multi_[3] = offset_multi3;
+}
+//軌道がリンク定義外になっていないかチェック。reutn 0:ok 1:out
+int Walk::CheckOrbit(OneLeg templateleg)
+{
     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);
+        //軌道が値域の外に出ないか計算で確かめる
+        float one_walk_time = GetOneWalkTime();
+        int step = one_walk_time / calctime_s_ * 2;
+        for (float j = 0; j < one_walk_time; j += step)
+        {
+            if (orbit_[i].GetOrbit(templateleg, j) == 1)
+                return 1; //解が出ないときは1を返す
+        }
+        phasetime_s_[i] = one_walk_time * offset_multi_[i];
     }
+    return 0;
 }
+void Walk::SetOneOrbit(int legnum, Orbit orbit)
+{
+    orbit_[legnum] = orbit;
+}
+void Walk::SetAllOrbit(Orbit orbit)
+{
+    for (int i = 0; i < 4; i++)
+        SetOneOrbit(i, orbit);
+}
+
+void Walk::ChangeOneParam(int legnum, EllipseParams param, float val)
+{
+    orbit_[legnum].ChangeOneParam(param, val);
+}
+void Walk::SetAllLegEllipseParam(float stridetime_s, float risetime_s,
+                                 float stride_m, float height_m, float ground_m,
+                                 float ellipsecenter_x_m, float ellipsecenter_y_m)
+{
+    for (int i = 0; i < 4; i++)
+        orbit_[i].SetParam(stridetime_s, risetime_s, stride_m, height_m, ground_m, ellipsecenter_x_m, ellipsecenter_y_m);
+}
+void Walk::SetAllLegStandParam(float x_m, float y_m, float time_s)
+{
+    Orbit stand(FREELINES);
+    stand.SetStandParam(x_m, y_m, time_s); //defaultとなる軌道設定
+    SetAllOrbit(stand);                    //4足にコピー
+}