岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

Walk/orbit/triangle/triangle.cpp@50:36741e8ab197, 2019-03-11 (annotated)

Committer:
shimizuta
Date:
Mon Mar 11 10:38:07 2019 +0000
Revision:
50:36741e8ab197
Parent:
40:5eb3b3908edd 
a

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