岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

Walk/Walk.cpp@19:1adc7302cfd9, 2019-02-15 (annotated)

Committer:
shimizuta
Date:
Fri Feb 15 03:46:52 2019 +0000
Revision:
19:1adc7302cfd9
Parent:
18:0033ef1814ba
Child:
21:61971fc18b90
some change

Who changed what in which revision?

UserRevisionLine numberNew 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 19:1adc7302cfd9 11 void Orbit::SetStraightParam(float stridetime_s, float risetime_s, float stride_m, float height_m, float offset_x_m, float offset_y_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 19:1adc7302cfd9 17 offset_x_m_ = offset_x_m;
shimizuta 19:1adc7302cfd9 18 offset_y_m_ = offset_y_m;
yuto17320508 18:0033ef1814ba 19 }
shimizuta 19:1adc7302cfd9 20
shimizuta 19:1adc7302cfd9 21 void Orbit::SetStandParam(float offset_y_m)
yuto17320508 18:0033ef1814ba 22 {
shimizuta 19:1adc7302cfd9 23 stridetime_s_ = 1;
shimizuta 19:1adc7302cfd9 24 risetime_s_ = 0;
shimizuta 19:1adc7302cfd9 25 stride_m_ = 0;
shimizuta 19:1adc7302cfd9 26 height_m_ = 0;
shimizuta 19:1adc7302cfd9 27 offset_x_m_ = 0;
shimizuta 19:1adc7302cfd9 28 offset_y_m_ = offset_y_m;
shimizuta 14:d7cb429946f4 29 }
shimizuta 14:d7cb429946f4 30
shimizuta 11:e81425872740 31 //着地中の動き.直線軌道.等速
shimizuta 11:e81425872740 32 OneLeg Orbit::StrideLine_(OneLeg leg, float phasetime_s)
shimizuta 11:e81425872740 33 {
shimizuta 19:1adc7302cfd9 34 float x_m = -stride_m_ * phasetime_s / stridetime_s_ + stride_m_ * 0.5 + offset_x_m_;
shimizuta 19:1adc7302cfd9 35 float y_m = offset_y_m_;
shimizuta 11:e81425872740 36 leg.SetXY_m(x_m, y_m);
shimizuta 11:e81425872740 37 return leg;
shimizuta 11:e81425872740 38 }
shimizuta 11:e81425872740 39 //空中の動き.楕円軌道
shimizuta 11:e81425872740 40 OneLeg Orbit::RiseEllipse_(OneLeg leg, float phasetime_s)
shimizuta 11:e81425872740 41 {
shimizuta 14:d7cb429946f4 42 float rad = M_PI * (phasetime_s - stridetime_s_) / risetime_s_ + M_PI;
shimizuta 19:1adc7302cfd9 43 float x_m = stride_m_ * 0.5 * cos(rad) + offset_x_m_;
shimizuta 19:1adc7302cfd9 44 float y_m = height_m_ * sin(rad) + offset_y_m_;
shimizuta 11:e81425872740 45 leg.SetXY_m(x_m, y_m);
shimizuta 11:e81425872740 46 return leg;
shimizuta 11:e81425872740 47 }
shimizuta 11:e81425872740 48 //楕円軌道
shimizuta 11:e81425872740 49 OneLeg Orbit::OrbitEllipse_(OneLeg leg, float phasetime_s)
shimizuta 11:e81425872740 50 {
shimizuta 11:e81425872740 51 if (phasetime_s < stridetime_s_)
shimizuta 11:e81425872740 52 return StrideLine_(leg, phasetime_s);
shimizuta 11:e81425872740 53 else
shimizuta 11:e81425872740 54 return RiseEllipse_(leg, phasetime_s);
shimizuta 11:e81425872740 55 }
shimizuta 11:e81425872740 56 //将来的には引数などで軌道の種類を変えて出力できるようにしたい。
shimizuta 11:e81425872740 57 OneLeg Orbit::GetOrbit(OneLeg leg, float phasetime_s)
shimizuta 11:e81425872740 58 {
shimizuta 11:e81425872740 59 return OrbitEllipse_(leg, phasetime_s);
shimizuta 11:e81425872740 60 }
shimizuta 11:e81425872740 61 float Orbit::GetOneWalkTime()
shimizuta 11:e81425872740 62 {
shimizuta 11:e81425872740 63 return stridetime_s_ + risetime_s_;
shimizuta 11:e81425872740 64 }
shimizuta 11:e81425872740 65
shimizuta 11:e81425872740 66 Walk::Walk(Orbit orbit[4], float offsettime_s[4], float cycletime_s)
shimizuta 11:e81425872740 67 {
shimizuta 11:e81425872740 68 for (int i = 0; i < 4; i++)
shimizuta 11:e81425872740 69 {
shimizuta 11:e81425872740 70 orbit_[i] = orbit[i];
shimizuta 11:e81425872740 71 offsettime_s_[i] = offsettime_s[i];
shimizuta 11:e81425872740 72 }
shimizuta 11:e81425872740 73 cycletime_s_ = cycletime_s;
shimizuta 11:e81425872740 74 }
shimizuta 11:e81425872740 75 void Walk::Cal4LegsPosi(OneLeg (&leg)[4])
shimizuta 11:e81425872740 76 {
shimizuta 11:e81425872740 77 phasetime_s_ += cycletime_s_;
shimizuta 14:d7cb429946f4 78 while (phasetime_s_ > orbit_[0].GetOneWalkTime())
shimizuta 14:d7cb429946f4 79 phasetime_s_ -= orbit_[0].GetOneWalkTime();
shimizuta 11:e81425872740 80 for (int i = 0; i < 4; i++)
shimizuta 11:e81425872740 81 {
shimizuta 11:e81425872740 82 float phasetime_s = phasetime_s_ + offsettime_s_[i];
shimizuta 11:e81425872740 83 while (phasetime_s > orbit_[i].GetOneWalkTime())
shimizuta 11:e81425872740 84 phasetime_s -= orbit_[i].GetOneWalkTime();
shimizuta 11:e81425872740 85 leg[i] = orbit_[i].GetOrbit(leg[i], phasetime_s);
shimizuta 11:e81425872740 86 }
shimizuta 11:e81425872740 87 }