serbo4soku

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

Committer:
shimizuta
Date:
Mon Feb 11 01:56:49 2019 +0000
Revision:
9:905f93247688
Child:
10:7a340c52e270
made program readable

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimizuta 9:905f93247688 1 #include "ToePosi.h"
shimizuta 9:905f93247688 2 #include "math.h"
shimizuta 9:905f93247688 3 const float M_PI = 3.141592;
shimizuta 9:905f93247688 4 //足先の軌道を決める関数。要調整。取り敢えず楕円軌道で実装
shimizuta 9:905f93247688 5 float ToePosi::GetX_m()
shimizuta 9:905f93247688 6 {
shimizuta 9:905f93247688 7 float phase = GetPhase();
shimizuta 9:905f93247688 8 float x_m = cos(phase) * stride_m_;
shimizuta 9:905f93247688 9 return x_m;
shimizuta 9:905f93247688 10 }
shimizuta 9:905f93247688 11 float ToePosi::GetY_m()
shimizuta 9:905f93247688 12 {
shimizuta 9:905f93247688 13 float phase = GetPhase();
shimizuta 9:905f93247688 14 float y_m;
shimizuta 9:905f93247688 15 if (phase < M_PI) //最初の半周期は空中
shimizuta 9:905f93247688 16 y_m = sin(phase) * height_m_;
shimizuta 9:905f93247688 17 else //後の半周期は着地。直線移動
shimizuta 9:905f93247688 18 y_m = 0;
shimizuta 9:905f93247688 19 return y_m;
shimizuta 9:905f93247688 20 }
shimizuta 9:905f93247688 21
shimizuta 9:905f93247688 22 ToePosi::ToePosi(float stride_m, float height_m)
shimizuta 9:905f93247688 23 {
shimizuta 9:905f93247688 24 phase_ = 0;
shimizuta 9:905f93247688 25 stride_m_ = stride_m;
shimizuta 9:905f93247688 26 height_m_ = height_m;
shimizuta 9:905f93247688 27 }
shimizuta 9:905f93247688 28 float ToePosi::GetPhase()
shimizuta 9:905f93247688 29 {
shimizuta 9:905f93247688 30 return phase_;
shimizuta 9:905f93247688 31 }
shimizuta 9:905f93247688 32 void ToePosi::SetPhase(float phase)
shimizuta 9:905f93247688 33 {
shimizuta 9:905f93247688 34 //値域を[0,2pi]にする
shimizuta 9:905f93247688 35 while (phase > 2 * M_PI)
shimizuta 9:905f93247688 36 phase -= 2 * M_PI;
shimizuta 9:905f93247688 37 while (phase < 0)
shimizuta 9:905f93247688 38 phase += 2 * M_PI;
shimizuta 9:905f93247688 39
shimizuta 9:905f93247688 40 phase_ = phase;
shimizuta 9:905f93247688 41 }
shimizuta 9:905f93247688 42
shimizuta 9:905f93247688 43