test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: main.cpp
- Revision:
- 39:87dcdff27797
- Parent:
- 38:e664b1c12da9
- Child:
- 41:38d79b6513c0
diff -r e664b1c12da9 -r 87dcdff27797 main.cpp --- a/main.cpp Mon Mar 04 10:51:46 2019 +0000 +++ b/main.cpp Mon Mar 04 23:41:48 2019 +0000 @@ -68,6 +68,7 @@ { STANDUP, LRFPOSTURE, + OVERCOME, FRONTLEG_ON_SANDDUNE, //前足かける }; @@ -106,20 +107,25 @@ stridetime_s, toptime_s, buffer_time_s); walk.SetOffsetTime(0, 0.5, 0.5, 0); break; - - case FRONTLEG_ON_SANDDUNE: //前足かける - // for(int q = 0; q < 4; q++) - // SetOneLegFreeLinesParam(walk, q, over_come_front, sizeof(over_come_front) / sizeof(over_come_front[0])); - // walk.SetOffsetTime(0, 0.5, 0.5, 0); - + case FRONTLEG_ON_SANDDUNE: + { //前足かける + /* for (int q = 0; q < 4; q++) + SetOneLegFreeLinesParam(walk, q, over_come_front, sizeof(over_come_front) / sizeof(over_come_front[0])); + walk.SetOffsetTime(0, 0.5, 0.5, 0); */ OverCome overcome(offset_x_m, start_y_m, d_x_m, goal_y_m, overcome_height_m, gravity_dist, walk.leg); walk.Copy(overcome.walk); + } + break; + case OVERCOME: //4点歩行でトロットし続ける + 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; } return walk.CheckOrbit(); } -Walk* walk; -OneLeg leg[4]; //各足の位置 int main() { @@ -127,64 +133,60 @@ if (FileOpen()) //csv fileに書き込み return 1; //異常終了したら強制終了 #endif - + OneLeg leg[4]; //各足の位置 for (int i = 0; i < 4; i++) leg[i] = OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2); - walk = new Walk(leg); //歩行法はここに入れていく + Walk walk(leg); //歩行法はここに入れていく Walk::calctime_s_ = 0.03; printf("Stand up?\r\n"); WaitStdin('y'); // ボタンを押したら立つ - if (SetWalk(*walk, STANDUP) == 1) + if (SetWalk(walk, STANDUP) == 1) { printf("error: stand move\r\n"); return 1; //強制終了.errorは内部の関数からprintfで知らせる } - if (MoveOneCycle(*walk, leg) == 1) - { - printf("error: stand move\r\n"); - return 1; //強制終了.errorは内部の関数からprintfで知らせる - } + MoveOneCycle(walk, leg); printf("Move?\r\n"); WaitStdin('y'); // ボタンを押したらスタート #ifdef USE_ROS nh_mbed.getHardware()->setBaud(115200); nh_mbed.initNode(); nh_mbed.subscribe(sub_vel); - while (1){ + while (1) + { + SetWalk(walk, LRFPOSTURE); + MoveOneCycle(walk, leg); nh_mbed.spinOnce(); - } + } #else - - if (SetWalk(*walk, FRONTLEG_ON_SANDDUNE) == 1) + if (SetWalk(walk, OVERCOME) == 1) + { + printf("error: OVERCOME\r\n"); + return 1; //強制終了 + } + for (int i = 0; i < 2; i++) //歩数 + MoveOneCycle(walk, leg); + /* + if (SetWalk(walk, FRONTLEG_ON_SANDDUNE) == 1) { printf("error: triangle\r\n"); return 1; //強制終了.errorは内部の関数からprintfで知らせる } - for (int i = 0; i < 2; i++) - { - if (MoveOneCycle(*walk, leg) == 1) - { - printf("error: triangle move\r\n"); - return 1; //強制終了.errorは内部の関数からprintfで知らせる - } - } + MoveOneCycle(walk, leg); + offset_y_m[LEFT_F] = 0.29; offset_y_m[RIGHT_F] = 0.29; - if (SetWalk(*walk, LRFPOSTURE) == 1) + */ + /* + if (SetWalk(walk, LRFPOSTURE) == 1) { printf("error: triangle\r\n"); return 1; //強制終了.errorは内部の関数からprintfで知らせる } - - for (int i = 0; i < 20; i++) - { - if (MoveOneCycle(*walk, leg) == 1) - { - printf("error: triangle move\r\n"); - return 1; //強制終了.errorは内部の関数からprintfで知らせる - } - } + for (int i = 0; i < 2; i++) + MoveOneCycle(walk, leg); + */ printf("program end\r\n"); #ifdef VSCODE @@ -278,7 +280,7 @@ { float left_vel = cmd_vel.x; float right_vel = cmd_vel.y; - int state = cmd_vel.z; +// int state = cmd_vel.z; //閾値は要検討 if (right_vel < left_vel) { @@ -294,7 +296,5 @@ stride_m[LEFT_B] = 0.2; stride_m[RIGHT_B] = 0.2; } - SetWalk(*walk, LRFPOSTURE); - MoveOneCycle(*walk, leg); } #endif \ No newline at end of file