Diff: Walk/Walk.cpp

Revision:

35:b4e1b8f25cd7

Parent:

34:89d701e15cdf

Child:

43:2ed84f3558c1

--- a/Walk/Walk.cpp	Fri Mar 01 12:07:23 2019 +0000
+++ b/Walk/Walk.cpp	Mon Mar 04 09:54:47 2019 +0000
@@ -1,61 +1,23 @@
-#define _USE_MATH_DEFINES
-#include "math.h"
+#include "Walk.h"
 #include <stdio.h>
-#include "stdlib.h"
-#include "Walk.h"
-#include "pi.h"
-//Orbitは足毎の軌道をあらわす。
-const float kGravity = 9.8;
-
-Orbit::Orbit(OrbitPattern pattern)
-{
-    pattern_ = pattern;
-}
-float Orbit::GetOneWalkTime()
+Walk::Walk(OneLeg legs[4])
 {
-    float time_s;
-    switch (pattern_)
-    {
-    case FREELINES:
-        time_s = FreeLineOrbit::GetOneWalkTime();
-        break;
-    default:
-        time_s = TriangleOrbit::GetOneWalkTime();
-        break;
+    for (int i = 0; i < 4; i++){
+        offset_multi[i] = 0;
+        leg[i] = legs[i];
     }
-    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 = TriangleOrbit::GetOrbit(leg, phasetime_s);
-        break;
-    }
-    return ret;
-}
-
-Walk::Walk()
-{
-    for (int i = 0; i < 4; i++)
-        offset_multi_[i] = 0;
 }
 float Walk::calctime_s_;
-int Walk::Cal4LegsPosi(OneLeg leg[4]) //失敗したら1を返す。成功なら0
+int Walk::Cal4LegsPosi(OneLeg legs[4]) //失敗したら1を返す。成功なら0
 {
     int is_out = 0;
     for (int i = 0; i < 4; i++)
     {
-        float one_walk_time = orbit_[i].GetOneWalkTime();
+        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;
-        if (orbit_[i].GetOrbit(leg[i], phasetime_s_[i]) == 1)
+        if (orbit[i].GetOrbit(legs[i], phasetime_s_[i]) == 1)
         {
             printf("error:leg %d in Cal4LegsPosi\r\n", i);
             is_out = 1;
@@ -63,322 +25,40 @@
     }
     return is_out;
 }
-float Walk::GetOneWalkTime()
-{
-    return orbit_[0].GetOneWalkTime(); //4足全て同じ時間のはずなので一例としてorbit_[0]のものを返している.
-};
 void Walk::SetOffsetTime(float offset_multi0, float offset_multi1, float offset_multi2, float offset_multi3)
 {
-    offset_multi_[0] = offset_multi0;
-    offset_multi_[1] = offset_multi1;
-    offset_multi_[2] = offset_multi2;
-    offset_multi_[3] = offset_multi3;
+    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)
+int Walk::CheckOrbit()
 {
     for (int i = 0; i < 4; i++)
     {
         //軌道が値域の外に出ないか計算で確かめる
-        float one_walk_time = GetOneWalkTime();
+        float one_walk_time = orbit[i].GetOneWalkTime();
         float step = calctime_s_ * 0.1;
         for (float j = 0; j < one_walk_time; j += step)
         {
-            if (orbit_[i].GetOrbit(templateleg, j) == 1)
+            if (orbit[i].GetOrbit(leg[i], j) == 1)
             {
                 printf("error:leg %d, time %f", i, j);
                 return 1; //解が出ないときは1を返す
             }
         }
-        phasetime_s_[i] = one_walk_time * offset_multi_[i];
+        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::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);
-    SetOneOrbit(legnum, triangle);
-}
-void Walk::SetAllLegStandParam(float x_m, float y_m, float time_s)
-{
-    for (int i = 0; i < 4; i++)
-        SetOneLegStandParam(i, x_m, y_m, 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(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 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, offset_x_m, offset_y_m, stride_m, height_m, buffer_height_m,
-                               stridetime_s, toptime_s, buffer_time_s);
-}
-
-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)
+void Walk::Copy(Walk &walk)
 {
     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); //論文よりこれが最適らしい
-    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; //着地直前で止める高さ
-    stridetime_s_ = stridetime_s;
-    toptime_s_ = toptime_s;         //頂点に行くまでの時間
-    buffer_time_s_ = buffer_time_s; //頂点から一時停止点までの時間.
-
-    //事前に計算しておく
-    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_y_m_ = -buffer_height_m_ + offset_y_m_;
-}
-//足一周の時間
-float TriangleOrbit::GetOneWalkTime()
-{
-    return stridetime_s_ + toptime_s_ + buffer_time_s_;
-};
-int TriangleOrbit::GetOrbit(OneLeg &leg, float phasetime_s)
-{
-    int ret = 0;
-    if (phasetime_s < stridetime_s_)
-        ret = StrideLineAccel_(leg, phasetime_s);
-    else if (phasetime_s < stridetime_s_ + toptime_s_)
-        ret = leg.SetXY_m(top_x_m_, top_y_m_);
-    else
-        ret = leg.SetXY_m(buffer_x_m_, buffer_y_m_);
-    return ret;
-};
-int TriangleOrbit::StrideLine_(OneLeg &leg, float phasetime_s)
-{
-    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 = 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 = offset_y_m_;
-    //x,yを代入
-    return leg.SetXY_m(x_m, y_m);
-}
-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;
-    }
-    CalOtherParam();
-}
-
-void FreeLineOrbit::SetFreeLinesParam(LineParam lineparams[], int point_num)
-{
-    point_num_ = point_num;
-    for (int i = 0; i < point_num_; i++)
-        lineparams_[i] = lineparams[i];
-}
-int FreeLineOrbit::GetOrbit(OneLeg &leg, float phasetime_s)
-{
-    ;
-    //現在の最終到達pointを決定
-    int next_point;
-    for (next_point = 0; next_point < point_num_ - 1; next_point++) // point_num - 1が配列の最大index.これを超えないようにしている
-    {
-        if (phasetime_s < lineparams_[next_point].time_s)
-            break;
-    }
-    //x,yを計算
-    float x_m, y_m;
-    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() //足一周の時間
-{
-    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 = 0;
-    lineparams_[1].x_m = x_m;
-    lineparams_[1].y_m = y_m;
-    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);
-*/
+       { 
+           orbit[i].Copy(walk.orbit[i]);
+        phasetime_s_[i] = walk.phasetime_s_[i];
+        offset_multi[i] = walk.offset_multi[i];
+        leg[i] = walk.leg[i];
+       }
 }
\ No newline at end of file