岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

main.cpp@45:0654364226c9, 2019-03-07 (annotated)

Committer:
shimizuta
Date:
Thu Mar 07 08:49:57 2019 +0000
Revision:
45:0654364226c9
Parent:
44:4aac39b8670b
Child:
46:621ea6492dea
turn

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