test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: main.cpp
- Revision:
- 50:36741e8ab197
- Parent:
- 49:198030e84936
--- a/main.cpp Mon Mar 11 06:36:56 2019 +0000 +++ b/main.cpp Mon Mar 11 10:38:07 2019 +0000 @@ -31,11 +31,8 @@ STOP, AREA1_LRFWALK_STATE, SANDDUNE, - AREA2_LRFWALK_STATE, ROPE_STATE, SLOPE_STATE, - TURNRIGHT_STATE, - TRUNLEFT_STATE, }; ROS_STATE state_from_ros = STOP; //ここを変えると動く方法が変わる #ifdef USE_ROS @@ -65,9 +62,9 @@ BACKLEG_ON_SANDDUNE, //後ろ脚を段差に載せる OVERCOME2, //後ろ脚が乗った状態で進む AFTER_OVERCOME, //段差を終了したら座って指示まち + WALK_BEFORE_ROPE, //紐前で歩く ROPE, //rope前足超える ROPE_BACK, //rope後ろ足超える - ROPE_DOWN, //rope機体高さをLRF用に落とす SLOPE, TRUNRIGHT, TRUNLEFT, @@ -91,10 +88,12 @@ float overcome_start_y_m[] = {0.38, 0.38, 0.38, 0.38}; float flontleg_sand_goal_y_m[4] = {0.38, 0.27, 0.38, 0.27}; float backleg_sand_goal_y_m[4] = {0.27, 0.38, 0.27, 0.38}; + float d_time = 0.2, d_time_slow = 0.3; //旋回 float turn_start_x_m = 0, turn_start_y_m = 0.275, - turn_stride_m = 0.05, turn_height_m = 0.05, - turn_stridetime_s = 0.6, turn_risetime_s = 0.3; + turn_stride_m = 0.075, turn_height_m = 0.05, + turn_stridetime_s = 0.5, turn_risetime_s = 0.2; + float d_time_rope = 0.6, d_time_slow_rope = 0.3; switch (way) { case STANDUP: @@ -141,7 +140,8 @@ float overcome_height_m[] = {0.05, 0.15, 0.05, 0.15}; float gravity_dist[] = {0.05, 0, 0.05, -0.05}; OverCome overcome(offset_x_m, overcome_start_y_m, d_x_m, flontleg_sand_goal_y_m, - overcome_height_m, gravity_dist, walk.leg, raise_offset_x_m); + overcome_height_m, gravity_dist, walk.leg, + raise_offset_x_m, d_time, d_time_slow); walk.Copy(overcome.walk); break; } @@ -163,7 +163,8 @@ float overcome_height_m[] = {0.05, 0.05, 0.05, 0.05}; float gravity_dist[] = {0.1, 0, 0.1, 0}; OverCome overcome(start_x_m, flontleg_sand_goal_y_m, d_x_m, overcome_start_y_m, - overcome_height_m, gravity_dist, walk.leg, raise_offset_x_m); + overcome_height_m, gravity_dist, walk.leg, + raise_offset_x_m, d_time, d_time_slow); walk.Copy(overcome.walk); break; } @@ -177,13 +178,14 @@ float overcome_height_m[] = {0.2, 0.1, 0.2, 0.1}; float gravity_dist[] = {0.1, 0, 0.1, 0}; OverCome overcome(start_x_m, start_y_m, d_x_m, backleg_sand_goal_y_m, - overcome_height_m, gravity_dist, walk.leg, raise_offset_x_m); + overcome_height_m, gravity_dist, walk.leg, + raise_offset_x_m, d_time, d_time_slow); walk.Copy(overcome.walk); break; } case OVERCOME2: { //後ろ脚が乗った状態で進む - float offset_x_m[4] = {0.1,0,0.1,0}; + float offset_x_m[4] = {0.1, 0, 0.1, 0}; float offset_y_m[4] = {0.27, 0.33, 0.27, 0.33}; float stride_m[4] = {0.2, 0.2, 0.2, 0.2}, height_m[4] = {0.1, 0.1, 0.1, 0.1}, buffer_height_m = 0.05, @@ -202,47 +204,47 @@ walk.SetOffsetTime(0, 0, 0, 0); break; } + case WALK_BEFORE_ROPE: + { + // float stride_m[] = {0.2, 0.2, 0.125, 0.125}; + float stride_m[] = {0.075, 0.075, 0.2, 0.2}; + for (int i = 0; i < 4; i++) + SetOneLegFourPointParam(walk, i, offset_x_m[i], area1_offset_y_m[i], + stride_m[i], height_m[i], buffer_height_m, + stridetime_s, toptime_s, buffer_time_s); + walk.SetOffsetTime(0, 0.5, 0.5, 0); + break; + } case ROPE: { float offset_x_m[4] = {-0.05, 0, -0.05, 0}; float d_x_m = 0.2; float start_y_m[4] = {0.41, 0.41, 0.41, 0.41}; float goal_y_m[4] = {0.41, 0.41, 0.41, 0.41}; - float raise_offset_x_m[] = {0, -0.08, 0, -0.08}; + float raise_offset_x_m[] = {0, -0.15, 0, -0.15}; float overcome_height_m[] = {0.05, 0.2, 0.05, 0.2}; float gravity_dist[] = {0.05, -0.05, 0.05, -0.05}; OverCome overcome(offset_x_m, start_y_m, d_x_m, goal_y_m, - overcome_height_m, gravity_dist, walk.leg, raise_offset_x_m); + overcome_height_m, gravity_dist, walk.leg, + raise_offset_x_m, d_time_rope, d_time_slow_rope); walk.Copy(overcome.walk); break; } case ROPE_BACK: { float offset_x_m[4] = {}; - float d_x_m = 0.15; - float start_y_m[4] = {0.41, 0.41, 0.41, 0.41}; - float goal_y_m[4] = {0.41, 0.41, 0.41, 0.41}; - float raise_offset_x_m[] = {-0.03, 0, -0.03, 0}; + float d_x_m = 0.2; + float start_y_m[4] = {0.41, 0.38, 0.41, 0.38}; + float goal_y_m[4] = {0.41, 0.38, 0.41, 0.38}; + float raise_offset_x_m[] = {-0.1, 0, -0.1, 0}; float overcome_height_m[] = {0.2, 0.05, 0.2, 0.05}; - float gravity_dist[] = {0.08, 0, 0.08, 0}; + float gravity_dist[] = {0.25, -0.05, 0.05, -0.05}; OverCome overcome(offset_x_m, start_y_m, d_x_m, goal_y_m, - overcome_height_m, gravity_dist, walk.leg, raise_offset_x_m); + overcome_height_m, gravity_dist, walk.leg, + raise_offset_x_m, d_time_rope, d_time_slow_rope); walk.Copy(overcome.walk); break; } - case ROPE_DOWN: - { - for (int i = 0; i < 4; i++) - { - LineParam lines[] = { - {.time_s = 0, .x_m = walk.leg[i].GetX_m(), .y_m = walk.leg[i].GetY_m(), .is_point_to_point = 0}, - {.time_s = 0.5, .x_m = offset_x_m[i], .y_m = area1_offset_y_m[i], .is_point_to_point = 0}, - }; - SetOneLegFreeLinesParam(walk, i, lines, sizeof(lines) / sizeof(lines[0])); - } - walk.SetOffsetTime(0, 0, 0, 0); - break; - } case SLOPE: { float offset_x_m[4] = {-0.15, 0.15, -0.15, 0.15}, @@ -306,63 +308,51 @@ nh_mbed.advertise(pub_vel); nh_mbed.spinOnce(); //一度受信 #endif - ROS_STATE old_state = STOP; + /////立つ -#ifndef USE_ROS - state_from_ros = SANDDUNE; //遠してやらないならやりたいSTATEに変更 -#endif + state_from_ros = ROPE_STATE; + switch (state_from_ros) + { + case SANDDUNE: + SetWalk(walk, STANDUP_SANDDUNE); + break; + default: + SetWalk(walk, STANDUP); + break; + } + printf("Stand up?\r\n"); + WaitStdin('y'); // キーボード入力されるまでまで待つ + MoveOneCycle(walk, leg); + printf("Move?\r\n"); + WaitStdin('y'); //プログラムの最初のほうにあるstate_from_rosで動かすものを切り替える.caseはenum ROS_STATEで分ける - // while (1) - // { switch (state_from_ros) { case STOP: break; - case AREA1_LRFWALK_STATE: - printf("Stand up?\r\n"); - WaitStdin('y'); // キーボード入力されるまでまで待つ - SetWalk(walk, STANDUP); - MoveOneCycle(walk, leg); - printf("Move?\r\n"); - WaitStdin('y'); //キーボード入力されるまでまで待つ + case AREA1_LRFWALK_STATE: //キーボード入力されるまでまで待つ SmoothChange(walk, leg, AREA1_LRFWALK, 0.5); //切り替え for (int i = 0; i < 20; i++) MoveOneCycle(walk, leg); - state_from_ros = STOP; - break; - case AREA2_LRFWALK_STATE: - if (old_state != state_from_ros) - SmoothChange(walk, leg, AREA2_LRFWALK, 0.5); //切り替え - else - SetWalk(walk, AREA2_LRFWALK); - MoveOneCycle(walk, leg); break; case SANDDUNE: - //前足を段差にかける - printf("Stand up?\r\n"); - WaitStdin('y'); // キーボード入力されるまでまで待つ - SetWalk(walk, STANDUP_SANDDUNE); - MoveOneCycle(walk, leg); - printf("Move?\r\n"); - WaitStdin('y'); //キーボード入力されるまでまで待つ + //前足を段差にかける //キーボード入力されるまでまで待つ SmoothChange(walk, leg, FRONTLEG_ON_SANDDUNE, 0.5); //切り替えスムーズ MoveOneCycle(walk, leg); //前足が段差に乗った状態で進む SmoothChange(walk, leg, OVERCOME, 0.5); //切り替えスムーズ for (int i = 0; i < 2; i++) MoveOneCycle(walk, leg); - //前足を降ろす + //前足を降ろす SmoothChange(walk, leg, FALL_FRONTLEG, 0.5); MoveOneCycle(walk, leg); //またいだ状態で歩く SmoothChange(walk, leg, AREA1_LRFWALK, 0.5); MoveOneCycle(walk, leg); - //後ろ脚載せる SmoothChange(walk, leg, BACKLEG_ON_SANDDUNE, 0.5); //切り替えスムーズ MoveOneCycle(walk, leg); //後ろ脚乗った状態で進む - // SmoothChange(walk, leg, OVERCOME2, 0.5); //切り替えスムーズ SetWalk(walk, OVERCOME2); for (int i = 0; i < 3; i++) MoveOneCycle(walk, leg); @@ -370,46 +360,72 @@ SetWalk(walk, AREA1_LRFWALK); for (int i = 0; i < 4; i++) MoveOneCycle(walk, leg); - /* //数歩左に曲がる - SmoothChange(walk, leg, TRUNLEFT, 0.5); //切り替えスムーズ + break; + + case ROPE_STATE: + //紐前歩く + SetWalk(walk, WALK_BEFORE_ROPE); + for (int i = 0; i < 5; i++) + MoveOneCycle(walk, leg); + int is_arrived = 0; + while (is_arrived == 0) + { + MoveOneCycle(walk, leg); + unsigned int dist_cm = GetDist_cm(); + if (dist_cm > 90 && dist_cm < 150) + ++is_arrived; + printf("%d\r\n", dist_cm); + } + /* + SetWalk(walk, AREA1_LRFWALK); + for (int i = 0; i < 5; i++) + MoveOneCycle(walk, leg); + SmoothChange(walk, leg, TRUNRIGHT, 0.5); for (int i = 0; i < 2; i++) + MoveOneCycle(walk, leg); + */ + /*曲がりながら歩く + SmoothChange(walk, leg, WALK_BEFORE_ROPE, 0.5); + for (int i = 0; i < 7; i++) MoveOneCycle(walk, leg); */ - state_from_ros = AREA2_LRFWALK_STATE; - break; - case ROPE_STATE: //前足だけ紐越え SmoothChange(walk, leg, ROPE, 0.5); MoveOneCycle(walk, leg); //LRFが取れる高さで歩行 - SmoothChange(walk, leg, AREA1_LRFWALK, 0.5); + SmoothChange(walk, leg, AREA1_LRFWALK, 1.5); for (int i = 0; i < 3; i++) MoveOneCycle(walk, leg); //後ろ足越え SmoothChange(walk, leg, ROPE_BACK, 0.5); MoveOneCycle(walk, leg); - state_from_ros = AREA2_LRFWALK_STATE; + //紐間歩く + SmoothChange(walk, leg, AREA1_LRFWALK, 0.5); + for (int i = 0; i < 1; i++) + MoveOneCycle(walk, leg); + //2本目 + //前足だけ紐越え + SmoothChange(walk, leg, ROPE, 0.5); + MoveOneCycle(walk, leg); + //LRFが取れる高さで歩行 + SmoothChange(walk, leg, AREA1_LRFWALK, 1.5); + for (int i = 0; i < 3; i++) + MoveOneCycle(walk, leg); + //後ろ足越え + SmoothChange(walk, leg, ROPE_BACK, 0.5); + MoveOneCycle(walk, leg); + //紐後歩く + SmoothChange(walk, leg, AREA1_LRFWALK, 1.5); + for (int i = 0; i < 10; i++) + MoveOneCycle(walk, leg); break; + case SLOPE_STATE: SmoothChange(walk, leg, SLOPE, 0.5); for (int i = 0; i < 30; i++) MoveOneCycle(walk, leg); - state_from_ros = STOP; - break; - case TURNRIGHT_STATE: - 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: - 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