test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
triangle.cpp
00001 #include "triangle.h" 00002 #include "pi.h" 00003 #include <math.h> 00004 void Triangle::SetTriangleParam(float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m, 00005 float stridetime_s, float toptime_s, float buffer_time_s) 00006 { 00007 offset_x_m_ = offset_x_m; 00008 offset_y_m_ = offset_y_m; 00009 stride_m_ = stride_m; 00010 height_m_ = height_m; //足上げ幅 00011 buffer_height_m_ = buffer_height_m; //着地直前で止める高さ 00012 stridetime_s_ = stridetime_s; 00013 toptime_s_ = toptime_s; //頂点に行くまでの時間 00014 buffer_time_s_ = buffer_time_s; //頂点から一時停止点までの時間. 00015 00016 //事前に計算しておく 00017 CalOtherParam(); 00018 } 00019 void Triangle::CalOtherParam() 00020 { 00021 beta_degree_ = 81; //論文よりこれが最適らしい 00022 reverse_tanbeta_ = 1.0 / tan(beta_degree_ / 180.0 * M_PI); 00023 top_x_m_ = offset_x_m_ + stride_m_ * 0.5 + height_m_ * reverse_tanbeta_; 00024 top_y_m_ = -height_m_ + offset_y_m_; 00025 buffer_x_m_ = offset_x_m_ + stride_m_ * 0.5 + buffer_height_m_ * reverse_tanbeta_; 00026 buffer_y_m_ = -buffer_height_m_ + offset_y_m_; 00027 } 00028 //足一周の時間 00029 float Triangle::GetOneWalkTime() 00030 { 00031 return stridetime_s_ + toptime_s_ + buffer_time_s_; 00032 }; 00033 int Triangle::GetOrbit(OneLeg &leg, float phasetime_s) 00034 { 00035 int ret = 0; 00036 if (phasetime_s < stridetime_s_) 00037 ret = StrideLineAccel_(leg, phasetime_s); 00038 else if (phasetime_s < stridetime_s_ + toptime_s_) 00039 ret = leg.SetXY_m(top_x_m_, top_y_m_); 00040 else 00041 ret = leg.SetXY_m(buffer_x_m_, buffer_y_m_); 00042 return ret; 00043 }; 00044 int Triangle::StrideLine_(OneLeg &leg, float phasetime_s) 00045 { 00046 float x_m = -stride_m_ * phasetime_s / stridetime_s_ + stride_m_ * 0.5 + offset_x_m_; 00047 float y_m = offset_y_m_; 00048 return leg.SetXY_m(x_m, y_m); 00049 } 00050 int Triangle::StrideLineAccel_(OneLeg &leg, float phasetime_s) 00051 { 00052 ///////////x,yを計算 00053 float s0 = stride_m_ * 0.5; 00054 float s1 = -stride_m_ * 0.5; 00055 float g_h = sqrtf(kGravity / offset_y_m_); 00056 float t = phasetime_s / stridetime_s_; 00057 float denominator = expf(g_h) - expf(-g_h); //分母 00058 00059 float x_m = -(s0 * expf(-g_h) - s1) * expf(g_h * t) / denominator + (s0 * expf(g_h) - s1) * expf(-g_h * t) / denominator; 00060 x_m += offset_x_m_; 00061 float y_m = offset_y_m_; 00062 //x,yを代入 00063 return leg.SetXY_m(x_m, y_m); 00064 } 00065 void Triangle::ChangeOneParam(TriangleParams param, float val) 00066 { 00067 switch (param) 00068 { 00069 case OFFSET_X_M: 00070 offset_x_m_ = val; 00071 break; 00072 case OFFSET_Y_M: 00073 offset_y_m_ = val; 00074 break; 00075 case STRIDE_M: 00076 stride_m_ = val; 00077 break; 00078 case HEIGHT_M: 00079 height_m_ = val; 00080 break; 00081 case BUFFER_HEIGHT_M: 00082 buffer_height_m_ = val; 00083 break; 00084 } 00085 CalOtherParam(); 00086 } 00087 void Triangle::Copy(const Triangle &origin) 00088 { 00089 *this = origin; 00090 } 00091 00092 void FourPoint::CalOtherParam() 00093 { 00094 beta_degree_ = 60; //論文よりこれが最適らしい 00095 reverse_tanbeta_ = 1.0 / tan(beta_degree_ / 180.0 * M_PI); 00096 top_x_m_ = offset_x_m_; 00097 top_y_m_ = -height_m_ + offset_y_m_; 00098 buffer_x_m_ = offset_x_m_ + stride_m_ * 0.5 + buffer_height_m_ * reverse_tanbeta_; 00099 buffer_y_m_ = -buffer_height_m_ + offset_y_m_; 00100 } 00101 void FourPoint::SetFourPointParam(float offset_x_m, float offset_y_m, float stride_m, float height_m, float buffer_height_m, 00102 float stridetime_s, float toptime_s, float buffer_time_s) 00103 { 00104 SetTriangleParam(offset_x_m, offset_y_m, stride_m, height_m, buffer_height_m, 00105 stridetime_s, toptime_s, buffer_time_s); 00106 CalOtherParam(); 00107 }
Generated on Thu Jul 14 2022 13:15:26 by 1.7.2