岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

Walk/Walk.cpp@34:89d701e15cdf, 2019-03-01 (annotated)

Committer:
shimizuta
Date:
Fri Mar 01 12:07:23 2019 +0000
Revision:
34:89d701e15cdf
Parent:
32:dc684a0b8448
Child:
35:b4e1b8f25cd7
over come and rope

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 34:89d701e15cdf 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 32:dc684a0b8448 19 case FREELINES:
shimizuta 32:dc684a0b8448 20 time_s = FreeLineOrbit::GetOneWalkTime();
shimizuta 32:dc684a0b8448 21 break;
shimizuta 27:79b4b932a6dd 22 default:
shimizuta 29:7d8b8011a88d 23 time_s = TriangleOrbit::GetOneWalkTime();
shimizuta 27:79b4b932a6dd 24 break;
shimizuta 27:79b4b932a6dd 25 }
shimizuta 27:79b4b932a6dd 26 return time_s;
shimizuta 27:79b4b932a6dd 27 }
shimizuta 27:79b4b932a6dd 28 int Orbit::GetOrbit(OneLeg &leg, float phasetime_s)
shimizuta 27:79b4b932a6dd 29 {
shimizuta 27:79b4b932a6dd 30 int ret;
shimizuta 27:79b4b932a6dd 31 switch (pattern_)
shimizuta 27:79b4b932a6dd 32 {
shimizuta 32:dc684a0b8448 33 case FREELINES:
shimizuta 32:dc684a0b8448 34 ret = FreeLineOrbit::GetOrbit(leg, phasetime_s);
shimizuta 32:dc684a0b8448 35 break;
shimizuta 27:79b4b932a6dd 36 default:
shimizuta 29:7d8b8011a88d 37 ret = TriangleOrbit::GetOrbit(leg, phasetime_s);
shimizuta 27:79b4b932a6dd 38 break;
shimizuta 27:79b4b932a6dd 39 }
shimizuta 27:79b4b932a6dd 40 return ret;
shimizuta 11:e81425872740 41 }
shimizuta 11:e81425872740 42
shimizuta 27:79b4b932a6dd 43 Walk::Walk()
shimizuta 27:79b4b932a6dd 44 {
shimizuta 27:79b4b932a6dd 45 for (int i = 0; i < 4; i++)
shimizuta 27:79b4b932a6dd 46 offset_multi_[i] = 0;
shimizuta 27:79b4b932a6dd 47 }
shimizuta 27:79b4b932a6dd 48 float Walk::calctime_s_;
shimizuta 29:7d8b8011a88d 49 int Walk::Cal4LegsPosi(OneLeg leg[4]) //失敗したら1を返す。成功なら0
shimizuta 11:e81425872740 50 {
shimizuta 29:7d8b8011a88d 51 int is_out = 0;
shimizuta 11:e81425872740 52 for (int i = 0; i < 4; i++)
shimizuta 11:e81425872740 53 {
shimizuta 27:79b4b932a6dd 54 float one_walk_time = orbit_[i].GetOneWalkTime();
shimizuta 27:79b4b932a6dd 55 phasetime_s_[i] += calctime_s_;
shimizuta 27:79b4b932a6dd 56 while (phasetime_s_[i] > one_walk_time)
shimizuta 27:79b4b932a6dd 57 phasetime_s_[i] -= one_walk_time;
shimizuta 29:7d8b8011a88d 58 if (orbit_[i].GetOrbit(leg[i], phasetime_s_[i]) == 1)
shimizuta 29:7d8b8011a88d 59 {
shimizuta 29:7d8b8011a88d 60 printf("error:leg %d in Cal4LegsPosi\r\n", i);
shimizuta 29:7d8b8011a88d 61 is_out = 1;
shimizuta 29:7d8b8011a88d 62 }
shimizuta 11:e81425872740 63 }
shimizuta 29:7d8b8011a88d 64 return is_out;
shimizuta 11:e81425872740 65 }
shimizuta 27:79b4b932a6dd 66 float Walk::GetOneWalkTime()
shimizuta 27:79b4b932a6dd 67 {
shimizuta 27:79b4b932a6dd 68 return orbit_[0].GetOneWalkTime(); //4足全て同じ時間のはずなので一例としてorbit_[0]のものを返している.
shimizuta 27:79b4b932a6dd 69 };
shimizuta 29:7d8b8011a88d 70 void Walk::SetOffsetTime(float offset_multi0, float offset_multi1, float offset_multi2, float offset_multi3)
shimizuta 11:e81425872740 71 {
shimizuta 27:79b4b932a6dd 72 offset_multi_[0] = offset_multi0;
shimizuta 27:79b4b932a6dd 73 offset_multi_[1] = offset_multi1;
shimizuta 27:79b4b932a6dd 74 offset_multi_[2] = offset_multi2;
shimizuta 27:79b4b932a6dd 75 offset_multi_[3] = offset_multi3;
shimizuta 27:79b4b932a6dd 76 }
shimizuta 27:79b4b932a6dd 77 //軌道がリンク定義外になっていないかチェック。reutn 0:ok 1:out
shimizuta 27:79b4b932a6dd 78 int Walk::CheckOrbit(OneLeg templateleg)
shimizuta 27:79b4b932a6dd 79 {
shimizuta 11:e81425872740 80 for (int i = 0; i < 4; i++)
shimizuta 11:e81425872740 81 {
shimizuta 27:79b4b932a6dd 82 //軌道が値域の外に出ないか計算で確かめる
shimizuta 27:79b4b932a6dd 83 float one_walk_time = GetOneWalkTime();
shimizuta 34:89d701e15cdf 84 float step = calctime_s_ * 0.1;
shimizuta 27:79b4b932a6dd 85 for (float j = 0; j < one_walk_time; j += step)
shimizuta 27:79b4b932a6dd 86 {
shimizuta 27:79b4b932a6dd 87 if (orbit_[i].GetOrbit(templateleg, j) == 1)
shimizuta 29:7d8b8011a88d 88 {
shimizuta 29:7d8b8011a88d 89 printf("error:leg %d, time %f", i, j);
shimizuta 27:79b4b932a6dd 90 return 1; //解が出ないときは1を返す
shimizuta 29:7d8b8011a88d 91 }
shimizuta 27:79b4b932a6dd 92 }
shimizuta 27:79b4b932a6dd 93 phasetime_s_[i] = one_walk_time * offset_multi_[i];
shimizuta 11:e81425872740 94 }
shimizuta 27:79b4b932a6dd 95 return 0;
shimizuta 11:e81425872740 96 }
shimizuta 27:79b4b932a6dd 97 void Walk::SetOneOrbit(int legnum, Orbit orbit)
shimizuta 27:79b4b932a6dd 98 {
shimizuta 27:79b4b932a6dd 99 orbit_[legnum] = orbit;
shimizuta 27:79b4b932a6dd 100 }
shimizuta 27:79b4b932a6dd 101 void Walk::SetAllOrbit(Orbit orbit)
shimizuta 27:79b4b932a6dd 102 {
shimizuta 27:79b4b932a6dd 103 for (int i = 0; i < 4; i++)
shimizuta 27:79b4b932a6dd 104 SetOneOrbit(i, orbit);
shimizuta 27:79b4b932a6dd 105 }
shimizuta 32:dc684a0b8448 106 void Walk::SetOneLegStandParam(int legnum, float x_m, float y_m, float time_s)
shimizuta 29:7d8b8011a88d 107 {
shimizuta 29:7d8b8011a88d 108 Orbit triangle(TRIANGLE);
shimizuta 32:dc684a0b8448 109 triangle.SetTriangleParam(x_m, y_m, 0, 0, 0, time_s, 0, 0);
shimizuta 29:7d8b8011a88d 110 SetOneOrbit(legnum, triangle);
shimizuta 29:7d8b8011a88d 111 }
shimizuta 32:dc684a0b8448 112 void Walk::SetAllLegStandParam(float x_m, float y_m, float time_s)
shimizuta 27:79b4b932a6dd 113 {
shimizuta 32:dc684a0b8448 114 for (int i = 0; i < 4; i++)
shimizuta 29:7d8b8011a88d 115 SetOneLegStandParam(i, x_m, y_m, time_s);
shimizuta 27:79b4b932a6dd 116 }
shimizuta 32:dc684a0b8448 117 void Walk::SetOneLegTriangleParam(int legnum, float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m,
shimizuta 32:dc684a0b8448 118 float stridetime_s, float toptime_s, float buffer_time_s)
shimizuta 29:7d8b8011a88d 119 {
shimizuta 29:7d8b8011a88d 120 Orbit triangle(TRIANGLE);
shimizuta 32:dc684a0b8448 121 triangle.SetTriangleParam(offset_x_m, offset_y_m, stride_m, height_m, buffer_height_m,
shimizuta 29:7d8b8011a88d 122 stridetime_s, toptime_s, buffer_time_s);
shimizuta 29:7d8b8011a88d 123 SetOneOrbit(legnum, triangle);
shimizuta 29:7d8b8011a88d 124 }
shimizuta 32:dc684a0b8448 125 void Walk::SetAllLegTriangleParam(float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m,
shimizuta 29:7d8b8011a88d 126 float stridetime_s, float toptime_s, float buffer_time_s)
shimizuta 27:79b4b932a6dd 127 {
shimizuta 27:79b4b932a6dd 128 for (int i = 0; i < 4; i++)
shimizuta 32:dc684a0b8448 129 SetOneLegTriangleParam(i, offset_x_m, offset_y_m, stride_m, height_m, buffer_height_m,
shimizuta 29:7d8b8011a88d 130 stridetime_s, toptime_s, buffer_time_s);
shimizuta 27:79b4b932a6dd 131 }
shimizuta 29:7d8b8011a88d 132
shimizuta 32:dc684a0b8448 133 void Walk::SetOneLegFreeLinesParam(int legnum, LineParam lineparams[], int point_num)
shimizuta 32:dc684a0b8448 134 {
shimizuta 32:dc684a0b8448 135 Orbit freeline(FREELINES);
shimizuta 32:dc684a0b8448 136 freeline.SetFreeLinesParam(lineparams, point_num);
shimizuta 32:dc684a0b8448 137 SetOneOrbit(legnum, freeline);
shimizuta 32:dc684a0b8448 138 }
shimizuta 32:dc684a0b8448 139 void Walk::SetAllLegFreeLinesParam(LineParam lineparams[], int point_num)
shimizuta 32:dc684a0b8448 140 {
shimizuta 32:dc684a0b8448 141 for (int i = 0; i < 4; i++)
shimizuta 32:dc684a0b8448 142 SetOneLegFreeLinesParam(i, lineparams, point_num);
shimizuta 32:dc684a0b8448 143 }
shimizuta 32:dc684a0b8448 144 void Walk::ChangeOneParam(int legnum, TriangleParams param, float val)
shimizuta 32:dc684a0b8448 145 {
shimizuta 32:dc684a0b8448 146 orbit_[legnum].ChangeOneParam(param, val);
shimizuta 32:dc684a0b8448 147 }
shimizuta 32:dc684a0b8448 148 void TriangleOrbit::SetTriangleParam(float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m,
shimizuta 29:7d8b8011a88d 149 float stridetime_s, float toptime_s, float buffer_time_s)
shimizuta 29:7d8b8011a88d 150 {
shimizuta 29:7d8b8011a88d 151 reverse_tanbeta_ = 1.0 / tan(81.0 / 180.0 * M_PI); //論文よりこれが最適らしい
shimizuta 32:dc684a0b8448 152 offset_x_m_ = offset_x_m;
shimizuta 32:dc684a0b8448 153 offset_y_m_ = offset_y_m;
shimizuta 29:7d8b8011a88d 154 stride_m_ = stride_m;
shimizuta 29:7d8b8011a88d 155 height_m_ = height_m; //足上げ幅
shimizuta 29:7d8b8011a88d 156 buffer_height_m_ = buffer_height_m; //着地直前で止める高さ
shimizuta 29:7d8b8011a88d 157 stridetime_s_ = stridetime_s;
shimizuta 29:7d8b8011a88d 158 toptime_s_ = toptime_s; //頂点に行くまでの時間
shimizuta 29:7d8b8011a88d 159 buffer_time_s_ = buffer_time_s; //頂点から一時停止点までの時間.
shimizuta 29:7d8b8011a88d 160
shimizuta 29:7d8b8011a88d 161 //事前に計算しておく
shimizuta 34:89d701e15cdf 162 CalOtherParam();
shimizuta 34:89d701e15cdf 163 }
shimizuta 34:89d701e15cdf 164 void TriangleOrbit::CalOtherParam()
shimizuta 34:89d701e15cdf 165 {
shimizuta 34:89d701e15cdf 166 top_x_m_ = offset_x_m_ + stride_m_ * 0.5 - height_m_ * reverse_tanbeta_;
shimizuta 32:dc684a0b8448 167 top_y_m_ = -height_m_ + offset_y_m_;
shimizuta 34:89d701e15cdf 168 buffer_x_m_ = offset_x_m_ + stride_m_ * 0.5 - buffer_height_m_ * reverse_tanbeta_;
shimizuta 32:dc684a0b8448 169 buffer_y_m_ = -buffer_height_m_ + offset_y_m_;
shimizuta 29:7d8b8011a88d 170 }
shimizuta 29:7d8b8011a88d 171 //足一周の時間
shimizuta 29:7d8b8011a88d 172 float TriangleOrbit::GetOneWalkTime()
shimizuta 29:7d8b8011a88d 173 {
shimizuta 29:7d8b8011a88d 174 return stridetime_s_ + toptime_s_ + buffer_time_s_;
shimizuta 29:7d8b8011a88d 175 };
shimizuta 29:7d8b8011a88d 176 int TriangleOrbit::GetOrbit(OneLeg &leg, float phasetime_s)
shimizuta 27:79b4b932a6dd 177 {
shimizuta 29:7d8b8011a88d 178 int ret = 0;
shimizuta 29:7d8b8011a88d 179 if (phasetime_s < stridetime_s_)
shimizuta 29:7d8b8011a88d 180 ret = StrideLineAccel_(leg, phasetime_s);
shimizuta 29:7d8b8011a88d 181 else if (phasetime_s < stridetime_s_ + toptime_s_)
shimizuta 29:7d8b8011a88d 182 ret = leg.SetXY_m(top_x_m_, top_y_m_);
shimizuta 29:7d8b8011a88d 183 else
shimizuta 29:7d8b8011a88d 184 ret = leg.SetXY_m(buffer_x_m_, buffer_y_m_);
shimizuta 29:7d8b8011a88d 185 return ret;
shimizuta 29:7d8b8011a88d 186 };
shimizuta 29:7d8b8011a88d 187 int TriangleOrbit::StrideLine_(OneLeg &leg, float phasetime_s)
shimizuta 29:7d8b8011a88d 188 {
shimizuta 32:dc684a0b8448 189 float x_m = -stride_m_ * phasetime_s / stridetime_s_ + stride_m_ * 0.5 + offset_x_m_;
shimizuta 32:dc684a0b8448 190 float y_m = offset_y_m_;
shimizuta 29:7d8b8011a88d 191 return leg.SetXY_m(x_m, y_m);
shimizuta 27:79b4b932a6dd 192 }
shimizuta 29:7d8b8011a88d 193 int TriangleOrbit::StrideLineAccel_(OneLeg &leg, float phasetime_s)
shimizuta 29:7d8b8011a88d 194 {
shimizuta 29:7d8b8011a88d 195 ///////////x,yを計算
shimizuta 32:dc684a0b8448 196 float s0 = stride_m_ * 0.5 + offset_x_m_;
shimizuta 34:89d701e15cdf 197 float s1 = offset_x_m_ - stride_m_ * 0.5;
shimizuta 32:dc684a0b8448 198 float g_h = sqrtf(kGravity / offset_y_m_);
shimizuta 29:7d8b8011a88d 199 float t = phasetime_s / stridetime_s_;
shimizuta 29:7d8b8011a88d 200 float denominator = expf(g_h) - expf(-g_h); //分母
shimizuta 29:7d8b8011a88d 201
shimizuta 29:7d8b8011a88d 202 float x_m = -(s0 * expf(-g_h) - s1) * expf(g_h * t) / denominator + (s0 * expf(g_h) - s1) * expf(-g_h * t) / denominator;
shimizuta 32:dc684a0b8448 203 float y_m = offset_y_m_;
shimizuta 29:7d8b8011a88d 204 //x,yを代入
shimizuta 29:7d8b8011a88d 205 return leg.SetXY_m(x_m, y_m);
shimizuta 32:dc684a0b8448 206 }
shimizuta 32:dc684a0b8448 207 void TriangleOrbit::ChangeOneParam(TriangleParams param, float val)
shimizuta 32:dc684a0b8448 208 {
shimizuta 32:dc684a0b8448 209 switch (param)
shimizuta 32:dc684a0b8448 210 {
shimizuta 32:dc684a0b8448 211 case OFFSET_X_M:
shimizuta 32:dc684a0b8448 212 offset_x_m_ = val;
shimizuta 32:dc684a0b8448 213 break;
shimizuta 32:dc684a0b8448 214 case OFFSET_Y_M:
shimizuta 32:dc684a0b8448 215 offset_y_m_ = val;
shimizuta 32:dc684a0b8448 216 break;
shimizuta 32:dc684a0b8448 217 case STRIDE_M:
shimizuta 32:dc684a0b8448 218 stride_m_ = val;
shimizuta 32:dc684a0b8448 219 break;
shimizuta 32:dc684a0b8448 220 case HEIGHT_M:
shimizuta 32:dc684a0b8448 221 height_m_ = val;
shimizuta 32:dc684a0b8448 222 break;
shimizuta 32:dc684a0b8448 223 case BUFFER_HEIGHT_M:
shimizuta 32:dc684a0b8448 224 buffer_height_m_ = val;
shimizuta 32:dc684a0b8448 225 break;
shimizuta 32:dc684a0b8448 226 }
shimizuta 34:89d701e15cdf 227 CalOtherParam();
shimizuta 32:dc684a0b8448 228 }
shimizuta 32:dc684a0b8448 229
shimizuta 32:dc684a0b8448 230 void FreeLineOrbit::SetFreeLinesParam(LineParam lineparams[], int point_num)
shimizuta 32:dc684a0b8448 231 {
shimizuta 32:dc684a0b8448 232 point_num_ = point_num;
shimizuta 32:dc684a0b8448 233 for (int i = 0; i < point_num_; i++)
shimizuta 32:dc684a0b8448 234 lineparams_[i] = lineparams[i];
shimizuta 32:dc684a0b8448 235 }
shimizuta 32:dc684a0b8448 236 int FreeLineOrbit::GetOrbit(OneLeg &leg, float phasetime_s)
shimizuta 32:dc684a0b8448 237 {
shimizuta 32:dc684a0b8448 238 ;
shimizuta 32:dc684a0b8448 239 //現在の最終到達pointを決定
shimizuta 34:89d701e15cdf 240 int next_point;
shimizuta 34:89d701e15cdf 241 for (next_point = 0; next_point < point_num_ - 1; next_point++) // point_num - 1が配列の最大index.これを超えないようにしている
shimizuta 32:dc684a0b8448 242 {
shimizuta 34:89d701e15cdf 243 if (phasetime_s < lineparams_[next_point].time_s)
shimizuta 32:dc684a0b8448 244 break;
shimizuta 32:dc684a0b8448 245 }
shimizuta 32:dc684a0b8448 246 //x,yを計算
shimizuta 32:dc684a0b8448 247 float x_m, y_m;
shimizuta 34:89d701e15cdf 248 float d_phase = phasetime_s - lineparams_[next_point - 1].time_s;
shimizuta 34:89d701e15cdf 249 float d_time = lineparams_[next_point].time_s - lineparams_[next_point - 1].time_s;
shimizuta 34:89d701e15cdf 250 float phase_rate = d_phase / d_time;
shimizuta 34:89d701e15cdf 251 x_m = lineparams_[next_point - 1].x_m +
shimizuta 34:89d701e15cdf 252 (lineparams_[next_point].x_m - lineparams_[next_point - 1].x_m) * phase_rate;
shimizuta 34:89d701e15cdf 253 y_m = lineparams_[next_point - 1].y_m +
shimizuta 34:89d701e15cdf 254 (lineparams_[next_point].y_m - lineparams_[next_point - 1].y_m) * phase_rate;
shimizuta 32:dc684a0b8448 255 return leg.SetXY_m(x_m, y_m);
shimizuta 32:dc684a0b8448 256 }
shimizuta 32:dc684a0b8448 257 float FreeLineOrbit::GetOneWalkTime() //足一周の時間
shimizuta 32:dc684a0b8448 258 {
shimizuta 34:89d701e15cdf 259 return lineparams_[point_num_ - 1].time_s;
shimizuta 32:dc684a0b8448 260 }
shimizuta 32:dc684a0b8448 261 void FreeLineOrbit::SetStandParam(float x_m, float y_m, float time_s)
shimizuta 32:dc684a0b8448 262 {
shimizuta 32:dc684a0b8448 263 point_num_ = 2;
shimizuta 32:dc684a0b8448 264 lineparams_[0].x_m = x_m;
shimizuta 32:dc684a0b8448 265 lineparams_[0].y_m = y_m;
shimizuta 34:89d701e15cdf 266 lineparams_[0].time_s = 0;
shimizuta 32:dc684a0b8448 267 lineparams_[1].x_m = x_m;
shimizuta 32:dc684a0b8448 268 lineparams_[1].y_m = y_m;
shimizuta 34:89d701e15cdf 269 lineparams_[1].time_s = time_s;
shimizuta 34:89d701e15cdf 270 }
shimizuta 34:89d701e15cdf 271 OverCome::OverCome(float start_x_m[4], float start_y_m[4],
shimizuta 34:89d701e15cdf 272 float d_x_m, float goal_y_m[4], float height_m[4], float gravity_dist[4])
shimizuta 34:89d701e15cdf 273 {
shimizuta 34:89d701e15cdf 274 for (int i = 0; i < 4; i++)
shimizuta 34:89d701e15cdf 275 {
shimizuta 34:89d701e15cdf 276 start_x_m_[i] = start_x_m[i]; //足のスタートx
shimizuta 34:89d701e15cdf 277 start_y_m_[i] = start_y_m[i]; //足のスタートy
shimizuta 34:89d701e15cdf 278 goal_y_m_[i] = goal_y_m[i]; //目標地点までのy
shimizuta 34:89d701e15cdf 279 gravity_dist_[i] = gravity_dist[i];
shimizuta 34:89d701e15cdf 280 height_m_[i] = height_m[i];
shimizuta 34:89d701e15cdf 281 }
shimizuta 34:89d701e15cdf 282 d_x_m_ = d_x_m; //目標地点までのx
shimizuta 34:89d701e15cdf 283
shimizuta 34:89d701e15cdf 284 d_time_ = 0.5; //各動きの時間
shimizuta 34:89d701e15cdf 285 d_time_slow_ = 0.5;
shimizuta 34:89d701e15cdf 286 next_point_ = 0; //次のparamのindex
shimizuta 34:89d701e15cdf 287 GetLine();
shimizuta 34:89d701e15cdf 288 for (int i = 0; i < 4; i++)
shimizuta 34:89d701e15cdf 289 walk.SetOneLegFreeLinesParam(i, legs_[i], next_point_);
shimizuta 34:89d701e15cdf 290 }
shimizuta 34:89d701e15cdf 291
shimizuta 34:89d701e15cdf 292 void OverCome::Rise(int legnum)
shimizuta 34:89d701e15cdf 293 {
shimizuta 34:89d701e15cdf 294 for (int i = 0; i < 4; i++)
shimizuta 34:89d701e15cdf 295 {
shimizuta 34:89d701e15cdf 296 legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_;
shimizuta 34:89d701e15cdf 297 legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m;
shimizuta 34:89d701e15cdf 298 legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m;
shimizuta 34:89d701e15cdf 299 }
shimizuta 34:89d701e15cdf 300 legs_[legnum][next_point_].y_m -= height_m_[legnum];
shimizuta 34:89d701e15cdf 301 next_point_++;
shimizuta 34:89d701e15cdf 302 }
shimizuta 34:89d701e15cdf 303 void OverCome::Forward(int legnum)
shimizuta 34:89d701e15cdf 304 {
shimizuta 34:89d701e15cdf 305 for (int i = 0; i < 4; i++)
shimizuta 34:89d701e15cdf 306 {
shimizuta 34:89d701e15cdf 307 legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_;
shimizuta 34:89d701e15cdf 308 legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m;
shimizuta 34:89d701e15cdf 309 legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m;
shimizuta 34:89d701e15cdf 310 }
shimizuta 34:89d701e15cdf 311 legs_[legnum][next_point_].x_m += d_x_m_;
shimizuta 34:89d701e15cdf 312 next_point_++;
shimizuta 34:89d701e15cdf 313 }
shimizuta 34:89d701e15cdf 314 void OverCome::Land(int legnum)
shimizuta 34:89d701e15cdf 315 {
shimizuta 34:89d701e15cdf 316 for (int i = 0; i < 4; i++)
shimizuta 34:89d701e15cdf 317 {
shimizuta 34:89d701e15cdf 318 legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_slow_;
shimizuta 34:89d701e15cdf 319 legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m;
shimizuta 34:89d701e15cdf 320 legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m;
shimizuta 34:89d701e15cdf 321 }
shimizuta 34:89d701e15cdf 322 legs_[legnum][next_point_].y_m = goal_y_m_[legnum]; //着地
shimizuta 34:89d701e15cdf 323 next_point_++;
shimizuta 32:dc684a0b8448 324 }
shimizuta 34:89d701e15cdf 325 void OverCome::Step(int legnum)
shimizuta 34:89d701e15cdf 326 {
shimizuta 34:89d701e15cdf 327 Rise(legnum);
shimizuta 34:89d701e15cdf 328 Forward(legnum);
shimizuta 34:89d701e15cdf 329 Land(legnum);
shimizuta 34:89d701e15cdf 330 }
shimizuta 34:89d701e15cdf 331 //重心移動、param direct:動く正負。+-1を入れる
shimizuta 34:89d701e15cdf 332 void OverCome::GravityMove(int legnum)
shimizuta 34:89d701e15cdf 333 {
shimizuta 34:89d701e15cdf 334 for (int i = 0; i < 4; i++)
shimizuta 34:89d701e15cdf 335 {
shimizuta 34:89d701e15cdf 336 legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_;
shimizuta 34:89d701e15cdf 337 legs_[i][next_point_].x_m = legs_[i][next_point_ - 1].x_m - gravity_dist_[legnum];
shimizuta 34:89d701e15cdf 338 legs_[i][next_point_].y_m = legs_[i][next_point_ - 1].y_m;
shimizuta 34:89d701e15cdf 339 }
shimizuta 34:89d701e15cdf 340 ++next_point_;
shimizuta 34:89d701e15cdf 341 }
shimizuta 34:89d701e15cdf 342 void OverCome::StartPoint()
shimizuta 34:89d701e15cdf 343 {
shimizuta 34:89d701e15cdf 344 for (int i = 0; i < 4; i++)
shimizuta 34:89d701e15cdf 345 {
shimizuta 34:89d701e15cdf 346 legs_[i][next_point_].time_s = 0;
shimizuta 34:89d701e15cdf 347 legs_[i][next_point_].x_m = start_x_m_[i];
shimizuta 34:89d701e15cdf 348 legs_[i][next_point_].y_m = start_y_m_[i];
shimizuta 34:89d701e15cdf 349 }
shimizuta 34:89d701e15cdf 350 ++next_point_;
shimizuta 34:89d701e15cdf 351 }
shimizuta 34:89d701e15cdf 352 void OverCome::GoalPoint()
shimizuta 34:89d701e15cdf 353 {
shimizuta 34:89d701e15cdf 354 for (int i = 0; i < 4; i++)
shimizuta 34:89d701e15cdf 355 {
shimizuta 34:89d701e15cdf 356 legs_[i][next_point_].time_s = legs_[i][next_point_ - 1].time_s + d_time_;
shimizuta 34:89d701e15cdf 357 legs_[i][next_point_].x_m = start_x_m_[i];
shimizuta 34:89d701e15cdf 358 legs_[i][next_point_].y_m = goal_y_m_[i];
shimizuta 34:89d701e15cdf 359 }
shimizuta 34:89d701e15cdf 360 ++next_point_;
shimizuta 34:89d701e15cdf 361 }
shimizuta 34:89d701e15cdf 362 void OverCome::GetLine()
shimizuta 34:89d701e15cdf 363 {
shimizuta 34:89d701e15cdf 364 StartPoint(); //スタート時点
shimizuta 34:89d701e15cdf 365 GravityMove(RIGHT_F);
shimizuta 34:89d701e15cdf 366 Step(RIGHT_F);
shimizuta 34:89d701e15cdf 367 GravityMove(LEFT_B);
shimizuta 34:89d701e15cdf 368 Step(LEFT_B);
shimizuta 34:89d701e15cdf 369 GravityMove(LEFT_F);
shimizuta 34:89d701e15cdf 370 Step(LEFT_F);
shimizuta 34:89d701e15cdf 371 GravityMove(RIGHT_B);
shimizuta 34:89d701e15cdf 372 Step(RIGHT_B);
shimizuta 34:89d701e15cdf 373 GoalPoint();
shimizuta 34:89d701e15cdf 374 /* GravityMove(1);
shimizuta 34:89d701e15cdf 375 Step(RIGHT_B);
shimizuta 34:89d701e15cdf 376 GravityMove(-1);
shimizuta 34:89d701e15cdf 377 Step(RIGHT_F);
shimizuta 34:89d701e15cdf 378 GravityMove(1);
shimizuta 34:89d701e15cdf 379 Step(LEFT_B);
shimizuta 34:89d701e15cdf 380 GravityMove(-1);
shimizuta 34:89d701e15cdf 381 Step(LEFT_F);
shimizuta 34:89d701e15cdf 382 GravityMove(1);
shimizuta 34:89d701e15cdf 383 */
shimizuta 34:89d701e15cdf 384 }