test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: Walk/Walk.cpp
- Revision:
- 14:d7cb429946f4
- Parent:
- 13:e7ecdb20665a
- Child:
- 18:0033ef1814ba
diff -r e7ecdb20665a -r d7cb429946f4 Walk/Walk.cpp --- a/Walk/Walk.cpp Tue Feb 12 07:51:07 2019 +0000 +++ b/Walk/Walk.cpp Tue Feb 12 12:50:45 2019 +0000 @@ -1,13 +1,21 @@ #include "Walk.h" #include "math.h" -const float M_PI = 3.141592; - +#include "pi.h" //Orbitは足毎の軌道をあらわす。 -Orbit::Orbit(int orbitType) +Orbit::Orbit(OrbitPattern orbitType) { orbitType_ = orbitType; } +void Orbit::SetStraightParam(float stridetime_s, float risetime_s, float stride_m, float height_m, float ground_m) +{ + stridetime_s_ = stridetime_s; + risetime_s_ = risetime_s; + stride_m_ = stride_m; + height_m_ = height_m; + ground_m_ = ground_m; +} + //着地中の動き.直線軌道.等速 OneLeg Orbit::StrideLine_(OneLeg leg, float phasetime_s) { @@ -19,7 +27,7 @@ //空中の動き.楕円軌道 OneLeg Orbit::RiseEllipse_(OneLeg leg, float phasetime_s) { - float rad = M_PI * (phasetime_s -stridetime_s_)/ risetime_s_ + M_PI; + float rad = M_PI * (phasetime_s - stridetime_s_) / risetime_s_ + M_PI; float x_m = stride_m_ * 0.5 * cos(rad); float y_m = height_m_ * sin(rad) + ground_m_; leg.SetXY_m(x_m, y_m); @@ -55,6 +63,8 @@ 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];