岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

Walk/Walk.cpp@26:24ae5a4f5b1f, 2019-02-22 (annotated)

Committer:
A0413
Date:
Fri Feb 22 05:20:28 2019 +0000
Revision:
26:24ae5a4f5b1f
Parent:
24:dec983da455c
Child:
27:79b4b932a6dd
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimizuta 11:e81425872740 1 #include "Walk.h"
shimizuta 11:e81425872740 2 #include "math.h"
shimizuta 14:d7cb429946f4 3 #include "pi.h"
yuto17320508 13:e7ecdb20665a 4 //Orbitは足毎の軌道をあらわす。
yuto17320508 13:e7ecdb20665a 5
shimizuta 21:61971fc18b90 6 void Orbit::SetStraightParam(float stridetime_s, float risetime_s,
shimizuta 21:61971fc18b90 7 float stride_m, float height_m, float ground_m,
shimizuta 21:61971fc18b90 8 float ellipse_center_x_m, float ellipse_center_y_m)
shimizuta 14:d7cb429946f4 9 {
shimizuta 14:d7cb429946f4 10 stridetime_s_ = stridetime_s;
shimizuta 14:d7cb429946f4 11 risetime_s_ = risetime_s;
shimizuta 14:d7cb429946f4 12 stride_m_ = stride_m;
shimizuta 14:d7cb429946f4 13 height_m_ = height_m;
shimizuta 21:61971fc18b90 14 ground_m_ = ground_m;
shimizuta 21:61971fc18b90 15 ellipse_center_x_m_ = ellipse_center_x_m;
shimizuta 21:61971fc18b90 16 ellipse_center_y_m_ = ellipse_center_y_m;
yuto17320508 18:0033ef1814ba 17 }
shimizuta 19:1adc7302cfd9 18
shimizuta 21:61971fc18b90 19 void Orbit::SetStandParam(float ground_m)
yuto17320508 18:0033ef1814ba 20 {
shimizuta 19:1adc7302cfd9 21 stridetime_s_ = 1;
shimizuta 19:1adc7302cfd9 22 risetime_s_ = 0;
shimizuta 19:1adc7302cfd9 23 stride_m_ = 0;
shimizuta 19:1adc7302cfd9 24 height_m_ = 0;
shimizuta 21:61971fc18b90 25 ground_m_ = ground_m;
shimizuta 21:61971fc18b90 26 ellipse_center_x_m_ = 0;
shimizuta 21:61971fc18b90 27 ellipse_center_y_m_ = ground_m;
A0413 26:24ae5a4f5b1f 28
A0413 26:24ae5a4f5b1f 29 ellipse_long_m_=stride_m*0.5/cosf(asinf(ground_m_-ellipse_center_y_m_));
A0413 26:24ae5a4f5b1f 30 ellipse_short_m_=2*(height_m_-grounf_m_+ellipse_center_y_m_);
shimizuta 14:d7cb429946f4 31 }
shimizuta 14:d7cb429946f4 32
shimizuta 11:e81425872740 33 //着地中の動き.直線軌道.等速
shimizuta 11:e81425872740 34 OneLeg Orbit::StrideLine_(OneLeg leg, float phasetime_s)
shimizuta 11:e81425872740 35 {
shimizuta 21:61971fc18b90 36 float x_m = -stride_m_ * phasetime_s / stridetime_s_ + stride_m_ * 0.5 + ellipse_center_x_m_;
shimizuta 21:61971fc18b90 37 float y_m = ellipse_center_y_m_;
shimizuta 11:e81425872740 38 leg.SetXY_m(x_m, y_m);
shimizuta 11:e81425872740 39 return leg;
shimizuta 11:e81425872740 40 }
shimizuta 23:97f34a7dc63c 41
shimizuta 23:97f34a7dc63c 42 //着地中の動き.直線軌道.加速
shimizuta 23:97f34a7dc63c 43 OneLeg Orbit::StrideLineAccel_(OneLeg leg, float phasetime_s)
shimizuta 23:97f34a7dc63c 44 {
shimizuta 23:97f34a7dc63c 45 ///////////x,yを計算.注：計算は正しくないので直す必要がある。
A0413 26:24ae5a4f5b1f 46 float s0=stride_m_*0.5+ellipse_center_x_m_;
A0413 26:24ae5a4f5b1f 47 float s1=-stride_m_*0.5+ellipse_center_x_m_;
A0413 24:dec983da455c 48
A0413 24:dec983da455c 49 float gravity=9.8;
A0413 24:dec983da455c 50 float g_h=sqrtf(gravity/ground_m_);
A0413 26:24ae5a4f5b1f 51 float t=phasetime_s/stridetime_s_;
A0413 24:dec983da455c 52 float denominator=expf(g_h)-expf(-g_h);//分母
A0413 24:dec983da455c 53
A0413 24:dec983da455c 54 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 55 float y_m = ground_m_;
shimizuta 23:97f34a7dc63c 56 ///////////
shimizuta 23:97f34a7dc63c 57 //x,yを代入
shimizuta 23:97f34a7dc63c 58 leg.SetXY_m(x_m, y_m);
shimizuta 23:97f34a7dc63c 59 return leg;
shimizuta 23:97f34a7dc63c 60 }
shimizuta 23:97f34a7dc63c 61
shimizuta 21:61971fc18b90 62 //空中の動き.半分にきれいに切れる楕円軌道
shimizuta 11:e81425872740 63 OneLeg Orbit::RiseEllipse_(OneLeg leg, float phasetime_s)
shimizuta 11:e81425872740 64 {
shimizuta 14:d7cb429946f4 65 float rad = M_PI * (phasetime_s - stridetime_s_) / risetime_s_ + M_PI;
shimizuta 21:61971fc18b90 66 float x_m = stride_m_ * 0.5 * cos(rad) + ellipse_center_x_m_;
shimizuta 21:61971fc18b90 67 float y_m = height_m_ * sin(rad) + ellipse_center_y_m_;
shimizuta 21:61971fc18b90 68 leg.SetXY_m(x_m, y_m);
shimizuta 21:61971fc18b90 69 return leg;
shimizuta 21:61971fc18b90 70 }
shimizuta 21:61971fc18b90 71
shimizuta 21:61971fc18b90 72 //空中の動き.完全に半分には切れない楕円軌道
shimizuta 21:61971fc18b90 73 OneLeg Orbit::RiseEllipse2_(OneLeg leg, float phasetime_s)
shimizuta 21:61971fc18b90 74 {
shimizuta 21:61971fc18b90 75 ///////////x,yを計算.注：計算は正しくないので直す必要がある。
A0413 22:787a764271dd 76 float theta1=acosf(stride_m_/ellipse_long_m_*0.5); //stride の始まりの位置の角度（数学基準）
A0413 22:787a764271dd 77 float theta2=acosf(-stride_m_/ellipse_long_m_*0.5); //stride の終わりの位置の角度（数学基準）
A0413 22:787a764271dd 78 float omega=(2*M_PI-theta2+theta1)/risetime_s_; //楕円軌道における角速度、一定の値とする（適当）
A0413 22:787a764271dd 79 float rad=(phasetime_s-stridetime_s_)*omega+theta2; //角度の基準はtheta1とする（適当）
A0413 22:787a764271dd 80 float x_m = ellipse_long_m_* cos(rad) + ellipse_center_x_m_;
A0413 22:787a764271dd 81 float y_m =ellipse_short_m_ * sin(rad) + ellipse_center_y_m_;
shimizuta 21:61971fc18b90 82 ///////////
shimizuta 21:61971fc18b90 83 //x,yを代入
shimizuta 11:e81425872740 84 leg.SetXY_m(x_m, y_m);
shimizuta 11:e81425872740 85 return leg;
shimizuta 11:e81425872740 86 }
shimizuta 11:e81425872740 87 //楕円軌道
shimizuta 11:e81425872740 88 OneLeg Orbit::OrbitEllipse_(OneLeg leg, float phasetime_s)
shimizuta 11:e81425872740 89 {
shimizuta 11:e81425872740 90 if (phasetime_s < stridetime_s_)
shimizuta 11:e81425872740 91 return StrideLine_(leg, phasetime_s);
shimizuta 11:e81425872740 92 else
shimizuta 11:e81425872740 93 return RiseEllipse_(leg, phasetime_s);
shimizuta 11:e81425872740 94 }
shimizuta 11:e81425872740 95 //将来的には引数などで軌道の種類を変えて出力できるようにしたい。
shimizuta 11:e81425872740 96 OneLeg Orbit::GetOrbit(OneLeg leg, float phasetime_s)
shimizuta 11:e81425872740 97 {
shimizuta 11:e81425872740 98 return OrbitEllipse_(leg, phasetime_s);
shimizuta 11:e81425872740 99 }
shimizuta 11:e81425872740 100 float Orbit::GetOneWalkTime()
shimizuta 11:e81425872740 101 {
shimizuta 11:e81425872740 102 return stridetime_s_ + risetime_s_;
shimizuta 11:e81425872740 103 }
shimizuta 11:e81425872740 104
shimizuta 11:e81425872740 105 Walk::Walk(Orbit orbit[4], float offsettime_s[4], float cycletime_s)
shimizuta 11:e81425872740 106 {
shimizuta 11:e81425872740 107 for (int i = 0; i < 4; i++)
shimizuta 11:e81425872740 108 {
shimizuta 11:e81425872740 109 orbit_[i] = orbit[i];
shimizuta 11:e81425872740 110 offsettime_s_[i] = offsettime_s[i];
shimizuta 11:e81425872740 111 }
shimizuta 11:e81425872740 112 cycletime_s_ = cycletime_s;
shimizuta 11:e81425872740 113 }
shimizuta 11:e81425872740 114 void Walk::Cal4LegsPosi(OneLeg (&leg)[4])
shimizuta 11:e81425872740 115 {
shimizuta 11:e81425872740 116 phasetime_s_ += cycletime_s_;
shimizuta 14:d7cb429946f4 117 while (phasetime_s_ > orbit_[0].GetOneWalkTime())
shimizuta 14:d7cb429946f4 118 phasetime_s_ -= orbit_[0].GetOneWalkTime();
shimizuta 11:e81425872740 119 for (int i = 0; i < 4; i++)
shimizuta 11:e81425872740 120 {
shimizuta 11:e81425872740 121 float phasetime_s = phasetime_s_ + offsettime_s_[i];
shimizuta 11:e81425872740 122 while (phasetime_s > orbit_[i].GetOneWalkTime())
shimizuta 11:e81425872740 123 phasetime_s -= orbit_[i].GetOneWalkTime();
shimizuta 11:e81425872740 124 leg[i] = orbit_[i].GetOrbit(leg[i], phasetime_s);
shimizuta 11:e81425872740 125 }
shimizuta 11:e81425872740 126 }