Diff: Walk/Walk.cpp

Revision:

32:dc684a0b8448

Parent:

29:7d8b8011a88d

Child:

34:89d701e15cdf

--- a/Walk/Walk.cpp	Thu Feb 28 03:52:09 2019 +0000
+++ b/Walk/Walk.cpp	Thu Feb 28 08:48:21 2019 +0000
@@ -16,6 +16,9 @@
     float time_s;
     switch (pattern_)
     {
+    case FREELINES:
+        time_s = FreeLineOrbit::GetOneWalkTime();
+        break;
     default:
         time_s = TriangleOrbit::GetOneWalkTime();
         break;
@@ -27,6 +30,9 @@
     int ret;
     switch (pattern_)
     {
+    case FREELINES:
+        ret = FreeLineOrbit::GetOrbit(leg, phasetime_s);
+        break;
     default:
         ret = TriangleOrbit::GetOrbit(leg, phasetime_s);
         break;
@@ -97,39 +103,54 @@
     for (int i = 0; i < 4; i++)
         SetOneOrbit(i, orbit);
 }
-void Walk::SetOneLegStandParam(int legnum,float x_m, float y_m, float time_s)
+void Walk::SetOneLegStandParam(int legnum, float x_m, float y_m, float time_s)
 {
     Orbit triangle(TRIANGLE);
-    triangle.SetTriangleParam(x_m, y_m, 0, 0, 0, time_s, 0,0);
+    triangle.SetTriangleParam(x_m, y_m, 0, 0, 0, time_s, 0, 0);
     SetOneOrbit(legnum, triangle);
 }
-void Walk::SetAllLegStandParam(float x_m, float y_m, float time_s) 
+void Walk::SetAllLegStandParam(float x_m, float y_m, float time_s)
 {
-    for (int i = 0; i < 4; i ++)
+    for (int i = 0; i < 4; i++)
         SetOneLegStandParam(i, x_m, y_m, time_s);
 }
-void Walk::SetOneLegTriangleParam(int legnum, float start_x_m, float start_y_m, float stride_m, float height_m, float buffer_height_m,
-                                                                                                     float stridetime_s, float toptime_s, float buffer_time_s)
+void Walk::SetOneLegTriangleParam(int legnum, float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m,
+                                  float stridetime_s, float toptime_s, float buffer_time_s)
 {
     Orbit triangle(TRIANGLE);
-    triangle.SetTriangleParam(start_x_m, start_y_m, stride_m, height_m, buffer_height_m,
+    triangle.SetTriangleParam(offset_x_m, offset_y_m, stride_m, height_m, buffer_height_m,
                               stridetime_s, toptime_s, buffer_time_s);
     SetOneOrbit(legnum, triangle);
 }
-void Walk::SetAllLegTriangleParam(float start_x_m, float start_y_m, float stride_m, float height_m, float buffer_height_m,
+void Walk::SetAllLegTriangleParam(float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m,
                                   float stridetime_s, float toptime_s, float buffer_time_s)
 {
     for (int i = 0; i < 4; i++)
-        SetOneLegTriangleParam(i, start_x_m, start_y_m, stride_m, height_m, buffer_height_m,
+        SetOneLegTriangleParam(i, offset_x_m, offset_y_m, stride_m, height_m, buffer_height_m,
                                stridetime_s, toptime_s, buffer_time_s);
 }
 
-void TriangleOrbit::SetTriangleParam(float start_x_m, float start_y_m, float stride_m, float height_m, float buffer_height_m,
+void Walk::SetOneLegFreeLinesParam(int legnum, LineParam lineparams[], int point_num)
+{
+    Orbit freeline(FREELINES);
+    freeline.SetFreeLinesParam(lineparams, point_num);
+    SetOneOrbit(legnum, freeline);
+}
+void Walk::SetAllLegFreeLinesParam(LineParam lineparams[], int point_num)
+{
+    for (int i = 0; i < 4; i++)
+        SetOneLegFreeLinesParam(i, lineparams, point_num);
+}
+void Walk::ChangeOneParam(int legnum, TriangleParams param, float val)
+{
+    orbit_[legnum].ChangeOneParam(param, val);
+}
+void TriangleOrbit::SetTriangleParam(float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m,
                                      float stridetime_s, float toptime_s, float buffer_time_s)
 {
     reverse_tanbeta_ = 1.0 / tan(81.0 / 180.0 * M_PI); //論文よりこれが最適らしい
-    start_x_m_ = start_x_m;
-    start_y_m_ = start_y_m;
+    offset_x_m_ = offset_x_m;
+    offset_y_m_ = offset_y_m;
     stride_m_ = stride_m;
     height_m_ = height_m;               //足上げ幅
     buffer_height_m_ = buffer_height_m; //着地直前で止める高さ
@@ -138,10 +159,10 @@
     buffer_time_s_ = buffer_time_s; //頂点から一時停止点までの時間.
 
     //事前に計算しておく
-    top_x_m_ = start_x_m_ - height_m * reverse_tanbeta_;
-    top_y_m_ = -height_m_ + start_y_m_;
-    buffer_x_m_ = start_x_m_ - buffer_height_m_ * reverse_tanbeta_;
-    buffer_y_m_ = -buffer_height_m_ + start_y_m_;
+    top_x_m_ = offset_x_m_ + stride_m * 0.5 - height_m * reverse_tanbeta_;
+    top_y_m_ = -height_m_ + offset_y_m_;
+    buffer_x_m_ = offset_x_m_ + stride_m * 0.5 - buffer_height_m_ * reverse_tanbeta_;
+    buffer_y_m_ = -buffer_height_m_ + offset_y_m_;
 }
 //足一周の時間
 float TriangleOrbit::GetOneWalkTime()
@@ -161,22 +182,94 @@
 };
 int TriangleOrbit::StrideLine_(OneLeg &leg, float phasetime_s)
 {
-    float x_m = start_x_m_ - stride_m_ * phasetime_s / stridetime_s_;
-    float y_m = start_y_m_;
+    float x_m = -stride_m_ * phasetime_s / stridetime_s_ + stride_m_ * 0.5 + offset_x_m_;
+    float y_m = offset_y_m_;
     return leg.SetXY_m(x_m, y_m);
 }
-
 int TriangleOrbit::StrideLineAccel_(OneLeg &leg, float phasetime_s)
 {
     ///////////x,yを計算
-    float s0 = start_x_m_;
-    float s1 = start_x_m_ - stride_m_;
-    float g_h = sqrtf(kGravity / start_y_m_);
+    float s0 = stride_m_ * 0.5 + offset_x_m_;
+    float s1 = offset_x_m_  -stride_m_ * 0.5;
+    float g_h = sqrtf(kGravity / offset_y_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 = start_y_m_;
+    float y_m = offset_y_m_;
     //x,yを代入
     return leg.SetXY_m(x_m, y_m);
-}
\ No newline at end of file
+}
+void TriangleOrbit::ChangeOneParam(TriangleParams param, float val)
+{
+    switch (param)
+    {
+    case OFFSET_X_M:
+        offset_x_m_ = val;
+        break;
+    case OFFSET_Y_M:
+        offset_y_m_ = val;
+        break;
+    case STRIDE_M:
+        stride_m_ = val;
+        break;
+    case HEIGHT_M:
+        height_m_ = val;
+        break;
+    case BUFFER_HEIGHT_M:
+        buffer_height_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)
+{
+    ;
+    //現在の最終到達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 - sum_time + lineparams_[arrived_point].time_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 - sum_time + lineparams_[arrived_point].time_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_ = 2;
+    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;
+}