serbo4soku

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

Committer:
shimizuta
Date:
Mon Feb 11 04:07:51 2019 +0000
Revision:
10:7a340c52e270
Parent:
9:905f93247688
printf debug finish

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 10:7a340c52e270 14 float y_m = offsety_m_;
shimizuta 10:7a340c52e270 15 if (phase < M_PI) //空中にいるとき(最初の半周期)のみy座標は変化
shimizuta 10:7a340c52e270 16 y_m += sin(phase) * height_m_;
shimizuta 9:905f93247688 17 return y_m;
shimizuta 9:905f93247688 18 }
shimizuta 9:905f93247688 19
shimizuta 10:7a340c52e270 20 ToePosi::ToePosi(float stride_m, float height_m, float offsety_m)
shimizuta 9:905f93247688 21 {
shimizuta 9:905f93247688 22 phase_ = 0;
shimizuta 9:905f93247688 23 stride_m_ = stride_m;
shimizuta 9:905f93247688 24 height_m_ = height_m;
shimizuta 10:7a340c52e270 25 offsety_m_ = offsety_m;
shimizuta 9:905f93247688 26 }
shimizuta 9:905f93247688 27 float ToePosi::GetPhase()
shimizuta 9:905f93247688 28 {
shimizuta 9:905f93247688 29 return phase_;
shimizuta 9:905f93247688 30 }
shimizuta 9:905f93247688 31 void ToePosi::SetPhase(float phase)
shimizuta 9:905f93247688 32 {
shimizuta 9:905f93247688 33 //値域を[0,2pi]にする
shimizuta 9:905f93247688 34 while (phase > 2 * M_PI)
shimizuta 9:905f93247688 35 phase -= 2 * M_PI;
shimizuta 9:905f93247688 36 while (phase < 0)
shimizuta 9:905f93247688 37 phase += 2 * M_PI;
shimizuta 9:905f93247688 38 phase_ = phase;
shimizuta 9:905f93247688 39 }