test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Walk/Walk.cpp@32:dc684a0b8448, 2019-02-28 (annotated)
- Committer:
- shimizuta
- Date:
- Thu Feb 28 08:48:21 2019 +0000
- Revision:
- 32:dc684a0b8448
- Parent:
- 29:7d8b8011a88d
- Child:
- 34:89d701e15cdf
a
Who changed what in which revision?
User | Revision | Line number | New 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 | 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 | 27:79b4b932a6dd | 84 | int step = one_walk_time / calctime_s_ * 2; |
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 | 32:dc684a0b8448 | 162 | top_x_m_ = offset_x_m_ + stride_m * 0.5 - height_m * reverse_tanbeta_; |
shimizuta | 32:dc684a0b8448 | 163 | top_y_m_ = -height_m_ + offset_y_m_; |
shimizuta | 32:dc684a0b8448 | 164 | buffer_x_m_ = offset_x_m_ + stride_m * 0.5 - buffer_height_m_ * reverse_tanbeta_; |
shimizuta | 32:dc684a0b8448 | 165 | buffer_y_m_ = -buffer_height_m_ + offset_y_m_; |
shimizuta | 29:7d8b8011a88d | 166 | } |
shimizuta | 29:7d8b8011a88d | 167 | //足一周の時間 |
shimizuta | 29:7d8b8011a88d | 168 | float TriangleOrbit::GetOneWalkTime() |
shimizuta | 29:7d8b8011a88d | 169 | { |
shimizuta | 29:7d8b8011a88d | 170 | return stridetime_s_ + toptime_s_ + buffer_time_s_; |
shimizuta | 29:7d8b8011a88d | 171 | }; |
shimizuta | 29:7d8b8011a88d | 172 | int TriangleOrbit::GetOrbit(OneLeg &leg, float phasetime_s) |
shimizuta | 27:79b4b932a6dd | 173 | { |
shimizuta | 29:7d8b8011a88d | 174 | int ret = 0; |
shimizuta | 29:7d8b8011a88d | 175 | if (phasetime_s < stridetime_s_) |
shimizuta | 29:7d8b8011a88d | 176 | ret = StrideLineAccel_(leg, phasetime_s); |
shimizuta | 29:7d8b8011a88d | 177 | else if (phasetime_s < stridetime_s_ + toptime_s_) |
shimizuta | 29:7d8b8011a88d | 178 | ret = leg.SetXY_m(top_x_m_, top_y_m_); |
shimizuta | 29:7d8b8011a88d | 179 | else |
shimizuta | 29:7d8b8011a88d | 180 | ret = leg.SetXY_m(buffer_x_m_, buffer_y_m_); |
shimizuta | 29:7d8b8011a88d | 181 | return ret; |
shimizuta | 29:7d8b8011a88d | 182 | }; |
shimizuta | 29:7d8b8011a88d | 183 | int TriangleOrbit::StrideLine_(OneLeg &leg, float phasetime_s) |
shimizuta | 29:7d8b8011a88d | 184 | { |
shimizuta | 32:dc684a0b8448 | 185 | float x_m = -stride_m_ * phasetime_s / stridetime_s_ + stride_m_ * 0.5 + offset_x_m_; |
shimizuta | 32:dc684a0b8448 | 186 | float y_m = offset_y_m_; |
shimizuta | 29:7d8b8011a88d | 187 | return leg.SetXY_m(x_m, y_m); |
shimizuta | 27:79b4b932a6dd | 188 | } |
shimizuta | 29:7d8b8011a88d | 189 | int TriangleOrbit::StrideLineAccel_(OneLeg &leg, float phasetime_s) |
shimizuta | 29:7d8b8011a88d | 190 | { |
shimizuta | 29:7d8b8011a88d | 191 | ///////////x,yを計算 |
shimizuta | 32:dc684a0b8448 | 192 | float s0 = stride_m_ * 0.5 + offset_x_m_; |
shimizuta | 32:dc684a0b8448 | 193 | float s1 = offset_x_m_ -stride_m_ * 0.5; |
shimizuta | 32:dc684a0b8448 | 194 | float g_h = sqrtf(kGravity / offset_y_m_); |
shimizuta | 29:7d8b8011a88d | 195 | float t = phasetime_s / stridetime_s_; |
shimizuta | 29:7d8b8011a88d | 196 | float denominator = expf(g_h) - expf(-g_h); //分母 |
shimizuta | 29:7d8b8011a88d | 197 | |
shimizuta | 29:7d8b8011a88d | 198 | 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 | 199 | float y_m = offset_y_m_; |
shimizuta | 29:7d8b8011a88d | 200 | //x,yを代入 |
shimizuta | 29:7d8b8011a88d | 201 | return leg.SetXY_m(x_m, y_m); |
shimizuta | 32:dc684a0b8448 | 202 | } |
shimizuta | 32:dc684a0b8448 | 203 | void TriangleOrbit::ChangeOneParam(TriangleParams param, float val) |
shimizuta | 32:dc684a0b8448 | 204 | { |
shimizuta | 32:dc684a0b8448 | 205 | switch (param) |
shimizuta | 32:dc684a0b8448 | 206 | { |
shimizuta | 32:dc684a0b8448 | 207 | case OFFSET_X_M: |
shimizuta | 32:dc684a0b8448 | 208 | offset_x_m_ = val; |
shimizuta | 32:dc684a0b8448 | 209 | break; |
shimizuta | 32:dc684a0b8448 | 210 | case OFFSET_Y_M: |
shimizuta | 32:dc684a0b8448 | 211 | offset_y_m_ = val; |
shimizuta | 32:dc684a0b8448 | 212 | break; |
shimizuta | 32:dc684a0b8448 | 213 | case STRIDE_M: |
shimizuta | 32:dc684a0b8448 | 214 | stride_m_ = val; |
shimizuta | 32:dc684a0b8448 | 215 | break; |
shimizuta | 32:dc684a0b8448 | 216 | case HEIGHT_M: |
shimizuta | 32:dc684a0b8448 | 217 | height_m_ = val; |
shimizuta | 32:dc684a0b8448 | 218 | break; |
shimizuta | 32:dc684a0b8448 | 219 | case BUFFER_HEIGHT_M: |
shimizuta | 32:dc684a0b8448 | 220 | buffer_height_m_ = val; |
shimizuta | 32:dc684a0b8448 | 221 | break; |
shimizuta | 32:dc684a0b8448 | 222 | } |
shimizuta | 32:dc684a0b8448 | 223 | } |
shimizuta | 32:dc684a0b8448 | 224 | |
shimizuta | 32:dc684a0b8448 | 225 | void FreeLineOrbit::SetFreeLinesParam(LineParam lineparams[], int point_num) |
shimizuta | 32:dc684a0b8448 | 226 | { |
shimizuta | 32:dc684a0b8448 | 227 | point_num_ = point_num; |
shimizuta | 32:dc684a0b8448 | 228 | if (point_num_ == 1) |
shimizuta | 32:dc684a0b8448 | 229 | { |
shimizuta | 32:dc684a0b8448 | 230 | printf("error:point_num = 1. You should put >=2 to make line."); |
shimizuta | 32:dc684a0b8448 | 231 | while (1) |
shimizuta | 32:dc684a0b8448 | 232 | ; |
shimizuta | 32:dc684a0b8448 | 233 | } |
shimizuta | 32:dc684a0b8448 | 234 | for (int i = 0; i < point_num_; i++) |
shimizuta | 32:dc684a0b8448 | 235 | lineparams_[i] = lineparams[i]; |
shimizuta | 32:dc684a0b8448 | 236 | } |
shimizuta | 32:dc684a0b8448 | 237 | int FreeLineOrbit::GetOrbit(OneLeg &leg, float phasetime_s) |
shimizuta | 32:dc684a0b8448 | 238 | { |
shimizuta | 32:dc684a0b8448 | 239 | ; |
shimizuta | 32:dc684a0b8448 | 240 | //現在の最終到達pointを決定 |
shimizuta | 32:dc684a0b8448 | 241 | int arrived_point; |
shimizuta | 32:dc684a0b8448 | 242 | float sum_time = 0; |
shimizuta | 32:dc684a0b8448 | 243 | for (arrived_point = 0; arrived_point < point_num_ - 2; arrived_point++) //arrived_point = point_num - 1(これが配列の最大index)の状態で終わらないように-2している |
shimizuta | 32:dc684a0b8448 | 244 | { |
shimizuta | 32:dc684a0b8448 | 245 | sum_time += lineparams_[arrived_point].time_s; |
shimizuta | 32:dc684a0b8448 | 246 | if (phasetime_s < sum_time) |
shimizuta | 32:dc684a0b8448 | 247 | break; |
shimizuta | 32:dc684a0b8448 | 248 | } |
shimizuta | 32:dc684a0b8448 | 249 | //x,yを計算 |
shimizuta | 32:dc684a0b8448 | 250 | float x_m, y_m; |
shimizuta | 32:dc684a0b8448 | 251 | x_m = lineparams_[arrived_point].x_m + |
shimizuta | 32:dc684a0b8448 | 252 | (lineparams_[arrived_point + 1].x_m - lineparams_[arrived_point].x_m) * |
shimizuta | 32:dc684a0b8448 | 253 | (phasetime_s - sum_time + lineparams_[arrived_point].time_s) / lineparams_[arrived_point].time_s; |
shimizuta | 32:dc684a0b8448 | 254 | y_m = lineparams_[arrived_point].y_m + |
shimizuta | 32:dc684a0b8448 | 255 | (lineparams_[arrived_point + 1].y_m - lineparams_[arrived_point].y_m) * |
shimizuta | 32:dc684a0b8448 | 256 | (phasetime_s - sum_time + lineparams_[arrived_point].time_s) / lineparams_[arrived_point].time_s; |
shimizuta | 32:dc684a0b8448 | 257 | return leg.SetXY_m(x_m, y_m); |
shimizuta | 32:dc684a0b8448 | 258 | } |
shimizuta | 32:dc684a0b8448 | 259 | float FreeLineOrbit::GetOneWalkTime() //足一周の時間 |
shimizuta | 32:dc684a0b8448 | 260 | { |
shimizuta | 32:dc684a0b8448 | 261 | float sum_time = 0; |
shimizuta | 32:dc684a0b8448 | 262 | for (int i = 0; i < point_num_; i++) |
shimizuta | 32:dc684a0b8448 | 263 | sum_time += lineparams_[i].time_s; |
shimizuta | 32:dc684a0b8448 | 264 | return sum_time; |
shimizuta | 32:dc684a0b8448 | 265 | } |
shimizuta | 32:dc684a0b8448 | 266 | void FreeLineOrbit::SetStandParam(float x_m, float y_m, float time_s) |
shimizuta | 32:dc684a0b8448 | 267 | { |
shimizuta | 32:dc684a0b8448 | 268 | point_num_ = 2; |
shimizuta | 32:dc684a0b8448 | 269 | lineparams_[0].x_m = x_m; |
shimizuta | 32:dc684a0b8448 | 270 | lineparams_[0].y_m = y_m; |
shimizuta | 32:dc684a0b8448 | 271 | lineparams_[0].time_s = time_s; |
shimizuta | 32:dc684a0b8448 | 272 | lineparams_[1].x_m = x_m; |
shimizuta | 32:dc684a0b8448 | 273 | lineparams_[1].y_m = y_m; |
shimizuta | 32:dc684a0b8448 | 274 | lineparams_[1].time_s = 0; |
shimizuta | 32:dc684a0b8448 | 275 | } |