岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

Walk/orbit/triangle/triangle.cpp@35:b4e1b8f25cd7, 2019-03-04 (annotated)

Committer:
shimizuta
Date:
Mon Mar 04 09:54:47 2019 +0000
Revision:
35:b4e1b8f25cd7
Child:
39:87dcdff27797
new MR2;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimizuta 35:b4e1b8f25cd7 1 #include "triangle.h"
shimizuta 35:b4e1b8f25cd7 2 #include "pi.h"
shimizuta 35:b4e1b8f25cd7 3 #include <math.h>
shimizuta 35:b4e1b8f25cd7 4 void Triangle::SetTriangleParam(float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m,
shimizuta 35:b4e1b8f25cd7 5 float stridetime_s, float toptime_s, float buffer_time_s)
shimizuta 35:b4e1b8f25cd7 6 {
shimizuta 35:b4e1b8f25cd7 7 reverse_tanbeta_ = 1.0 / tan(81.0 / 180.0 * M_PI); //論文よりこれが最適らしい
shimizuta 35:b4e1b8f25cd7 8 offset_x_m_ = offset_x_m;
shimizuta 35:b4e1b8f25cd7 9 offset_y_m_ = offset_y_m;
shimizuta 35:b4e1b8f25cd7 10 stride_m_ = stride_m;
shimizuta 35:b4e1b8f25cd7 11 height_m_ = height_m; //足上げ幅
shimizuta 35:b4e1b8f25cd7 12 buffer_height_m_ = buffer_height_m; //着地直前で止める高さ
shimizuta 35:b4e1b8f25cd7 13 stridetime_s_ = stridetime_s;
shimizuta 35:b4e1b8f25cd7 14 toptime_s_ = toptime_s; //頂点に行くまでの時間
shimizuta 35:b4e1b8f25cd7 15 buffer_time_s_ = buffer_time_s; //頂点から一時停止点までの時間.
shimizuta 35:b4e1b8f25cd7 16
shimizuta 35:b4e1b8f25cd7 17 //事前に計算しておく
shimizuta 35:b4e1b8f25cd7 18 CalOtherParam();
shimizuta 35:b4e1b8f25cd7 19 }
shimizuta 35:b4e1b8f25cd7 20 void Triangle::CalOtherParam()
shimizuta 35:b4e1b8f25cd7 21 {
shimizuta 35:b4e1b8f25cd7 22 top_x_m_ = offset_x_m_ + stride_m_ * 0.5 - height_m_ * reverse_tanbeta_;
shimizuta 35:b4e1b8f25cd7 23 top_y_m_ = -height_m_ + offset_y_m_;
shimizuta 35:b4e1b8f25cd7 24 buffer_x_m_ = offset_x_m_ + stride_m_ * 0.5 - buffer_height_m_ * reverse_tanbeta_;
shimizuta 35:b4e1b8f25cd7 25 buffer_y_m_ = -buffer_height_m_ + offset_y_m_;
shimizuta 35:b4e1b8f25cd7 26 }
shimizuta 35:b4e1b8f25cd7 27 //足一周の時間
shimizuta 35:b4e1b8f25cd7 28 float Triangle::GetOneWalkTime()
shimizuta 35:b4e1b8f25cd7 29 {
shimizuta 35:b4e1b8f25cd7 30 return stridetime_s_ + toptime_s_ + buffer_time_s_;
shimizuta 35:b4e1b8f25cd7 31 };
shimizuta 35:b4e1b8f25cd7 32 int Triangle::GetOrbit(OneLeg &leg, float phasetime_s)
shimizuta 35:b4e1b8f25cd7 33 {
shimizuta 35:b4e1b8f25cd7 34 int ret = 0;
shimizuta 35:b4e1b8f25cd7 35 if (phasetime_s < stridetime_s_)
shimizuta 35:b4e1b8f25cd7 36 ret = StrideLineAccel_(leg, phasetime_s);
shimizuta 35:b4e1b8f25cd7 37 else if (phasetime_s < stridetime_s_ + toptime_s_)
shimizuta 35:b4e1b8f25cd7 38 ret = leg.SetXY_m(top_x_m_, top_y_m_);
shimizuta 35:b4e1b8f25cd7 39 else
shimizuta 35:b4e1b8f25cd7 40 ret = leg.SetXY_m(buffer_x_m_, buffer_y_m_);
shimizuta 35:b4e1b8f25cd7 41 return ret;
shimizuta 35:b4e1b8f25cd7 42 };
shimizuta 35:b4e1b8f25cd7 43 int Triangle::StrideLine_(OneLeg &leg, float phasetime_s)
shimizuta 35:b4e1b8f25cd7 44 {
shimizuta 35:b4e1b8f25cd7 45 float x_m = -stride_m_ * phasetime_s / stridetime_s_ + stride_m_ * 0.5 + offset_x_m_;
shimizuta 35:b4e1b8f25cd7 46 float y_m = offset_y_m_;
shimizuta 35:b4e1b8f25cd7 47 return leg.SetXY_m(x_m, y_m);
shimizuta 35:b4e1b8f25cd7 48 }
shimizuta 35:b4e1b8f25cd7 49 int Triangle::StrideLineAccel_(OneLeg &leg, float phasetime_s)
shimizuta 35:b4e1b8f25cd7 50 {
shimizuta 35:b4e1b8f25cd7 51 ///////////x,yを計算
shimizuta 35:b4e1b8f25cd7 52 float s0 = stride_m_ * 0.5 + offset_x_m_;
shimizuta 35:b4e1b8f25cd7 53 float s1 = offset_x_m_ - stride_m_ * 0.5;
shimizuta 35:b4e1b8f25cd7 54 float g_h = sqrtf(kGravity / offset_y_m_);
shimizuta 35:b4e1b8f25cd7 55 float t = phasetime_s / stridetime_s_;
shimizuta 35:b4e1b8f25cd7 56 float denominator = expf(g_h) - expf(-g_h); //分母
shimizuta 35:b4e1b8f25cd7 57
shimizuta 35:b4e1b8f25cd7 58 float x_m = -(s0 * expf(-g_h) - s1) * expf(g_h * t) / denominator + (s0 * expf(g_h) - s1) * expf(-g_h * t) / denominator;
shimizuta 35:b4e1b8f25cd7 59 float y_m = offset_y_m_;
shimizuta 35:b4e1b8f25cd7 60 //x,yを代入
shimizuta 35:b4e1b8f25cd7 61 return leg.SetXY_m(x_m, y_m);
shimizuta 35:b4e1b8f25cd7 62 }
shimizuta 35:b4e1b8f25cd7 63 void Triangle::ChangeOneParam(TriangleParams param, float val)
shimizuta 35:b4e1b8f25cd7 64 {
shimizuta 35:b4e1b8f25cd7 65 switch (param)
shimizuta 35:b4e1b8f25cd7 66 {
shimizuta 35:b4e1b8f25cd7 67 case OFFSET_X_M:
shimizuta 35:b4e1b8f25cd7 68 offset_x_m_ = val;
shimizuta 35:b4e1b8f25cd7 69 break;
shimizuta 35:b4e1b8f25cd7 70 case OFFSET_Y_M:
shimizuta 35:b4e1b8f25cd7 71 offset_y_m_ = val;
shimizuta 35:b4e1b8f25cd7 72 break;
shimizuta 35:b4e1b8f25cd7 73 case STRIDE_M:
shimizuta 35:b4e1b8f25cd7 74 stride_m_ = val;
shimizuta 35:b4e1b8f25cd7 75 break;
shimizuta 35:b4e1b8f25cd7 76 case HEIGHT_M:
shimizuta 35:b4e1b8f25cd7 77 height_m_ = val;
shimizuta 35:b4e1b8f25cd7 78 break;
shimizuta 35:b4e1b8f25cd7 79 case BUFFER_HEIGHT_M:
shimizuta 35:b4e1b8f25cd7 80 buffer_height_m_ = val;
shimizuta 35:b4e1b8f25cd7 81 break;
shimizuta 35:b4e1b8f25cd7 82 }
shimizuta 35:b4e1b8f25cd7 83 CalOtherParam();
shimizuta 35:b4e1b8f25cd7 84 }
shimizuta 35:b4e1b8f25cd7 85 void Triangle::Copy(const Triangle & origin){
shimizuta 35:b4e1b8f25cd7 86 *this = origin;
shimizuta 35:b4e1b8f25cd7 87 }