test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Walk/orbit/triangle/triangle.cpp
- Committer:
- shimizuta
- Date:
- 2019-03-04
- Revision:
- 39:87dcdff27797
- Parent:
- 35:b4e1b8f25cd7
- Child:
- 40:5eb3b3908edd
File content as of revision 39:87dcdff27797:
#include "triangle.h" #include "pi.h" #include <math.h> void Triangle::SetTriangleParam(float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m, float stridetime_s, float toptime_s, float buffer_time_s) { offset_x_m_ = offset_x_m; offset_y_m_ = offset_y_m; stride_m_ = stride_m; height_m_ = height_m; //足上げ幅 buffer_height_m_ = buffer_height_m; //着地直前で止める高さ stridetime_s_ = stridetime_s; toptime_s_ = toptime_s; //頂点に行くまでの時間 buffer_time_s_ = buffer_time_s; //頂点から一時停止点までの時間. //事前に計算しておく CalOtherParam(); } void Triangle::CalOtherParam() { beta_degree_ = 81; //論文よりこれが最適らしい reverse_tanbeta_ = 1.0 / tan(beta_degree_ / 180.0 * M_PI); top_x_m_ = offset_x_m_ + stride_m_ * 0.5 + height_m_ * reverse_tanbeta_; top_y_m_ = -height_m_ + offset_y_m_; buffer_x_m_ = offset_x_m_ + stride_m_ * 0.5 + buffer_height_m_ * reverse_tanbeta_; buffer_y_m_ = -buffer_height_m_ + offset_y_m_; } //足一周の時間 float Triangle::GetOneWalkTime() { return stridetime_s_ + toptime_s_ + buffer_time_s_; }; int Triangle::GetOrbit(OneLeg &leg, float phasetime_s) { int ret = 0; if (phasetime_s < stridetime_s_) ret = StrideLineAccel_(leg, phasetime_s); else if (phasetime_s < stridetime_s_ + toptime_s_) ret = leg.SetXY_m(top_x_m_, top_y_m_); else ret = leg.SetXY_m(buffer_x_m_, buffer_y_m_); return ret; }; int Triangle::StrideLine_(OneLeg &leg, float phasetime_s) { float x_m = -stride_m_ * phasetime_s / stridetime_s_ + stride_m_ * 0.5 + offset_x_m_; float y_m = offset_y_m_; return leg.SetXY_m(x_m, y_m); } int Triangle::StrideLineAccel_(OneLeg &leg, float phasetime_s) { ///////////x,yを計算 float s0 = stride_m_ * 0.5; float s1 = -stride_m_ * 0.5; float g_h = sqrtf(kGravity / offset_y_m_); float t = phasetime_s / stridetime_s_; float denominator = expf(g_h) - expf(-g_h); //分母 float x_m = -(s0 * expf(-g_h) - s1) * expf(g_h * t) / denominator + (s0 * expf(g_h) - s1) * expf(-g_h * t) / denominator; x_m += offset_x_m_; float y_m = offset_y_m_; //x,yを代入 return leg.SetXY_m(x_m, y_m); } void Triangle::ChangeOneParam(TriangleParams param, float val) { switch (param) { case OFFSET_X_M: offset_x_m_ = val; break; case OFFSET_Y_M: offset_y_m_ = val; break; case STRIDE_M: stride_m_ = val; break; case HEIGHT_M: height_m_ = val; break; case BUFFER_HEIGHT_M: buffer_height_m_ = val; break; } CalOtherParam(); } void Triangle::Copy(const Triangle &origin) { *this = origin; } void FourPoint::CalOtherParam() { beta_degree_ = 60; //論文よりこれが最適らしい reverse_tanbeta_ = 1.0 / tan(beta_degree_ / 180.0 * M_PI); top_x_m_ = offset_x_m_; top_y_m_ = -height_m_ + offset_y_m_; buffer_x_m_ = offset_x_m_ + stride_m_ * 0.5 - buffer_height_m_ * reverse_tanbeta_; buffer_y_m_ = -buffer_height_m_ + offset_y_m_; } void FourPoint::SetFourPointParam(float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m, float stridetime_s, float toptime_s, float buffer_time_s) { SetTriangleParam(offset_x_m, offset_y_m, stride_m, height_m, buffer_height_m, stridetime_s, toptime_s, buffer_time_s); CalOtherParam(); }