岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

main.cpp@47:c2c579909787, 2019-03-08 (annotated)

Committer:
yuto17320508
Date:
Fri Mar 08 04:34:29 2019 +0000
Revision:
47:c2c579909787
Parent:
46:621ea6492dea
Child:
48:1aad3cc386e8
hi

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimizuta 44:4aac39b8670b 1 /* NHK2019MR2 馬型機構プログラム.
shimizuta 44:4aac39b8670b 2 //基本プログラム
shimizuta 44:4aac39b8670b 3 SetWalk(walk, 歩行パターン);//歩行パターン設定
shimizuta 44:4aac39b8670b 4 MoveOneCycle(walk, leg);//1回呼び出すと一サイクル分進む。2歩以上進みたいときはforで回して回数分呼び出す
shimizuta 44:4aac39b8670b 5
shimizuta 44:4aac39b8670b 6 で動く。
shimizuta 44:4aac39b8670b 7 SetWalkの引数はenum WalkWayの定数。SetWalk内でswitchしている。SetWalk内ではchange_walk.hの関数で軌道を設定。
shimizuta 44:4aac39b8670b 8 ROSありだと、ROSからのstateに従いswitchで切り替えている。
shimizuta 44:4aac39b8670b 9 */
shimizuta 43:2ed84f3558c1 10 #include "pi.h"
shimizuta 27:79b4b932a6dd 11 #include <math.h>
shimizuta 27:79b4b932a6dd 12 #include <stdio.h>
shimizuta 43:2ed84f3558c1 13 #ifndef VSCODE
yuto17320508 0:f000d896d188 14 #include "mbed.h"
shimizuta 9:905f93247688 15 #include "pinnames.h"
shimizuta 16:0069a56f11a3 16 #include "can.h"
shimizuta 44:4aac39b8670b 17 #define USE_ROS //ROSを使うときはコメントアウトを外す
shimizuta 43:2ed84f3558c1 18 #include "ros.h"
shimizuta 43:2ed84f3558c1 19 #include "geometry_msgs/Vector3.h"
shimizuta 43:2ed84f3558c1 20 #include "std_msgs/Int16.h"
shimizuta 27:79b4b932a6dd 21 #endif
shimizuta 27:79b4b932a6dd 22 //#define DEBUG_ON //デバッグ用。使わないときはコメントアウト
shimizuta 27:79b4b932a6dd 23 #include "debug.h"
shimizuta 11:e81425872740 24 #include "OneLeg.h" ///足先の座標を保存するクラス。x,yやサーボの角度の保存、サーボの駆動も行う。他の足を考慮した処理は別のクラスに任せる。
shimizuta 11:e81425872740 25 #include "Walk.h" //歩き方に関するファイル
shimizuta 35:b4e1b8f25cd7 26 #include "OverCome.h"
shimizuta 35:b4e1b8f25cd7 27 #include "change_walk.h"
shimizuta 43:2ed84f3558c1 28 #include "servo_and_movefunc.h"
shimizuta 43:2ed84f3558c1 29 enum ROS_STATE
shimizuta 43:2ed84f3558c1 30 {
shimizuta 43:2ed84f3558c1 31 STOP,
shimizuta 46:621ea6492dea 32 AREA1_LRFWALK_STATE,
shimizuta 43:2ed84f3558c1 33 SANDDUNE,
shimizuta 46:621ea6492dea 34 AREA2_LRFWALK_STATE,
shimizuta 45:0654364226c9 35 ROPE_STATE,
shimizuta 45:0654364226c9 36 SLOPE_STATE,
shimizuta 46:621ea6492dea 37 TURNRIGHT_STATE,
shimizuta 46:621ea6492dea 38 TRUNLEFT_STATE,
shimizuta 43:2ed84f3558c1 39 };
shimizuta 46:621ea6492dea 40 ROS_STATE state_from_ros = STOP; //ここを変えると動く方法が変わる
shimizuta 43:2ed84f3558c1 41 #ifdef USE_ROS
shimizuta 43:2ed84f3558c1 42 ros::NodeHandle nh_mbed;
shimizuta 43:2ed84f3558c1 43 void callback(const geometry_msgs::Vector3 &cmd_vel);
shimizuta 43:2ed84f3558c1 44 void callback_state(const std_msgs::Int16 &cmd_vel);
shimizuta 44:4aac39b8670b 45 geometry_msgs::Vector3 back_vel;
shimizuta 43:2ed84f3558c1 46 ros::Subscriber<geometry_msgs::Vector3> sub_vel("/cmd_vel", &callback);
shimizuta 43:2ed84f3558c1 47 ros::Subscriber<std_msgs::Int16> sub_state("/state", &callback_state);
shimizuta 44:4aac39b8670b 48 ros::Publisher pub_vel("/back_vel", &back_vel);
shimizuta 43:2ed84f3558c1 49 #endif
shimizuta 43:2ed84f3558c1 50 ////////////あまり変化させないパラメーター
shimizuta 35:b4e1b8f25cd7 51 const float kBetweenServoHalf_m = 0.034 * 0.5; //サーボ間の距離の半分
shimizuta 35:b4e1b8f25cd7 52 float kLegLength1[2] = {0.15, 0.15};
shimizuta 35:b4e1b8f25cd7 53 float kLegLength2[2] = {0.33, 0.34};
shimizuta 9:905f93247688 54 ///////////////
shimizuta 27:79b4b932a6dd 55 ////////調整するべきパラメータ
shimizuta 44:4aac39b8670b 56 enum WalkWay //歩行軌道
shimizuta 27:79b4b932a6dd 57 {
shimizuta 43:2ed84f3558c1 58 STANDUP, //受け渡し用に待つ
shimizuta 46:621ea6492dea 59 AREA1_LRFWALK, //段差までLRFありで歩く.元LRFPOSTURE
shimizuta 46:621ea6492dea 60 AREA2_LRFWALK, //段差後からLRFありで歩く
shimizuta 43:2ed84f3558c1 61 STANDUP_SANDDUNE, //段差前で立つ
shimizuta 43:2ed84f3558c1 62 BEFORE_SANDDUNE, //段差に壁当て
shimizuta 42:982064594ba6 63 FRONTLEG_ON_SANDDUNE, //前足を段差にかける
shimizuta 42:982064594ba6 64 OVERCOME, //前足が乗った状態で進む
shimizuta 42:982064594ba6 65 BACKLEG_ON_SANDDUNE, //後ろ脚を段差に載せる
shimizuta 42:982064594ba6 66 OVERCOME2, //後ろ脚が乗った状態で進む
shimizuta 45:0654364226c9 67 AFTER_OVERCOME, //段差を終了したら座って指示まち
shimizuta 43:2ed84f3558c1 68 ROPE, //rope前足超える
shimizuta 43:2ed84f3558c1 69 ROPE_BACK, //rope後ろ足超える
shimizuta 43:2ed84f3558c1 70 ROPE_DOWN, //rope機体高さをLRF用に落とす
shimizuta 43:2ed84f3558c1 71 SLOPE,
shimizuta 45:0654364226c9 72 TRUNRIGHT,
shimizuta 45:0654364226c9 73 TRUNLEFT,
shimizuta 43:2ed84f3558c1 74 CHECK, //check用に最後に置いておく
shimizuta 27:79b4b932a6dd 75 };
shimizuta 46:621ea6492dea 76 //LRF使うやつだけだけspinOnceで変更するためグローバルで
shimizuta 43:2ed84f3558c1 77 float stride_m[4] = {0.2, 0.2, 0.2, 0.2}; //ropeのときは0.15だった
shimizuta 44:4aac39b8670b 78 //各歩行軌道のパラメータ設定。switchで羅列している。新規に歩行パターンを増やすときはcaseを増やす。
shimizuta 44:4aac39b8670b 79 //param walk:結果を入れる箱。way:歩行軌道を指定するWalkWayの定数。
shimizuta 44:4aac39b8670b 80 int SetWalk(Walk &walk, WalkWay way)
shimizuta 42:982064594ba6 81 {
shimizuta 46:621ea6492dea 82 //複数歩行に共有するパラメータはここに書く。それ以外はcase内に入れる。
shimizuta 46:621ea6492dea 83 //LRFを使って歩行する際のパラメータ.stride_mだけグローバルにある。
shimizuta 46:621ea6492dea 84 float area1_offset_y_m[4] = {0.27, 0.27, 0.27, 0.27};
yuto17320508 47:c2c579909787 85 float area2_offset_y_m[4] = {0.27, 0.27, 0.27, 0.27};
shimizuta 46:621ea6492dea 86 float offset_x_m[4] = {-0.15, 0.15, -0.15, 0.15};
shimizuta 46:621ea6492dea 87 float height_m[4] = {0.05, 0.05, 0.05, 0.05}, buffer_height_m = 0.02,
shimizuta 46:621ea6492dea 88 stridetime_s = 1, toptime_s = 0.2, buffer_time_s = 0.1;
shimizuta 46:621ea6492dea 89
shimizuta 46:621ea6492dea 90 //段差
shimizuta 43:2ed84f3558c1 91 float overcome_start_y_m[] = {0.41, 0.41, 0.41, 0.41};
shimizuta 46:621ea6492dea 92
shimizuta 46:621ea6492dea 93 //旋回
shimizuta 45:0654364226c9 94 float turn_start_x_m = 0, turn_start_y_m = 0.275,
shimizuta 45:0654364226c9 95 turn_stride_m = 0.05, turn_height_m = 0.05,
shimizuta 45:0654364226c9 96 turn_stridetime_s = 0.6, turn_risetime_s = 0.3;
shimizuta 35:b4e1b8f25cd7 97 switch (way)
shimizuta 35:b4e1b8f25cd7 98 {
shimizuta 43:2ed84f3558c1 99 case STANDUP:
shimizuta 43:2ed84f3558c1 100 { //受け渡し用に待つ
shimizuta 43:2ed84f3558c1 101 float offset_x_m[4] = {},
yuto17320508 47:c2c579909787 102 offset_y_m[4] = {0.275, 0.275, 0.275, 0.275};
shimizuta 35:b4e1b8f25cd7 103 for (int i = 0; i < 4; i++)
shimizuta 43:2ed84f3558c1 104 SetOneLegStandParam(walk, i, offset_x_m[i], offset_y_m[i], 0.5);
shimizuta 43:2ed84f3558c1 105 walk.SetOffsetTime(0, 0, 0, 0);
shimizuta 35:b4e1b8f25cd7 106 break;
shimizuta 43:2ed84f3558c1 107 }
shimizuta 46:621ea6492dea 108 case AREA1_LRFWALK: //LRF用に変数はグローバルにある
shimizuta 35:b4e1b8f25cd7 109 for (int i = 0; i < 4; i++)
shimizuta 46:621ea6492dea 110 SetOneLegTriangleParam(walk, i, offset_x_m[i], area1_offset_y_m[i],
shimizuta 46:621ea6492dea 111 stride_m[i], height_m[i], buffer_height_m,
shimizuta 46:621ea6492dea 112 stridetime_s, toptime_s, buffer_time_s);
shimizuta 46:621ea6492dea 113 walk.SetOffsetTime(0, 0.5, 0.5, 0);
shimizuta 46:621ea6492dea 114 break;
shimizuta 46:621ea6492dea 115 case AREA2_LRFWALK:
shimizuta 46:621ea6492dea 116 for (int i = 0; i < 4; i++)
shimizuta 46:621ea6492dea 117 SetOneLegTriangleParam(walk, i, offset_x_m[i], area2_offset_y_m[i],
shimizuta 35:b4e1b8f25cd7 118 stride_m[i], height_m[i], buffer_height_m,
shimizuta 35:b4e1b8f25cd7 119 stridetime_s, toptime_s, buffer_time_s);
shimizuta 35:b4e1b8f25cd7 120 walk.SetOffsetTime(0, 0.5, 0.5, 0);
shimizuta 35:b4e1b8f25cd7 121 break;
shimizuta 43:2ed84f3558c1 122 case BEFORE_SANDDUNE:
shimizuta 43:2ed84f3558c1 123 {
shimizuta 43:2ed84f3558c1 124 float stride_short_m[] = {0.1, 0.1, 0.1, 0.1};
shimizuta 43:2ed84f3558c1 125 for (int i = 0; i < 4; i++)
shimizuta 43:2ed84f3558c1 126 SetOneLegTriangleParam(walk, i, offset_x_m[i], overcome_start_y_m[i],
shimizuta 43:2ed84f3558c1 127 stride_short_m[i], height_m[i], buffer_height_m,
shimizuta 43:2ed84f3558c1 128 stridetime_s, toptime_s, buffer_time_s);
shimizuta 43:2ed84f3558c1 129 walk.SetOffsetTime(0, 0.5, 0.5, 0);
shimizuta 44:4aac39b8670b 130 break;
shimizuta 43:2ed84f3558c1 131 }
shimizuta 43:2ed84f3558c1 132 case STANDUP_SANDDUNE:
shimizuta 42:982064594ba6 133 {
shimizuta 43:2ed84f3558c1 134 float start_y_m[4] = {0.41, 0.41, 0.41, 0.41};
shimizuta 43:2ed84f3558c1 135 for (int i = 0; i < 4; i++)
shimizuta 43:2ed84f3558c1 136 {
shimizuta 43:2ed84f3558c1 137 LineParam lines[] = {
shimizuta 43:2ed84f3558c1 138 {.time_s = 0, .x_m = walk.leg[i].GetX_m(), .y_m = walk.leg[i].GetY_m(), .is_point_to_point = 0},
shimizuta 43:2ed84f3558c1 139 {.time_s = 0.5, .x_m = offset_x_m[i], .y_m = start_y_m[i], .is_point_to_point = 0},
shimizuta 43:2ed84f3558c1 140 {.time_s = 1.5, .x_m = offset_x_m[i], .y_m = start_y_m[i], .is_point_to_point = 0},
shimizuta 43:2ed84f3558c1 141 };
shimizuta 43:2ed84f3558c1 142 SetOneLegFreeLinesParam(walk, i, lines, sizeof(lines) / sizeof(lines[0]));
shimizuta 43:2ed84f3558c1 143 }
shimizuta 43:2ed84f3558c1 144 walk.SetOffsetTime(0, 0, 0, 0);
shimizuta 44:4aac39b8670b 145 break;
shimizuta 43:2ed84f3558c1 146 }
shimizuta 43:2ed84f3558c1 147 case FRONTLEG_ON_SANDDUNE:
shimizuta 43:2ed84f3558c1 148 { //前足を段差にかける
shimizuta 42:982064594ba6 149 float d_x_m = 0.1;
shimizuta 42:982064594ba6 150 float goal_y_m[4] = {0.41, 0.29, 0.41, 0.29};
shimizuta 43:2ed84f3558c1 151 float raise_offset_x_m[4] = {0, -0.02, 0, 0};
shimizuta 42:982064594ba6 152 float overcome_height_m[] = {0.1, 0.2, 0.1, 0.2};
shimizuta 42:982064594ba6 153 float gravity_dist[] = {0.05, 0, 0.05, -0.05};
shimizuta 43:2ed84f3558c1 154 OverCome overcome(offset_x_m, overcome_start_y_m, d_x_m, goal_y_m,
shimizuta 43:2ed84f3558c1 155 overcome_height_m, gravity_dist, walk.leg, raise_offset_x_m);
shimizuta 35:b4e1b8f25cd7 156 walk.Copy(overcome.walk);
shimizuta 42:982064594ba6 157 break;
shimizuta 39:87dcdff27797 158 }
shimizuta 43:2ed84f3558c1 159 case BACKLEG_ON_SANDDUNE:
shimizuta 43:2ed84f3558c1 160 { //後ろ脚を乗せる
shimizuta 42:982064594ba6 161 float d_x_m = 0.1;
shimizuta 43:2ed84f3558c1 162 float start_x_m[4] = {0, 0, 0, 0};
shimizuta 42:982064594ba6 163 float start_y_m[4] = {0.41, 0.41, 0.41, 0.41};
shimizuta 42:982064594ba6 164 float goal_y_m[4] = {0.29, 0.41, 0.29, 0.41};
shimizuta 43:2ed84f3558c1 165 float raise_offset_x_m[4] = {};
shimizuta 42:982064594ba6 166 float overcome_height_m[] = {0.2, 0.1, 0.2, 0.1};
shimizuta 43:2ed84f3558c1 167 float gravity_dist[] = {0.1, 0, 0.1, 0};
shimizuta 43:2ed84f3558c1 168 OverCome overcome(start_x_m, start_y_m, d_x_m, goal_y_m,
shimizuta 43:2ed84f3558c1 169 overcome_height_m, gravity_dist, walk.leg, raise_offset_x_m);
shimizuta 42:982064594ba6 170 walk.Copy(overcome.walk);
shimizuta 42:982064594ba6 171 break;
shimizuta 42:982064594ba6 172 }
shimizuta 43:2ed84f3558c1 173 case OVERCOME:
shimizuta 43:2ed84f3558c1 174 { //前足が乗った状態で進む
shimizuta 42:982064594ba6 175 float offset_x_m[4] = {-0.15, 0.15, -0.15, 0.15},
shimizuta 43:2ed84f3558c1 176 offset_y_m[4] = {0.38, 0.35, 0.38, 0.35};
shimizuta 42:982064594ba6 177 float stride_m[4] = {0.2, 0.2, 0.2, 0.2},
shimizuta 42:982064594ba6 178 height_m[4] = {0.1, 0.1, 0.1, 0.1}, buffer_height_m = 0.05,
shimizuta 42:982064594ba6 179 stridetime_s = 1, toptime_s = 0.2, buffer_time_s = 0.2;
shimizuta 39:87dcdff27797 180 for (int i = 0; i < 4; i++)
shimizuta 39:87dcdff27797 181 SetOneLegFourPointParam(walk, i, offset_x_m[i], offset_y_m[i],
shimizuta 39:87dcdff27797 182 stride_m[i], height_m[i], buffer_height_m,
shimizuta 39:87dcdff27797 183 stridetime_s, toptime_s, buffer_time_s);
shimizuta 39:87dcdff27797 184 walk.SetOffsetTime(0, 0.5, 0.5, 0);
shimizuta 35:b4e1b8f25cd7 185 break;
shimizuta 35:b4e1b8f25cd7 186 }
shimizuta 43:2ed84f3558c1 187 case OVERCOME2:
shimizuta 43:2ed84f3558c1 188 { //後ろ脚が乗った状態で進む
shimizuta 42:982064594ba6 189 float offset_x_m[4] = {},
shimizuta 42:982064594ba6 190 offset_y_m[4] = {0.35, 0.35, 0.35, 0.35};
shimizuta 42:982064594ba6 191 float stride_m[4] = {0.2, 0.2, 0.2, 0.2},
shimizuta 42:982064594ba6 192 height_m[4] = {0.1, 0.1, 0.1, 0.1}, buffer_height_m = 0.05,
shimizuta 42:982064594ba6 193 stridetime_s = 1, toptime_s = 0.2, buffer_time_s = 0.2;
shimizuta 42:982064594ba6 194 for (int i = 0; i < 4; i++)
shimizuta 42:982064594ba6 195 SetOneLegFourPointParam(walk, i, offset_x_m[i], offset_y_m[i],
shimizuta 42:982064594ba6 196 stride_m[i], height_m[i], buffer_height_m,
shimizuta 42:982064594ba6 197 stridetime_s, toptime_s, buffer_time_s);
shimizuta 42:982064594ba6 198 walk.SetOffsetTime(0, 0.5, 0.5, 0);
shimizuta 42:982064594ba6 199 break;
shimizuta 42:982064594ba6 200 }
shimizuta 44:4aac39b8670b 201 case AFTER_OVERCOME:
shimizuta 43:2ed84f3558c1 202 {
shimizuta 44:4aac39b8670b 203 for (int i = 0; i < 4; i++)
shimizuta 46:621ea6492dea 204 SetOneLegStandParam(walk, i, offset_x_m[i], area1_offset_y_m[i], 0.5);
shimizuta 44:4aac39b8670b 205 walk.SetOffsetTime(0, 0, 0, 0);
shimizuta 44:4aac39b8670b 206 break;
shimizuta 44:4aac39b8670b 207 }
shimizuta 45:0654364226c9 208 case ROPE:
shimizuta 44:4aac39b8670b 209 {
shimizuta 43:2ed84f3558c1 210 float offset_x_m[4] = {-0.05, 0, -0.05, 0};
shimizuta 43:2ed84f3558c1 211 float d_x_m = 0.2;
shimizuta 43:2ed84f3558c1 212 float start_y_m[4] = {0.41, 0.41, 0.41, 0.41};
shimizuta 43:2ed84f3558c1 213 float goal_y_m[4] = {0.41, 0.41, 0.41, 0.41};
shimizuta 43:2ed84f3558c1 214 float raise_offset_x_m[] = {0, -0.08, 0, -0.08};
shimizuta 43:2ed84f3558c1 215 float overcome_height_m[] = {0.05, 0.2, 0.05, 0.2};
shimizuta 43:2ed84f3558c1 216 float gravity_dist[] = {0.05, -0.05, 0.05, -0.05};
shimizuta 43:2ed84f3558c1 217 OverCome overcome(offset_x_m, start_y_m, d_x_m, goal_y_m,
shimizuta 43:2ed84f3558c1 218 overcome_height_m, gravity_dist, walk.leg, raise_offset_x_m);
shimizuta 43:2ed84f3558c1 219 walk.Copy(overcome.walk);
shimizuta 44:4aac39b8670b 220 break;
shimizuta 43:2ed84f3558c1 221 }
shimizuta 43:2ed84f3558c1 222 case ROPE_BACK:
shimizuta 43:2ed84f3558c1 223 {
shimizuta 43:2ed84f3558c1 224 float offset_x_m[4] = {};
shimizuta 43:2ed84f3558c1 225 float d_x_m = 0.15;
shimizuta 43:2ed84f3558c1 226 float start_y_m[4] = {0.41, 0.41, 0.41, 0.41};
shimizuta 43:2ed84f3558c1 227 float goal_y_m[4] = {0.41, 0.41, 0.41, 0.41};
shimizuta 43:2ed84f3558c1 228 float raise_offset_x_m[] = {-0.03, 0, -0.03, 0};
shimizuta 43:2ed84f3558c1 229 float overcome_height_m[] = {0.2, 0.05, 0.2, 0.05};
shimizuta 43:2ed84f3558c1 230 float gravity_dist[] = {0.08, 0, 0.08, 0};
shimizuta 43:2ed84f3558c1 231 OverCome overcome(offset_x_m, start_y_m, d_x_m, goal_y_m,
shimizuta 43:2ed84f3558c1 232 overcome_height_m, gravity_dist, walk.leg, raise_offset_x_m);
shimizuta 43:2ed84f3558c1 233 walk.Copy(overcome.walk);
shimizuta 44:4aac39b8670b 234 break;
shimizuta 43:2ed84f3558c1 235 }
shimizuta 43:2ed84f3558c1 236 case ROPE_DOWN:
shimizuta 43:2ed84f3558c1 237 {
shimizuta 43:2ed84f3558c1 238 for (int i = 0; i < 4; i++)
shimizuta 43:2ed84f3558c1 239 {
shimizuta 43:2ed84f3558c1 240 LineParam lines[] = {
shimizuta 43:2ed84f3558c1 241 {.time_s = 0, .x_m = walk.leg[i].GetX_m(), .y_m = walk.leg[i].GetY_m(), .is_point_to_point = 0},
shimizuta 46:621ea6492dea 242 {.time_s = 0.5, .x_m = offset_x_m[i], .y_m = area1_offset_y_m[i], .is_point_to_point = 0},
shimizuta 43:2ed84f3558c1 243 };
shimizuta 43:2ed84f3558c1 244 SetOneLegFreeLinesParam(walk, i, lines, sizeof(lines) / sizeof(lines[0]));
shimizuta 43:2ed84f3558c1 245 }
shimizuta 43:2ed84f3558c1 246 walk.SetOffsetTime(0, 0, 0, 0);
shimizuta 44:4aac39b8670b 247 break;
shimizuta 43:2ed84f3558c1 248 }
shimizuta 43:2ed84f3558c1 249 case SLOPE:
shimizuta 43:2ed84f3558c1 250 {
shimizuta 43:2ed84f3558c1 251 float offset_x_m[4] = {-0.15, 0.15, -0.15, 0.15},
shimizuta 43:2ed84f3558c1 252 offset_y_m[4] = {0.32, 0.22, 0.32, 0.22};
shimizuta 43:2ed84f3558c1 253 float stride_m[4] = {0.18, 0.18, 0.2, 0.2}; //ropeのときは0.15
shimizuta 43:2ed84f3558c1 254 float height_m[4] = {0.05, 0.05, 0.05, 0.05}, buffer_height_m = 0.02,
shimizuta 43:2ed84f3558c1 255 stridetime_s = 1, toptime_s = 0.2, buffer_time_s = 0.1;
shimizuta 43:2ed84f3558c1 256 for (int i = 0; i < 4; i++)
shimizuta 43:2ed84f3558c1 257 SetOneLegTriangleParam(walk, i, offset_x_m[i], offset_y_m[i],
shimizuta 43:2ed84f3558c1 258 stride_m[i], height_m[i], buffer_height_m,
shimizuta 43:2ed84f3558c1 259 stridetime_s, toptime_s, buffer_time_s);
shimizuta 43:2ed84f3558c1 260 walk.SetOffsetTime(0, 0.5, 0.5, 0);
shimizuta 44:4aac39b8670b 261 break;
shimizuta 43:2ed84f3558c1 262 }
shimizuta 45:0654364226c9 263 case TRUNRIGHT:
shimizuta 45:0654364226c9 264 Turn(walk, 1, turn_start_x_m, turn_start_y_m, turn_stride_m,
shimizuta 45:0654364226c9 265 turn_height_m, turn_stridetime_s, turn_risetime_s);
shimizuta 45:0654364226c9 266 break;
shimizuta 45:0654364226c9 267 case TRUNLEFT:
shimizuta 45:0654364226c9 268 Turn(walk, 0, turn_start_x_m, turn_start_y_m, turn_stride_m,
shimizuta 45:0654364226c9 269 turn_height_m, turn_stridetime_s, turn_risetime_s);
shimizuta 45:0654364226c9 270 break;
shimizuta 42:982064594ba6 271 default:
shimizuta 42:982064594ba6 272 printf("error: there is no WalkWay\r\n");
shimizuta 44:4aac39b8670b 273 return 1; //以上終了
shimizuta 42:982064594ba6 274 }
shimizuta 44:4aac39b8670b 275 walk.ResetPhase();
shimizuta 44:4aac39b8670b 276 return 0; //正常終了
yuto17320508 18:0033ef1814ba 277 }
shimizuta 9:905f93247688 278 int main()
yuto17320508 5:556d5a5e9d24 279 {
shimizuta 44:4aac39b8670b 280 printf("program start\r\n");
shimizuta 29:7d8b8011a88d 281 #ifdef VSCODE
shimizuta 27:79b4b932a6dd 282 if (FileOpen()) //csv fileに書き込み
shimizuta 44:4aac39b8670b 283 return 1; //異常なら強制終了
shimizuta 29:7d8b8011a88d 284 #endif
shimizuta 46:621ea6492dea 285
shimizuta 44:4aac39b8670b 286 /////足の長さ、計算周期の設定
shimizuta 39:87dcdff27797 287 OneLeg leg[4]; //各足の位置
shimizuta 35:b4e1b8f25cd7 288 for (int i = 0; i < 4; i++)
shimizuta 35:b4e1b8f25cd7 289 leg[i] = OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2);
shimizuta 39:87dcdff27797 290 Walk walk(leg); //歩行法はここに入れていく
shimizuta 35:b4e1b8f25cd7 291 Walk::calctime_s_ = 0.03;
shimizuta 44:4aac39b8670b 292 /////事前に軌道を全チェック
shimizuta 42:982064594ba6 293 for (int i = 0; i < CHECK; i++)
shimizuta 42:982064594ba6 294 {
shimizuta 44:4aac39b8670b 295 SetWalk(walk, (WalkWay)i);
shimizuta 44:4aac39b8670b 296 if (walk.CheckOrbit() == 1)
shimizuta 42:982064594ba6 297 {
shimizuta 42:982064594ba6 298 printf("error: move %d\r\n", i);
shimizuta 42:982064594ba6 299 return 1; //強制終了.errorは内部の関数からprintfで知らせる
shimizuta 42:982064594ba6 300 }
shimizuta 42:982064594ba6 301 }
shimizuta 46:621ea6492dea 302 //軌道チェックしてからROS完全起動
shimizuta 46:621ea6492dea 303 #ifdef USE_ROS
shimizuta 46:621ea6492dea 304 nh_mbed.getHardware()->setBaud(115200);
shimizuta 46:621ea6492dea 305 nh_mbed.initNode();
shimizuta 46:621ea6492dea 306 nh_mbed.subscribe(sub_vel);
shimizuta 46:621ea6492dea 307 nh_mbed.subscribe(sub_state);
shimizuta 46:621ea6492dea 308 nh_mbed.advertise(pub_vel);
shimizuta 46:621ea6492dea 309 nh_mbed.spinOnce(); //一度受信
shimizuta 46:621ea6492dea 310 #else
shimizuta 35:b4e1b8f25cd7 311 printf("Stand up?\r\n");
shimizuta 44:4aac39b8670b 312 WaitStdin('y'); // キーボード入力されるまでまで待つ
shimizuta 43:2ed84f3558c1 313 #endif
shimizuta 44:4aac39b8670b 314 /////立つ
yuto17320508 47:c2c579909787 315 SetWalk(walk, STANDUP);
yuto17320508 47:c2c579909787 316 // SetWalk(walk, AFTER_OVERCOME); //段差越え後調整用に一時的に入れた
shimizuta 43:2ed84f3558c1 317 MoveOneCycle(walk, leg);
shimizuta 43:2ed84f3558c1 318 #ifndef USE_ROS
shimizuta 43:2ed84f3558c1 319 printf("Move?\r\n");
shimizuta 44:4aac39b8670b 320 WaitStdin('y'); //キーボード入力されるまでまで待つ
shimizuta 46:621ea6492dea 321 state_from_ros = SANDDUNE; //遠してやらないならやりたいSTATEに変更
shimizuta 46:621ea6492dea 322 #endif
shimizuta 46:621ea6492dea 323 int loopcount = 0;
shimizuta 46:621ea6492dea 324 //プログラムの最初のほうにあるstate_from_rosで動かすものを切り替える.caseはenum ROS_STATEで分ける
shimizuta 39:87dcdff27797 325 while (1)
shimizuta 39:87dcdff27797 326 {
shimizuta 43:2ed84f3558c1 327 switch (state_from_ros)
shimizuta 42:982064594ba6 328 {
shimizuta 43:2ed84f3558c1 329 case STOP:
shimizuta 43:2ed84f3558c1 330 break;
shimizuta 46:621ea6492dea 331 case AREA1_LRFWALK_STATE:
shimizuta 46:621ea6492dea 332 SetWalk(walk, AREA1_LRFWALK);
shimizuta 46:621ea6492dea 333 MoveOneCycle(walk, leg);
shimizuta 46:621ea6492dea 334 break;
shimizuta 46:621ea6492dea 335 case AREA2_LRFWALK_STATE:
shimizuta 46:621ea6492dea 336 SetWalk(walk, AREA2_LRFWALK);
shimizuta 43:2ed84f3558c1 337 MoveOneCycle(walk, leg);
shimizuta 43:2ed84f3558c1 338 break;
shimizuta 43:2ed84f3558c1 339 case SANDDUNE:
shimizuta 45:0654364226c9 340 // #endif
shimizuta 44:4aac39b8670b 341 SetWalk(walk, STANDUP_SANDDUNE);
shimizuta 44:4aac39b8670b 342 MoveOneCycle(walk, leg);
shimizuta 42:982064594ba6 343 //前足を段差にかける
shimizuta 44:4aac39b8670b 344 SetWalk(walk, FRONTLEG_ON_SANDDUNE);
shimizuta 42:982064594ba6 345 for (int i = 0; i < 1; i++)
shimizuta 42:982064594ba6 346 MoveOneCycle(walk, leg);
shimizuta 42:982064594ba6 347 //前足が段差に乗った状態で進む
shimizuta 44:4aac39b8670b 348 SetWalk(walk, OVERCOME);
shimizuta 42:982064594ba6 349 for (int i = 0; i < 5; i++)
shimizuta 42:982064594ba6 350 MoveOneCycle(walk, leg);
shimizuta 42:982064594ba6 351 //後ろ脚載せる
shimizuta 44:4aac39b8670b 352 SetWalk(walk, BACKLEG_ON_SANDDUNE);
shimizuta 42:982064594ba6 353 for (int i = 0; i < 1; i++)
shimizuta 42:982064594ba6 354 MoveOneCycle(walk, leg);
shimizuta 42:982064594ba6 355 //後ろ脚乗った状態で進む
shimizuta 44:4aac39b8670b 356 SetWalk(walk, OVERCOME2);
shimizuta 43:2ed84f3558c1 357 for (int i = 0; i < 3; i++)
shimizuta 42:982064594ba6 358 MoveOneCycle(walk, leg);
shimizuta 45:0654364226c9 359 SetWalk(walk, TRUNLEFT);
shimizuta 45:0654364226c9 360 for (int i = 0; i < 2; i++)
shimizuta 45:0654364226c9 361 MoveOneCycle(walk, leg);
shimizuta 46:621ea6492dea 362 state_from_ros = AREA2_LRFWALK_STATE;
shimizuta 45:0654364226c9 363 // #ifdef USE_ROS
shimizuta 45:0654364226c9 364 break;
shimizuta 45:0654364226c9 365 case ROPE_STATE:
shimizuta 45:0654364226c9 366 SetWalk(walk, ROPE);
shimizuta 45:0654364226c9 367 MoveOneCycle(walk, leg);
shimizuta 45:0654364226c9 368 SetWalk(walk, ROPE_DOWN);
shimizuta 45:0654364226c9 369 MoveOneCycle(walk, leg);
shimizuta 46:621ea6492dea 370 SetWalk(walk, AREA1_LRFWALK);
shimizuta 45:0654364226c9 371 for (int i = 0; i < 3; i++)
shimizuta 45:0654364226c9 372 MoveOneCycle(walk, leg);
shimizuta 45:0654364226c9 373 SetWalk(walk, ROPE_BACK);
shimizuta 45:0654364226c9 374 MoveOneCycle(walk, leg);
shimizuta 46:621ea6492dea 375 state_from_ros = AREA2_LRFWALK_STATE;
shimizuta 45:0654364226c9 376 break;
shimizuta 45:0654364226c9 377 case SLOPE_STATE:
shimizuta 45:0654364226c9 378 SetWalk(walk, SLOPE);
shimizuta 45:0654364226c9 379 for (int i = 0; i < 30; i++)
shimizuta 45:0654364226c9 380 MoveOneCycle(walk, leg);
shimizuta 46:621ea6492dea 381 state_from_ros = STOP;
shimizuta 46:621ea6492dea 382 break;
shimizuta 46:621ea6492dea 383 case TURNRIGHT_STATE:
shimizuta 46:621ea6492dea 384 SetWalk(walk, TRUNRIGHT);
shimizuta 46:621ea6492dea 385 for (int i = 0; i < 5; i++)
shimizuta 46:621ea6492dea 386 MoveOneCycle(walk, leg);
shimizuta 46:621ea6492dea 387 state_from_ros = AREA2_LRFWALK_STATE;
shimizuta 46:621ea6492dea 388 break;
shimizuta 46:621ea6492dea 389 case TRUNLEFT_STATE:
shimizuta 46:621ea6492dea 390 SetWalk(walk, TRUNLEFT);
shimizuta 46:621ea6492dea 391 for (int i = 0; i < 5; i++)
shimizuta 46:621ea6492dea 392 MoveOneCycle(walk, leg);
shimizuta 46:621ea6492dea 393 state_from_ros = AREA2_LRFWALK_STATE;
shimizuta 43:2ed84f3558c1 394 break;
shimizuta 42:982064594ba6 395 }
shimizuta 46:621ea6492dea 396 #ifdef USE_ROS
shimizuta 43:2ed84f3558c1 397 nh_mbed.spinOnce();
shimizuta 46:621ea6492dea 398 #endif
shimizuta 46:621ea6492dea 399 #ifdef VSCODE
shimizuta 46:621ea6492dea 400 ++loopcount;
shimizuta 46:621ea6492dea 401 if (loopcount > 1)
shimizuta 46:621ea6492dea 402 break; //VSCODEでデバックする際は一ループで抜ける
shimizuta 46:621ea6492dea 403 #endif
yuto17320508 41:38d79b6513c0 404 }
shimizuta 34:89d701e15cdf 405 printf("program end\r\n");
shimizuta 29:7d8b8011a88d 406 #ifdef VSCODE
shimizuta 27:79b4b932a6dd 407 fclose(fp);
shimizuta 27:79b4b932a6dd 408 #endif
shimizuta 2:a92568bdeb5c 409 }
shimizuta 27:79b4b932a6dd 410 #ifdef USE_ROS
shimizuta 27:79b4b932a6dd 411 void callback(const geometry_msgs::Vector3 &cmd_vel)
shimizuta 27:79b4b932a6dd 412 {
shimizuta 46:621ea6492dea 413 if (state_from_ros == STOP)
shimizuta 46:621ea6492dea 414 state_from_ros = AREA1_LRFWALK_STATE;
shimizuta 42:982064594ba6 415 stride_m[LEFT_F] = cmd_vel.x;
shimizuta 42:982064594ba6 416 stride_m[LEFT_B] = cmd_vel.x;
shimizuta 42:982064594ba6 417 stride_m[RIGHT_F] = cmd_vel.y;
shimizuta 42:982064594ba6 418 stride_m[RIGHT_B] = cmd_vel.y;
shimizuta 44:4aac39b8670b 419 back_vel = cmd_vel;
shimizuta 44:4aac39b8670b 420 pub_vel.publish(&back_vel);
yuto17320508 47:c2c579909787 421 // led[0] = !led[0];
shimizuta 42:982064594ba6 422 }
shimizuta 43:2ed84f3558c1 423 void callback_state(const std_msgs::Int16 &cmd)
shimizuta 42:982064594ba6 424 {
yuto17320508 47:c2c579909787 425 state_from_ros = (ROS_STATE)cmd.data;
yuto17320508 47:c2c579909787 426 led[state_from_ros % 4] = 1;
shimizuta 27:79b4b932a6dd 427 }
shimizuta 35:b4e1b8f25cd7 428 #endif