岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

main.cpp@49:198030e84936, 2019-03-11 (annotated)

Committer:
shimizuta
Date:
Mon Mar 11 06:36:56 2019 +0000
Revision:
49:198030e84936
Parent:
48:1aad3cc386e8
Child:
50:36741e8ab197
from start to sanddune, 40s  (if you use restart)

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 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 42:982064594ba6 62 FRONTLEG_ON_SANDDUNE, //前足を段差にかける
shimizuta 42:982064594ba6 63 OVERCOME, //前足が乗った状態で進む
shimizuta 49:198030e84936 64 FALL_FRONTLEG, //前足を段差から降ろす
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 49:198030e84936 84 float area1_offset_y_m[4] = {0.3, 0.3, 0.3, 0.3};
yuto17320508 47:c2c579909787 85 float area2_offset_y_m[4] = {0.27, 0.27, 0.27, 0.27};
shimizuta 49:198030e84936 86 float offset_x_m[4] = {0, 0, 0, 0};
shimizuta 46:621ea6492dea 87 float height_m[4] = {0.05, 0.05, 0.05, 0.05}, buffer_height_m = 0.02,
shimizuta 49:198030e84936 88 stridetime_s = 0.45, toptime_s = 0.2, buffer_time_s = 0.1;
shimizuta 46:621ea6492dea 89
shimizuta 46:621ea6492dea 90 //段差
shimizuta 49:198030e84936 91 float overcome_start_y_m[] = {0.38, 0.38, 0.38, 0.38};
shimizuta 49:198030e84936 92 float flontleg_sand_goal_y_m[4] = {0.38, 0.27, 0.38, 0.27};
shimizuta 49:198030e84936 93 float backleg_sand_goal_y_m[4] = {0.27, 0.38, 0.27, 0.38};
shimizuta 46:621ea6492dea 94 //旋回
shimizuta 45:0654364226c9 95 float turn_start_x_m = 0, turn_start_y_m = 0.275,
shimizuta 45:0654364226c9 96 turn_stride_m = 0.05, turn_height_m = 0.05,
shimizuta 45:0654364226c9 97 turn_stridetime_s = 0.6, turn_risetime_s = 0.3;
shimizuta 35:b4e1b8f25cd7 98 switch (way)
shimizuta 35:b4e1b8f25cd7 99 {
shimizuta 43:2ed84f3558c1 100 case STANDUP:
shimizuta 43:2ed84f3558c1 101 { //受け渡し用に待つ
shimizuta 43:2ed84f3558c1 102 float offset_x_m[4] = {},
shimizuta 48:1aad3cc386e8 103 offset_y_m[4] = {0.3, 0.3, 0.3, 0.3};
shimizuta 35:b4e1b8f25cd7 104 for (int i = 0; i < 4; i++)
shimizuta 43:2ed84f3558c1 105 SetOneLegStandParam(walk, i, offset_x_m[i], offset_y_m[i], 0.5);
shimizuta 43:2ed84f3558c1 106 walk.SetOffsetTime(0, 0, 0, 0);
shimizuta 35:b4e1b8f25cd7 107 break;
shimizuta 43:2ed84f3558c1 108 }
shimizuta 46:621ea6492dea 109 case AREA1_LRFWALK: //LRF用に変数はグローバルにある
shimizuta 35:b4e1b8f25cd7 110 for (int i = 0; i < 4; i++)
shimizuta 49:198030e84936 111 SetOneLegFourPointParam(walk, i, offset_x_m[i], area1_offset_y_m[i],
shimizuta 49:198030e84936 112 stride_m[i], height_m[i], buffer_height_m,
shimizuta 49:198030e84936 113 stridetime_s, toptime_s, buffer_time_s);
shimizuta 46:621ea6492dea 114 walk.SetOffsetTime(0, 0.5, 0.5, 0);
shimizuta 46:621ea6492dea 115 break;
shimizuta 46:621ea6492dea 116 case AREA2_LRFWALK:
shimizuta 46:621ea6492dea 117 for (int i = 0; i < 4; i++)
shimizuta 46:621ea6492dea 118 SetOneLegTriangleParam(walk, i, offset_x_m[i], area2_offset_y_m[i],
shimizuta 35:b4e1b8f25cd7 119 stride_m[i], height_m[i], buffer_height_m,
shimizuta 35:b4e1b8f25cd7 120 stridetime_s, toptime_s, buffer_time_s);
shimizuta 35:b4e1b8f25cd7 121 walk.SetOffsetTime(0, 0.5, 0.5, 0);
shimizuta 35:b4e1b8f25cd7 122 break;
shimizuta 43:2ed84f3558c1 123 case STANDUP_SANDDUNE:
shimizuta 42:982064594ba6 124 {
shimizuta 43:2ed84f3558c1 125 for (int i = 0; i < 4; i++)
shimizuta 43:2ed84f3558c1 126 {
shimizuta 43:2ed84f3558c1 127 LineParam lines[] = {
shimizuta 43:2ed84f3558c1 128 {.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 129 {.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 130 {.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 131 };
shimizuta 43:2ed84f3558c1 132 SetOneLegFreeLinesParam(walk, i, lines, sizeof(lines) / sizeof(lines[0]));
shimizuta 43:2ed84f3558c1 133 }
shimizuta 43:2ed84f3558c1 134 walk.SetOffsetTime(0, 0, 0, 0);
shimizuta 44:4aac39b8670b 135 break;
shimizuta 43:2ed84f3558c1 136 }
shimizuta 43:2ed84f3558c1 137 case FRONTLEG_ON_SANDDUNE:
shimizuta 43:2ed84f3558c1 138 { //前足を段差にかける
shimizuta 42:982064594ba6 139 float d_x_m = 0.1;
shimizuta 43:2ed84f3558c1 140 float raise_offset_x_m[4] = {0, -0.02, 0, 0};
shimizuta 49:198030e84936 141 float overcome_height_m[] = {0.05, 0.15, 0.05, 0.15};
shimizuta 42:982064594ba6 142 float gravity_dist[] = {0.05, 0, 0.05, -0.05};
shimizuta 49:198030e84936 143 OverCome overcome(offset_x_m, overcome_start_y_m, d_x_m, flontleg_sand_goal_y_m,
shimizuta 49:198030e84936 144 overcome_height_m, gravity_dist, walk.leg, raise_offset_x_m);
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 43:2ed84f3558c1 166 overcome_height_m, gravity_dist, walk.leg, raise_offset_x_m);
shimizuta 35:b4e1b8f25cd7 167 walk.Copy(overcome.walk);
shimizuta 42:982064594ba6 168 break;
shimizuta 39:87dcdff27797 169 }
shimizuta 43:2ed84f3558c1 170 case BACKLEG_ON_SANDDUNE:
shimizuta 43:2ed84f3558c1 171 { //後ろ脚を乗せる
shimizuta 49:198030e84936 172 float d_x_m = 0.15;
shimizuta 43:2ed84f3558c1 173 float start_x_m[4] = {0, 0, 0, 0};
shimizuta 42:982064594ba6 174 float start_y_m[4] = {0.41, 0.41, 0.41, 0.41};
shimizuta 49:198030e84936 175
shimizuta 43:2ed84f3558c1 176 float raise_offset_x_m[4] = {};
shimizuta 42:982064594ba6 177 float overcome_height_m[] = {0.2, 0.1, 0.2, 0.1};
shimizuta 43:2ed84f3558c1 178 float gravity_dist[] = {0.1, 0, 0.1, 0};
shimizuta 49:198030e84936 179 OverCome overcome(start_x_m, start_y_m, d_x_m, backleg_sand_goal_y_m,
shimizuta 43:2ed84f3558c1 180 overcome_height_m, gravity_dist, walk.leg, raise_offset_x_m);
shimizuta 42:982064594ba6 181 walk.Copy(overcome.walk);
shimizuta 42:982064594ba6 182 break;
shimizuta 42:982064594ba6 183 }
shimizuta 43:2ed84f3558c1 184 case OVERCOME2:
shimizuta 43:2ed84f3558c1 185 { //後ろ脚が乗った状態で進む
shimizuta 49:198030e84936 186 float offset_x_m[4] = {0.1,0,0.1,0};
shimizuta 49:198030e84936 187 float offset_y_m[4] = {0.27, 0.33, 0.27, 0.33};
shimizuta 42:982064594ba6 188 float stride_m[4] = {0.2, 0.2, 0.2, 0.2},
shimizuta 42:982064594ba6 189 height_m[4] = {0.1, 0.1, 0.1, 0.1}, buffer_height_m = 0.05,
shimizuta 42:982064594ba6 190 stridetime_s = 1, toptime_s = 0.2, buffer_time_s = 0.2;
shimizuta 42:982064594ba6 191 for (int i = 0; i < 4; i++)
shimizuta 42:982064594ba6 192 SetOneLegFourPointParam(walk, i, offset_x_m[i], offset_y_m[i],
shimizuta 42:982064594ba6 193 stride_m[i], height_m[i], buffer_height_m,
shimizuta 42:982064594ba6 194 stridetime_s, toptime_s, buffer_time_s);
shimizuta 42:982064594ba6 195 walk.SetOffsetTime(0, 0.5, 0.5, 0);
shimizuta 42:982064594ba6 196 break;
shimizuta 42:982064594ba6 197 }
shimizuta 44:4aac39b8670b 198 case AFTER_OVERCOME:
shimizuta 43:2ed84f3558c1 199 {
shimizuta 44:4aac39b8670b 200 for (int i = 0; i < 4; i++)
shimizuta 46:621ea6492dea 201 SetOneLegStandParam(walk, i, offset_x_m[i], area1_offset_y_m[i], 0.5);
shimizuta 44:4aac39b8670b 202 walk.SetOffsetTime(0, 0, 0, 0);
shimizuta 44:4aac39b8670b 203 break;
shimizuta 44:4aac39b8670b 204 }
shimizuta 45:0654364226c9 205 case ROPE:
shimizuta 44:4aac39b8670b 206 {
shimizuta 43:2ed84f3558c1 207 float offset_x_m[4] = {-0.05, 0, -0.05, 0};
shimizuta 43:2ed84f3558c1 208 float d_x_m = 0.2;
shimizuta 43:2ed84f3558c1 209 float start_y_m[4] = {0.41, 0.41, 0.41, 0.41};
shimizuta 43:2ed84f3558c1 210 float goal_y_m[4] = {0.41, 0.41, 0.41, 0.41};
shimizuta 43:2ed84f3558c1 211 float raise_offset_x_m[] = {0, -0.08, 0, -0.08};
shimizuta 43:2ed84f3558c1 212 float overcome_height_m[] = {0.05, 0.2, 0.05, 0.2};
shimizuta 43:2ed84f3558c1 213 float gravity_dist[] = {0.05, -0.05, 0.05, -0.05};
shimizuta 43:2ed84f3558c1 214 OverCome overcome(offset_x_m, start_y_m, d_x_m, goal_y_m,
shimizuta 43:2ed84f3558c1 215 overcome_height_m, gravity_dist, walk.leg, raise_offset_x_m);
shimizuta 43:2ed84f3558c1 216 walk.Copy(overcome.walk);
shimizuta 44:4aac39b8670b 217 break;
shimizuta 43:2ed84f3558c1 218 }
shimizuta 43:2ed84f3558c1 219 case ROPE_BACK:
shimizuta 43:2ed84f3558c1 220 {
shimizuta 43:2ed84f3558c1 221 float offset_x_m[4] = {};
shimizuta 43:2ed84f3558c1 222 float d_x_m = 0.15;
shimizuta 43:2ed84f3558c1 223 float start_y_m[4] = {0.41, 0.41, 0.41, 0.41};
shimizuta 43:2ed84f3558c1 224 float goal_y_m[4] = {0.41, 0.41, 0.41, 0.41};
shimizuta 43:2ed84f3558c1 225 float raise_offset_x_m[] = {-0.03, 0, -0.03, 0};
shimizuta 43:2ed84f3558c1 226 float overcome_height_m[] = {0.2, 0.05, 0.2, 0.05};
shimizuta 43:2ed84f3558c1 227 float gravity_dist[] = {0.08, 0, 0.08, 0};
shimizuta 43:2ed84f3558c1 228 OverCome overcome(offset_x_m, start_y_m, d_x_m, goal_y_m,
shimizuta 43:2ed84f3558c1 229 overcome_height_m, gravity_dist, walk.leg, raise_offset_x_m);
shimizuta 43:2ed84f3558c1 230 walk.Copy(overcome.walk);
shimizuta 44:4aac39b8670b 231 break;
shimizuta 43:2ed84f3558c1 232 }
shimizuta 43:2ed84f3558c1 233 case ROPE_DOWN:
shimizuta 43:2ed84f3558c1 234 {
shimizuta 43:2ed84f3558c1 235 for (int i = 0; i < 4; i++)
shimizuta 43:2ed84f3558c1 236 {
shimizuta 43:2ed84f3558c1 237 LineParam lines[] = {
shimizuta 43:2ed84f3558c1 238 {.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 239 {.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 240 };
shimizuta 43:2ed84f3558c1 241 SetOneLegFreeLinesParam(walk, i, lines, sizeof(lines) / sizeof(lines[0]));
shimizuta 43:2ed84f3558c1 242 }
shimizuta 43:2ed84f3558c1 243 walk.SetOffsetTime(0, 0, 0, 0);
shimizuta 44:4aac39b8670b 244 break;
shimizuta 43:2ed84f3558c1 245 }
shimizuta 43:2ed84f3558c1 246 case SLOPE:
shimizuta 43:2ed84f3558c1 247 {
shimizuta 43:2ed84f3558c1 248 float offset_x_m[4] = {-0.15, 0.15, -0.15, 0.15},
shimizuta 43:2ed84f3558c1 249 offset_y_m[4] = {0.32, 0.22, 0.32, 0.22};
shimizuta 43:2ed84f3558c1 250 float stride_m[4] = {0.18, 0.18, 0.2, 0.2}; //ropeのときは0.15
shimizuta 43:2ed84f3558c1 251 float height_m[4] = {0.05, 0.05, 0.05, 0.05}, buffer_height_m = 0.02,
shimizuta 49:198030e84936 252 stridetime_s = 1, toptime_s = 0.15, buffer_time_s = 0.05;
shimizuta 43:2ed84f3558c1 253 for (int i = 0; i < 4; i++)
shimizuta 43:2ed84f3558c1 254 SetOneLegTriangleParam(walk, i, offset_x_m[i], offset_y_m[i],
shimizuta 43:2ed84f3558c1 255 stride_m[i], height_m[i], buffer_height_m,
shimizuta 43:2ed84f3558c1 256 stridetime_s, toptime_s, buffer_time_s);
shimizuta 43:2ed84f3558c1 257 walk.SetOffsetTime(0, 0.5, 0.5, 0);
shimizuta 44:4aac39b8670b 258 break;
shimizuta 43:2ed84f3558c1 259 }
shimizuta 45:0654364226c9 260 case TRUNRIGHT:
shimizuta 45:0654364226c9 261 Turn(walk, 1, turn_start_x_m, turn_start_y_m, turn_stride_m,
shimizuta 45:0654364226c9 262 turn_height_m, turn_stridetime_s, turn_risetime_s);
shimizuta 45:0654364226c9 263 break;
shimizuta 45:0654364226c9 264 case TRUNLEFT:
shimizuta 45:0654364226c9 265 Turn(walk, 0, turn_start_x_m, turn_start_y_m, turn_stride_m,
shimizuta 45:0654364226c9 266 turn_height_m, turn_stridetime_s, turn_risetime_s);
shimizuta 45:0654364226c9 267 break;
shimizuta 42:982064594ba6 268 default:
shimizuta 42:982064594ba6 269 printf("error: there is no WalkWay\r\n");
shimizuta 44:4aac39b8670b 270 return 1; //以上終了
shimizuta 42:982064594ba6 271 }
shimizuta 44:4aac39b8670b 272 walk.ResetPhase();
shimizuta 44:4aac39b8670b 273 return 0; //正常終了
yuto17320508 18:0033ef1814ba 274 }
shimizuta 48:1aad3cc386e8 275 void SmoothChange(Walk &walk, OneLeg leg[4], WalkWay nextway, float time_s);
shimizuta 9:905f93247688 276 int main()
yuto17320508 5:556d5a5e9d24 277 {
shimizuta 44:4aac39b8670b 278 printf("program start\r\n");
shimizuta 29:7d8b8011a88d 279 #ifdef VSCODE
shimizuta 27:79b4b932a6dd 280 if (FileOpen()) //csv fileに書き込み
shimizuta 44:4aac39b8670b 281 return 1; //異常なら強制終了
shimizuta 29:7d8b8011a88d 282 #endif
shimizuta 46:621ea6492dea 283
shimizuta 44:4aac39b8670b 284 /////足の長さ、計算周期の設定
shimizuta 39:87dcdff27797 285 OneLeg leg[4]; //各足の位置
shimizuta 35:b4e1b8f25cd7 286 for (int i = 0; i < 4; i++)
shimizuta 35:b4e1b8f25cd7 287 leg[i] = OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2);
shimizuta 39:87dcdff27797 288 Walk walk(leg); //歩行法はここに入れていく
shimizuta 35:b4e1b8f25cd7 289 Walk::calctime_s_ = 0.03;
shimizuta 44:4aac39b8670b 290 /////事前に軌道を全チェック
shimizuta 42:982064594ba6 291 for (int i = 0; i < CHECK; i++)
shimizuta 42:982064594ba6 292 {
shimizuta 44:4aac39b8670b 293 SetWalk(walk, (WalkWay)i);
shimizuta 44:4aac39b8670b 294 if (walk.CheckOrbit() == 1)
shimizuta 42:982064594ba6 295 {
shimizuta 42:982064594ba6 296 printf("error: move %d\r\n", i);
shimizuta 42:982064594ba6 297 return 1; //強制終了.errorは内部の関数からprintfで知らせる
shimizuta 42:982064594ba6 298 }
shimizuta 42:982064594ba6 299 }
shimizuta 46:621ea6492dea 300 //軌道チェックしてからROS完全起動
shimizuta 46:621ea6492dea 301 #ifdef USE_ROS
shimizuta 46:621ea6492dea 302 nh_mbed.getHardware()->setBaud(115200);
shimizuta 46:621ea6492dea 303 nh_mbed.initNode();
shimizuta 46:621ea6492dea 304 nh_mbed.subscribe(sub_vel);
shimizuta 46:621ea6492dea 305 nh_mbed.subscribe(sub_state);
shimizuta 46:621ea6492dea 306 nh_mbed.advertise(pub_vel);
shimizuta 46:621ea6492dea 307 nh_mbed.spinOnce(); //一度受信
shimizuta 43:2ed84f3558c1 308 #endif
shimizuta 49:198030e84936 309 ROS_STATE old_state = STOP;
shimizuta 44:4aac39b8670b 310 /////立つ
shimizuta 43:2ed84f3558c1 311 #ifndef USE_ROS
shimizuta 46:621ea6492dea 312 state_from_ros = SANDDUNE; //遠してやらないならやりたいSTATEに変更
shimizuta 46:621ea6492dea 313 #endif
shimizuta 46:621ea6492dea 314 //プログラムの最初のほうにあるstate_from_rosで動かすものを切り替える.caseはenum ROS_STATEで分ける
shimizuta 49:198030e84936 315 // while (1)
shimizuta 49:198030e84936 316 // {
shimizuta 49:198030e84936 317 switch (state_from_ros)
shimizuta 39:87dcdff27797 318 {
shimizuta 49:198030e84936 319 case STOP:
shimizuta 49:198030e84936 320 break;
shimizuta 49:198030e84936 321 case AREA1_LRFWALK_STATE:
shimizuta 49:198030e84936 322 printf("Stand up?\r\n");
shimizuta 49:198030e84936 323 WaitStdin('y'); // キーボード入力されるまでまで待つ
shimizuta 49:198030e84936 324 SetWalk(walk, STANDUP);
shimizuta 49:198030e84936 325 MoveOneCycle(walk, leg);
shimizuta 49:198030e84936 326 printf("Move?\r\n");
shimizuta 49:198030e84936 327 WaitStdin('y'); //キーボード入力されるまでまで待つ
shimizuta 49:198030e84936 328 SmoothChange(walk, leg, AREA1_LRFWALK, 0.5); //切り替え
shimizuta 49:198030e84936 329 for (int i = 0; i < 20; i++)
shimizuta 46:621ea6492dea 330 MoveOneCycle(walk, leg);
shimizuta 49:198030e84936 331 state_from_ros = STOP;
shimizuta 49:198030e84936 332 break;
shimizuta 49:198030e84936 333 case AREA2_LRFWALK_STATE:
shimizuta 49:198030e84936 334 if (old_state != state_from_ros)
shimizuta 49:198030e84936 335 SmoothChange(walk, leg, AREA2_LRFWALK, 0.5); //切り替え
shimizuta 49:198030e84936 336 else
shimizuta 49:198030e84936 337 SetWalk(walk, AREA2_LRFWALK);
shimizuta 49:198030e84936 338 MoveOneCycle(walk, leg);
shimizuta 49:198030e84936 339 break;
shimizuta 49:198030e84936 340 case SANDDUNE:
shimizuta 49:198030e84936 341 //前足を段差にかける
shimizuta 49:198030e84936 342 printf("Stand up?\r\n");
shimizuta 49:198030e84936 343 WaitStdin('y'); // キーボード入力されるまでまで待つ
shimizuta 49:198030e84936 344 SetWalk(walk, STANDUP_SANDDUNE);
shimizuta 49:198030e84936 345 MoveOneCycle(walk, leg);
shimizuta 49:198030e84936 346 printf("Move?\r\n");
shimizuta 49:198030e84936 347 WaitStdin('y'); //キーボード入力されるまでまで待つ
shimizuta 49:198030e84936 348 SmoothChange(walk, leg, FRONTLEG_ON_SANDDUNE, 0.5); //切り替えスムーズ
shimizuta 49:198030e84936 349 MoveOneCycle(walk, leg);
shimizuta 49:198030e84936 350 //前足が段差に乗った状態で進む
shimizuta 49:198030e84936 351 SmoothChange(walk, leg, OVERCOME, 0.5); //切り替えスムーズ
shimizuta 49:198030e84936 352 for (int i = 0; i < 2; i++)
shimizuta 44:4aac39b8670b 353 MoveOneCycle(walk, leg);
shimizuta 49:198030e84936 354 //前足を降ろす
shimizuta 49:198030e84936 355 SmoothChange(walk, leg, FALL_FRONTLEG, 0.5);
shimizuta 49:198030e84936 356 MoveOneCycle(walk, leg);
shimizuta 49:198030e84936 357 //またいだ状態で歩く
shimizuta 49:198030e84936 358 SmoothChange(walk, leg, AREA1_LRFWALK, 0.5);
shimizuta 49:198030e84936 359 MoveOneCycle(walk, leg);
shimizuta 49:198030e84936 360
shimizuta 49:198030e84936 361 //後ろ脚載せる
shimizuta 49:198030e84936 362 SmoothChange(walk, leg, BACKLEG_ON_SANDDUNE, 0.5); //切り替えスムーズ
shimizuta 49:198030e84936 363 MoveOneCycle(walk, leg);
shimizuta 49:198030e84936 364 //後ろ脚乗った状態で進む
shimizuta 49:198030e84936 365 // SmoothChange(walk, leg, OVERCOME2, 0.5); //切り替えスムーズ
shimizuta 49:198030e84936 366 SetWalk(walk, OVERCOME2);
shimizuta 49:198030e84936 367 for (int i = 0; i < 3; i++)
shimizuta 45:0654364226c9 368 MoveOneCycle(walk, leg);
shimizuta 49:198030e84936 369 //普通に進む
shimizuta 49:198030e84936 370 SetWalk(walk, AREA1_LRFWALK);
shimizuta 49:198030e84936 371 for (int i = 0; i < 4; i++)
shimizuta 49:198030e84936 372 MoveOneCycle(walk, leg);
shimizuta 49:198030e84936 373 /* //数歩左に曲がる
shimizuta 49:198030e84936 374 SmoothChange(walk, leg, TRUNLEFT, 0.5); //切り替えスムーズ
shimizuta 49:198030e84936 375 for (int i = 0; i < 2; i++)
shimizuta 45:0654364226c9 376 MoveOneCycle(walk, leg);
shimizuta 49:198030e84936 377 */
shimizuta 49:198030e84936 378 state_from_ros = AREA2_LRFWALK_STATE;
shimizuta 49:198030e84936 379 break;
shimizuta 49:198030e84936 380 case ROPE_STATE:
shimizuta 49:198030e84936 381 //前足だけ紐越え
shimizuta 49:198030e84936 382 SmoothChange(walk, leg, ROPE, 0.5);
shimizuta 49:198030e84936 383 MoveOneCycle(walk, leg);
shimizuta 49:198030e84936 384 //LRFが取れる高さで歩行
shimizuta 49:198030e84936 385 SmoothChange(walk, leg, AREA1_LRFWALK, 0.5);
shimizuta 49:198030e84936 386 for (int i = 0; i < 3; i++)
shimizuta 49:198030e84936 387 MoveOneCycle(walk, leg);
shimizuta 49:198030e84936 388 //後ろ足越え
shimizuta 49:198030e84936 389 SmoothChange(walk, leg, ROPE_BACK, 0.5);
shimizuta 49:198030e84936 390 MoveOneCycle(walk, leg);
shimizuta 49:198030e84936 391 state_from_ros = AREA2_LRFWALK_STATE;
shimizuta 49:198030e84936 392 break;
shimizuta 49:198030e84936 393 case SLOPE_STATE:
shimizuta 49:198030e84936 394 SmoothChange(walk, leg, SLOPE, 0.5);
shimizuta 49:198030e84936 395 for (int i = 0; i < 30; i++)
shimizuta 49:198030e84936 396 MoveOneCycle(walk, leg);
shimizuta 49:198030e84936 397 state_from_ros = STOP;
shimizuta 49:198030e84936 398 break;
shimizuta 49:198030e84936 399 case TURNRIGHT_STATE:
shimizuta 49:198030e84936 400 SmoothChange(walk, leg, TRUNRIGHT, 0.5);
shimizuta 49:198030e84936 401 for (int i = 0; i < 5; i++)
shimizuta 49:198030e84936 402 MoveOneCycle(walk, leg);
shimizuta 49:198030e84936 403 state_from_ros = AREA2_LRFWALK_STATE;
shimizuta 49:198030e84936 404 break;
shimizuta 49:198030e84936 405 case TRUNLEFT_STATE:
shimizuta 49:198030e84936 406 SmoothChange(walk, leg, TRUNLEFT, 0.5);
shimizuta 49:198030e84936 407 for (int i = 0; i < 5; i++)
shimizuta 49:198030e84936 408 MoveOneCycle(walk, leg);
shimizuta 49:198030e84936 409 state_from_ros = AREA2_LRFWALK_STATE;
shimizuta 49:198030e84936 410 break;
shimizuta 49:198030e84936 411 }
shimizuta 49:198030e84936 412 // old_state = state_from_ros;
shimizuta 46:621ea6492dea 413 #ifdef USE_ROS
shimizuta 49:198030e84936 414 nh_mbed.spinOnce();
shimizuta 46:621ea6492dea 415 #endif
shimizuta 34:89d701e15cdf 416 printf("program end\r\n");
shimizuta 29:7d8b8011a88d 417 #ifdef VSCODE
shimizuta 27:79b4b932a6dd 418 fclose(fp);
shimizuta 27:79b4b932a6dd 419 #endif
shimizuta 2:a92568bdeb5c 420 }
shimizuta 27:79b4b932a6dd 421 #ifdef USE_ROS
shimizuta 27:79b4b932a6dd 422 void callback(const geometry_msgs::Vector3 &cmd_vel)
shimizuta 27:79b4b932a6dd 423 {
shimizuta 46:621ea6492dea 424 if (state_from_ros == STOP)
shimizuta 46:621ea6492dea 425 state_from_ros = AREA1_LRFWALK_STATE;
shimizuta 42:982064594ba6 426 stride_m[LEFT_F] = cmd_vel.x;
shimizuta 42:982064594ba6 427 stride_m[LEFT_B] = cmd_vel.x;
shimizuta 42:982064594ba6 428 stride_m[RIGHT_F] = cmd_vel.y;
shimizuta 42:982064594ba6 429 stride_m[RIGHT_B] = cmd_vel.y;
shimizuta 44:4aac39b8670b 430 back_vel = cmd_vel;
shimizuta 44:4aac39b8670b 431 pub_vel.publish(&back_vel);
shimizuta 42:982064594ba6 432 }
shimizuta 43:2ed84f3558c1 433 void callback_state(const std_msgs::Int16 &cmd)
shimizuta 42:982064594ba6 434 {
yuto17320508 47:c2c579909787 435 state_from_ros = (ROS_STATE)cmd.data;
yuto17320508 47:c2c579909787 436 led[state_from_ros % 4] = 1;
shimizuta 27:79b4b932a6dd 437 }
shimizuta 48:1aad3cc386e8 438 #endif
shimizuta 48:1aad3cc386e8 439 void SmoothChange(Walk &walk, OneLeg leg[4], WalkWay nextway, float time_s)
shimizuta 48:1aad3cc386e8 440 {
shimizuta 48:1aad3cc386e8 441 LineParam lines[4][2];
shimizuta 48:1aad3cc386e8 442 //前回位置を保存
shimizuta 48:1aad3cc386e8 443 for (size_t i = 0; i < sizeof(lines) / sizeof(lines[0]); i++)
shimizuta 48:1aad3cc386e8 444 {
shimizuta 48:1aad3cc386e8 445 lines[i][0].time_s = 0;
shimizuta 48:1aad3cc386e8 446 lines[i][0].x_m = leg[i].GetX_m();
shimizuta 48:1aad3cc386e8 447 lines[i][0].y_m = leg[i].GetY_m();
shimizuta 48:1aad3cc386e8 448 }
shimizuta 48:1aad3cc386e8 449 Walk tempwalk(leg);
shimizuta 48:1aad3cc386e8 450 SetWalk(tempwalk, nextway);
shimizuta 48:1aad3cc386e8 451 tempwalk.Cal4LegsPosi(tempwalk.leg); //次の一歩目をwalk.legに書き込む
shimizuta 48:1aad3cc386e8 452 for (size_t i = 0; i < sizeof(lines) / sizeof(lines[0]); i++)
shimizuta 48:1aad3cc386e8 453 {
shimizuta 48:1aad3cc386e8 454 lines[i][1].time_s = time_s;
shimizuta 48:1aad3cc386e8 455 lines[i][1].x_m = tempwalk.leg[i].GetX_m();
shimizuta 48:1aad3cc386e8 456 lines[i][1].y_m = tempwalk.leg[i].GetY_m();
shimizuta 48:1aad3cc386e8 457 lines[i][1].is_point_to_point = 0;
shimizuta 48:1aad3cc386e8 458 }
shimizuta 48:1aad3cc386e8 459 //前回終点から次回始点まで直線移動
shimizuta 48:1aad3cc386e8 460 for (int i = 0; i < 4; i++)
shimizuta 48:1aad3cc386e8 461 SetOneLegFreeLinesParam(tempwalk, i, lines[i], sizeof(lines[i]) / sizeof(lines[i][0]));
shimizuta 48:1aad3cc386e8 462 tempwalk.ResetPhase();
shimizuta 48:1aad3cc386e8 463 MoveOneCycle(tempwalk, leg);
shimizuta 48:1aad3cc386e8 464 SetWalk(walk, nextway);
shimizuta 48:1aad3cc386e8 465 };