File content as of revision 27:79b4b932a6dd:

#define _USE_MATH_DEFINES
#include "math.h"
#include <stdio.h>
#include "stdlib.h"
#include "Walk.h"
 #include "pi.h"
//Orbitは足毎の軌道をあらわす。
const float kGravity = 9.8;
void EllipseOrbit::SetParam(float stridetime_s, float risetime_s,
                            float stride_m, float height_m, float ground_m,
                            float ellipsecenter_x_m, float ellipsecenter_y_m)
{
    stridetime_s_ = stridetime_s;
    risetime_s_ = risetime_s;
    stride_m_ = stride_m;
    height_m_ = height_m;
    ground_m_ = ground_m;
    ellipsecenter_x_m_ = ellipsecenter_x_m;
    ellipsecenter_y_m_ = ellipsecenter_y_m;
}

//着地中の動き.直線軌道.等速
int EllipseOrbit::StrideLine_(OneLeg &leg, float phasetime_s)
{
    float x_m = -stride_m_ * phasetime_s / stridetime_s_ + stride_m_ * 0.5 + ellipsecenter_x_m_;
    float y_m = ellipsecenter_y_m_;
    return leg.SetXY_m(x_m, y_m);
}
int EllipseOrbit::StrideLineAccel_(OneLeg &leg, float phasetime_s)
{
    ///////////x,yを計算
    float s0 = stride_m_ * 0.5 + ellipsecenter_x_m_;
    float s1 = -stride_m_ * 0.5 + ellipsecenter_x_m_;
    float g_h = sqrtf(kGravity / ground_m_);
    float t = phasetime_s / stridetime_s_;
    float denominator = expf(g_h) - expf(-g_h); //分母

    float x_m = -(s0 * expf(-g_h) - s1) * expf(g_h * t) / denominator + (s0 * expf(g_h) - s1) * expf(-g_h * t) / denominator;
    float y_m = ground_m_;
    //x,yを代入
    return leg.SetXY_m(x_m, y_m);
}
//空中の動き.半分にきれいに切れる楕円軌道
int EllipseOrbit::RiseEllipse_(OneLeg &leg, float phasetime_s)
{
    float rad = M_PI * (phasetime_s - stridetime_s_) / risetime_s_ + M_PI;
    float x_m = stride_m_ * 0.5 * cosf(rad) + ellipsecenter_x_m_;
    float y_m = height_m_ * sinf(rad) + ellipsecenter_y_m_;
    return leg.SetXY_m(x_m, y_m);
}
//空中の動き.完全に半分には切れない楕円軌道
int EllipseOrbit::RiseEllipse2_(OneLeg &leg, float phasetime_s)
{
    ///////////x,yを計算
    float ellipselong_m = stride_m_ * 0.5 / cosf(asinf(ground_m_ - ellipsecenter_y_m_));
    float ellipseshort_m = 2 * (height_m_ - ground_m_ + ellipsecenter_y_m_);
    float theta1 = acosf(stride_m_ / ellipselong_m * 0.5);      //stride の始まりの位置の角度（数学基準）
    float theta2 = acosf(-stride_m_ / ellipselong_m * 0.5);     //stride の終わりの位置の角度（数学基準）
    float omega = (2 * M_PI - theta2 + theta1) / risetime_s_;   //楕円軌道における角速度、一定の値とする（適当）
    float rad = (phasetime_s - stridetime_s_) * omega + theta2; //角度の基準はtheta1とする（適当）
    float x_m = ellipselong_m * cos(rad) + ellipsecenter_x_m_;
    float y_m = ellipseshort_m * sin(rad) + ellipsecenter_y_m_;
    //x,yを代入
    return leg.SetXY_m(x_m, y_m);
}
//楕円軌道
int EllipseOrbit::GetOrbit(OneLeg &leg, float phasetime_s)
{
    if (phasetime_s < stridetime_s_)
        return StrideLine_(leg, phasetime_s);
    else
        return RiseEllipse_(leg, phasetime_s);
}
float EllipseOrbit::GetOneWalkTime()
{
    return stridetime_s_ + risetime_s_;
}
void EllipseOrbit::ChangeOneParam(EllipseParams param, float val)
{
    switch (param)
    {
    case STRIDE_M:
        stride_m_ = val;
        break;
    case HEIGHT_M:
        height_m_ = val;
        break;
    case GROUND_M:
        ground_m_ = val;
        break;
    case ELLIPSE_CENTER_X_M:
        ellipsecenter_x_m_ = val;
        break;
    case ELLIPSE_CENTER_Y_M:
        ellipsecenter_y_m_ = val;
        break;
    }
}
void FreeLineOrbit::SetFreeLinesParam(LineParam lineparams[], int point_num)
{
    point_num_ = point_num;
    if (point_num_ == 1)
    {
        printf("error:point_num = 1. You should put >=2 to make line.");
        while (1)
            ;
    }
    for (int i = 0; i < point_num_; i++)
        lineparams_[i] = lineparams[i];
}
int FreeLineOrbit::GetOrbit(OneLeg &leg, float phasetime_s)
{
    ;
    //現在の最終到達pointを決定
    int arrived_point;
    float sum_time = 0;
    for (arrived_point = 0; arrived_point < point_num_ - 2; arrived_point++) //arrived_point = point_num-1(これが配列の最大index)の状態で終わらないように-2している
    {
        sum_time += lineparams_[arrived_point].time_s;
        if (phasetime_s < sum_time)
            break;
    }
    //x,yを計算
    float x_m, y_m;
    x_m = lineparams_[arrived_point].x_m +
          (lineparams_[arrived_point + 1].x_m - lineparams_[arrived_point].x_m) * phasetime_s / lineparams_[arrived_point].time_s;
    y_m = lineparams_[arrived_point].y_m +
          (lineparams_[arrived_point + 1].y_m - lineparams_[arrived_point].y_m) * phasetime_s / lineparams_[arrived_point].time_s;
    return leg.SetXY_m(x_m, y_m);
}
float FreeLineOrbit::GetOneWalkTime() //足一周の時間
{
    float sum_time = 0;
    for (int i = 0; i < point_num_; i++)
        sum_time += lineparams_[i].time_s;
    return sum_time;
}
void FreeLineOrbit::SetStandParam(float x_m, float y_m, float time_s)
{
    point_num_ = 1;
    lineparams_[0].x_m = x_m;
    lineparams_[0].y_m = y_m;
    lineparams_[0].time_s = time_s;
    lineparams_[1].x_m = x_m;
    lineparams_[1].y_m = y_m;
    lineparams_[1].time_s = 0;
}

