岡本 陸 / Mbed 2 deprecated 0311

test

Dependencies:   mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM

main.cpp@43:2ed84f3558c1, 2019-03-06 (annotated)

Committer:
shimizuta
Date:
Wed Mar 06 12:13:46 2019 +0000
Revision:
43:2ed84f3558c1
Parent:
42:982064594ba6
Child:
44:4aac39b8670b
moved 3/06

Who changed what in which revision?

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