File content as of revision 45:0654364226c9:

/* 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,
    RUN,
    SANDDUNE,
    TURNRIGHT_STATE,
    ROPE_STATE,
    SLOPE_STATE,

};
int state_from_ros = 0;
#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,              //受け渡し用に待つ
    LRFPOSTURE,           //LRF用
    STANDUP_SANDDUNE,     //段差前で立つ
    BEFORE_SANDDUNE,      //段差に壁当て
    FRONTLEG_ON_SANDDUNE, //前足を段差にかける
    OVERCOME,             //前足が乗った状態で進む
    BACKLEG_ON_SANDDUNE,  //後ろ脚を段差に載せる
    OVERCOME2,            //後ろ脚が乗った状態で進む
    AFTER_OVERCOME,       //段差を終了したら座って指示まち
    ROPE,                 //rope前足超える
    ROPE_BACK,            //rope後ろ足超える
    ROPE_DOWN,            //rope機体高さをLRF用に落とす
    SLOPE,
    TRUNRIGHT,
    TRUNLEFT,
    CHECK, //check用に最後に置いておく
};
//LRFPOSTUREだけspinOnceで変更するためグローバルで
float offset_x_m[4] = {-0.15, 0.15, -0.15, 0.15},
      offset_y_m[4] = {0.27, 0.27, 0.27, 0.27};
float stride_m[4] = {0.2, 0.2, 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.2, buffer_time_s = 0.1;
//各歩行軌道のパラメータ設定。switchで羅列している。新規に歩行パターンを増やすときはcaseを増やす。
//param walk:結果を入れる箱。way:歩行軌道を指定するWalkWayの定数。
int SetWalk(Walk &walk, WalkWay way)
{
    float overcome_start_y_m[] = {0.41, 0.41, 0.41, 0.41};
    float turn_start_x_m = 0, turn_start_y_m = 0.275,
          turn_stride_m = 0.05, turn_height_m = 0.05,
          turn_stridetime_s = 0.6, turn_risetime_s = 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 LRFPOSTURE: //LRF用に変数はグローバルにある
        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 BEFORE_SANDDUNE:
    {
        float stride_short_m[] = {0.1, 0.1, 0.1, 0.1};
        for (int i = 0; i < 4; i++)
            SetOneLegTriangleParam(walk, i, offset_x_m[i], overcome_start_y_m[i],
                                   stride_short_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:
    {
        float start_y_m[4] = {0.41, 0.41, 0.41, 0.41};
        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 = start_y_m[i], .is_point_to_point = 0},
                {.time_s = 1.5, .x_m = offset_x_m[i], .y_m = 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 goal_y_m[4] = {0.41, 0.29, 0.41, 0.29};
        float raise_offset_x_m[4] = {0, -0.02, 0, 0};
        float overcome_height_m[] = {0.1, 0.2, 0.1, 0.2};
        float gravity_dist[] = {0.05, 0, 0.05, -0.05};
        OverCome overcome(offset_x_m, overcome_start_y_m, d_x_m, goal_y_m,
                          overcome_height_m, gravity_dist, walk.leg, raise_offset_x_m);
        walk.Copy(overcome.walk);
        break;
    }
    case BACKLEG_ON_SANDDUNE:
    { //後ろ脚を乗せる
        float d_x_m = 0.1;
        float start_x_m[4] = {0, 0, 0, 0};
        float start_y_m[4] = {0.41, 0.41, 0.41, 0.41};
        float goal_y_m[4] = {0.29, 0.41, 0.29, 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, goal_y_m,
                          overcome_height_m, gravity_dist, walk.leg, raise_offset_x_m);
        walk.Copy(overcome.walk);
        break;
    }
    case OVERCOME:
    { //前足が乗った状態で進む
        float offset_x_m[4] = {-0.15, 0.15, -0.15, 0.15},
              offset_y_m[4] = {0.38, 0.35, 0.38, 0.35};
        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 OVERCOME2:
    { //後ろ脚が乗った状態で進む
        float offset_x_m[4] = {},
              offset_y_m[4] = {0.35, 0.35, 0.35, 0.35};
        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], offset_y_m[i], 0.5);
        walk.SetOffsetTime(0, 0, 0, 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.08, 0, -0.08};
        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);
        walk.Copy(overcome.walk);
        break;
    }
    case ROPE_BACK:
    {
        float offset_x_m[4] = {};
        float d_x_m = 0.15;
        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.03, 0, -0.03, 0};
        float overcome_height_m[] = {0.2, 0.05, 0.2, 0.05};
        float gravity_dist[] = {0.08, 0, 0.08, 0};
        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);
        walk.Copy(overcome.walk);
        break;
    }
    case ROPE_DOWN:
    {
        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 = offset_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 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.2, buffer_time_s = 0.1;
        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; //正常終了
}
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で知らせる
        }
    }
#ifndef USE_ROS
    printf("Stand up?\r\n");
    WaitStdin('y'); // キーボード入力されるまでまで待つ
#endif
    /////立つ
    // SetWalk(walk, STANDUP);
    SetWalk(walk, AFTER_OVERCOME); //段差越え後調整用に一時的に入れた
    MoveOneCycle(walk, leg);
#ifndef USE_ROS
    printf("Move?\r\n");
    WaitStdin('y'); //キーボード入力されるまでまで待つ
#else
    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(); //一度受信
    SetWalk(walk, LRFPOSTURE);
    while (1)
    {
        switch (state_from_ros)
        {
        case STOP:
            break;
        case RUN:
            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);
            MoveOneCycle(walk, leg);
            break;
        case SANDDUNE:
            // #endif
            //2歩進んで当て処理
            //            for (int i = 0; i < 2; i++)
            //                MoveOneCycle(walk, leg);
            SetWalk(walk, STANDUP_SANDDUNE);
            MoveOneCycle(walk, leg);
            //前足を段差にかける
            SetWalk(walk, FRONTLEG_ON_SANDDUNE);
            for (int i = 0; i < 1; i++)
                MoveOneCycle(walk, leg);
            //前足が段差に乗った状態で進む
            SetWalk(walk, OVERCOME);
            for (int i = 0; i < 5; i++)
                MoveOneCycle(walk, leg);
            //後ろ脚載せる
            SetWalk(walk, BACKLEG_ON_SANDDUNE);
            for (int i = 0; i < 1; i++)
                MoveOneCycle(walk, leg);
            //後ろ脚乗った状態で進む
            SetWalk(walk, OVERCOME2);
            for (int i = 0; i < 3; i++)
                MoveOneCycle(walk, leg);
            SetWalk(walk, TRUNLEFT);
            for (int i = 0; i < 2; i++)
                MoveOneCycle(walk, leg);
            state_from_ros = 1;
            // #ifdef USE_ROS
            break;
        case TURNRIGHT_STATE:
            SetWalk(walk, TRUNRIGHT);
            for (int i = 0; i < 5; i++)
                MoveOneCycle(walk, leg);
            state_from_ros = 1;
            break;
        case ROPE_STATE:
            SetWalk(walk, ROPE);
            MoveOneCycle(walk, leg);
            SetWalk(walk, ROPE_DOWN);
            MoveOneCycle(walk, leg);
            SetWalk(walk, LRFPOSTURE);
            for (int i = 0; i < 3; i++)
                MoveOneCycle(walk, leg);
            SetWalk(walk, ROPE_BACK);
            MoveOneCycle(walk, leg);
            state_from_ros = 1;
            break;
        case SLOPE_STATE:
            SetWalk(walk, SLOPE);
            for (int i = 0; i < 30; i++)
                MoveOneCycle(walk, leg);
            state_from_ros = 0;
            break;
        }
        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 == 0)
        state_from_ros = 1;
    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);
    led[0] = !led[0];
}
void callback_state(const std_msgs::Int16 &cmd)
{
    state_from_ros = cmd.data;
    //    led[state_from_ros] = 1;
}
#endif