Orbit::Orbit(OrbitPattern pattern)
{
    pattern_ = pattern;
}
float Orbit::GetOneWalkTime()
{
    float time_s;
    switch (pattern_)
    {
    case FREELINES:
        time_s = FreeLineOrbit::GetOneWalkTime();
        break;

    default:
        time_s = EllipseOrbit::GetOneWalkTime();
        break;
    }
    return time_s;
}
int Orbit::GetOrbit(OneLeg &leg, float phasetime_s)
{
    int ret;
    switch (pattern_)
    {
    case FREELINES:
        ret = FreeLineOrbit::GetOrbit(leg, phasetime_s);
        break;
    default:
        ret = EllipseOrbit::GetOrbit(leg, phasetime_s);
        break;
    }
    return ret;
}

Walk::Walk()
{
    for (int i = 0; i < 4; i++)
        offset_multi_[i] = 0;
}
float Walk::calctime_s_;
void Walk::Cal4LegsPosi(OneLeg leg[4])
{
    for (int i = 0; i < 4; i++)
    {
        float one_walk_time = orbit_[i].GetOneWalkTime();
        phasetime_s_[i] += calctime_s_;
        while (phasetime_s_[i] > one_walk_time)
            phasetime_s_[i] -= one_walk_time;
        orbit_[i].GetOrbit(leg[i], phasetime_s_[i]);
    }
}
float Walk::GetOneWalkTime()
{
    return orbit_[0].GetOneWalkTime(); //4足全て同じ時間のはずなので一例としてorbit_[0]のものを返している.
};
void Walk::SetOffset(float offset_multi0, float offset_multi1, float offset_multi2, float offset_multi3)
{
    offset_multi_[0] = offset_multi0;
    offset_multi_[1] = offset_multi1;
    offset_multi_[2] = offset_multi2;
    offset_multi_[3] = offset_multi3;
}
//軌道がリンク定義外になっていないかチェック。reutn 0:ok 1:out
int Walk::CheckOrbit(OneLeg templateleg)
{
    for (int i = 0; i < 4; i++)
    {
        //軌道が値域の外に出ないか計算で確かめる
        float one_walk_time = GetOneWalkTime();
        int step = one_walk_time / calctime_s_ * 2;
        for (float j = 0; j < one_walk_time; j += step)
        {
            if (orbit_[i].GetOrbit(templateleg, j) == 1)
                return 1; //解が出ないときは1を返す
        }
        phasetime_s_[i] = one_walk_time * offset_multi_[i];
    }
    return 0;
}
void Walk::SetOneOrbit(int legnum, Orbit orbit)
{
    orbit_[legnum] = orbit;
}
void Walk::SetAllOrbit(Orbit orbit)
{
    for (int i = 0; i < 4; i++)
        SetOneOrbit(i, orbit);
}

void Walk::ChangeOneParam(int legnum, EllipseParams param, float val)
{
    orbit_[legnum].ChangeOneParam(param, val);
}
void Walk::SetAllLegEllipseParam(float stridetime_s, float risetime_s,
                                 float stride_m, float height_m, float ground_m,
                                 float ellipsecenter_x_m, float ellipsecenter_y_m)
{
    for (int i = 0; i < 4; i++)
        orbit_[i].SetParam(stridetime_s, risetime_s, stride_m, height_m, ground_m, ellipsecenter_x_m, ellipsecenter_y_m);
}
void Walk::SetAllLegStandParam(float x_m, float y_m, float time_s)
{
    Orbit stand(FREELINES);
    stand.SetStandParam(x_m, y_m, time_s); //defaultとなる軌道設定
    SetAllOrbit(stand);                    //4足にコピー
}