serbo4soku

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

ToePosi/ToePosi.cpp

Committer:
shimizuta
Date:
5 weeks ago
Revision:
10:7a340c52e270
Parent:
9:905f93247688

File content as of revision 10:7a340c52e270:

#include "ToePosi.h"
#include "math.h"
const float M_PI = 3.141592;
//足先の軌道を決める関数。要調整。取り敢えず楕円軌道で実装
float ToePosi::GetX_m()
{
    float phase = GetPhase();
    float x_m = cos(phase) * stride_m_;
    return x_m;
}
float ToePosi::GetY_m()
{
    float phase = GetPhase();
    float y_m = offsety_m_;
    if (phase < M_PI) //空中にいるとき(最初の半周期)のみy座標は変化
        y_m += sin(phase) * height_m_;
    return y_m;
}

ToePosi::ToePosi(float stride_m, float height_m, float offsety_m)
{
    phase_ = 0;
    stride_m_ = stride_m;
    height_m_ = height_m;
    offsety_m_ = offsety_m;
}
float ToePosi::GetPhase()
{
    return phase_;
}
void ToePosi::SetPhase(float phase)
{
    //値域を[0,2pi]にする
    while (phase > 2 * M_PI)
        phase -= 2 * M_PI;
    while (phase < 0)
        phase += 2 * M_PI;
    phase_ = phase;
}