File content as of revision 50:36741e8ab197:

/* NHK2019MR2 馬型機構プログラム.
//基本プログラム
SetWalk(walk, 歩行パターン);//歩行パターン設定
MoveOneCycle(walk, leg);//1回呼び出すと一サイクル分進む。2歩以上進みたいときはforで回して回数分呼び出す

で動く。
SetWalkの引数はenum WalkWayの定数。SetWalk内でswitchしている。SetWalk内ではchange_walk.hの関数で軌道を設定。
ROSありだと、ROSからのstateに従いswitchで切り替えている。
 */
#include "pi.h"
#include <math.h>
#include <stdio.h>
#ifndef VSCODE
#include "mbed.h"
#include "pinnames.h"
#include "can.h"
//#define USE_ROS //ROSを使うときはコメントアウトを外す
#include "ros.h"
#include "geometry_msgs/Vector3.h"
#include "std_msgs/Int16.h"
#endif
//#define DEBUG_ON //デバッグ用。使わないときはコメントアウト
#include "debug.h"
#include "OneLeg.h" ///足先の座標を保存するクラス。x,yやサーボの角度の保存、サーボの駆動も行う。他の足を考慮した処理は別のクラスに任せる。
#include "Walk.h"   //歩き方に関するファイル
#include "OverCome.h"
#include "change_walk.h"
#include "servo_and_movefunc.h"
enum ROS_STATE
{
    STOP,
    AREA1_LRFWALK_STATE,
    SANDDUNE,
    ROPE_STATE,
    SLOPE_STATE,
};
ROS_STATE state_from_ros = STOP; //ここを変えると動く方法が変わる
#ifdef USE_ROS
ros::NodeHandle nh_mbed;
void callback(const geometry_msgs::Vector3 &cmd_vel);
void callback_state(const std_msgs::Int16 &cmd_vel);
geometry_msgs::Vector3 back_vel;
ros::Subscriber<geometry_msgs::Vector3> sub_vel("/cmd_vel", &callback);
ros::Subscriber<std_msgs::Int16> sub_state("/state", &callback_state);
ros::Publisher pub_vel("/back_vel", &back_vel);
#endif
////////////あまり変化させないパラメーター
const float kBetweenServoHalf_m = 0.034 * 0.5; //サーボ間の距離の半分
float kLegLength1[2] = {0.15, 0.15};
float kLegLength2[2] = {0.33, 0.34};
///////////////
////////調整するべきパラメータ
enum WalkWay //歩行軌道
{
    STANDUP,              //受け渡し用に待つ
    AREA1_LRFWALK,        //段差までLRFありで歩く.元LRFPOSTURE
    AREA2_LRFWALK,        //段差後からLRFありで歩く
    STANDUP_SANDDUNE,     //段差前で立つ
    FRONTLEG_ON_SANDDUNE, //前足を段差にかける
    OVERCOME,             //前足が乗った状態で進む
    FALL_FRONTLEG,        //前足を段差から降ろす
    BACKLEG_ON_SANDDUNE,  //後ろ脚を段差に載せる
    OVERCOME2,            //後ろ脚が乗った状態で進む
    AFTER_OVERCOME,       //段差を終了したら座って指示まち
    WALK_BEFORE_ROPE,     //紐前で歩く
    ROPE,                 //rope前足超える
    ROPE_BACK,            //rope後ろ足超える
    SLOPE,
    TRUNRIGHT,
    TRUNLEFT,
    CHECK, //check用に最後に置いておく
};
//LRF使うやつだけだけspinOnceで変更するためグローバルで
float stride_m[4] = {0.2, 0.2, 0.2, 0.2}; //ropeのときは0.15だった
//各歩行軌道のパラメータ設定。switchで羅列している。新規に歩行パターンを増やすときはcaseを増やす。
//param walk:結果を入れる箱。way:歩行軌道を指定するWalkWayの定数。
int SetWalk(Walk &walk, WalkWay way)
{
    //複数歩行に共有するパラメータはここに書く。それ以外はcase内に入れる。
    //LRFを使って歩行する際のパラメータ.stride_mだけグローバルにある。
    float area1_offset_y_m[4] = {0.3, 0.3, 0.3, 0.3};
    float area2_offset_y_m[4] = {0.27, 0.27, 0.27, 0.27};
    float offset_x_m[4] = {0, 0, 0, 0};
    float height_m[4] = {0.05, 0.05, 0.05, 0.05}, buffer_height_m = 0.02,
          stridetime_s = 0.45, toptime_s = 0.2, buffer_time_s = 0.1;

    //段差
    float overcome_start_y_m[] = {0.38, 0.38, 0.38, 0.38};
    float flontleg_sand_goal_y_m[4] = {0.38, 0.27, 0.38, 0.27};
    float backleg_sand_goal_y_m[4] = {0.27, 0.38, 0.27, 0.38};
    float d_time = 0.2, d_time_slow = 0.3;
    //旋回
    float turn_start_x_m = 0, turn_start_y_m = 0.275,
          turn_stride_m = 0.075, turn_height_m = 0.05,
          turn_stridetime_s = 0.5, turn_risetime_s = 0.2;
    float d_time_rope = 0.6, d_time_slow_rope = 0.3;
    switch (way)
    {
    case STANDUP:
    { //受け渡し用に待つ
        float offset_x_m[4] = {},
              offset_y_m[4] = {0.3, 0.3, 0.3, 0.3};
        for (int i = 0; i < 4; i++)
            SetOneLegStandParam(walk, i, offset_x_m[i], offset_y_m[i], 0.5);
        walk.SetOffsetTime(0, 0, 0, 0);
        break;
    }
    case AREA1_LRFWALK: //LRF用に変数はグローバルにある
        for (int i = 0; i < 4; i++)
            SetOneLegFourPointParam(walk, i, offset_x_m[i], area1_offset_y_m[i],
                                    stride_m[i], height_m[i], buffer_height_m,
                                    stridetime_s, toptime_s, buffer_time_s);
        walk.SetOffsetTime(0, 0.5, 0.5, 0);
        break;
    case AREA2_LRFWALK:
        for (int i = 0; i < 4; i++)
            SetOneLegTriangleParam(walk, i, offset_x_m[i], area2_offset_y_m[i],
                                   stride_m[i], height_m[i], buffer_height_m,
                                   stridetime_s, toptime_s, buffer_time_s);
        walk.SetOffsetTime(0, 0.5, 0.5, 0);
        break;
    case STANDUP_SANDDUNE:
    {
        for (int i = 0; i < 4; i++)
        {
            LineParam lines[] = {
                {.time_s = 0, .x_m = walk.leg[i].GetX_m(), .y_m = walk.leg[i].GetY_m(), .is_point_to_point = 0},
                {.time_s = 0.5, .x_m = offset_x_m[i], .y_m = overcome_start_y_m[i], .is_point_to_point = 0},
                {.time_s = 1.5, .x_m = offset_x_m[i], .y_m = overcome_start_y_m[i], .is_point_to_point = 0},
            };
            SetOneLegFreeLinesParam(walk, i, lines, sizeof(lines) / sizeof(lines[0]));
        }
        walk.SetOffsetTime(0, 0, 0, 0);
        break;
    }
    case FRONTLEG_ON_SANDDUNE:
    { //前足を段差にかける
        float d_x_m = 0.1;
        float raise_offset_x_m[4] = {0, -0.02, 0, 0};
        float overcome_height_m[] = {0.05, 0.15, 0.05, 0.15};
        float gravity_dist[] = {0.05, 0, 0.05, -0.05};
        OverCome overcome(offset_x_m, overcome_start_y_m, d_x_m, flontleg_sand_goal_y_m,
                          overcome_height_m, gravity_dist, walk.leg,
                          raise_offset_x_m, d_time, d_time_slow);
        walk.Copy(overcome.walk);
        break;
    }
    case OVERCOME:
    { //前足が乗った状態で進む
        float stride_m[4] = {0.07, 0.07, 0.07, 0.07};
        for (int i = 0; i < 4; i++)
            SetOneLegFourPointParam(walk, i, offset_x_m[i], flontleg_sand_goal_y_m[i],
                                    stride_m[i], height_m[i], buffer_height_m,
                                    stridetime_s, toptime_s, buffer_time_s);
        walk.SetOffsetTime(0, 0.5, 0.5, 0);
        break;
    }
    case FALL_FRONTLEG: //前足を段差から降ろす
    {
        float d_x_m = 0.15;
        float start_x_m[4] = {0, 0, 0, 0};
        float raise_offset_x_m[4] = {};
        float overcome_height_m[] = {0.05, 0.05, 0.05, 0.05};
        float gravity_dist[] = {0.1, 0, 0.1, 0};
        OverCome overcome(start_x_m, flontleg_sand_goal_y_m, d_x_m, overcome_start_y_m,
                          overcome_height_m, gravity_dist, walk.leg,
                          raise_offset_x_m, d_time, d_time_slow);
        walk.Copy(overcome.walk);
        break;
    }
    case BACKLEG_ON_SANDDUNE:
    { //後ろ脚を乗せる
        float d_x_m = 0.15;
        float start_x_m[4] = {0, 0, 0, 0};
        float start_y_m[4] = {0.41, 0.41, 0.41, 0.41};

        float raise_offset_x_m[4] = {};
        float overcome_height_m[] = {0.2, 0.1, 0.2, 0.1};
        float gravity_dist[] = {0.1, 0, 0.1, 0};
        OverCome overcome(start_x_m, start_y_m, d_x_m, backleg_sand_goal_y_m,
                          overcome_height_m, gravity_dist, walk.leg,
                          raise_offset_x_m, d_time, d_time_slow);
        walk.Copy(overcome.walk);
        break;
    }
    case OVERCOME2:
    { //後ろ脚が乗った状態で進む
        float offset_x_m[4] = {0.1, 0, 0.1, 0};
        float offset_y_m[4] = {0.27, 0.33, 0.27, 0.33};
        float stride_m[4] = {0.2, 0.2, 0.2, 0.2},
              height_m[4] = {0.1, 0.1, 0.1, 0.1}, buffer_height_m = 0.05,
              stridetime_s = 1, toptime_s = 0.2, buffer_time_s = 0.2;
        for (int i = 0; i < 4; i++)
            SetOneLegFourPointParam(walk, i, offset_x_m[i], offset_y_m[i],
                                    stride_m[i], height_m[i], buffer_height_m,
                                    stridetime_s, toptime_s, buffer_time_s);
        walk.SetOffsetTime(0, 0.5, 0.5, 0);
        break;
    }
    case AFTER_OVERCOME:
    {
        for (int i = 0; i < 4; i++)
            SetOneLegStandParam(walk, i, offset_x_m[i], area1_offset_y_m[i], 0.5);
        walk.SetOffsetTime(0, 0, 0, 0);
        break;
    }
    case WALK_BEFORE_ROPE:
    {
        // float stride_m[] = {0.2, 0.2, 0.125, 0.125};
        float stride_m[] = {0.075, 0.075, 0.2, 0.2};
        for (int i = 0; i < 4; i++)
            SetOneLegFourPointParam(walk, i, offset_x_m[i], area1_offset_y_m[i],
                                    stride_m[i], height_m[i], buffer_height_m,
                                    stridetime_s, toptime_s, buffer_time_s);
        walk.SetOffsetTime(0, 0.5, 0.5, 0);
        break;
    }
    case ROPE:
    {
        float offset_x_m[4] = {-0.05, 0, -0.05, 0};
        float d_x_m = 0.2;
        float start_y_m[4] = {0.41, 0.41, 0.41, 0.41};
        float goal_y_m[4] = {0.41, 0.41, 0.41, 0.41};
        float raise_offset_x_m[] = {0, -0.15, 0, -0.15};
        float overcome_height_m[] = {0.05, 0.2, 0.05, 0.2};
        float gravity_dist[] = {0.05, -0.05, 0.05, -0.05};
        OverCome overcome(offset_x_m, start_y_m, d_x_m, goal_y_m,
                          overcome_height_m, gravity_dist, walk.leg,
                          raise_offset_x_m, d_time_rope, d_time_slow_rope);
        walk.Copy(overcome.walk);
        break;
    }
    case ROPE_BACK:
    {
        float offset_x_m[4] = {};
        float d_x_m = 0.2;
        float start_y_m[4] = {0.41, 0.38, 0.41, 0.38};
        float goal_y_m[4] = {0.41, 0.38, 0.41, 0.38};
        float raise_offset_x_m[] = {-0.1, 0, -0.1, 0};
        float overcome_height_m[] = {0.2, 0.05, 0.2, 0.05};
        float gravity_dist[] = {0.25, -0.05, 0.05, -0.05};
        OverCome overcome(offset_x_m, start_y_m, d_x_m, goal_y_m,
                          overcome_height_m, gravity_dist, walk.leg,
                          raise_offset_x_m, d_time_rope, d_time_slow_rope);
        walk.Copy(overcome.walk);
        break;
    }
    case SLOPE:
    {
        float offset_x_m[4] = {-0.15, 0.15, -0.15, 0.15},
              offset_y_m[4] = {0.32, 0.22, 0.32, 0.22};
        float stride_m[4] = {0.18, 0.18, 0.2, 0.2}; //ropeのときは0.15
        float height_m[4] = {0.05, 0.05, 0.05, 0.05}, buffer_height_m = 0.02,
              stridetime_s = 1, toptime_s = 0.15, buffer_time_s = 0.05;
        for (int i = 0; i < 4; i++)
            SetOneLegTriangleParam(walk, i, offset_x_m[i], offset_y_m[i],
                                   stride_m[i], height_m[i], buffer_height_m,
                                   stridetime_s, toptime_s, buffer_time_s);
        walk.SetOffsetTime(0, 0.5, 0.5, 0);
        break;
    }
    case TRUNRIGHT:
        Turn(walk, 1, turn_start_x_m, turn_start_y_m, turn_stride_m,
             turn_height_m, turn_stridetime_s, turn_risetime_s);
        break;
    case TRUNLEFT:
        Turn(walk, 0, turn_start_x_m, turn_start_y_m, turn_stride_m,
             turn_height_m, turn_stridetime_s, turn_risetime_s);
        break;
    default:
        printf("error: there is no WalkWay\r\n");
        return 1; //以上終了
    }
    walk.ResetPhase();
    return 0; //正常終了
}
void SmoothChange(Walk &walk, OneLeg leg[4], WalkWay nextway, float time_s);
int main()
{
    printf("program start\r\n");
#ifdef VSCODE
    if (FileOpen()) //csv fileに書き込み
        return 1;   //異常なら強制終了
#endif

    /////足の長さ、計算周期の設定
    OneLeg leg[4]; //各足の位置
    for (int i = 0; i < 4; i++)
        leg[i] = OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2);
    Walk walk(leg); //歩行法はここに入れていく
    Walk::calctime_s_ = 0.03;
    /////事前に軌道を全チェック
    for (int i = 0; i < CHECK; i++)
    {
        SetWalk(walk, (WalkWay)i);
        if (walk.CheckOrbit() == 1)
        {
            printf("error: move %d\r\n", i);
            return 1; //強制終了.errorは内部の関数からprintfで知らせる
        }
    }
