岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

main.cpp@44:4aac39b8670b, 2019-03-07 (annotated)

Committer:
shimizuta
Date:
Thu Mar 07 06:36:34 2019 +0000
Revision:
44:4aac39b8670b
Parent:
43:2ed84f3558c1
Child:
45:0654364226c9
over come finished

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