serbo4soku

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

ToePosi/ToePosi.cpp

Committer:
shimizuta
Date:
2 months ago
Revision:
9:905f93247688
Child:
10:7a340c52e270

File content as of revision 9:905f93247688:

#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;
    if (phase < M_PI) //最初の半周期は空中
        y_m = sin(phase) * height_m_;
    else //後の半周期は着地。直線移動
        y_m = 0;
    return y_m;
}

ToePosi::ToePosi(float stride_m, float height_m)
{
    phase_ = 0;
    stride_m_ = stride_m;
    height_m_ = height_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;
}