岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

Walk/Walk.cpp@29:7d8b8011a88d, 2019-02-27 (annotated)

Committer:
shimizuta
Date:
Wed Feb 27 12:16:18 2019 +0000
Revision:
29:7d8b8011a88d
Parent:
27:79b4b932a6dd
Child:
32:dc684a0b8448
can move triangle orbit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimizuta 27:79b4b932a6dd 1 #define _USE_MATH_DEFINES
shimizuta 11:e81425872740 2 #include "math.h"
shimizuta 27:79b4b932a6dd 3 #include <stdio.h>
shimizuta 27:79b4b932a6dd 4 #include "stdlib.h"
shimizuta 27:79b4b932a6dd 5 #include "Walk.h"
shimizuta 27:79b4b932a6dd 6 #include "pi.h"
yuto17320508 13:e7ecdb20665a 7 //Orbitは足毎の軌道をあらわす。
shimizuta 27:79b4b932a6dd 8 const float kGravity = 9.8;
shimizuta 27:79b4b932a6dd 9
shimizuta 27:79b4b932a6dd 10 Orbit::Orbit(OrbitPattern pattern)
shimizuta 27:79b4b932a6dd 11 {
shimizuta 27:79b4b932a6dd 12 pattern_ = pattern;
shimizuta 11:e81425872740 13 }
shimizuta 11:e81425872740 14 float Orbit::GetOneWalkTime()
shimizuta 11:e81425872740 15 {
shimizuta 27:79b4b932a6dd 16 float time_s;
shimizuta 27:79b4b932a6dd 17 switch (pattern_)
shimizuta 27:79b4b932a6dd 18 {
shimizuta 27:79b4b932a6dd 19 default:
shimizuta 29:7d8b8011a88d 20 time_s = TriangleOrbit::GetOneWalkTime();
shimizuta 27:79b4b932a6dd 21 break;
shimizuta 27:79b4b932a6dd 22 }
shimizuta 27:79b4b932a6dd 23 return time_s;
shimizuta 27:79b4b932a6dd 24 }
shimizuta 27:79b4b932a6dd 25 int Orbit::GetOrbit(OneLeg &leg, float phasetime_s)
shimizuta 27:79b4b932a6dd 26 {
shimizuta 27:79b4b932a6dd 27 int ret;
shimizuta 27:79b4b932a6dd 28 switch (pattern_)
shimizuta 27:79b4b932a6dd 29 {
shimizuta 27:79b4b932a6dd 30 default:
shimizuta 29:7d8b8011a88d 31 ret = TriangleOrbit::GetOrbit(leg, phasetime_s);
shimizuta 27:79b4b932a6dd 32 break;
shimizuta 27:79b4b932a6dd 33 }
shimizuta 27:79b4b932a6dd 34 return ret;
shimizuta 11:e81425872740 35 }
shimizuta 11:e81425872740 36
shimizuta 27:79b4b932a6dd 37 Walk::Walk()
shimizuta 27:79b4b932a6dd 38 {
shimizuta 27:79b4b932a6dd 39 for (int i = 0; i < 4; i++)
shimizuta 27:79b4b932a6dd 40 offset_multi_[i] = 0;
shimizuta 27:79b4b932a6dd 41 }
shimizuta 27:79b4b932a6dd 42 float Walk::calctime_s_;
shimizuta 29:7d8b8011a88d 43 int Walk::Cal4LegsPosi(OneLeg leg[4]) //失敗したら1を返す。成功なら0
shimizuta 11:e81425872740 44 {
shimizuta 29:7d8b8011a88d 45 int is_out = 0;
shimizuta 11:e81425872740 46 for (int i = 0; i < 4; i++)
shimizuta 11:e81425872740 47 {
shimizuta 27:79b4b932a6dd 48 float one_walk_time = orbit_[i].GetOneWalkTime();
shimizuta 27:79b4b932a6dd 49 phasetime_s_[i] += calctime_s_;
shimizuta 27:79b4b932a6dd 50 while (phasetime_s_[i] > one_walk_time)
shimizuta 27:79b4b932a6dd 51 phasetime_s_[i] -= one_walk_time;
shimizuta 29:7d8b8011a88d 52 if (orbit_[i].GetOrbit(leg[i], phasetime_s_[i]) == 1)
shimizuta 29:7d8b8011a88d 53 {
shimizuta 29:7d8b8011a88d 54 printf("error:leg %d in Cal4LegsPosi\r\n", i);
shimizuta 29:7d8b8011a88d 55 is_out = 1;
shimizuta 29:7d8b8011a88d 56 }
shimizuta 11:e81425872740 57 }
shimizuta 29:7d8b8011a88d 58 return is_out;
shimizuta 11:e81425872740 59 }
shimizuta 27:79b4b932a6dd 60 float Walk::GetOneWalkTime()
shimizuta 27:79b4b932a6dd 61 {
shimizuta 27:79b4b932a6dd 62 return orbit_[0].GetOneWalkTime(); //4足全て同じ時間のはずなので一例としてorbit_[0]のものを返している.
shimizuta 27:79b4b932a6dd 63 };
shimizuta 29:7d8b8011a88d 64 void Walk::SetOffsetTime(float offset_multi0, float offset_multi1, float offset_multi2, float offset_multi3)
shimizuta 11:e81425872740 65 {
shimizuta 27:79b4b932a6dd 66 offset_multi_[0] = offset_multi0;
shimizuta 27:79b4b932a6dd 67 offset_multi_[1] = offset_multi1;
shimizuta 27:79b4b932a6dd 68 offset_multi_[2] = offset_multi2;
shimizuta 27:79b4b932a6dd 69 offset_multi_[3] = offset_multi3;
shimizuta 27:79b4b932a6dd 70 }
shimizuta 27:79b4b932a6dd 71 //軌道がリンク定義外になっていないかチェック。reutn 0:ok 1:out
shimizuta 27:79b4b932a6dd 72 int Walk::CheckOrbit(OneLeg templateleg)
shimizuta 27:79b4b932a6dd 73 {
shimizuta 11:e81425872740 74 for (int i = 0; i < 4; i++)
shimizuta 11:e81425872740 75 {
shimizuta 27:79b4b932a6dd 76 //軌道が値域の外に出ないか計算で確かめる
shimizuta 27:79b4b932a6dd 77 float one_walk_time = GetOneWalkTime();
shimizuta 27:79b4b932a6dd 78 int step = one_walk_time / calctime_s_ * 2;
shimizuta 27:79b4b932a6dd 79 for (float j = 0; j < one_walk_time; j += step)
shimizuta 27:79b4b932a6dd 80 {
shimizuta 27:79b4b932a6dd 81 if (orbit_[i].GetOrbit(templateleg, j) == 1)
shimizuta 29:7d8b8011a88d 82 {
shimizuta 29:7d8b8011a88d 83 printf("error:leg %d, time %f", i, j);
shimizuta 27:79b4b932a6dd 84 return 1; //解が出ないときは1を返す
shimizuta 29:7d8b8011a88d 85 }
shimizuta 27:79b4b932a6dd 86 }
shimizuta 27:79b4b932a6dd 87 phasetime_s_[i] = one_walk_time * offset_multi_[i];
shimizuta 11:e81425872740 88 }
shimizuta 27:79b4b932a6dd 89 return 0;
shimizuta 11:e81425872740 90 }
shimizuta 27:79b4b932a6dd 91 void Walk::SetOneOrbit(int legnum, Orbit orbit)
shimizuta 27:79b4b932a6dd 92 {
shimizuta 27:79b4b932a6dd 93 orbit_[legnum] = orbit;
shimizuta 27:79b4b932a6dd 94 }
shimizuta 27:79b4b932a6dd 95 void Walk::SetAllOrbit(Orbit orbit)
shimizuta 27:79b4b932a6dd 96 {
shimizuta 27:79b4b932a6dd 97 for (int i = 0; i < 4; i++)
shimizuta 27:79b4b932a6dd 98 SetOneOrbit(i, orbit);
shimizuta 27:79b4b932a6dd 99 }
shimizuta 29:7d8b8011a88d 100 void Walk::SetOneLegStandParam(int legnum,float x_m, float y_m, float time_s)
shimizuta 29:7d8b8011a88d 101 {
shimizuta 29:7d8b8011a88d 102 Orbit triangle(TRIANGLE);
shimizuta 29:7d8b8011a88d 103 triangle.SetTriangleParam(x_m, y_m, 0, 0, 0, time_s, 0,0);
shimizuta 29:7d8b8011a88d 104 SetOneOrbit(legnum, triangle);
shimizuta 29:7d8b8011a88d 105 }
shimizuta 29:7d8b8011a88d 106 void Walk::SetAllLegStandParam(float x_m, float y_m, float time_s)
shimizuta 27:79b4b932a6dd 107 {
shimizuta 29:7d8b8011a88d 108 for (int i = 0; i < 4; i ++)
shimizuta 29:7d8b8011a88d 109 SetOneLegStandParam(i, x_m, y_m, time_s);
shimizuta 27:79b4b932a6dd 110 }
shimizuta 29:7d8b8011a88d 111 void Walk::SetOneLegTriangleParam(int legnum, float start_x_m, float start_y_m, float stride_m, float height_m, float buffer_height_m,
shimizuta 29:7d8b8011a88d 112 float stridetime_s, float toptime_s, float buffer_time_s)
shimizuta 29:7d8b8011a88d 113 {
shimizuta 29:7d8b8011a88d 114 Orbit triangle(TRIANGLE);
shimizuta 29:7d8b8011a88d 115 triangle.SetTriangleParam(start_x_m, start_y_m, stride_m, height_m, buffer_height_m,
shimizuta 29:7d8b8011a88d 116 stridetime_s, toptime_s, buffer_time_s);
shimizuta 29:7d8b8011a88d 117 SetOneOrbit(legnum, triangle);
shimizuta 29:7d8b8011a88d 118 }
shimizuta 29:7d8b8011a88d 119 void Walk::SetAllLegTriangleParam(float start_x_m, float start_y_m, float stride_m, float height_m, float buffer_height_m,
shimizuta 29:7d8b8011a88d 120 float stridetime_s, float toptime_s, float buffer_time_s)
shimizuta 27:79b4b932a6dd 121 {
shimizuta 27:79b4b932a6dd 122 for (int i = 0; i < 4; i++)
shimizuta 29:7d8b8011a88d 123 SetOneLegTriangleParam(i, start_x_m, start_y_m, stride_m, height_m, buffer_height_m,
shimizuta 29:7d8b8011a88d 124 stridetime_s, toptime_s, buffer_time_s);
shimizuta 27:79b4b932a6dd 125 }
shimizuta 29:7d8b8011a88d 126
shimizuta 29:7d8b8011a88d 127 void TriangleOrbit::SetTriangleParam(float start_x_m, float start_y_m, float stride_m, float height_m, float buffer_height_m,
shimizuta 29:7d8b8011a88d 128 float stridetime_s, float toptime_s, float buffer_time_s)
shimizuta 29:7d8b8011a88d 129 {
shimizuta 29:7d8b8011a88d 130 reverse_tanbeta_ = 1.0 / tan(81.0 / 180.0 * M_PI); //論文よりこれが最適らしい
shimizuta 29:7d8b8011a88d 131 start_x_m_ = start_x_m;
shimizuta 29:7d8b8011a88d 132 start_y_m_ = start_y_m;
shimizuta 29:7d8b8011a88d 133 stride_m_ = stride_m;
shimizuta 29:7d8b8011a88d 134 height_m_ = height_m; //足上げ幅
shimizuta 29:7d8b8011a88d 135 buffer_height_m_ = buffer_height_m; //着地直前で止める高さ
shimizuta 29:7d8b8011a88d 136 stridetime_s_ = stridetime_s;
shimizuta 29:7d8b8011a88d 137 toptime_s_ = toptime_s; //頂点に行くまでの時間
shimizuta 29:7d8b8011a88d 138 buffer_time_s_ = buffer_time_s; //頂点から一時停止点までの時間.
shimizuta 29:7d8b8011a88d 139
shimizuta 29:7d8b8011a88d 140 //事前に計算しておく
shimizuta 29:7d8b8011a88d 141 top_x_m_ = start_x_m_ - height_m * reverse_tanbeta_;
shimizuta 29:7d8b8011a88d 142 top_y_m_ = -height_m_ + start_y_m_;
shimizuta 29:7d8b8011a88d 143 buffer_x_m_ = start_x_m_ - buffer_height_m_ * reverse_tanbeta_;
shimizuta 29:7d8b8011a88d 144 buffer_y_m_ = -buffer_height_m_ + start_y_m_;
shimizuta 29:7d8b8011a88d 145 }
shimizuta 29:7d8b8011a88d 146 //足一周の時間
shimizuta 29:7d8b8011a88d 147 float TriangleOrbit::GetOneWalkTime()
shimizuta 29:7d8b8011a88d 148 {
shimizuta 29:7d8b8011a88d 149 return stridetime_s_ + toptime_s_ + buffer_time_s_;
shimizuta 29:7d8b8011a88d 150 };
shimizuta 29:7d8b8011a88d 151 int TriangleOrbit::GetOrbit(OneLeg &leg, float phasetime_s)
shimizuta 27:79b4b932a6dd 152 {
shimizuta 29:7d8b8011a88d 153 int ret = 0;
shimizuta 29:7d8b8011a88d 154 if (phasetime_s < stridetime_s_)
shimizuta 29:7d8b8011a88d 155 ret = StrideLineAccel_(leg, phasetime_s);
shimizuta 29:7d8b8011a88d 156 else if (phasetime_s < stridetime_s_ + toptime_s_)
shimizuta 29:7d8b8011a88d 157 ret = leg.SetXY_m(top_x_m_, top_y_m_);
shimizuta 29:7d8b8011a88d 158 else
shimizuta 29:7d8b8011a88d 159 ret = leg.SetXY_m(buffer_x_m_, buffer_y_m_);
shimizuta 29:7d8b8011a88d 160 return ret;
shimizuta 29:7d8b8011a88d 161 };
shimizuta 29:7d8b8011a88d 162 int TriangleOrbit::StrideLine_(OneLeg &leg, float phasetime_s)
shimizuta 29:7d8b8011a88d 163 {
shimizuta 29:7d8b8011a88d 164 float x_m = start_x_m_ - stride_m_ * phasetime_s / stridetime_s_;
shimizuta 29:7d8b8011a88d 165 float y_m = start_y_m_;
shimizuta 29:7d8b8011a88d 166 return leg.SetXY_m(x_m, y_m);
shimizuta 27:79b4b932a6dd 167 }
shimizuta 29:7d8b8011a88d 168
shimizuta 29:7d8b8011a88d 169 int TriangleOrbit::StrideLineAccel_(OneLeg &leg, float phasetime_s)
shimizuta 29:7d8b8011a88d 170 {
shimizuta 29:7d8b8011a88d 171 ///////////x,yを計算
shimizuta 29:7d8b8011a88d 172 float s0 = start_x_m_;
shimizuta 29:7d8b8011a88d 173 float s1 = start_x_m_ - stride_m_;
shimizuta 29:7d8b8011a88d 174 float g_h = sqrtf(kGravity / start_y_m_);
shimizuta 29:7d8b8011a88d 175 float t = phasetime_s / stridetime_s_;
shimizuta 29:7d8b8011a88d 176 float denominator = expf(g_h) - expf(-g_h); //分母
shimizuta 29:7d8b8011a88d 177
shimizuta 29:7d8b8011a88d 178 float x_m = -(s0 * expf(-g_h) - s1) * expf(g_h * t) / denominator + (s0 * expf(g_h) - s1) * expf(-g_h * t) / denominator;
shimizuta 29:7d8b8011a88d 179 float y_m = start_y_m_;
shimizuta 29:7d8b8011a88d 180 //x,yを代入
shimizuta 29:7d8b8011a88d 181 return leg.SetXY_m(x_m, y_m);
shimizuta 29:7d8b8011a88d 182 }