test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Walk/Walk.cpp
- Committer:
- shimizuta
- Date:
- 2019-02-16
- Revision:
- 21:61971fc18b90
- Parent:
- 19:1adc7302cfd9
- Child:
- 22:787a764271dd
File content as of revision 21:61971fc18b90:
#include "Walk.h" #include "math.h" #include "pi.h" //Orbitは足毎の軌道をあらわす。 void Orbit::SetStraightParam(float stridetime_s, float risetime_s, float stride_m, float height_m, float ground_m, float ellipse_center_x_m, float ellipse_center_y_m) { stridetime_s_ = stridetime_s; risetime_s_ = risetime_s; stride_m_ = stride_m; height_m_ = height_m; ground_m_ = ground_m; ellipse_center_x_m_ = ellipse_center_x_m; ellipse_center_y_m_ = ellipse_center_y_m; } void Orbit::SetStandParam(float ground_m) { stridetime_s_ = 1; risetime_s_ = 0; stride_m_ = 0; height_m_ = 0; ground_m_ = ground_m; ellipse_center_x_m_ = 0; ellipse_center_y_m_ = ground_m; } //着地中の動き.直線軌道.等速 OneLeg Orbit::StrideLine_(OneLeg leg, float phasetime_s) { float x_m = -stride_m_ * phasetime_s / stridetime_s_ + stride_m_ * 0.5 + ellipse_center_x_m_; float y_m = ellipse_center_y_m_; leg.SetXY_m(x_m, y_m); return leg; } //空中の動き.半分にきれいに切れる楕円軌道 OneLeg Orbit::RiseEllipse_(OneLeg leg, float phasetime_s) { float rad = M_PI * (phasetime_s - stridetime_s_) / risetime_s_ + M_PI; float x_m = stride_m_ * 0.5 * cos(rad) + ellipse_center_x_m_; float y_m = height_m_ * sin(rad) + ellipse_center_y_m_; leg.SetXY_m(x_m, y_m); return leg; } //空中の動き.完全に半分には切れない楕円軌道 OneLeg Orbit::RiseEllipse2_(OneLeg leg, float phasetime_s) { ///////////x,yを計算.注:計算は正しくないので直す必要がある。 float rad = M_PI * (phasetime_s - stridetime_s_) / risetime_s_ + M_PI; float x_m = stride_m_ * 0.5 * cos(rad) + ellipse_center_x_m_; float y_m = height_m_ * sin(rad) + ellipse_center_y_m_; /////////// //x,yを代入 leg.SetXY_m(x_m, y_m); return leg; } //楕円軌道 OneLeg Orbit::OrbitEllipse_(OneLeg leg, float phasetime_s) { if (phasetime_s < stridetime_s_) return StrideLine_(leg, phasetime_s); else return RiseEllipse_(leg, phasetime_s); } //将来的には引数などで軌道の種類を変えて出力できるようにしたい。 OneLeg Orbit::GetOrbit(OneLeg leg, float phasetime_s) { return OrbitEllipse_(leg, phasetime_s); } float Orbit::GetOneWalkTime() { return stridetime_s_ + risetime_s_; } Walk::Walk(Orbit orbit[4], float offsettime_s[4], float cycletime_s) { for (int i = 0; i < 4; i++) { orbit_[i] = orbit[i]; offsettime_s_[i] = offsettime_s[i]; } cycletime_s_ = cycletime_s; } void Walk::Cal4LegsPosi(OneLeg (&leg)[4]) { phasetime_s_ += cycletime_s_; while (phasetime_s_ > orbit_[0].GetOneWalkTime()) phasetime_s_ -= orbit_[0].GetOneWalkTime(); for (int i = 0; i < 4; i++) { float phasetime_s = phasetime_s_ + offsettime_s_[i]; while (phasetime_s > orbit_[i].GetOneWalkTime()) phasetime_s -= orbit_[i].GetOneWalkTime(); leg[i] = orbit_[i].GetOrbit(leg[i], phasetime_s); } }