test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: main.cpp
- Revision:
- 45:0654364226c9
- Parent:
- 44:4aac39b8670b
- Child:
- 46:621ea6492dea
--- a/main.cpp Thu Mar 07 06:36:34 2019 +0000 +++ b/main.cpp Thu Mar 07 08:49:57 2019 +0000 @@ -7,7 +7,6 @@ SetWalkの引数はenum WalkWayの定数。SetWalk内でswitchしている。SetWalk内ではchange_walk.hの関数で軌道を設定。 ROSありだと、ROSからのstateに従いswitchで切り替えている。 */ - #include "pi.h" #include <math.h> #include <stdio.h> @@ -32,7 +31,10 @@ STOP, RUN, SANDDUNE, - ROPE_STATE + TURNRIGHT_STATE, + ROPE_STATE, + SLOPE_STATE, + }; int state_from_ros = 0; #ifdef USE_ROS @@ -60,11 +62,13 @@ OVERCOME, //前足が乗った状態で進む BACKLEG_ON_SANDDUNE, //後ろ脚を段差に載せる OVERCOME2, //後ろ脚が乗った状態で進む - AFTER_OVERCOME, //段差を終了したら座って指示まち + AFTER_OVERCOME, //段差を終了したら座って指示まち ROPE, //rope前足超える ROPE_BACK, //rope後ろ足超える ROPE_DOWN, //rope機体高さをLRF用に落とす SLOPE, + TRUNRIGHT, + TRUNLEFT, CHECK, //check用に最後に置いておく }; //LRFPOSTUREだけspinOnceで変更するためグローバルで @@ -78,6 +82,9 @@ int SetWalk(Walk &walk, WalkWay way) { float overcome_start_y_m[] = {0.41, 0.41, 0.41, 0.41}; + 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; switch (way) { case STANDUP: @@ -182,7 +189,7 @@ walk.SetOffsetTime(0, 0, 0, 0); break; } - case ROPE: + case ROPE: { float offset_x_m[4] = {-0.05, 0, -0.05, 0}; float d_x_m = 0.2; @@ -237,6 +244,14 @@ walk.SetOffsetTime(0, 0.5, 0.5, 0); break; } + case TRUNRIGHT: + Turn(walk, 1, turn_start_x_m, turn_start_y_m, turn_stride_m, + turn_height_m, turn_stridetime_s, turn_risetime_s); + break; + case TRUNLEFT: + Turn(walk, 0, turn_start_x_m, turn_start_y_m, turn_stride_m, + turn_height_m, turn_stridetime_s, turn_risetime_s); + break; default: printf("error: there is no WalkWay\r\n"); return 1; //以上終了 @@ -273,30 +288,12 @@ #endif /////立つ // SetWalk(walk, STANDUP); - SetWalk(walk, AFTER_OVERCOME);//段差越え後調整用に一時的に入れた + SetWalk(walk, AFTER_OVERCOME); //段差越え後調整用に一時的に入れた MoveOneCycle(walk, leg); #ifndef USE_ROS printf("Move?\r\n"); WaitStdin('y'); //キーボード入力されるまでまで待つ -#endif -/* -///SLOPE - SetWalk(walk, SLOPE); - for (int i = 0; i < 30; i++) - MoveOneCycle(walk, leg); - - /////////////ROPEを試す - SetWalk(walk, ROPE); - MoveOneCycle(walk, leg); - SetWalk(walk, ROPE_DOWN); - MoveOneCycle(walk, leg); - SetWalk(walk, LRFPOSTURE); - for (int i = 0; i < 3; i++) - MoveOneCycle(walk, leg); - SetWalk(walk, ROPE_BACK); - MoveOneCycle(walk, leg); -*/ -#ifdef USE_ROS +#else nh_mbed.getHardware()->setBaud(115200); nh_mbed.initNode(); nh_mbed.subscribe(sub_vel); @@ -318,7 +315,7 @@ MoveOneCycle(walk, leg); break; case SANDDUNE: -#endif + // #endif //2歩進んで当て処理 // for (int i = 0; i < 2; i++) // MoveOneCycle(walk, leg); @@ -340,7 +337,35 @@ SetWalk(walk, OVERCOME2); for (int i = 0; i < 3; i++) MoveOneCycle(walk, leg); -#ifdef USE_ROS + SetWalk(walk, TRUNLEFT); + for (int i = 0; i < 2; i++) + MoveOneCycle(walk, leg); + state_from_ros = 1; + // #ifdef USE_ROS + break; + case TURNRIGHT_STATE: + SetWalk(walk, TRUNRIGHT); + for (int i = 0; i < 5; i++) + MoveOneCycle(walk, leg); + state_from_ros = 1; + break; + case ROPE_STATE: + SetWalk(walk, ROPE); + MoveOneCycle(walk, leg); + SetWalk(walk, ROPE_DOWN); + MoveOneCycle(walk, leg); + SetWalk(walk, LRFPOSTURE); + for (int i = 0; i < 3; i++) + MoveOneCycle(walk, leg); + SetWalk(walk, ROPE_BACK); + MoveOneCycle(walk, leg); + state_from_ros = 1; + break; + case SLOPE_STATE: + SetWalk(walk, SLOPE); + for (int i = 0; i < 30; i++) + MoveOneCycle(walk, leg); + state_from_ros = 0; break; } nh_mbed.spinOnce(); @@ -362,10 +387,11 @@ 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 = cmd.data; - led[state_from_ros] = 1; + // led[state_from_ros] = 1; } #endif \ No newline at end of file