test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Walk/Walk.cpp@18:0033ef1814ba, 2019-02-14 (annotated)
- Committer:
- yuto17320508
- Date:
- Thu Feb 14 09:04:25 2019 +0000
- Revision:
- 18:0033ef1814ba
- Parent:
- 14:d7cb429946f4
- Child:
- 19:1adc7302cfd9
move
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shimizuta | 11:e81425872740 | 1 | #include "Walk.h" |
shimizuta | 11:e81425872740 | 2 | #include "math.h" |
shimizuta | 14:d7cb429946f4 | 3 | #include "pi.h" |
yuto17320508 | 13:e7ecdb20665a | 4 | //Orbitは足毎の軌道をあらわす。 |
yuto17320508 | 13:e7ecdb20665a | 5 | |
shimizuta | 14:d7cb429946f4 | 6 | Orbit::Orbit(OrbitPattern orbitType) |
shimizuta | 11:e81425872740 | 7 | { |
yuto17320508 | 13:e7ecdb20665a | 8 | orbitType_ = orbitType; |
shimizuta | 11:e81425872740 | 9 | } |
yuto17320508 | 18:0033ef1814ba | 10 | |
shimizuta | 14:d7cb429946f4 | 11 | void Orbit::SetStraightParam(float stridetime_s, float risetime_s, float stride_m, float height_m, float ground_m) |
shimizuta | 14:d7cb429946f4 | 12 | { |
shimizuta | 14:d7cb429946f4 | 13 | stridetime_s_ = stridetime_s; |
shimizuta | 14:d7cb429946f4 | 14 | risetime_s_ = risetime_s; |
shimizuta | 14:d7cb429946f4 | 15 | stride_m_ = stride_m; |
shimizuta | 14:d7cb429946f4 | 16 | height_m_ = height_m; |
shimizuta | 14:d7cb429946f4 | 17 | ground_m_ = ground_m; |
yuto17320508 | 18:0033ef1814ba | 18 | offset_x_m_ = 0; |
yuto17320508 | 18:0033ef1814ba | 19 | } |
yuto17320508 | 18:0033ef1814ba | 20 | void Orbit::SetStraightParam(float stridetime_s, float risetime_s, float stride_m, float height_m, float ground_m, float offset_x_m) |
yuto17320508 | 18:0033ef1814ba | 21 | { |
yuto17320508 | 18:0033ef1814ba | 22 | stridetime_s_ = stridetime_s; |
yuto17320508 | 18:0033ef1814ba | 23 | risetime_s_ = risetime_s; |
yuto17320508 | 18:0033ef1814ba | 24 | stride_m_ = stride_m; |
yuto17320508 | 18:0033ef1814ba | 25 | height_m_ = height_m; |
yuto17320508 | 18:0033ef1814ba | 26 | ground_m_ = ground_m; |
yuto17320508 | 18:0033ef1814ba | 27 | offset_x_m_ = offset_x_m; |
shimizuta | 14:d7cb429946f4 | 28 | } |
shimizuta | 14:d7cb429946f4 | 29 | |
shimizuta | 11:e81425872740 | 30 | //着地中の動き.直線軌道.等速 |
shimizuta | 11:e81425872740 | 31 | OneLeg Orbit::StrideLine_(OneLeg leg, float phasetime_s) |
shimizuta | 11:e81425872740 | 32 | { |
yuto17320508 | 18:0033ef1814ba | 33 | float x_m = -stride_m_ * phasetime_s / stridetime_s_ + stride_m_ * 0.5 - offset_x_m_; |
shimizuta | 11:e81425872740 | 34 | float y_m = ground_m_; |
shimizuta | 11:e81425872740 | 35 | leg.SetXY_m(x_m, y_m); |
shimizuta | 11:e81425872740 | 36 | return leg; |
shimizuta | 11:e81425872740 | 37 | } |
shimizuta | 11:e81425872740 | 38 | //空中の動き.楕円軌道 |
shimizuta | 11:e81425872740 | 39 | OneLeg Orbit::RiseEllipse_(OneLeg leg, float phasetime_s) |
shimizuta | 11:e81425872740 | 40 | { |
shimizuta | 14:d7cb429946f4 | 41 | float rad = M_PI * (phasetime_s - stridetime_s_) / risetime_s_ + M_PI; |
yuto17320508 | 18:0033ef1814ba | 42 | float x_m = stride_m_ * 0.5 * cos(rad) - offset_x_m_; |
shimizuta | 11:e81425872740 | 43 | float y_m = height_m_ * sin(rad) + ground_m_; |
shimizuta | 11:e81425872740 | 44 | leg.SetXY_m(x_m, y_m); |
shimizuta | 11:e81425872740 | 45 | return leg; |
shimizuta | 11:e81425872740 | 46 | } |
shimizuta | 11:e81425872740 | 47 | //楕円軌道 |
shimizuta | 11:e81425872740 | 48 | OneLeg Orbit::OrbitEllipse_(OneLeg leg, float phasetime_s) |
shimizuta | 11:e81425872740 | 49 | { |
shimizuta | 11:e81425872740 | 50 | if (phasetime_s < stridetime_s_) |
shimizuta | 11:e81425872740 | 51 | return StrideLine_(leg, phasetime_s); |
shimizuta | 11:e81425872740 | 52 | else |
shimizuta | 11:e81425872740 | 53 | return RiseEllipse_(leg, phasetime_s); |
shimizuta | 11:e81425872740 | 54 | } |
shimizuta | 11:e81425872740 | 55 | //将来的には引数などで軌道の種類を変えて出力できるようにしたい。 |
shimizuta | 11:e81425872740 | 56 | OneLeg Orbit::GetOrbit(OneLeg leg, float phasetime_s) |
shimizuta | 11:e81425872740 | 57 | { |
shimizuta | 11:e81425872740 | 58 | return OrbitEllipse_(leg, phasetime_s); |
shimizuta | 11:e81425872740 | 59 | } |
shimizuta | 11:e81425872740 | 60 | float Orbit::GetOneWalkTime() |
shimizuta | 11:e81425872740 | 61 | { |
shimizuta | 11:e81425872740 | 62 | return stridetime_s_ + risetime_s_; |
shimizuta | 11:e81425872740 | 63 | } |
shimizuta | 11:e81425872740 | 64 | |
shimizuta | 11:e81425872740 | 65 | Walk::Walk(Orbit orbit[4], float offsettime_s[4], float cycletime_s) |
shimizuta | 11:e81425872740 | 66 | { |
shimizuta | 11:e81425872740 | 67 | for (int i = 0; i < 4; i++) |
shimizuta | 11:e81425872740 | 68 | { |
shimizuta | 11:e81425872740 | 69 | orbit_[i] = orbit[i]; |
shimizuta | 11:e81425872740 | 70 | offsettime_s_[i] = offsettime_s[i]; |
shimizuta | 11:e81425872740 | 71 | } |
shimizuta | 11:e81425872740 | 72 | cycletime_s_ = cycletime_s; |
shimizuta | 11:e81425872740 | 73 | } |
shimizuta | 11:e81425872740 | 74 | void Walk::Cal4LegsPosi(OneLeg (&leg)[4]) |
shimizuta | 11:e81425872740 | 75 | { |
shimizuta | 11:e81425872740 | 76 | phasetime_s_ += cycletime_s_; |
shimizuta | 14:d7cb429946f4 | 77 | while (phasetime_s_ > orbit_[0].GetOneWalkTime()) |
shimizuta | 14:d7cb429946f4 | 78 | phasetime_s_ -= orbit_[0].GetOneWalkTime(); |
shimizuta | 11:e81425872740 | 79 | for (int i = 0; i < 4; i++) |
shimizuta | 11:e81425872740 | 80 | { |
shimizuta | 11:e81425872740 | 81 | float phasetime_s = phasetime_s_ + offsettime_s_[i]; |
shimizuta | 11:e81425872740 | 82 | while (phasetime_s > orbit_[i].GetOneWalkTime()) |
shimizuta | 11:e81425872740 | 83 | phasetime_s -= orbit_[i].GetOneWalkTime(); |
shimizuta | 11:e81425872740 | 84 | leg[i] = orbit_[i].GetOrbit(leg[i], phasetime_s); |
shimizuta | 11:e81425872740 | 85 | } |
shimizuta | 11:e81425872740 | 86 | } |