岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

Walk/Walk.cpp@27:79b4b932a6dd, 2019-02-27 (annotated)

Committer:
shimizuta
Date:
Wed Feb 27 01:24:00 2019 +0000
Revision:
27:79b4b932a6dd
Parent:
26:24ae5a4f5b1f
Child:
29:7d8b8011a88d
debug was ok. ROS is not debuged

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 void EllipseOrbit::SetParam(float stridetime_s, float risetime_s,
shimizuta 27:79b4b932a6dd 10 float stride_m, float height_m, float ground_m,
shimizuta 27:79b4b932a6dd 11 float ellipsecenter_x_m, float ellipsecenter_y_m)
shimizuta 14:d7cb429946f4 12 {
shimizuta 14:d7cb429946f4 13 stridetime_s_ = stridetime_s;
shimizuta 14:d7cb429946f4 14 risetime_s_ = risetime_s;
shimizuta 14:d7cb429946f4 15 stride_m_ = stride_m;
shimizuta 14:d7cb429946f4 16 height_m_ = height_m;
shimizuta 21:61971fc18b90 17 ground_m_ = ground_m;
shimizuta 27:79b4b932a6dd 18 ellipsecenter_x_m_ = ellipsecenter_x_m;
shimizuta 27:79b4b932a6dd 19 ellipsecenter_y_m_ = ellipsecenter_y_m;
shimizuta 14:d7cb429946f4 20 }
shimizuta 14:d7cb429946f4 21
shimizuta 11:e81425872740 22 //着地中の動き.直線軌道.等速
shimizuta 27:79b4b932a6dd 23 int EllipseOrbit::StrideLine_(OneLeg &leg, float phasetime_s)
shimizuta 11:e81425872740 24 {
shimizuta 27:79b4b932a6dd 25 float x_m = -stride_m_ * phasetime_s / stridetime_s_ + stride_m_ * 0.5 + ellipsecenter_x_m_;
shimizuta 27:79b4b932a6dd 26 float y_m = ellipsecenter_y_m_;
shimizuta 27:79b4b932a6dd 27 return leg.SetXY_m(x_m, y_m);
shimizuta 11:e81425872740 28 }
shimizuta 27:79b4b932a6dd 29 int EllipseOrbit::StrideLineAccel_(OneLeg &leg, float phasetime_s)
shimizuta 23:97f34a7dc63c 30 {
shimizuta 27:79b4b932a6dd 31 ///////////x,yを計算
shimizuta 27:79b4b932a6dd 32 float s0 = stride_m_ * 0.5 + ellipsecenter_x_m_;
shimizuta 27:79b4b932a6dd 33 float s1 = -stride_m_ * 0.5 + ellipsecenter_x_m_;
shimizuta 27:79b4b932a6dd 34 float g_h = sqrtf(kGravity / ground_m_);
shimizuta 27:79b4b932a6dd 35 float t = phasetime_s / stridetime_s_;
shimizuta 27:79b4b932a6dd 36 float denominator = expf(g_h) - expf(-g_h); //分母
shimizuta 27:79b4b932a6dd 37
shimizuta 27:79b4b932a6dd 38 float x_m = -(s0 * expf(-g_h) - s1) * expf(g_h * t) / denominator + (s0 * expf(g_h) - s1) * expf(-g_h * t) / denominator;
A0413 24:dec983da455c 39 float y_m = ground_m_;
shimizuta 23:97f34a7dc63c 40 //x,yを代入
shimizuta 27:79b4b932a6dd 41 return leg.SetXY_m(x_m, y_m);
shimizuta 23:97f34a7dc63c 42 }
shimizuta 21:61971fc18b90 43 //空中の動き.半分にきれいに切れる楕円軌道
shimizuta 27:79b4b932a6dd 44 int EllipseOrbit::RiseEllipse_(OneLeg &leg, float phasetime_s)
shimizuta 11:e81425872740 45 {
shimizuta 14:d7cb429946f4 46 float rad = M_PI * (phasetime_s - stridetime_s_) / risetime_s_ + M_PI;
shimizuta 27:79b4b932a6dd 47 float x_m = stride_m_ * 0.5 * cosf(rad) + ellipsecenter_x_m_;
shimizuta 27:79b4b932a6dd 48 float y_m = height_m_ * sinf(rad) + ellipsecenter_y_m_;
shimizuta 27:79b4b932a6dd 49 return leg.SetXY_m(x_m, y_m);
shimizuta 21:61971fc18b90 50 }
shimizuta 21:61971fc18b90 51 //空中の動き.完全に半分には切れない楕円軌道
shimizuta 27:79b4b932a6dd 52 int EllipseOrbit::RiseEllipse2_(OneLeg &leg, float phasetime_s)
shimizuta 21:61971fc18b90 53 {
shimizuta 27:79b4b932a6dd 54 ///////////x,yを計算
shimizuta 27:79b4b932a6dd 55 float ellipselong_m = stride_m_ * 0.5 / cosf(asinf(ground_m_ - ellipsecenter_y_m_));
shimizuta 27:79b4b932a6dd 56 float ellipseshort_m = 2 * (height_m_ - ground_m_ + ellipsecenter_y_m_);
shimizuta 27:79b4b932a6dd 57 float theta1 = acosf(stride_m_ / ellipselong_m * 0.5); //stride の始まりの位置の角度（数学基準）
shimizuta 27:79b4b932a6dd 58 float theta2 = acosf(-stride_m_ / ellipselong_m * 0.5); //stride の終わりの位置の角度（数学基準）
shimizuta 27:79b4b932a6dd 59 float omega = (2 * M_PI - theta2 + theta1) / risetime_s_; //楕円軌道における角速度、一定の値とする（適当）
shimizuta 27:79b4b932a6dd 60 float rad = (phasetime_s - stridetime_s_) * omega + theta2; //角度の基準はtheta1とする（適当）
shimizuta 27:79b4b932a6dd 61 float x_m = ellipselong_m * cos(rad) + ellipsecenter_x_m_;
shimizuta 27:79b4b932a6dd 62 float y_m = ellipseshort_m * sin(rad) + ellipsecenter_y_m_;
shimizuta 21:61971fc18b90 63 //x,yを代入
shimizuta 27:79b4b932a6dd 64 return leg.SetXY_m(x_m, y_m);
shimizuta 11:e81425872740 65 }
shimizuta 11:e81425872740 66 //楕円軌道
shimizuta 27:79b4b932a6dd 67 int EllipseOrbit::GetOrbit(OneLeg &leg, float phasetime_s)
shimizuta 11:e81425872740 68 {
shimizuta 11:e81425872740 69 if (phasetime_s < stridetime_s_)
shimizuta 11:e81425872740 70 return StrideLine_(leg, phasetime_s);
shimizuta 11:e81425872740 71 else
shimizuta 11:e81425872740 72 return RiseEllipse_(leg, phasetime_s);
shimizuta 11:e81425872740 73 }
shimizuta 27:79b4b932a6dd 74 float EllipseOrbit::GetOneWalkTime()
shimizuta 27:79b4b932a6dd 75 {
shimizuta 27:79b4b932a6dd 76 return stridetime_s_ + risetime_s_;
shimizuta 27:79b4b932a6dd 77 }
shimizuta 27:79b4b932a6dd 78 void EllipseOrbit::ChangeOneParam(EllipseParams param, float val)
shimizuta 27:79b4b932a6dd 79 {
shimizuta 27:79b4b932a6dd 80 switch (param)
shimizuta 27:79b4b932a6dd 81 {
shimizuta 27:79b4b932a6dd 82 case STRIDE_M:
shimizuta 27:79b4b932a6dd 83 stride_m_ = val;
shimizuta 27:79b4b932a6dd 84 break;
shimizuta 27:79b4b932a6dd 85 case HEIGHT_M:
shimizuta 27:79b4b932a6dd 86 height_m_ = val;
shimizuta 27:79b4b932a6dd 87 break;
shimizuta 27:79b4b932a6dd 88 case GROUND_M:
shimizuta 27:79b4b932a6dd 89 ground_m_ = val;
shimizuta 27:79b4b932a6dd 90 break;
shimizuta 27:79b4b932a6dd 91 case ELLIPSE_CENTER_X_M:
shimizuta 27:79b4b932a6dd 92 ellipsecenter_x_m_ = val;
shimizuta 27:79b4b932a6dd 93 break;
shimizuta 27:79b4b932a6dd 94 case ELLIPSE_CENTER_Y_M:
shimizuta 27:79b4b932a6dd 95 ellipsecenter_y_m_ = val;
shimizuta 27:79b4b932a6dd 96 break;
shimizuta 27:79b4b932a6dd 97 }
shimizuta 27:79b4b932a6dd 98 }
shimizuta 27:79b4b932a6dd 99 void FreeLineOrbit::SetFreeLinesParam(LineParam lineparams[], int point_num)
shimizuta 27:79b4b932a6dd 100 {
shimizuta 27:79b4b932a6dd 101 point_num_ = point_num;
shimizuta 27:79b4b932a6dd 102 if (point_num_ == 1)
shimizuta 27:79b4b932a6dd 103 {
shimizuta 27:79b4b932a6dd 104 printf("error:point_num = 1. You should put >=2 to make line.");
shimizuta 27:79b4b932a6dd 105 while (1)
shimizuta 27:79b4b932a6dd 106 ;
shimizuta 27:79b4b932a6dd 107 }
shimizuta 27:79b4b932a6dd 108 for (int i = 0; i < point_num_; i++)
shimizuta 27:79b4b932a6dd 109 lineparams_[i] = lineparams[i];
shimizuta 27:79b4b932a6dd 110 }
shimizuta 27:79b4b932a6dd 111 int FreeLineOrbit::GetOrbit(OneLeg &leg, float phasetime_s)
shimizuta 11:e81425872740 112 {
shimizuta 27:79b4b932a6dd 113 ;
shimizuta 27:79b4b932a6dd 114 //現在の最終到達pointを決定
shimizuta 27:79b4b932a6dd 115 int arrived_point;
shimizuta 27:79b4b932a6dd 116 float sum_time = 0;
shimizuta 27:79b4b932a6dd 117 for (arrived_point = 0; arrived_point < point_num_ - 2; arrived_point++) //arrived_point = point_num-1(これが配列の最大index)の状態で終わらないように-2している
shimizuta 27:79b4b932a6dd 118 {
shimizuta 27:79b4b932a6dd 119 sum_time += lineparams_[arrived_point].time_s;
shimizuta 27:79b4b932a6dd 120 if (phasetime_s < sum_time)
shimizuta 27:79b4b932a6dd 121 break;
shimizuta 27:79b4b932a6dd 122 }
shimizuta 27:79b4b932a6dd 123 //x,yを計算
shimizuta 27:79b4b932a6dd 124 float x_m, y_m;
shimizuta 27:79b4b932a6dd 125 x_m = lineparams_[arrived_point].x_m +
shimizuta 27:79b4b932a6dd 126 (lineparams_[arrived_point + 1].x_m - lineparams_[arrived_point].x_m) * phasetime_s / lineparams_[arrived_point].time_s;
shimizuta 27:79b4b932a6dd 127 y_m = lineparams_[arrived_point].y_m +
shimizuta 27:79b4b932a6dd 128 (lineparams_[arrived_point + 1].y_m - lineparams_[arrived_point].y_m) * phasetime_s / lineparams_[arrived_point].time_s;
shimizuta 27:79b4b932a6dd 129 return leg.SetXY_m(x_m, y_m);
shimizuta 27:79b4b932a6dd 130 }
shimizuta 27:79b4b932a6dd 131 float FreeLineOrbit::GetOneWalkTime() //足一周の時間
shimizuta 27:79b4b932a6dd 132 {
shimizuta 27:79b4b932a6dd 133 float sum_time = 0;
shimizuta 27:79b4b932a6dd 134 for (int i = 0; i < point_num_; i++)
shimizuta 27:79b4b932a6dd 135 sum_time += lineparams_[i].time_s;
shimizuta 27:79b4b932a6dd 136 return sum_time;
shimizuta 27:79b4b932a6dd 137 }
shimizuta 27:79b4b932a6dd 138 void FreeLineOrbit::SetStandParam(float x_m, float y_m, float time_s)
shimizuta 27:79b4b932a6dd 139 {
shimizuta 27:79b4b932a6dd 140 point_num_ = 1;
shimizuta 27:79b4b932a6dd 141 lineparams_[0].x_m = x_m;
shimizuta 27:79b4b932a6dd 142 lineparams_[0].y_m = y_m;
shimizuta 27:79b4b932a6dd 143 lineparams_[0].time_s = time_s;
shimizuta 27:79b4b932a6dd 144 lineparams_[1].x_m = x_m;
shimizuta 27:79b4b932a6dd 145 lineparams_[1].y_m = y_m;
shimizuta 27:79b4b932a6dd 146 lineparams_[1].time_s = 0;
shimizuta 27:79b4b932a6dd 147 }
shimizuta 27:79b4b932a6dd 148
shimizuta 27:79b4b932a6dd 149 Orbit::Orbit(OrbitPattern pattern)
shimizuta 27:79b4b932a6dd 150 {
shimizuta 27:79b4b932a6dd 151 pattern_ = pattern;
shimizuta 11:e81425872740 152 }
shimizuta 11:e81425872740 153 float Orbit::GetOneWalkTime()
shimizuta 11:e81425872740 154 {
shimizuta 27:79b4b932a6dd 155 float time_s;
shimizuta 27:79b4b932a6dd 156 switch (pattern_)
shimizuta 27:79b4b932a6dd 157 {
shimizuta 27:79b4b932a6dd 158 case FREELINES:
shimizuta 27:79b4b932a6dd 159 time_s = FreeLineOrbit::GetOneWalkTime();
shimizuta 27:79b4b932a6dd 160 break;
shimizuta 27:79b4b932a6dd 161
shimizuta 27:79b4b932a6dd 162 default:
shimizuta 27:79b4b932a6dd 163 time_s = EllipseOrbit::GetOneWalkTime();
shimizuta 27:79b4b932a6dd 164 break;
shimizuta 27:79b4b932a6dd 165 }
shimizuta 27:79b4b932a6dd 166 return time_s;
shimizuta 27:79b4b932a6dd 167 }
shimizuta 27:79b4b932a6dd 168 int Orbit::GetOrbit(OneLeg &leg, float phasetime_s)
shimizuta 27:79b4b932a6dd 169 {
shimizuta 27:79b4b932a6dd 170 int ret;
shimizuta 27:79b4b932a6dd 171 switch (pattern_)
shimizuta 27:79b4b932a6dd 172 {
shimizuta 27:79b4b932a6dd 173 case FREELINES:
shimizuta 27:79b4b932a6dd 174 ret = FreeLineOrbit::GetOrbit(leg, phasetime_s);
shimizuta 27:79b4b932a6dd 175 break;
shimizuta 27:79b4b932a6dd 176 default:
shimizuta 27:79b4b932a6dd 177 ret = EllipseOrbit::GetOrbit(leg, phasetime_s);
shimizuta 27:79b4b932a6dd 178 break;
shimizuta 27:79b4b932a6dd 179 }
shimizuta 27:79b4b932a6dd 180 return ret;
shimizuta 11:e81425872740 181 }
shimizuta 11:e81425872740 182
shimizuta 27:79b4b932a6dd 183 Walk::Walk()
shimizuta 27:79b4b932a6dd 184 {
shimizuta 27:79b4b932a6dd 185 for (int i = 0; i < 4; i++)
shimizuta 27:79b4b932a6dd 186 offset_multi_[i] = 0;
shimizuta 27:79b4b932a6dd 187 }
shimizuta 27:79b4b932a6dd 188 float Walk::calctime_s_;
shimizuta 27:79b4b932a6dd 189 void Walk::Cal4LegsPosi(OneLeg leg[4])
shimizuta 11:e81425872740 190 {
shimizuta 11:e81425872740 191 for (int i = 0; i < 4; i++)
shimizuta 11:e81425872740 192 {
shimizuta 27:79b4b932a6dd 193 float one_walk_time = orbit_[i].GetOneWalkTime();
shimizuta 27:79b4b932a6dd 194 phasetime_s_[i] += calctime_s_;
shimizuta 27:79b4b932a6dd 195 while (phasetime_s_[i] > one_walk_time)
shimizuta 27:79b4b932a6dd 196 phasetime_s_[i] -= one_walk_time;
shimizuta 27:79b4b932a6dd 197 orbit_[i].GetOrbit(leg[i], phasetime_s_[i]);
shimizuta 11:e81425872740 198 }
shimizuta 11:e81425872740 199 }
shimizuta 27:79b4b932a6dd 200 float Walk::GetOneWalkTime()
shimizuta 27:79b4b932a6dd 201 {
shimizuta 27:79b4b932a6dd 202 return orbit_[0].GetOneWalkTime(); //4足全て同じ時間のはずなので一例としてorbit_[0]のものを返している.
shimizuta 27:79b4b932a6dd 203 };
shimizuta 27:79b4b932a6dd 204 void Walk::SetOffset(float offset_multi0, float offset_multi1, float offset_multi2, float offset_multi3)
shimizuta 11:e81425872740 205 {
shimizuta 27:79b4b932a6dd 206 offset_multi_[0] = offset_multi0;
shimizuta 27:79b4b932a6dd 207 offset_multi_[1] = offset_multi1;
shimizuta 27:79b4b932a6dd 208 offset_multi_[2] = offset_multi2;
shimizuta 27:79b4b932a6dd 209 offset_multi_[3] = offset_multi3;
shimizuta 27:79b4b932a6dd 210 }
shimizuta 27:79b4b932a6dd 211 //軌道がリンク定義外になっていないかチェック。reutn 0:ok 1:out
shimizuta 27:79b4b932a6dd 212 int Walk::CheckOrbit(OneLeg templateleg)
shimizuta 27:79b4b932a6dd 213 {
shimizuta 11:e81425872740 214 for (int i = 0; i < 4; i++)
shimizuta 11:e81425872740 215 {
shimizuta 27:79b4b932a6dd 216 //軌道が値域の外に出ないか計算で確かめる
shimizuta 27:79b4b932a6dd 217 float one_walk_time = GetOneWalkTime();
shimizuta 27:79b4b932a6dd 218 int step = one_walk_time / calctime_s_ * 2;
shimizuta 27:79b4b932a6dd 219 for (float j = 0; j < one_walk_time; j += step)
shimizuta 27:79b4b932a6dd 220 {
shimizuta 27:79b4b932a6dd 221 if (orbit_[i].GetOrbit(templateleg, j) == 1)
shimizuta 27:79b4b932a6dd 222 return 1; //解が出ないときは1を返す
shimizuta 27:79b4b932a6dd 223 }
shimizuta 27:79b4b932a6dd 224 phasetime_s_[i] = one_walk_time * offset_multi_[i];
shimizuta 11:e81425872740 225 }
shimizuta 27:79b4b932a6dd 226 return 0;
shimizuta 11:e81425872740 227 }
shimizuta 27:79b4b932a6dd 228 void Walk::SetOneOrbit(int legnum, Orbit orbit)
shimizuta 27:79b4b932a6dd 229 {
shimizuta 27:79b4b932a6dd 230 orbit_[legnum] = orbit;
shimizuta 27:79b4b932a6dd 231 }
shimizuta 27:79b4b932a6dd 232 void Walk::SetAllOrbit(Orbit orbit)
shimizuta 27:79b4b932a6dd 233 {
shimizuta 27:79b4b932a6dd 234 for (int i = 0; i < 4; i++)
shimizuta 27:79b4b932a6dd 235 SetOneOrbit(i, orbit);
shimizuta 27:79b4b932a6dd 236 }
shimizuta 27:79b4b932a6dd 237
shimizuta 27:79b4b932a6dd 238 void Walk::ChangeOneParam(int legnum, EllipseParams param, float val)
shimizuta 27:79b4b932a6dd 239 {
shimizuta 27:79b4b932a6dd 240 orbit_[legnum].ChangeOneParam(param, val);
shimizuta 27:79b4b932a6dd 241 }
shimizuta 27:79b4b932a6dd 242 void Walk::SetAllLegEllipseParam(float stridetime_s, float risetime_s,
shimizuta 27:79b4b932a6dd 243 float stride_m, float height_m, float ground_m,
shimizuta 27:79b4b932a6dd 244 float ellipsecenter_x_m, float ellipsecenter_y_m)
shimizuta 27:79b4b932a6dd 245 {
shimizuta 27:79b4b932a6dd 246 for (int i = 0; i < 4; i++)
shimizuta 27:79b4b932a6dd 247 orbit_[i].SetParam(stridetime_s, risetime_s, stride_m, height_m, ground_m, ellipsecenter_x_m, ellipsecenter_y_m);
shimizuta 27:79b4b932a6dd 248 }
shimizuta 27:79b4b932a6dd 249 void Walk::SetAllLegStandParam(float x_m, float y_m, float time_s)
shimizuta 27:79b4b932a6dd 250 {
shimizuta 27:79b4b932a6dd 251 Orbit stand(FREELINES);
shimizuta 27:79b4b932a6dd 252 stand.SetStandParam(x_m, y_m, time_s); //defaultとなる軌道設定
shimizuta 27:79b4b932a6dd 253 SetAllOrbit(stand); //4足にコピー
shimizuta 27:79b4b932a6dd 254 }