岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

main.cpp@48:1aad3cc386e8, 2019-03-08 (annotated)

Committer:
shimizuta
Date:
Fri Mar 08 06:09:31 2019 +0000
Revision:
48:1aad3cc386e8
Parent:
47:c2c579909787
Child:
49:198030e84936
changewalk

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] = {},
shimizuta 48:1aad3cc386e8 102 offset_y_m[4] = {0.3, 0.3, 0.3, 0.3};
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 48:1aad3cc386e8 278 void SmoothChange(Walk &walk, OneLeg leg[4], WalkWay nextway, float time_s);
shimizuta 9:905f93247688 279 int main()
yuto17320508 5:556d5a5e9d24 280 {
shimizuta 44:4aac39b8670b 281 printf("program start\r\n");
shimizuta 29:7d8b8011a88d 282 #ifdef VSCODE
shimizuta 27:79b4b932a6dd 283 if (FileOpen()) //csv fileに書き込み
shimizuta 44:4aac39b8670b 284 return 1; //異常なら強制終了
shimizuta 29:7d8b8011a88d 285 #endif
shimizuta 46:621ea6492dea 286
shimizuta 44:4aac39b8670b 287 /////足の長さ、計算周期の設定
shimizuta 39:87dcdff27797 288 OneLeg leg[4]; //各足の位置
shimizuta 35:b4e1b8f25cd7 289 for (int i = 0; i < 4; i++)
shimizuta 35:b4e1b8f25cd7 290 leg[i] = OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2);
shimizuta 39:87dcdff27797 291 Walk walk(leg); //歩行法はここに入れていく
shimizuta 35:b4e1b8f25cd7 292 Walk::calctime_s_ = 0.03;
shimizuta 44:4aac39b8670b 293 /////事前に軌道を全チェック
shimizuta 42:982064594ba6 294 for (int i = 0; i < CHECK; i++)
shimizuta 42:982064594ba6 295 {
shimizuta 44:4aac39b8670b 296 SetWalk(walk, (WalkWay)i);
shimizuta 44:4aac39b8670b 297 if (walk.CheckOrbit() == 1)
shimizuta 42:982064594ba6 298 {
shimizuta 42:982064594ba6 299 printf("error: move %d\r\n", i);
shimizuta 42:982064594ba6 300 return 1; //強制終了.errorは内部の関数からprintfで知らせる
shimizuta 42:982064594ba6 301 }
shimizuta 42:982064594ba6 302 }
shimizuta 46:621ea6492dea 303 //軌道チェックしてからROS完全起動
shimizuta 46:621ea6492dea 304 #ifdef USE_ROS
shimizuta 46:621ea6492dea 305 nh_mbed.getHardware()->setBaud(115200);
shimizuta 46:621ea6492dea 306 nh_mbed.initNode();
shimizuta 46:621ea6492dea 307 nh_mbed.subscribe(sub_vel);
shimizuta 46:621ea6492dea 308 nh_mbed.subscribe(sub_state);
shimizuta 46:621ea6492dea 309 nh_mbed.advertise(pub_vel);
shimizuta 46:621ea6492dea 310 nh_mbed.spinOnce(); //一度受信
shimizuta 46:621ea6492dea 311 #else
shimizuta 35:b4e1b8f25cd7 312 printf("Stand up?\r\n");
shimizuta 44:4aac39b8670b 313 WaitStdin('y'); // キーボード入力されるまでまで待つ
shimizuta 43:2ed84f3558c1 314 #endif
shimizuta 44:4aac39b8670b 315 /////立つ
shimizuta 48:1aad3cc386e8 316 SetWalk(walk, STANDUP);
shimizuta 48:1aad3cc386e8 317 // SetWalk(walk, AFTER_OVERCOME); //段差越え後調整用に一時的に入れた
shimizuta 43:2ed84f3558c1 318 MoveOneCycle(walk, leg);
shimizuta 43:2ed84f3558c1 319 #ifndef USE_ROS
shimizuta 43:2ed84f3558c1 320 printf("Move?\r\n");
shimizuta 48:1aad3cc386e8 321 WaitStdin('y'); //キーボード入力されるまでまで待つ
shimizuta 46:621ea6492dea 322 state_from_ros = SANDDUNE; //遠してやらないならやりたいSTATEに変更
shimizuta 46:621ea6492dea 323 #endif
shimizuta 48:1aad3cc386e8 324 int loopcount = 0;
shimizuta 48:1aad3cc386e8 325 ROS_STATE old_state = state_from_ros;
shimizuta 46:621ea6492dea 326 //プログラムの最初のほうにあるstate_from_rosで動かすものを切り替える.caseはenum ROS_STATEで分ける
shimizuta 39:87dcdff27797 327 while (1)
shimizuta 39:87dcdff27797 328 {
shimizuta 43:2ed84f3558c1 329 switch (state_from_ros)
shimizuta 42:982064594ba6 330 {
shimizuta 43:2ed84f3558c1 331 case STOP:
shimizuta 43:2ed84f3558c1 332 break;
shimizuta 46:621ea6492dea 333 case AREA1_LRFWALK_STATE:
shimizuta 48:1aad3cc386e8 334 if (old_state != state_from_ros)
shimizuta 48:1aad3cc386e8 335 SmoothChange(walk, leg, AREA1_LRFWALK, 0.5); //切り替え
shimizuta 48:1aad3cc386e8 336 else
shimizuta 48:1aad3cc386e8 337 SetWalk(walk, AREA1_LRFWALK);
shimizuta 46:621ea6492dea 338 MoveOneCycle(walk, leg);
shimizuta 46:621ea6492dea 339 break;
shimizuta 46:621ea6492dea 340 case AREA2_LRFWALK_STATE:
shimizuta 48:1aad3cc386e8 341 if (old_state != state_from_ros)
shimizuta 48:1aad3cc386e8 342 SmoothChange(walk, leg, AREA2_LRFWALK, 0.5); //切り替え
shimizuta 48:1aad3cc386e8 343 else
shimizuta 48:1aad3cc386e8 344 SetWalk(walk, AREA2_LRFWALK);
shimizuta 43:2ed84f3558c1 345 MoveOneCycle(walk, leg);
shimizuta 43:2ed84f3558c1 346 break;
shimizuta 43:2ed84f3558c1 347 case SANDDUNE:
shimizuta 48:1aad3cc386e8 348 //前足を段差にかける
shimizuta 48:1aad3cc386e8 349 SmoothChange(walk, leg, FRONTLEG_ON_SANDDUNE, 0.5); //切り替えスムーズ
shimizuta 44:4aac39b8670b 350 MoveOneCycle(walk, leg);
shimizuta 42:982064594ba6 351 //前足が段差に乗った状態で進む
shimizuta 48:1aad3cc386e8 352 SmoothChange(walk, leg, OVERCOME, 0.5); //切り替えスムーズ
shimizuta 42:982064594ba6 353 for (int i = 0; i < 5; i++)
shimizuta 42:982064594ba6 354 MoveOneCycle(walk, leg);
shimizuta 42:982064594ba6 355 //後ろ脚載せる
shimizuta 48:1aad3cc386e8 356 SmoothChange(walk, leg, BACKLEG_ON_SANDDUNE, 0.5); //切り替えスムーズ
shimizuta 42:982064594ba6 357 for (int i = 0; i < 1; i++)
shimizuta 42:982064594ba6 358 MoveOneCycle(walk, leg);
shimizuta 42:982064594ba6 359 //後ろ脚乗った状態で進む
shimizuta 48:1aad3cc386e8 360 SmoothChange(walk ,leg, OVERCOME2, 0.5); //切り替えスムーズ
shimizuta 43:2ed84f3558c1 361 for (int i = 0; i < 3; i++)
shimizuta 42:982064594ba6 362 MoveOneCycle(walk, leg);
shimizuta 48:1aad3cc386e8 363 //数歩左に曲がる
shimizuta 48:1aad3cc386e8 364 SmoothChange(walk, leg, TRUNLEFT, 0.5); //切り替えスムーズ
shimizuta 45:0654364226c9 365 for (int i = 0; i < 2; i++)
shimizuta 45:0654364226c9 366 MoveOneCycle(walk, leg);
shimizuta 46:621ea6492dea 367 state_from_ros = AREA2_LRFWALK_STATE;
shimizuta 45:0654364226c9 368 break;
shimizuta 45:0654364226c9 369 case ROPE_STATE:
shimizuta 48:1aad3cc386e8 370 //前足だけ紐越え
shimizuta 48:1aad3cc386e8 371 SmoothChange(walk, leg, ROPE, 0.5);
shimizuta 45:0654364226c9 372 MoveOneCycle(walk, leg);
shimizuta 48:1aad3cc386e8 373 //LRFが取れる高さで歩行
shimizuta 48:1aad3cc386e8 374 SmoothChange(walk, leg, AREA1_LRFWALK, 0.5);
shimizuta 45:0654364226c9 375 for (int i = 0; i < 3; i++)
shimizuta 45:0654364226c9 376 MoveOneCycle(walk, leg);
shimizuta 48:1aad3cc386e8 377 //後ろ足越え
shimizuta 48:1aad3cc386e8 378 SmoothChange(walk,leg, ROPE_BACK, 0.5);
shimizuta 45:0654364226c9 379 MoveOneCycle(walk, leg);
shimizuta 46:621ea6492dea 380 state_from_ros = AREA2_LRFWALK_STATE;
shimizuta 45:0654364226c9 381 break;
shimizuta 45:0654364226c9 382 case SLOPE_STATE:
shimizuta 48:1aad3cc386e8 383 SmoothChange(walk, leg, SLOPE, 0.5);
shimizuta 45:0654364226c9 384 for (int i = 0; i < 30; i++)
shimizuta 45:0654364226c9 385 MoveOneCycle(walk, leg);
shimizuta 46:621ea6492dea 386 state_from_ros = STOP;
shimizuta 46:621ea6492dea 387 break;
shimizuta 46:621ea6492dea 388 case TURNRIGHT_STATE:
shimizuta 48:1aad3cc386e8 389 SmoothChange(walk, leg, TRUNRIGHT, 0.5);
shimizuta 46:621ea6492dea 390 for (int i = 0; i < 5; i++)
shimizuta 46:621ea6492dea 391 MoveOneCycle(walk, leg);
shimizuta 46:621ea6492dea 392 state_from_ros = AREA2_LRFWALK_STATE;
shimizuta 46:621ea6492dea 393 break;
shimizuta 46:621ea6492dea 394 case TRUNLEFT_STATE:
shimizuta 48:1aad3cc386e8 395 SmoothChange(walk, leg, TRUNLEFT, 0.5);
shimizuta 46:621ea6492dea 396 for (int i = 0; i < 5; i++)
shimizuta 46:621ea6492dea 397 MoveOneCycle(walk, leg);
shimizuta 46:621ea6492dea 398 state_from_ros = AREA2_LRFWALK_STATE;
shimizuta 43:2ed84f3558c1 399 break;
shimizuta 42:982064594ba6 400 }
shimizuta 48:1aad3cc386e8 401 old_state = state_from_ros;
shimizuta 46:621ea6492dea 402 #ifdef USE_ROS
shimizuta 43:2ed84f3558c1 403 nh_mbed.spinOnce();
shimizuta 46:621ea6492dea 404 #endif
shimizuta 46:621ea6492dea 405 #ifdef VSCODE
shimizuta 46:621ea6492dea 406 ++loopcount;
shimizuta 46:621ea6492dea 407 if (loopcount > 1)
shimizuta 46:621ea6492dea 408 break; //VSCODEでデバックする際は一ループで抜ける
shimizuta 46:621ea6492dea 409 #endif
yuto17320508 41:38d79b6513c0 410 }
shimizuta 34:89d701e15cdf 411 printf("program end\r\n");
shimizuta 29:7d8b8011a88d 412 #ifdef VSCODE
shimizuta 27:79b4b932a6dd 413 fclose(fp);
shimizuta 27:79b4b932a6dd 414 #endif
shimizuta 2:a92568bdeb5c 415 }
shimizuta 27:79b4b932a6dd 416 #ifdef USE_ROS
shimizuta 27:79b4b932a6dd 417 void callback(const geometry_msgs::Vector3 &cmd_vel)
shimizuta 27:79b4b932a6dd 418 {
shimizuta 46:621ea6492dea 419 if (state_from_ros == STOP)
shimizuta 46:621ea6492dea 420 state_from_ros = AREA1_LRFWALK_STATE;
shimizuta 42:982064594ba6 421 stride_m[LEFT_F] = cmd_vel.x;
shimizuta 42:982064594ba6 422 stride_m[LEFT_B] = cmd_vel.x;
shimizuta 42:982064594ba6 423 stride_m[RIGHT_F] = cmd_vel.y;
shimizuta 42:982064594ba6 424 stride_m[RIGHT_B] = cmd_vel.y;
shimizuta 44:4aac39b8670b 425 back_vel = cmd_vel;
shimizuta 44:4aac39b8670b 426 pub_vel.publish(&back_vel);
shimizuta 42:982064594ba6 427 }
shimizuta 43:2ed84f3558c1 428 void callback_state(const std_msgs::Int16 &cmd)
shimizuta 42:982064594ba6 429 {
yuto17320508 47:c2c579909787 430 state_from_ros = (ROS_STATE)cmd.data;
yuto17320508 47:c2c579909787 431 led[state_from_ros % 4] = 1;
shimizuta 27:79b4b932a6dd 432 }
shimizuta 48:1aad3cc386e8 433 #endif
shimizuta 48:1aad3cc386e8 434 void SmoothChange(Walk &walk, OneLeg leg[4], WalkWay nextway, float time_s)
shimizuta 48:1aad3cc386e8 435 {
shimizuta 48:1aad3cc386e8 436 LineParam lines[4][2];
shimizuta 48:1aad3cc386e8 437 //前回位置を保存
shimizuta 48:1aad3cc386e8 438 for (size_t i = 0; i < sizeof(lines) / sizeof(lines[0]); i++)
shimizuta 48:1aad3cc386e8 439 {
shimizuta 48:1aad3cc386e8 440 lines[i][0].time_s = 0;
shimizuta 48:1aad3cc386e8 441 lines[i][0].x_m = leg[i].GetX_m();
shimizuta 48:1aad3cc386e8 442 lines[i][0].y_m = leg[i].GetY_m();
shimizuta 48:1aad3cc386e8 443 }
shimizuta 48:1aad3cc386e8 444 Walk tempwalk(leg);
shimizuta 48:1aad3cc386e8 445 SetWalk(tempwalk, nextway);
shimizuta 48:1aad3cc386e8 446 tempwalk.Cal4LegsPosi(tempwalk.leg); //次の一歩目をwalk.legに書き込む
shimizuta 48:1aad3cc386e8 447 for (size_t i = 0; i < sizeof(lines) / sizeof(lines[0]); i++)
shimizuta 48:1aad3cc386e8 448 {
shimizuta 48:1aad3cc386e8 449 lines[i][1].time_s = time_s;
shimizuta 48:1aad3cc386e8 450 lines[i][1].x_m = tempwalk.leg[i].GetX_m();
shimizuta 48:1aad3cc386e8 451 lines[i][1].y_m = tempwalk.leg[i].GetY_m();
shimizuta 48:1aad3cc386e8 452 lines[i][1].is_point_to_point = 0;
shimizuta 48:1aad3cc386e8 453 }
shimizuta 48:1aad3cc386e8 454 //前回終点から次回始点まで直線移動
shimizuta 48:1aad3cc386e8 455 for (int i = 0; i < 4; i++)
shimizuta 48:1aad3cc386e8 456 SetOneLegFreeLinesParam(tempwalk, i, lines[i], sizeof(lines[i]) / sizeof(lines[i][0]));
shimizuta 48:1aad3cc386e8 457 tempwalk.ResetPhase();
shimizuta 48:1aad3cc386e8 458 MoveOneCycle(tempwalk, leg);
shimizuta 48:1aad3cc386e8 459 SetWalk(walk, nextway);
shimizuta 48:1aad3cc386e8 460 };