test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: main.cpp
- Revision:
- 48:1aad3cc386e8
- Parent:
- 47:c2c579909787
- Child:
- 49:198030e84936
diff -r c2c579909787 -r 1aad3cc386e8 main.cpp --- a/main.cpp Fri Mar 08 04:34:29 2019 +0000 +++ b/main.cpp Fri Mar 08 06:09:31 2019 +0000 @@ -99,7 +99,7 @@ case STANDUP: { //受け渡し用に待つ float offset_x_m[4] = {}, - offset_y_m[4] = {0.275, 0.275, 0.275, 0.275}; + offset_y_m[4] = {0.3, 0.3, 0.3, 0.3}; for (int i = 0; i < 4; i++) SetOneLegStandParam(walk, i, offset_x_m[i], offset_y_m[i], 0.5); walk.SetOffsetTime(0, 0, 0, 0); @@ -275,6 +275,7 @@ walk.ResetPhase(); return 0; //正常終了 } +void SmoothChange(Walk &walk, OneLeg leg[4], WalkWay nextway, float time_s); int main() { printf("program start\r\n"); @@ -312,15 +313,16 @@ WaitStdin('y'); // キーボード入力されるまでまで待つ #endif /////立つ - SetWalk(walk, STANDUP); -// SetWalk(walk, AFTER_OVERCOME); //段差越え後調整用に一時的に入れた + SetWalk(walk, STANDUP); + // SetWalk(walk, AFTER_OVERCOME); //段差越え後調整用に一時的に入れた MoveOneCycle(walk, leg); #ifndef USE_ROS printf("Move?\r\n"); - WaitStdin('y'); //キーボード入力されるまでまで待つ + WaitStdin('y'); //キーボード入力されるまでまで待つ state_from_ros = SANDDUNE; //遠してやらないならやりたいSTATEに変更 #endif - int loopcount = 0; + int loopcount = 0; + ROS_STATE old_state = state_from_ros; //プログラムの最初のほうにあるstate_from_rosで動かすものを切り替える.caseはenum ROS_STATEで分ける while (1) { @@ -329,70 +331,74 @@ case STOP: break; case AREA1_LRFWALK_STATE: - SetWalk(walk, AREA1_LRFWALK); + if (old_state != state_from_ros) + SmoothChange(walk, leg, AREA1_LRFWALK, 0.5); //切り替え + else + SetWalk(walk, AREA1_LRFWALK); MoveOneCycle(walk, leg); break; case AREA2_LRFWALK_STATE: - SetWalk(walk, AREA2_LRFWALK); + if (old_state != state_from_ros) + SmoothChange(walk, leg, AREA2_LRFWALK, 0.5); //切り替え + else + SetWalk(walk, AREA2_LRFWALK); MoveOneCycle(walk, leg); break; case SANDDUNE: - // #endif - SetWalk(walk, STANDUP_SANDDUNE); + //前足を段差にかける + SmoothChange(walk, leg, FRONTLEG_ON_SANDDUNE, 0.5); //切り替えスムーズ MoveOneCycle(walk, leg); - //前足を段差にかける - SetWalk(walk, FRONTLEG_ON_SANDDUNE); - for (int i = 0; i < 1; i++) - MoveOneCycle(walk, leg); //前足が段差に乗った状態で進む - SetWalk(walk, OVERCOME); + SmoothChange(walk, leg, OVERCOME, 0.5); //切り替えスムーズ for (int i = 0; i < 5; i++) MoveOneCycle(walk, leg); //後ろ脚載せる - SetWalk(walk, BACKLEG_ON_SANDDUNE); + SmoothChange(walk, leg, BACKLEG_ON_SANDDUNE, 0.5); //切り替えスムーズ for (int i = 0; i < 1; i++) MoveOneCycle(walk, leg); //後ろ脚乗った状態で進む - SetWalk(walk, OVERCOME2); + SmoothChange(walk ,leg, OVERCOME2, 0.5); //切り替えスムーズ for (int i = 0; i < 3; i++) MoveOneCycle(walk, leg); - SetWalk(walk, TRUNLEFT); + //数歩左に曲がる + SmoothChange(walk, leg, TRUNLEFT, 0.5); //切り替えスムーズ for (int i = 0; i < 2; i++) MoveOneCycle(walk, leg); state_from_ros = AREA2_LRFWALK_STATE; - // #ifdef USE_ROS break; case ROPE_STATE: - SetWalk(walk, ROPE); + //前足だけ紐越え + SmoothChange(walk, leg, ROPE, 0.5); MoveOneCycle(walk, leg); - SetWalk(walk, ROPE_DOWN); - MoveOneCycle(walk, leg); - SetWalk(walk, AREA1_LRFWALK); + //LRFが取れる高さで歩行 + SmoothChange(walk, leg, AREA1_LRFWALK, 0.5); for (int i = 0; i < 3; i++) MoveOneCycle(walk, leg); - SetWalk(walk, ROPE_BACK); + //後ろ足越え + SmoothChange(walk,leg, ROPE_BACK, 0.5); MoveOneCycle(walk, leg); state_from_ros = AREA2_LRFWALK_STATE; break; case SLOPE_STATE: - SetWalk(walk, SLOPE); + SmoothChange(walk, leg, SLOPE, 0.5); for (int i = 0; i < 30; i++) MoveOneCycle(walk, leg); state_from_ros = STOP; break; case TURNRIGHT_STATE: - SetWalk(walk, TRUNRIGHT); + SmoothChange(walk, leg, TRUNRIGHT, 0.5); for (int i = 0; i < 5; i++) MoveOneCycle(walk, leg); state_from_ros = AREA2_LRFWALK_STATE; break; case TRUNLEFT_STATE: - SetWalk(walk, TRUNLEFT); + SmoothChange(walk, leg, TRUNLEFT, 0.5); for (int i = 0; i < 5; i++) MoveOneCycle(walk, leg); state_from_ros = AREA2_LRFWALK_STATE; break; } + old_state = state_from_ros; #ifdef USE_ROS nh_mbed.spinOnce(); #endif @@ -418,11 +424,37 @@ stride_m[RIGHT_B] = cmd_vel.y; back_vel = cmd_vel; pub_vel.publish(&back_vel); -// led[0] = !led[0]; } void callback_state(const std_msgs::Int16 &cmd) { state_from_ros = (ROS_STATE)cmd.data; led[state_from_ros % 4] = 1; } -#endif \ No newline at end of file +#endif +void SmoothChange(Walk &walk, OneLeg leg[4], WalkWay nextway, float time_s) +{ + LineParam lines[4][2]; + //前回位置を保存 + for (size_t i = 0; i < sizeof(lines) / sizeof(lines[0]); i++) + { + lines[i][0].time_s = 0; + lines[i][0].x_m = leg[i].GetX_m(); + lines[i][0].y_m = leg[i].GetY_m(); + } + Walk tempwalk(leg); + SetWalk(tempwalk, nextway); + tempwalk.Cal4LegsPosi(tempwalk.leg); //次の一歩目をwalk.legに書き込む + for (size_t i = 0; i < sizeof(lines) / sizeof(lines[0]); i++) + { + lines[i][1].time_s = time_s; + lines[i][1].x_m = tempwalk.leg[i].GetX_m(); + lines[i][1].y_m = tempwalk.leg[i].GetY_m(); + lines[i][1].is_point_to_point = 0; + } + //前回終点から次回始点まで直線移動 + for (int i = 0; i < 4; i++) + SetOneLegFreeLinesParam(tempwalk, i, lines[i], sizeof(lines[i]) / sizeof(lines[i][0])); + tempwalk.ResetPhase(); + MoveOneCycle(tempwalk, leg); + SetWalk(walk, nextway); +}; \ No newline at end of file