//軌道チェックしてからROS完全起動
#ifdef USE_ROS
    nh_mbed.getHardware()->setBaud(115200);
    nh_mbed.initNode();
    nh_mbed.subscribe(sub_vel);
    nh_mbed.subscribe(sub_state);
    nh_mbed.advertise(pub_vel);
    nh_mbed.spinOnce(); //一度受信
#endif

    /////立つ
    state_from_ros = ROPE_STATE;
    switch (state_from_ros)
    {
    case SANDDUNE:
        SetWalk(walk, STANDUP_SANDDUNE);
        break;
    default:
        SetWalk(walk, STANDUP);
        break;
    }
    printf("Stand up?\r\n");
    WaitStdin('y'); // キーボード入力されるまでまで待つ
    MoveOneCycle(walk, leg);
    printf("Move?\r\n");
    WaitStdin('y');
    //プログラムの最初のほうにあるstate_from_rosで動かすものを切り替える.caseはenum ROS_STATEで分ける
    switch (state_from_ros)
    {
    case STOP:
        break;
    case AREA1_LRFWALK_STATE:                        //キーボード入力されるまでまで待つ
        SmoothChange(walk, leg, AREA1_LRFWALK, 0.5); //切り替え
        for (int i = 0; i < 20; i++)
            MoveOneCycle(walk, leg);
        break;
    case SANDDUNE:
        //前足を段差にかける                               //キーボード入力されるまでまで待つ
        SmoothChange(walk, leg, FRONTLEG_ON_SANDDUNE, 0.5); //切り替えスムーズ
        MoveOneCycle(walk, leg);
        //前足が段差に乗った状態で進む
        SmoothChange(walk, leg, OVERCOME, 0.5); //切り替えスムーズ
        for (int i = 0; i < 2; i++)
            MoveOneCycle(walk, leg);
        //前足を降ろす
        SmoothChange(walk, leg, FALL_FRONTLEG, 0.5);
        MoveOneCycle(walk, leg);
        //またいだ状態で歩く
        SmoothChange(walk, leg, AREA1_LRFWALK, 0.5);
        MoveOneCycle(walk, leg);
        //後ろ脚載せる
        SmoothChange(walk, leg, BACKLEG_ON_SANDDUNE, 0.5); //切り替えスムーズ
        MoveOneCycle(walk, leg);
        //後ろ脚乗った状態で進む
        SetWalk(walk, OVERCOME2);
        for (int i = 0; i < 3; i++)
            MoveOneCycle(walk, leg);
        //普通に進む
        SetWalk(walk, AREA1_LRFWALK);
        for (int i = 0; i < 4; i++)
            MoveOneCycle(walk, leg);
        break;

    case ROPE_STATE:
        //紐前歩く
        SetWalk(walk, WALK_BEFORE_ROPE);
        for (int i = 0; i < 5; i++)
            MoveOneCycle(walk, leg);
        int is_arrived = 0;
        while (is_arrived == 0)
        {
            MoveOneCycle(walk, leg);
            unsigned int dist_cm = GetDist_cm();
            if (dist_cm > 90 && dist_cm < 150)
                ++is_arrived;
            printf("%d\r\n", dist_cm);
        }
        /*
        SetWalk(walk, AREA1_LRFWALK);
        for (int i = 0; i < 5; i++)
            MoveOneCycle(walk, leg);
        SmoothChange(walk, leg, TRUNRIGHT, 0.5);
        for (int i = 0; i < 2; i++)
            MoveOneCycle(walk, leg); 
        */
        /*曲がりながら歩く
        SmoothChange(walk, leg, WALK_BEFORE_ROPE, 0.5);
        for (int i = 0; i < 7; i++)
            MoveOneCycle(walk, leg);
        */
        //前足だけ紐越え
        SmoothChange(walk, leg, ROPE, 0.5);
        MoveOneCycle(walk, leg);
        //LRFが取れる高さで歩行
        SmoothChange(walk, leg, AREA1_LRFWALK, 1.5);
        for (int i = 0; i < 3; i++)
            MoveOneCycle(walk, leg);
        //後ろ足越え
        SmoothChange(walk, leg, ROPE_BACK, 0.5);
        MoveOneCycle(walk, leg);
        //紐間歩く
        SmoothChange(walk, leg, AREA1_LRFWALK, 0.5);
        for (int i = 0; i < 1; i++)
            MoveOneCycle(walk, leg);
        //2本目
        //前足だけ紐越え
        SmoothChange(walk, leg, ROPE, 0.5);
        MoveOneCycle(walk, leg);
        //LRFが取れる高さで歩行
        SmoothChange(walk, leg, AREA1_LRFWALK, 1.5);
        for (int i = 0; i < 3; i++)
            MoveOneCycle(walk, leg);
        //後ろ足越え
        SmoothChange(walk, leg, ROPE_BACK, 0.5);
        MoveOneCycle(walk, leg);
        //紐後歩く
        SmoothChange(walk, leg, AREA1_LRFWALK, 1.5);
        for (int i = 0; i < 10; i++)
            MoveOneCycle(walk, leg);
        break;

    case SLOPE_STATE:
        SmoothChange(walk, leg, SLOPE, 0.5);
        for (int i = 0; i < 30; i++)
            MoveOneCycle(walk, leg);
        break;
    }
