岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

Walk/Walk.cpp@13:e7ecdb20665a, 2019-02-12 (annotated)

Committer:
yuto17320508
Date:
Tue Feb 12 07:51:07 2019 +0000
Revision:
13:e7ecdb20665a
Parent:
12:2ac37fe6c3bb
Child:
14:d7cb429946f4
coment

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 11:e81425872740 3 const float M_PI = 3.141592;
yuto17320508 13:e7ecdb20665a 4
yuto17320508 13:e7ecdb20665a 5 //Orbitは足毎の軌道をあらわす。
yuto17320508 13:e7ecdb20665a 6
yuto17320508 13:e7ecdb20665a 7 Orbit::Orbit(int orbitType)
shimizuta 11:e81425872740 8 {
yuto17320508 13:e7ecdb20665a 9 orbitType_ = orbitType;
shimizuta 11:e81425872740 10 }
shimizuta 11:e81425872740 11 //着地中の動き.直線軌道.等速
shimizuta 11:e81425872740 12 OneLeg Orbit::StrideLine_(OneLeg leg, float phasetime_s)
shimizuta 11:e81425872740 13 {
shimizuta 11:e81425872740 14 float x_m = -stride_m_ * phasetime_s / stridetime_s_ + stride_m_ * 0.5;
shimizuta 11:e81425872740 15 float y_m = ground_m_;
shimizuta 11:e81425872740 16 leg.SetXY_m(x_m, y_m);
shimizuta 11:e81425872740 17 return leg;
shimizuta 11:e81425872740 18 }
shimizuta 11:e81425872740 19 //空中の動き.楕円軌道
shimizuta 11:e81425872740 20 OneLeg Orbit::RiseEllipse_(OneLeg leg, float phasetime_s)
shimizuta 11:e81425872740 21 {
shimizuta 12:2ac37fe6c3bb 22 float rad = M_PI * (phasetime_s -stridetime_s_)/ risetime_s_ + M_PI;
shimizuta 11:e81425872740 23 float x_m = stride_m_ * 0.5 * cos(rad);
shimizuta 11:e81425872740 24 float y_m = height_m_ * sin(rad) + ground_m_;
shimizuta 11:e81425872740 25 leg.SetXY_m(x_m, y_m);
shimizuta 11:e81425872740 26 return leg;
shimizuta 11:e81425872740 27 }
shimizuta 11:e81425872740 28 //楕円軌道
shimizuta 11:e81425872740 29 OneLeg Orbit::OrbitEllipse_(OneLeg leg, float phasetime_s)
shimizuta 11:e81425872740 30 {
shimizuta 11:e81425872740 31 if (phasetime_s < stridetime_s_)
shimizuta 11:e81425872740 32 return StrideLine_(leg, phasetime_s);
shimizuta 11:e81425872740 33 else
shimizuta 11:e81425872740 34 return RiseEllipse_(leg, phasetime_s);
shimizuta 11:e81425872740 35 }
shimizuta 11:e81425872740 36 //将来的には引数などで軌道の種類を変えて出力できるようにしたい。
shimizuta 11:e81425872740 37 OneLeg Orbit::GetOrbit(OneLeg leg, float phasetime_s)
shimizuta 11:e81425872740 38 {
shimizuta 11:e81425872740 39 return OrbitEllipse_(leg, phasetime_s);
shimizuta 11:e81425872740 40 }
shimizuta 11:e81425872740 41 float Orbit::GetOneWalkTime()
shimizuta 11:e81425872740 42 {
shimizuta 11:e81425872740 43 return stridetime_s_ + risetime_s_;
shimizuta 11:e81425872740 44 }
shimizuta 11:e81425872740 45
shimizuta 11:e81425872740 46 Walk::Walk(Orbit orbit[4], float offsettime_s[4], float cycletime_s)
shimizuta 11:e81425872740 47 {
shimizuta 11:e81425872740 48 for (int i = 0; i < 4; i++)
shimizuta 11:e81425872740 49 {
shimizuta 11:e81425872740 50 orbit_[i] = orbit[i];
shimizuta 11:e81425872740 51 offsettime_s_[i] = offsettime_s[i];
shimizuta 11:e81425872740 52 }
shimizuta 11:e81425872740 53 cycletime_s_ = cycletime_s;
shimizuta 11:e81425872740 54 }
shimizuta 11:e81425872740 55 void Walk::Cal4LegsPosi(OneLeg (&leg)[4])
shimizuta 11:e81425872740 56 {
shimizuta 11:e81425872740 57 phasetime_s_ += cycletime_s_;
shimizuta 11:e81425872740 58 for (int i = 0; i < 4; i++)
shimizuta 11:e81425872740 59 {
shimizuta 11:e81425872740 60 float phasetime_s = phasetime_s_ + offsettime_s_[i];
shimizuta 11:e81425872740 61 while (phasetime_s > orbit_[i].GetOneWalkTime())
shimizuta 11:e81425872740 62 phasetime_s -= orbit_[i].GetOneWalkTime();
shimizuta 11:e81425872740 63 leg[i] = orbit_[i].GetOrbit(leg[i], phasetime_s);
shimizuta 11:e81425872740 64 }
shimizuta 11:e81425872740 65 }