岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

main.cpp@50:36741e8ab197, 2019-03-11 (annotated)

Committer:
shimizuta
Date:
Mon Mar 11 10:38:07 2019 +0000
Revision:
50:36741e8ab197
Parent:
49:198030e84936 
a

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 49:198030e84936 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 45:0654364226c9 34 ROPE_STATE,
shimizuta 45:0654364226c9 35 SLOPE_STATE,
shimizuta 43:2ed84f3558c1 36 };
shimizuta 46:621ea6492dea 37 ROS_STATE state_from_ros = STOP; //ここを変えると動く方法が変わる
shimizuta 43:2ed84f3558c1 38 #ifdef USE_ROS
shimizuta 43:2ed84f3558c1 39 ros::NodeHandle nh_mbed;
shimizuta 43:2ed84f3558c1 40 void callback(const geometry_msgs::Vector3 &cmd_vel);
shimizuta 43:2ed84f3558c1 41 void callback_state(const std_msgs::Int16 &cmd_vel);
shimizuta 44:4aac39b8670b 42 geometry_msgs::Vector3 back_vel;
shimizuta 43:2ed84f3558c1 43 ros::Subscriber<geometry_msgs::Vector3> sub_vel("/cmd_vel", &callback);
shimizuta 43:2ed84f3558c1 44 ros::Subscriber<std_msgs::Int16> sub_state("/state", &callback_state);
shimizuta 44:4aac39b8670b 45 ros::Publisher pub_vel("/back_vel", &back_vel);
shimizuta 43:2ed84f3558c1 46 #endif
shimizuta 43:2ed84f3558c1 47 ////////////あまり変化させないパラメーター
shimizuta 35:b4e1b8f25cd7 48 const float kBetweenServoHalf_m = 0.034 * 0.5; //サーボ間の距離の半分
shimizuta 35:b4e1b8f25cd7 49 float kLegLength1[2] = {0.15, 0.15};
shimizuta 35:b4e1b8f25cd7 50 float kLegLength2[2] = {0.33, 0.34};
shimizuta 9:905f93247688 51 ///////////////
shimizuta 27:79b4b932a6dd 52 ////////調整するべきパラメータ
shimizuta 44:4aac39b8670b 53 enum WalkWay //歩行軌道
shimizuta 27:79b4b932a6dd 54 {
shimizuta 43:2ed84f3558c1 55 STANDUP, //受け渡し用に待つ
shimizuta 46:621ea6492dea 56 AREA1_LRFWALK, //段差までLRFありで歩く.元LRFPOSTURE
shimizuta 46:621ea6492dea 57 AREA2_LRFWALK, //段差後からLRFありで歩く
shimizuta 43:2ed84f3558c1 58 STANDUP_SANDDUNE, //段差前で立つ
shimizuta 42:982064594ba6 59 FRONTLEG_ON_SANDDUNE, //前足を段差にかける
shimizuta 42:982064594ba6 60 OVERCOME, //前足が乗った状態で進む
shimizuta 49:198030e84936 61 FALL_FRONTLEG, //前足を段差から降ろす
shimizuta 42:982064594ba6 62 BACKLEG_ON_SANDDUNE, //後ろ脚を段差に載せる
shimizuta 42:982064594ba6 63 OVERCOME2, //後ろ脚が乗った状態で進む
shimizuta 45:0654364226c9 64 AFTER_OVERCOME, //段差を終了したら座って指示まち
shimizuta 50:36741e8ab197 65 WALK_BEFORE_ROPE, //紐前で歩く
shimizuta 43:2ed84f3558c1 66 ROPE, //rope前足超える
shimizuta 43:2ed84f3558c1 67 ROPE_BACK, //rope後ろ足超える
shimizuta 43:2ed84f3558c1 68 SLOPE,
shimizuta 45:0654364226c9 69 TRUNRIGHT,
shimizuta 45:0654364226c9 70 TRUNLEFT,
shimizuta 43:2ed84f3558c1 71 CHECK, //check用に最後に置いておく
shimizuta 27:79b4b932a6dd 72 };
shimizuta 46:621ea6492dea 73 //LRF使うやつだけだけspinOnceで変更するためグローバルで
shimizuta 43:2ed84f3558c1 74 float stride_m[4] = {0.2, 0.2, 0.2, 0.2}; //ropeのときは0.15だった
shimizuta 44:4aac39b8670b 75 //各歩行軌道のパラメータ設定。switchで羅列している。新規に歩行パターンを増やすときはcaseを増やす。
shimizuta 44:4aac39b8670b 76 //param walk:結果を入れる箱。way:歩行軌道を指定するWalkWayの定数。
shimizuta 44:4aac39b8670b 77 int SetWalk(Walk &walk, WalkWay way)
shimizuta 42:982064594ba6 78 {
shimizuta 46:621ea6492dea 79 //複数歩行に共有するパラメータはここに書く。それ以外はcase内に入れる。
shimizuta 46:621ea6492dea 80 //LRFを使って歩行する際のパラメータ.stride_mだけグローバルにある。
shimizuta 49:198030e84936 81 float area1_offset_y_m[4] = {0.3, 0.3, 0.3, 0.3};
yuto17320508 47:c2c579909787 82 float area2_offset_y_m[4] = {0.27, 0.27, 0.27, 0.27};
shimizuta 49:198030e84936 83 float offset_x_m[4] = {0, 0, 0, 0};
shimizuta 46:621ea6492dea 84 float height_m[4] = {0.05, 0.05, 0.05, 0.05}, buffer_height_m = 0.02,
shimizuta 49:198030e84936 85 stridetime_s = 0.45, toptime_s = 0.2, buffer_time_s = 0.1;
shimizuta 46:621ea6492dea 86
shimizuta 46:621ea6492dea 87 //段差
shimizuta 49:198030e84936 88 float overcome_start_y_m[] = {0.38, 0.38, 0.38, 0.38};
shimizuta 49:198030e84936 89 float flontleg_sand_goal_y_m[4] = {0.38, 0.27, 0.38, 0.27};
shimizuta 49:198030e84936 90 float backleg_sand_goal_y_m[4] = {0.27, 0.38, 0.27, 0.38};
shimizuta 50:36741e8ab197 91 float d_time = 0.2, d_time_slow = 0.3;
shimizuta 46:621ea6492dea 92 //旋回
shimizuta 45:0654364226c9 93 float turn_start_x_m = 0, turn_start_y_m = 0.275,
shimizuta 50:36741e8ab197 94 turn_stride_m = 0.075, turn_height_m = 0.05,
shimizuta 50:36741e8ab197 95 turn_stridetime_s = 0.5, turn_risetime_s = 0.2;
shimizuta 50:36741e8ab197 96 float d_time_rope = 0.6, d_time_slow_rope = 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 49:198030e84936 110 SetOneLegFourPointParam(walk, i, offset_x_m[i], area1_offset_y_m[i],
shimizuta 49:198030e84936 111 stride_m[i], height_m[i], buffer_height_m,
shimizuta 49:198030e84936 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 STANDUP_SANDDUNE:
shimizuta 42:982064594ba6 123 {
shimizuta 43:2ed84f3558c1 124 for (int i = 0; i < 4; i++)
shimizuta 43:2ed84f3558c1 125 {
shimizuta 43:2ed84f3558c1 126 LineParam lines[] = {
shimizuta 43:2ed84f3558c1 127 {.time_s = 0, .x_m = walk.leg[i].GetX_m(), .y_m = walk.leg[i].GetY_m(), .is_point_to_point = 0},
shimizuta 49:198030e84936 128 {.time_s = 0.5, .x_m = offset_x_m[i], .y_m = overcome_start_y_m[i], .is_point_to_point = 0},
shimizuta 49:198030e84936 129 {.time_s = 1.5, .x_m = offset_x_m[i], .y_m = overcome_start_y_m[i], .is_point_to_point = 0},
shimizuta 43:2ed84f3558c1 130 };
shimizuta 43:2ed84f3558c1 131 SetOneLegFreeLinesParam(walk, i, lines, sizeof(lines) / sizeof(lines[0]));
shimizuta 43:2ed84f3558c1 132 }
shimizuta 43:2ed84f3558c1 133 walk.SetOffsetTime(0, 0, 0, 0);
shimizuta 44:4aac39b8670b 134 break;
shimizuta 43:2ed84f3558c1 135 }
shimizuta 43:2ed84f3558c1 136 case FRONTLEG_ON_SANDDUNE:
shimizuta 43:2ed84f3558c1 137 { //前足を段差にかける
shimizuta 42:982064594ba6 138 float d_x_m = 0.1;
shimizuta 43:2ed84f3558c1 139 float raise_offset_x_m[4] = {0, -0.02, 0, 0};
shimizuta 49:198030e84936 140 float overcome_height_m[] = {0.05, 0.15, 0.05, 0.15};
shimizuta 42:982064594ba6 141 float gravity_dist[] = {0.05, 0, 0.05, -0.05};
shimizuta 49:198030e84936 142 OverCome overcome(offset_x_m, overcome_start_y_m, d_x_m, flontleg_sand_goal_y_m,
shimizuta 50:36741e8ab197 143 overcome_height_m, gravity_dist, walk.leg,
shimizuta 50:36741e8ab197 144 raise_offset_x_m, d_time, d_time_slow);
shimizuta 49:198030e84936 145 walk.Copy(overcome.walk);
shimizuta 49:198030e84936 146 break;
shimizuta 49:198030e84936 147 }
shimizuta 49:198030e84936 148 case OVERCOME:
shimizuta 49:198030e84936 149 { //前足が乗った状態で進む
shimizuta 49:198030e84936 150 float stride_m[4] = {0.07, 0.07, 0.07, 0.07};
shimizuta 49:198030e84936 151 for (int i = 0; i < 4; i++)
shimizuta 49:198030e84936 152 SetOneLegFourPointParam(walk, i, offset_x_m[i], flontleg_sand_goal_y_m[i],
shimizuta 49:198030e84936 153 stride_m[i], height_m[i], buffer_height_m,
shimizuta 49:198030e84936 154 stridetime_s, toptime_s, buffer_time_s);
shimizuta 49:198030e84936 155 walk.SetOffsetTime(0, 0.5, 0.5, 0);
shimizuta 49:198030e84936 156 break;
shimizuta 49:198030e84936 157 }
shimizuta 49:198030e84936 158 case FALL_FRONTLEG: //前足を段差から降ろす
shimizuta 49:198030e84936 159 {
shimizuta 49:198030e84936 160 float d_x_m = 0.15;
shimizuta 49:198030e84936 161 float start_x_m[4] = {0, 0, 0, 0};
shimizuta 49:198030e84936 162 float raise_offset_x_m[4] = {};
shimizuta 49:198030e84936 163 float overcome_height_m[] = {0.05, 0.05, 0.05, 0.05};
shimizuta 49:198030e84936 164 float gravity_dist[] = {0.1, 0, 0.1, 0};
shimizuta 49:198030e84936 165 OverCome overcome(start_x_m, flontleg_sand_goal_y_m, d_x_m, overcome_start_y_m,
shimizuta 50:36741e8ab197 166 overcome_height_m, gravity_dist, walk.leg,
shimizuta 50:36741e8ab197 167 raise_offset_x_m, d_time, d_time_slow);
shimizuta 35:b4e1b8f25cd7 168 walk.Copy(overcome.walk);
shimizuta 42:982064594ba6 169 break;
shimizuta 39:87dcdff27797 170 }
shimizuta 43:2ed84f3558c1 171 case BACKLEG_ON_SANDDUNE:
shimizuta 43:2ed84f3558c1 172 { //後ろ脚を乗せる
shimizuta 49:198030e84936 173 float d_x_m = 0.15;
shimizuta 43:2ed84f3558c1 174 float start_x_m[4] = {0, 0, 0, 0};
shimizuta 42:982064594ba6 175 float start_y_m[4] = {0.41, 0.41, 0.41, 0.41};
shimizuta 49:198030e84936 176
shimizuta 43:2ed84f3558c1 177 float raise_offset_x_m[4] = {};
shimizuta 42:982064594ba6 178 float overcome_height_m[] = {0.2, 0.1, 0.2, 0.1};
shimizuta 43:2ed84f3558c1 179 float gravity_dist[] = {0.1, 0, 0.1, 0};
shimizuta 49:198030e84936 180 OverCome overcome(start_x_m, start_y_m, d_x_m, backleg_sand_goal_y_m,
shimizuta 50:36741e8ab197 181 overcome_height_m, gravity_dist, walk.leg,
shimizuta 50:36741e8ab197 182 raise_offset_x_m, d_time, d_time_slow);
shimizuta 42:982064594ba6 183 walk.Copy(overcome.walk);
shimizuta 42:982064594ba6 184 break;
shimizuta 42:982064594ba6 185 }
shimizuta 43:2ed84f3558c1 186 case OVERCOME2:
shimizuta 43:2ed84f3558c1 187 { //後ろ脚が乗った状態で進む
shimizuta 50:36741e8ab197 188 float offset_x_m[4] = {0.1, 0, 0.1, 0};
shimizuta 49:198030e84936 189 float offset_y_m[4] = {0.27, 0.33, 0.27, 0.33};
shimizuta 42:982064594ba6 190 float stride_m[4] = {0.2, 0.2, 0.2, 0.2},
shimizuta 42:982064594ba6 191 height_m[4] = {0.1, 0.1, 0.1, 0.1}, buffer_height_m = 0.05,
shimizuta 42:982064594ba6 192 stridetime_s = 1, toptime_s = 0.2, buffer_time_s = 0.2;
shimizuta 42:982064594ba6 193 for (int i = 0; i < 4; i++)
shimizuta 42:982064594ba6 194 SetOneLegFourPointParam(walk, i, offset_x_m[i], offset_y_m[i],
shimizuta 42:982064594ba6 195 stride_m[i], height_m[i], buffer_height_m,
shimizuta 42:982064594ba6 196 stridetime_s, toptime_s, buffer_time_s);
shimizuta 42:982064594ba6 197 walk.SetOffsetTime(0, 0.5, 0.5, 0);
shimizuta 42:982064594ba6 198 break;
shimizuta 42:982064594ba6 199 }
shimizuta 44:4aac39b8670b 200 case AFTER_OVERCOME:
shimizuta 43:2ed84f3558c1 201 {
shimizuta 44:4aac39b8670b 202 for (int i = 0; i < 4; i++)
shimizuta 46:621ea6492dea 203 SetOneLegStandParam(walk, i, offset_x_m[i], area1_offset_y_m[i], 0.5);
shimizuta 44:4aac39b8670b 204 walk.SetOffsetTime(0, 0, 0, 0);
shimizuta 44:4aac39b8670b 205 break;
shimizuta 44:4aac39b8670b 206 }
shimizuta 50:36741e8ab197 207 case WALK_BEFORE_ROPE:
shimizuta 50:36741e8ab197 208 {
shimizuta 50:36741e8ab197 209 // float stride_m[] = {0.2, 0.2, 0.125, 0.125};
shimizuta 50:36741e8ab197 210 float stride_m[] = {0.075, 0.075, 0.2, 0.2};
shimizuta 50:36741e8ab197 211 for (int i = 0; i < 4; i++)
shimizuta 50:36741e8ab197 212 SetOneLegFourPointParam(walk, i, offset_x_m[i], area1_offset_y_m[i],
shimizuta 50:36741e8ab197 213 stride_m[i], height_m[i], buffer_height_m,
shimizuta 50:36741e8ab197 214 stridetime_s, toptime_s, buffer_time_s);
shimizuta 50:36741e8ab197 215 walk.SetOffsetTime(0, 0.5, 0.5, 0);
shimizuta 50:36741e8ab197 216 break;
shimizuta 50:36741e8ab197 217 }
shimizuta 45:0654364226c9 218 case ROPE:
shimizuta 44:4aac39b8670b 219 {
shimizuta 43:2ed84f3558c1 220 float offset_x_m[4] = {-0.05, 0, -0.05, 0};
shimizuta 43:2ed84f3558c1 221 float d_x_m = 0.2;
shimizuta 43:2ed84f3558c1 222 float start_y_m[4] = {0.41, 0.41, 0.41, 0.41};
shimizuta 43:2ed84f3558c1 223 float goal_y_m[4] = {0.41, 0.41, 0.41, 0.41};
shimizuta 50:36741e8ab197 224 float raise_offset_x_m[] = {0, -0.15, 0, -0.15};
shimizuta 43:2ed84f3558c1 225 float overcome_height_m[] = {0.05, 0.2, 0.05, 0.2};
shimizuta 43:2ed84f3558c1 226 float gravity_dist[] = {0.05, -0.05, 0.05, -0.05};
shimizuta 43:2ed84f3558c1 227 OverCome overcome(offset_x_m, start_y_m, d_x_m, goal_y_m,
shimizuta 50:36741e8ab197 228 overcome_height_m, gravity_dist, walk.leg,
shimizuta 50:36741e8ab197 229 raise_offset_x_m, d_time_rope, d_time_slow_rope);
shimizuta 43:2ed84f3558c1 230 walk.Copy(overcome.walk);
shimizuta 44:4aac39b8670b 231 break;
shimizuta 43:2ed84f3558c1 232 }
shimizuta 43:2ed84f3558c1 233 case ROPE_BACK:
shimizuta 43:2ed84f3558c1 234 {
shimizuta 43:2ed84f3558c1 235 float offset_x_m[4] = {};
shimizuta 50:36741e8ab197 236 float d_x_m = 0.2;
shimizuta 50:36741e8ab197 237 float start_y_m[4] = {0.41, 0.38, 0.41, 0.38};
shimizuta 50:36741e8ab197 238 float goal_y_m[4] = {0.41, 0.38, 0.41, 0.38};
shimizuta 50:36741e8ab197 239 float raise_offset_x_m[] = {-0.1, 0, -0.1, 0};
shimizuta 43:2ed84f3558c1 240 float overcome_height_m[] = {0.2, 0.05, 0.2, 0.05};
shimizuta 50:36741e8ab197 241 float gravity_dist[] = {0.25, -0.05, 0.05, -0.05};
shimizuta 43:2ed84f3558c1 242 OverCome overcome(offset_x_m, start_y_m, d_x_m, goal_y_m,
shimizuta 50:36741e8ab197 243 overcome_height_m, gravity_dist, walk.leg,
shimizuta 50:36741e8ab197 244 raise_offset_x_m, d_time_rope, d_time_slow_rope);
shimizuta 43:2ed84f3558c1 245 walk.Copy(overcome.walk);
shimizuta 44:4aac39b8670b 246 break;
shimizuta 43:2ed84f3558c1 247 }
shimizuta 43:2ed84f3558c1 248 case SLOPE:
shimizuta 43:2ed84f3558c1 249 {
shimizuta 43:2ed84f3558c1 250 float offset_x_m[4] = {-0.15, 0.15, -0.15, 0.15},
shimizuta 43:2ed84f3558c1 251 offset_y_m[4] = {0.32, 0.22, 0.32, 0.22};
shimizuta 43:2ed84f3558c1 252 float stride_m[4] = {0.18, 0.18, 0.2, 0.2}; //ropeのときは0.15
shimizuta 43:2ed84f3558c1 253 float height_m[4] = {0.05, 0.05, 0.05, 0.05}, buffer_height_m = 0.02,
shimizuta 49:198030e84936 254 stridetime_s = 1, toptime_s = 0.15, buffer_time_s = 0.05;
shimizuta 43:2ed84f3558c1 255 for (int i = 0; i < 4; i++)
shimizuta 43:2ed84f3558c1 256 SetOneLegTriangleParam(walk, i, offset_x_m[i], offset_y_m[i],
shimizuta 43:2ed84f3558c1 257 stride_m[i], height_m[i], buffer_height_m,
shimizuta 43:2ed84f3558c1 258 stridetime_s, toptime_s, buffer_time_s);
shimizuta 43:2ed84f3558c1 259 walk.SetOffsetTime(0, 0.5, 0.5, 0);
shimizuta 44:4aac39b8670b 260 break;
shimizuta 43:2ed84f3558c1 261 }
shimizuta 45:0654364226c9 262 case TRUNRIGHT:
shimizuta 45:0654364226c9 263 Turn(walk, 1, turn_start_x_m, turn_start_y_m, turn_stride_m,
shimizuta 45:0654364226c9 264 turn_height_m, turn_stridetime_s, turn_risetime_s);
shimizuta 45:0654364226c9 265 break;
shimizuta 45:0654364226c9 266 case TRUNLEFT:
shimizuta 45:0654364226c9 267 Turn(walk, 0, turn_start_x_m, turn_start_y_m, turn_stride_m,
shimizuta 45:0654364226c9 268 turn_height_m, turn_stridetime_s, turn_risetime_s);
shimizuta 45:0654364226c9 269 break;
shimizuta 42:982064594ba6 270 default:
shimizuta 42:982064594ba6 271 printf("error: there is no WalkWay\r\n");
shimizuta 44:4aac39b8670b 272 return 1; //以上終了
shimizuta 42:982064594ba6 273 }
shimizuta 44:4aac39b8670b 274 walk.ResetPhase();
shimizuta 44:4aac39b8670b 275 return 0; //正常終了
yuto17320508 18:0033ef1814ba 276 }
shimizuta 48:1aad3cc386e8 277 void SmoothChange(Walk &walk, OneLeg leg[4], WalkWay nextway, float time_s);
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 43:2ed84f3558c1 310 #endif
shimizuta 50:36741e8ab197 311
shimizuta 44:4aac39b8670b 312 /////立つ
shimizuta 50:36741e8ab197 313 state_from_ros = ROPE_STATE;
shimizuta 50:36741e8ab197 314 switch (state_from_ros)
shimizuta 50:36741e8ab197 315 {
shimizuta 50:36741e8ab197 316 case SANDDUNE:
shimizuta 50:36741e8ab197 317 SetWalk(walk, STANDUP_SANDDUNE);
shimizuta 50:36741e8ab197 318 break;
shimizuta 50:36741e8ab197 319 default:
shimizuta 50:36741e8ab197 320 SetWalk(walk, STANDUP);
shimizuta 50:36741e8ab197 321 break;
shimizuta 50:36741e8ab197 322 }
shimizuta 50:36741e8ab197 323 printf("Stand up?\r\n");
shimizuta 50:36741e8ab197 324 WaitStdin('y'); // キーボード入力されるまでまで待つ
shimizuta 50:36741e8ab197 325 MoveOneCycle(walk, leg);
shimizuta 50:36741e8ab197 326 printf("Move?\r\n");
shimizuta 50:36741e8ab197 327 WaitStdin('y');
shimizuta 46:621ea6492dea 328 //プログラムの最初のほうにあるstate_from_rosで動かすものを切り替える.caseはenum ROS_STATEで分ける
shimizuta 49:198030e84936 329 switch (state_from_ros)
shimizuta 39:87dcdff27797 330 {
shimizuta 49:198030e84936 331 case STOP:
shimizuta 49:198030e84936 332 break;
shimizuta 50:36741e8ab197 333 case AREA1_LRFWALK_STATE: //キーボード入力されるまでまで待つ
shimizuta 49:198030e84936 334 SmoothChange(walk, leg, AREA1_LRFWALK, 0.5); //切り替え
shimizuta 49:198030e84936 335 for (int i = 0; i < 20; i++)
shimizuta 46:621ea6492dea 336 MoveOneCycle(walk, leg);
shimizuta 49:198030e84936 337 break;
shimizuta 49:198030e84936 338 case SANDDUNE:
shimizuta 50:36741e8ab197 339 //前足を段差にかける //キーボード入力されるまでまで待つ
shimizuta 49:198030e84936 340 SmoothChange(walk, leg, FRONTLEG_ON_SANDDUNE, 0.5); //切り替えスムーズ
shimizuta 49:198030e84936 341 MoveOneCycle(walk, leg);
shimizuta 49:198030e84936 342 //前足が段差に乗った状態で進む
shimizuta 49:198030e84936 343 SmoothChange(walk, leg, OVERCOME, 0.5); //切り替えスムーズ
shimizuta 49:198030e84936 344 for (int i = 0; i < 2; i++)
shimizuta 44:4aac39b8670b 345 MoveOneCycle(walk, leg);
shimizuta 50:36741e8ab197 346 //前足を降ろす
shimizuta 49:198030e84936 347 SmoothChange(walk, leg, FALL_FRONTLEG, 0.5);
shimizuta 49:198030e84936 348 MoveOneCycle(walk, leg);
shimizuta 49:198030e84936 349 //またいだ状態で歩く
shimizuta 49:198030e84936 350 SmoothChange(walk, leg, AREA1_LRFWALK, 0.5);
shimizuta 49:198030e84936 351 MoveOneCycle(walk, leg);
shimizuta 49:198030e84936 352 //後ろ脚載せる
shimizuta 49:198030e84936 353 SmoothChange(walk, leg, BACKLEG_ON_SANDDUNE, 0.5); //切り替えスムーズ
shimizuta 49:198030e84936 354 MoveOneCycle(walk, leg);
shimizuta 49:198030e84936 355 //後ろ脚乗った状態で進む
shimizuta 49:198030e84936 356 SetWalk(walk, OVERCOME2);
shimizuta 49:198030e84936 357 for (int i = 0; i < 3; i++)
shimizuta 45:0654364226c9 358 MoveOneCycle(walk, leg);
shimizuta 49:198030e84936 359 //普通に進む
shimizuta 49:198030e84936 360 SetWalk(walk, AREA1_LRFWALK);
shimizuta 49:198030e84936 361 for (int i = 0; i < 4; i++)
shimizuta 49:198030e84936 362 MoveOneCycle(walk, leg);
shimizuta 50:36741e8ab197 363 break;
shimizuta 50:36741e8ab197 364
shimizuta 50:36741e8ab197 365 case ROPE_STATE:
shimizuta 50:36741e8ab197 366 //紐前歩く
shimizuta 50:36741e8ab197 367 SetWalk(walk, WALK_BEFORE_ROPE);
shimizuta 50:36741e8ab197 368 for (int i = 0; i < 5; i++)
shimizuta 50:36741e8ab197 369 MoveOneCycle(walk, leg);
shimizuta 50:36741e8ab197 370 int is_arrived = 0;
shimizuta 50:36741e8ab197 371 while (is_arrived == 0)
shimizuta 50:36741e8ab197 372 {
shimizuta 50:36741e8ab197 373 MoveOneCycle(walk, leg);
shimizuta 50:36741e8ab197 374 unsigned int dist_cm = GetDist_cm();
shimizuta 50:36741e8ab197 375 if (dist_cm > 90 && dist_cm < 150)
shimizuta 50:36741e8ab197 376 ++is_arrived;
shimizuta 50:36741e8ab197 377 printf("%d\r\n", dist_cm);
shimizuta 50:36741e8ab197 378 }
shimizuta 50:36741e8ab197 379 /*
shimizuta 50:36741e8ab197 380 SetWalk(walk, AREA1_LRFWALK);
shimizuta 50:36741e8ab197 381 for (int i = 0; i < 5; i++)
shimizuta 50:36741e8ab197 382 MoveOneCycle(walk, leg);
shimizuta 50:36741e8ab197 383 SmoothChange(walk, leg, TRUNRIGHT, 0.5);
shimizuta 49:198030e84936 384 for (int i = 0; i < 2; i++)
shimizuta 50:36741e8ab197 385 MoveOneCycle(walk, leg);
shimizuta 50:36741e8ab197 386 */
shimizuta 50:36741e8ab197 387 /*曲がりながら歩く
shimizuta 50:36741e8ab197 388 SmoothChange(walk, leg, WALK_BEFORE_ROPE, 0.5);
shimizuta 50:36741e8ab197 389 for (int i = 0; i < 7; i++)
shimizuta 45:0654364226c9 390 MoveOneCycle(walk, leg);
shimizuta 49:198030e84936 391 */
shimizuta 49:198030e84936 392 //前足だけ紐越え
shimizuta 49:198030e84936 393 SmoothChange(walk, leg, ROPE, 0.5);
shimizuta 49:198030e84936 394 MoveOneCycle(walk, leg);
shimizuta 49:198030e84936 395 //LRFが取れる高さで歩行
shimizuta 50:36741e8ab197 396 SmoothChange(walk, leg, AREA1_LRFWALK, 1.5);
shimizuta 49:198030e84936 397 for (int i = 0; i < 3; i++)
shimizuta 49:198030e84936 398 MoveOneCycle(walk, leg);
shimizuta 49:198030e84936 399 //後ろ足越え
shimizuta 49:198030e84936 400 SmoothChange(walk, leg, ROPE_BACK, 0.5);
shimizuta 49:198030e84936 401 MoveOneCycle(walk, leg);
shimizuta 50:36741e8ab197 402 //紐間歩く
shimizuta 50:36741e8ab197 403 SmoothChange(walk, leg, AREA1_LRFWALK, 0.5);
shimizuta 50:36741e8ab197 404 for (int i = 0; i < 1; i++)
shimizuta 50:36741e8ab197 405 MoveOneCycle(walk, leg);
shimizuta 50:36741e8ab197 406 //2本目
shimizuta 50:36741e8ab197 407 //前足だけ紐越え
shimizuta 50:36741e8ab197 408 SmoothChange(walk, leg, ROPE, 0.5);
shimizuta 50:36741e8ab197 409 MoveOneCycle(walk, leg);
shimizuta 50:36741e8ab197 410 //LRFが取れる高さで歩行
shimizuta 50:36741e8ab197 411 SmoothChange(walk, leg, AREA1_LRFWALK, 1.5);
shimizuta 50:36741e8ab197 412 for (int i = 0; i < 3; i++)
shimizuta 50:36741e8ab197 413 MoveOneCycle(walk, leg);
shimizuta 50:36741e8ab197 414 //後ろ足越え
shimizuta 50:36741e8ab197 415 SmoothChange(walk, leg, ROPE_BACK, 0.5);
shimizuta 50:36741e8ab197 416 MoveOneCycle(walk, leg);
shimizuta 50:36741e8ab197 417 //紐後歩く
shimizuta 50:36741e8ab197 418 SmoothChange(walk, leg, AREA1_LRFWALK, 1.5);
shimizuta 50:36741e8ab197 419 for (int i = 0; i < 10; i++)
shimizuta 50:36741e8ab197 420 MoveOneCycle(walk, leg);
shimizuta 49:198030e84936 421 break;
shimizuta 50:36741e8ab197 422
shimizuta 49:198030e84936 423 case SLOPE_STATE:
shimizuta 49:198030e84936 424 SmoothChange(walk, leg, SLOPE, 0.5);
shimizuta 49:198030e84936 425 for (int i = 0; i < 30; i++)
shimizuta 49:198030e84936 426 MoveOneCycle(walk, leg);
shimizuta 49:198030e84936 427 break;
shimizuta 49:198030e84936 428 }
shimizuta 46:621ea6492dea 429 #ifdef USE_ROS
shimizuta 49:198030e84936 430 nh_mbed.spinOnce();
shimizuta 46:621ea6492dea 431 #endif
shimizuta 34:89d701e15cdf 432 printf("program end\r\n");
shimizuta 29:7d8b8011a88d 433 #ifdef VSCODE
shimizuta 27:79b4b932a6dd 434 fclose(fp);
shimizuta 27:79b4b932a6dd 435 #endif
shimizuta 2:a92568bdeb5c 436 }
shimizuta 27:79b4b932a6dd 437 #ifdef USE_ROS
shimizuta 27:79b4b932a6dd 438 void callback(const geometry_msgs::Vector3 &cmd_vel)
shimizuta 27:79b4b932a6dd 439 {
shimizuta 46:621ea6492dea 440 if (state_from_ros == STOP)
shimizuta 46:621ea6492dea 441 state_from_ros = AREA1_LRFWALK_STATE;
shimizuta 42:982064594ba6 442 stride_m[LEFT_F] = cmd_vel.x;
shimizuta 42:982064594ba6 443 stride_m[LEFT_B] = cmd_vel.x;
shimizuta 42:982064594ba6 444 stride_m[RIGHT_F] = cmd_vel.y;
shimizuta 42:982064594ba6 445 stride_m[RIGHT_B] = cmd_vel.y;
shimizuta 44:4aac39b8670b 446 back_vel = cmd_vel;
shimizuta 44:4aac39b8670b 447 pub_vel.publish(&back_vel);
shimizuta 42:982064594ba6 448 }
shimizuta 43:2ed84f3558c1 449 void callback_state(const std_msgs::Int16 &cmd)
shimizuta 42:982064594ba6 450 {
yuto17320508 47:c2c579909787 451 state_from_ros = (ROS_STATE)cmd.data;
yuto17320508 47:c2c579909787 452 led[state_from_ros % 4] = 1;
shimizuta 27:79b4b932a6dd 453 }
shimizuta 48:1aad3cc386e8 454 #endif
shimizuta 48:1aad3cc386e8 455 void SmoothChange(Walk &walk, OneLeg leg[4], WalkWay nextway, float time_s)
shimizuta 48:1aad3cc386e8 456 {
shimizuta 48:1aad3cc386e8 457 LineParam lines[4][2];
shimizuta 48:1aad3cc386e8 458 //前回位置を保存
shimizuta 48:1aad3cc386e8 459 for (size_t i = 0; i < sizeof(lines) / sizeof(lines[0]); i++)
shimizuta 48:1aad3cc386e8 460 {
shimizuta 48:1aad3cc386e8 461 lines[i][0].time_s = 0;
shimizuta 48:1aad3cc386e8 462 lines[i][0].x_m = leg[i].GetX_m();
shimizuta 48:1aad3cc386e8 463 lines[i][0].y_m = leg[i].GetY_m();
shimizuta 48:1aad3cc386e8 464 }
shimizuta 48:1aad3cc386e8 465 Walk tempwalk(leg);
shimizuta 48:1aad3cc386e8 466 SetWalk(tempwalk, nextway);
shimizuta 48:1aad3cc386e8 467 tempwalk.Cal4LegsPosi(tempwalk.leg); //次の一歩目をwalk.legに書き込む
shimizuta 48:1aad3cc386e8 468 for (size_t i = 0; i < sizeof(lines) / sizeof(lines[0]); i++)
shimizuta 48:1aad3cc386e8 469 {
shimizuta 48:1aad3cc386e8 470 lines[i][1].time_s = time_s;
shimizuta 48:1aad3cc386e8 471 lines[i][1].x_m = tempwalk.leg[i].GetX_m();
shimizuta 48:1aad3cc386e8 472 lines[i][1].y_m = tempwalk.leg[i].GetY_m();
shimizuta 48:1aad3cc386e8 473 lines[i][1].is_point_to_point = 0;
shimizuta 48:1aad3cc386e8 474 }
shimizuta 48:1aad3cc386e8 475 //前回終点から次回始点まで直線移動
shimizuta 48:1aad3cc386e8 476 for (int i = 0; i < 4; i++)
shimizuta 48:1aad3cc386e8 477 SetOneLegFreeLinesParam(tempwalk, i, lines[i], sizeof(lines[i]) / sizeof(lines[i][0]));
shimizuta 48:1aad3cc386e8 478 tempwalk.ResetPhase();
shimizuta 48:1aad3cc386e8 479 MoveOneCycle(tempwalk, leg);
shimizuta 48:1aad3cc386e8 480 SetWalk(walk, nextway);
shimizuta 48:1aad3cc386e8 481 };