test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: main.cpp
- Revision:
- 49:198030e84936
- Parent:
- 48:1aad3cc386e8
- Child:
- 50:36741e8ab197
--- a/main.cpp Fri Mar 08 06:09:31 2019 +0000 +++ b/main.cpp Mon Mar 11 06:36:56 2019 +0000 @@ -14,7 +14,7 @@ #include "mbed.h" #include "pinnames.h" #include "can.h" -#define USE_ROS //ROSを使うときはコメントアウトを外す +//#define USE_ROS //ROSを使うときはコメントアウトを外す #include "ros.h" #include "geometry_msgs/Vector3.h" #include "std_msgs/Int16.h" @@ -59,9 +59,9 @@ AREA1_LRFWALK, //段差までLRFありで歩く.元LRFPOSTURE AREA2_LRFWALK, //段差後からLRFありで歩く STANDUP_SANDDUNE, //段差前で立つ - BEFORE_SANDDUNE, //段差に壁当て FRONTLEG_ON_SANDDUNE, //前足を段差にかける OVERCOME, //前足が乗った状態で進む + FALL_FRONTLEG, //前足を段差から降ろす BACKLEG_ON_SANDDUNE, //後ろ脚を段差に載せる OVERCOME2, //後ろ脚が乗った状態で進む AFTER_OVERCOME, //段差を終了したら座って指示まち @@ -81,15 +81,16 @@ { //複数歩行に共有するパラメータはここに書く。それ以外はcase内に入れる。 //LRFを使って歩行する際のパラメータ.stride_mだけグローバルにある。 - float area1_offset_y_m[4] = {0.27, 0.27, 0.27, 0.27}; + float area1_offset_y_m[4] = {0.3, 0.3, 0.3, 0.3}; float area2_offset_y_m[4] = {0.27, 0.27, 0.27, 0.27}; - float offset_x_m[4] = {-0.15, 0.15, -0.15, 0.15}; + float offset_x_m[4] = {0, 0, 0, 0}; float height_m[4] = {0.05, 0.05, 0.05, 0.05}, buffer_height_m = 0.02, - stridetime_s = 1, toptime_s = 0.2, buffer_time_s = 0.1; + stridetime_s = 0.45, toptime_s = 0.2, buffer_time_s = 0.1; //段差 - float overcome_start_y_m[] = {0.41, 0.41, 0.41, 0.41}; - + 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 turn_start_x_m = 0, turn_start_y_m = 0.275, turn_stride_m = 0.05, turn_height_m = 0.05, @@ -107,9 +108,9 @@ } case AREA1_LRFWALK: //LRF用に変数はグローバルにある for (int i = 0; i < 4; i++) - SetOneLegTriangleParam(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); + 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 AREA2_LRFWALK: @@ -119,25 +120,14 @@ stridetime_s, toptime_s, buffer_time_s); walk.SetOffsetTime(0, 0.5, 0.5, 0); break; - case BEFORE_SANDDUNE: - { - float stride_short_m[] = {0.1, 0.1, 0.1, 0.1}; - for (int i = 0; i < 4; i++) - SetOneLegTriangleParam(walk, i, offset_x_m[i], overcome_start_y_m[i], - stride_short_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 STANDUP_SANDDUNE: { - float start_y_m[4] = {0.41, 0.41, 0.41, 0.41}; 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 = start_y_m[i], .is_point_to_point = 0}, - {.time_s = 1.5, .x_m = offset_x_m[i], .y_m = start_y_m[i], .is_point_to_point = 0}, + {.time_s = 0.5, .x_m = offset_x_m[i], .y_m = overcome_start_y_m[i], .is_point_to_point = 0}, + {.time_s = 1.5, .x_m = offset_x_m[i], .y_m = overcome_start_y_m[i], .is_point_to_point = 0}, }; SetOneLegFreeLinesParam(walk, i, lines, sizeof(lines) / sizeof(lines[0])); } @@ -147,47 +137,54 @@ case FRONTLEG_ON_SANDDUNE: { //前足を段差にかける float d_x_m = 0.1; - float goal_y_m[4] = {0.41, 0.29, 0.41, 0.29}; float raise_offset_x_m[4] = {0, -0.02, 0, 0}; - float overcome_height_m[] = {0.1, 0.2, 0.1, 0.2}; + 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, goal_y_m, + 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); + walk.Copy(overcome.walk); + break; + } + case OVERCOME: + { //前足が乗った状態で進む + float stride_m[4] = {0.07, 0.07, 0.07, 0.07}; + for (int i = 0; i < 4; i++) + SetOneLegFourPointParam(walk, i, offset_x_m[i], flontleg_sand_goal_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 FALL_FRONTLEG: //前足を段差から降ろす + { + float d_x_m = 0.15; + float start_x_m[4] = {0, 0, 0, 0}; + float raise_offset_x_m[4] = {}; + 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); walk.Copy(overcome.walk); break; } case BACKLEG_ON_SANDDUNE: { //後ろ脚を乗せる - float d_x_m = 0.1; + float d_x_m = 0.15; float start_x_m[4] = {0, 0, 0, 0}; float start_y_m[4] = {0.41, 0.41, 0.41, 0.41}; - float goal_y_m[4] = {0.29, 0.41, 0.29, 0.41}; + float raise_offset_x_m[4] = {}; 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, goal_y_m, + 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); walk.Copy(overcome.walk); break; } - case OVERCOME: - { //前足が乗った状態で進む - float offset_x_m[4] = {-0.15, 0.15, -0.15, 0.15}, - offset_y_m[4] = {0.38, 0.35, 0.38, 0.35}; - 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, - stridetime_s = 1, toptime_s = 0.2, buffer_time_s = 0.2; - for (int i = 0; i < 4; i++) - SetOneLegFourPointParam(walk, i, offset_x_m[i], 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 OVERCOME2: { //後ろ脚が乗った状態で進む - float offset_x_m[4] = {}, - offset_y_m[4] = {0.35, 0.35, 0.35, 0.35}; + 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, stridetime_s = 1, toptime_s = 0.2, buffer_time_s = 0.2; @@ -252,7 +249,7 @@ offset_y_m[4] = {0.32, 0.22, 0.32, 0.22}; float stride_m[4] = {0.18, 0.18, 0.2, 0.2}; //ropeのときは0.15 float height_m[4] = {0.05, 0.05, 0.05, 0.05}, buffer_height_m = 0.02, - stridetime_s = 1, toptime_s = 0.2, buffer_time_s = 0.1; + stridetime_s = 1, toptime_s = 0.15, buffer_time_s = 0.05; for (int i = 0; i < 4; i++) SetOneLegTriangleParam(walk, i, offset_x_m[i], offset_y_m[i], stride_m[i], height_m[i], buffer_height_m, @@ -308,106 +305,114 @@ nh_mbed.subscribe(sub_state); nh_mbed.advertise(pub_vel); nh_mbed.spinOnce(); //一度受信 -#else - printf("Stand up?\r\n"); - WaitStdin('y'); // キーボード入力されるまでまで待つ #endif + ROS_STATE old_state = STOP; /////立つ - SetWalk(walk, STANDUP); - // SetWalk(walk, AFTER_OVERCOME); //段差越え後調整用に一時的に入れた - MoveOneCycle(walk, leg); #ifndef USE_ROS - printf("Move?\r\n"); - WaitStdin('y'); //キーボード入力されるまでまで待つ state_from_ros = SANDDUNE; //遠してやらないならやりたいSTATEに変更 #endif - int loopcount = 0; - ROS_STATE old_state = state_from_ros; //プログラムの最初のほうにあるstate_from_rosで動かすものを切り替える.caseはenum ROS_STATEで分ける - while (1) + // while (1) + // { + switch (state_from_ros) { - switch (state_from_ros) - { - case STOP: - break; - case AREA1_LRFWALK_STATE: - if (old_state != state_from_ros) - SmoothChange(walk, leg, AREA1_LRFWALK, 0.5); //切り替え - else - SetWalk(walk, AREA1_LRFWALK); + 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'); //キーボード入力されるまでまで待つ + SmoothChange(walk, leg, AREA1_LRFWALK, 0.5); //切り替え + for (int i = 0; i < 20; i++) MoveOneCycle(walk, leg); - 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: - //前足を段差にかける - SmoothChange(walk, leg, FRONTLEG_ON_SANDDUNE, 0.5); //切り替えスムーズ + 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, OVERCOME, 0.5); //切り替えスムーズ - for (int i = 0; i < 5; i++) - MoveOneCycle(walk, leg); - //後ろ脚載せる - SmoothChange(walk, leg, BACKLEG_ON_SANDDUNE, 0.5); //切り替えスムーズ - for (int i = 0; i < 1; i++) - MoveOneCycle(walk, leg); - //後ろ脚乗った状態で進む - SmoothChange(walk ,leg, OVERCOME2, 0.5); //切り替えスムーズ - for (int i = 0; i < 3; i++) - MoveOneCycle(walk, leg); - //数歩左に曲がる - SmoothChange(walk, leg, TRUNLEFT, 0.5); //切り替えスムーズ - for (int i = 0; i < 2; i++) - MoveOneCycle(walk, leg); - state_from_ros = AREA2_LRFWALK_STATE; - break; - case ROPE_STATE: - //前足だけ紐越え - SmoothChange(walk, leg, ROPE, 0.5); + //前足を降ろす + 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); - //LRFが取れる高さで歩行 - SmoothChange(walk, leg, AREA1_LRFWALK, 0.5); - for (int i = 0; i < 3; i++) - MoveOneCycle(walk, leg); - //後ろ足越え - SmoothChange(walk,leg, ROPE_BACK, 0.5); + //普通に進む + SetWalk(walk, AREA1_LRFWALK); + for (int i = 0; i < 4; i++) + MoveOneCycle(walk, leg); + /* //数歩左に曲がる + SmoothChange(walk, leg, TRUNLEFT, 0.5); //切り替えスムーズ + for (int i = 0; i < 2; i++) MoveOneCycle(walk, leg); - state_from_ros = AREA2_LRFWALK_STATE; - 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; + */ + 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); + 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; + 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(); + nh_mbed.spinOnce(); #endif -#ifdef VSCODE - ++loopcount; - if (loopcount > 1) - break; //VSCODEでデバックする際は一ループで抜ける -#endif - } printf("program end\r\n"); #ifdef VSCODE fclose(fp);