Diff: Walk/Walk.cpp

Revision:

34:89d701e15cdf

Parent:

32:dc684a0b8448

Child:

35:b4e1b8f25cd7

diff -r 945d634d4c9b -r 89d701e15cdf Walk/Walk.cpp
--- a/Walk/Walk.cpp	Thu Feb 28 09:39:02 2019 +0000
+++ b/Walk/Walk.cpp	Fri Mar 01 12:07:23 2019 +0000
@@ -3,7 +3,7 @@
 #include <stdio.h>
 #include "stdlib.h"
 #include "Walk.h"
- #include "pi.h"
+#include "pi.h"
 //Orbitは足毎の軌道をあらわす。
 const float kGravity = 9.8;
 
@@ -81,7 +81,7 @@
     {
         //軌道が値域の外に出ないか計算で確かめる
         float one_walk_time = GetOneWalkTime();
-        int step = one_walk_time / calctime_s_ * 2;
+        float step = calctime_s_ * 0.1;
         for (float j = 0; j < one_walk_time; j += step)
         {
             if (orbit_[i].GetOrbit(templateleg, j) == 1)
@@ -159,9 +159,13 @@
     buffer_time_s_ = buffer_time_s; //頂点から一時停止点までの時間.
 
     //事前に計算しておく
-    top_x_m_ = offset_x_m_ + stride_m * 0.5 - height_m * reverse_tanbeta_;
+    CalOtherParam();
+}
+void TriangleOrbit::CalOtherParam()
+{
+    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_x_m_ = offset_x_m_ + stride_m_ * 0.5 - buffer_height_m_ * reverse_tanbeta_;
     buffer_y_m_ = -buffer_height_m_ + offset_y_m_;
 }
 //足一周の時間
@@ -190,7 +194,7 @@
 {
     ///////////x,yを計算
     float s0 = stride_m_ * 0.5 + offset_x_m_;
-    float s1 = offset_x_m_  -stride_m_ * 0.5;
+    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); //分母
@@ -220,17 +224,12 @@
         buffer_height_m_ = val;
         break;
     }
+    CalOtherParam();
 }
 
 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];
 }
@@ -238,38 +237,148 @@
 {
     ;
     //現在の最終到達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している
+    int next_point;
+    for (next_point = 0; next_point < point_num_ - 1; next_point++) // point_num - 1が配列の最大index.これを超えないようにしている
     {
-        sum_time += lineparams_[arrived_point].time_s;
-        if (phasetime_s < sum_time)
+        if (phasetime_s < lineparams_[next_point].time_s)
             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;
+    float d_phase = phasetime_s - lineparams_[next_point - 1].time_s;
+    float d_time = lineparams_[next_point].time_s - lineparams_[next_point - 1].time_s;
+    float phase_rate = d_phase / d_time;
+    x_m = lineparams_[next_point - 1].x_m +
+          (lineparams_[next_point].x_m - lineparams_[next_point - 1].x_m) * phase_rate;
+    y_m = lineparams_[next_point - 1].y_m +
+          (lineparams_[next_point].y_m - lineparams_[next_point - 1].y_m) * phase_rate;
     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;
+    return lineparams_[point_num_ - 1].time_s;
 }
 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_[0].time_s = 0;
     lineparams_[1].x_m = x_m;
     lineparams_[1].y_m = y_m;
-    lineparams_[1].time_s = 0;
+    lineparams_[1].time_s = time_s;
+}
+OverCome::OverCome(float start_x_m[4], float start_y_m[4],
+                   float d_x_m, float goal_y_m[4], float height_m[4], float gravity_dist[4])
+{
+    for (int i = 0; i < 4; i++)
+    {
+        start_x_m_[i] = start_x_m[i]; //足のスタートx
+        start_y_m_[i] = start_y_m[i]; //足のスタートy
+        goal_y_m_[i] = goal_y_m[i];   //目標地点までのy
+        gravity_dist_[i] = gravity_dist[i];
+        height_m_[i] = height_m[i];
+    }
+    d_x_m_ = d_x_m; //目標地点までのx
+
+    d_time_ = 0.5; //各動きの時間
+    d_time_slow_ = 0.5;
+    next_point_ = 0; //次のparamのindex
+    GetLine();
+    for (int i = 0; i < 4; i++)
+        walk.SetOneLegFreeLinesParam(i, legs_[i], next_point_);
+}
+
+void OverCome::Rise(int legnum)
+{
+    for (int i = 0; i < 4; i++)
+    {
+        legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_;
+        legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m;
+        legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m;
+    }
+    legs_[legnum][next_point_].y_m -= height_m_[legnum];
+    next_point_++;
+}
+void OverCome::Forward(int legnum)
+{
+    for (int i = 0; i < 4; i++)
+    {
+        legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_;
+        legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m;
+        legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m;
+    }
+    legs_[legnum][next_point_].x_m += d_x_m_;
+    next_point_++;
+}
+void OverCome::Land(int legnum)
+{
+    for (int i = 0; i < 4; i++)
+    {
+        legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_slow_;
+        legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m;
+        legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m;
+    }
+    legs_[legnum][next_point_].y_m = goal_y_m_[legnum]; //着地
+    next_point_++;
 }
+void OverCome::Step(int legnum)
+{
+    Rise(legnum);
+    Forward(legnum);
+    Land(legnum);
+}
+//重心移動、param direct:動く正負。+-1を入れる
+void OverCome::GravityMove(int legnum)
+{
+    for (int i = 0; i < 4; i++)
+    {
+        legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_;
+        legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m - gravity_dist_[legnum];
+        legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m;
+    }
+    ++next_point_;
+}
+void OverCome::StartPoint()
+{
+    for (int i = 0; i < 4; i++)
+    {
+        legs_[i][next_point_].time_s = 0;
+        legs_[i][next_point_].x_m = start_x_m_[i];
+        legs_[i][next_point_].y_m = start_y_m_[i];
+    }
+    ++next_point_;
+}
+void OverCome::GoalPoint()
+{
+    for (int i = 0; i < 4; i++)
+    {
+        legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_;
+        legs_[i][next_point_].x_m = start_x_m_[i];
+        legs_[i][next_point_].y_m = goal_y_m_[i];
+    }
+    ++next_point_;
+}
+void OverCome::GetLine()
+{
+    StartPoint(); //スタート時点
+    GravityMove(RIGHT_F);
+    Step(RIGHT_F);
+    GravityMove(LEFT_B);
+    Step(LEFT_B);
+    GravityMove(LEFT_F);
+    Step(LEFT_F);
+    GravityMove(RIGHT_B);
+    Step(RIGHT_B);
+    GoalPoint();
+    /*   GravityMove(1);
+    Step(RIGHT_B);
+    GravityMove(-1);
+    Step(RIGHT_F);
+    GravityMove(1);
+    Step(LEFT_B);
+    GravityMove(-1);
+    Step(LEFT_F);
+    GravityMove(1);
+*/
+}
\ No newline at end of file