#ifdef USE_ROS
    nh_mbed.spinOnce();
#endif
    printf("program end\r\n");
#ifdef VSCODE
    fclose(fp);
#endif
}
#ifdef USE_ROS
void callback(const geometry_msgs::Vector3 &cmd_vel)
{
    if (state_from_ros == STOP)
        state_from_ros = AREA1_LRFWALK_STATE;
    stride_m[LEFT_F] = cmd_vel.x;
    stride_m[LEFT_B] = cmd_vel.x;
    stride_m[RIGHT_F] = cmd_vel.y;
    stride_m[RIGHT_B] = cmd_vel.y;
    back_vel = cmd_vel;
    pub_vel.publish(&back_vel);
}
void callback_state(const std_msgs::Int16 &cmd)
{
    state_from_ros = (ROS_STATE)cmd.data;
    led[state_from_ros % 4] = 1;
}
#endif
void SmoothChange(Walk &walk, OneLeg leg[4], WalkWay nextway, float time_s)
{
    LineParam lines[4][2];
    //前回位置を保存
    for (size_t i = 0; i < sizeof(lines) / sizeof(lines[0]); i++)
    {
        lines[i][0].time_s = 0;
        lines[i][0].x_m = leg[i].GetX_m();
        lines[i][0].y_m = leg[i].GetY_m();
    }
    Walk tempwalk(leg);
    SetWalk(tempwalk, nextway);
    tempwalk.Cal4LegsPosi(tempwalk.leg); //次の一歩目をwalk.legに書き込む
    for (size_t i = 0; i < sizeof(lines) / sizeof(lines[0]); i++)
    {
        lines[i][1].time_s = time_s;
        lines[i][1].x_m = tempwalk.leg[i].GetX_m();
        lines[i][1].y_m = tempwalk.leg[i].GetY_m();
        lines[i][1].is_point_to_point = 0;
    }
    //前回終点から次回始点まで直線移動
    for (int i = 0; i < 4; i++)
        SetOneLegFreeLinesParam(tempwalk, i, lines[i], sizeof(lines[i]) / sizeof(lines[i][0]));
    tempwalk.ResetPhase();
    MoveOneCycle(tempwalk, leg);
    SetWalk(walk, nextway);
};