test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: main.cpp
- Revision:
- 46:621ea6492dea
- Parent:
- 45:0654364226c9
- Child:
- 47:c2c579909787
--- a/main.cpp Thu Mar 07 08:49:57 2019 +0000 +++ b/main.cpp Fri Mar 08 02:13:10 2019 +0000 @@ -29,14 +29,15 @@ enum ROS_STATE { STOP, - RUN, + AREA1_LRFWALK_STATE, SANDDUNE, - TURNRIGHT_STATE, + AREA2_LRFWALK_STATE, ROPE_STATE, SLOPE_STATE, - + TURNRIGHT_STATE, + TRUNLEFT_STATE, }; -int state_from_ros = 0; +ROS_STATE state_from_ros = STOP; //ここを変えると動く方法が変わる #ifdef USE_ROS ros::NodeHandle nh_mbed; void callback(const geometry_msgs::Vector3 &cmd_vel); @@ -55,7 +56,8 @@ enum WalkWay //歩行軌道 { STANDUP, //受け渡し用に待つ - LRFPOSTURE, //LRF用 + AREA1_LRFWALK, //段差までLRFありで歩く.元LRFPOSTURE + AREA2_LRFWALK, //段差後からLRFありで歩く STANDUP_SANDDUNE, //段差前で立つ BEFORE_SANDDUNE, //段差に壁当て FRONTLEG_ON_SANDDUNE, //前足を段差にかける @@ -71,17 +73,24 @@ TRUNLEFT, CHECK, //check用に最後に置いておく }; -//LRFPOSTUREだけspinOnceで変更するためグローバルで -float offset_x_m[4] = {-0.15, 0.15, -0.15, 0.15}, - offset_y_m[4] = {0.27, 0.27, 0.27, 0.27}; +//LRF使うやつだけだけspinOnceで変更するためグローバルで float stride_m[4] = {0.2, 0.2, 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; //各歩行軌道のパラメータ設定。switchで羅列している。新規に歩行パターンを増やすときはcaseを増やす。 //param walk:結果を入れる箱。way:歩行軌道を指定するWalkWayの定数。 int SetWalk(Walk &walk, WalkWay way) { + //複数歩行に共有するパラメータはここに書く。それ以外はcase内に入れる。 + //LRFを使って歩行する際のパラメータ.stride_mだけグローバルにある。 + float area1_offset_y_m[4] = {0.27, 0.27, 0.27, 0.27}; + float area2_offset_y_m[4] = {0.275, 0.275, 0.275, 0.275}; + float offset_x_m[4] = {-0.15, 0.15, -0.15, 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; + + //段差 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; @@ -96,9 +105,16 @@ walk.SetOffsetTime(0, 0, 0, 0); break; } - case LRFPOSTURE: //LRF用に変数はグローバルにある + case AREA1_LRFWALK: //LRF用に変数はグローバルにある for (int i = 0; i < 4; i++) - SetOneLegTriangleParam(walk, i, offset_x_m[i], offset_y_m[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); + walk.SetOffsetTime(0, 0.5, 0.5, 0); + break; + case AREA2_LRFWALK: + for (int i = 0; i < 4; i++) + SetOneLegTriangleParam(walk, i, offset_x_m[i], area2_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); @@ -185,7 +201,7 @@ case AFTER_OVERCOME: { for (int i = 0; i < 4; i++) - SetOneLegStandParam(walk, i, offset_x_m[i], offset_y_m[i], 0.5); + SetOneLegStandParam(walk, i, offset_x_m[i], area1_offset_y_m[i], 0.5); walk.SetOffsetTime(0, 0, 0, 0); break; } @@ -223,7 +239,7 @@ { 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 = offset_y_m[i], .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])); } @@ -266,6 +282,7 @@ if (FileOpen()) //csv fileに書き込み return 1; //異常なら強制終了 #endif + /////足の長さ、計算周期の設定 OneLeg leg[4]; //各足の位置 for (int i = 0; i < 4; i++) @@ -282,7 +299,15 @@ return 1; //強制終了.errorは内部の関数からprintfで知らせる } } -#ifndef USE_ROS +//軌道チェックしてからROS完全起動 +#ifdef USE_ROS + nh_mbed.getHardware()->setBaud(115200); + nh_mbed.initNode(); + nh_mbed.subscribe(sub_vel); + nh_mbed.subscribe(sub_state); + nh_mbed.advertise(pub_vel); + nh_mbed.spinOnce(); //一度受信 +#else printf("Stand up?\r\n"); WaitStdin('y'); // キーボード入力されるまでまで待つ #endif @@ -293,32 +318,26 @@ #ifndef USE_ROS printf("Move?\r\n"); WaitStdin('y'); //キーボード入力されるまでまで待つ -#else - nh_mbed.getHardware()->setBaud(115200); - nh_mbed.initNode(); - nh_mbed.subscribe(sub_vel); - nh_mbed.subscribe(sub_state); - nh_mbed.advertise(pub_vel); - nh_mbed.spinOnce(); //一度受信 - SetWalk(walk, LRFPOSTURE); + state_from_ros = SANDDUNE; //遠してやらないならやりたいSTATEに変更 +#endif + int loopcount = 0; + //プログラムの最初のほうにあるstate_from_rosで動かすものを切り替える.caseはenum ROS_STATEで分ける while (1) { switch (state_from_ros) { case STOP: break; - case RUN: - 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, - stridetime_s, toptime_s, buffer_time_s); + case AREA1_LRFWALK_STATE: + SetWalk(walk, AREA1_LRFWALK); + MoveOneCycle(walk, leg); + break; + case AREA2_LRFWALK_STATE: + SetWalk(walk, AREA2_LRFWALK); MoveOneCycle(walk, leg); break; case SANDDUNE: // #endif - //2歩進んで当て処理 - // for (int i = 0; i < 2; i++) - // MoveOneCycle(walk, leg); SetWalk(walk, STANDUP_SANDDUNE); MoveOneCycle(walk, leg); //前足を段差にかける @@ -340,37 +359,49 @@ SetWalk(walk, TRUNLEFT); for (int i = 0; i < 2; i++) MoveOneCycle(walk, leg); - state_from_ros = 1; + state_from_ros = AREA2_LRFWALK_STATE; // #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); + SetWalk(walk, AREA1_LRFWALK); for (int i = 0; i < 3; i++) MoveOneCycle(walk, leg); SetWalk(walk, ROPE_BACK); MoveOneCycle(walk, leg); - state_from_ros = 1; + state_from_ros = AREA2_LRFWALK_STATE; break; case SLOPE_STATE: SetWalk(walk, SLOPE); for (int i = 0; i < 30; i++) MoveOneCycle(walk, leg); - state_from_ros = 0; + state_from_ros = STOP; + break; + case TURNRIGHT_STATE: + SetWalk(walk, TRUNRIGHT); + for (int i = 0; i < 5; i++) + MoveOneCycle(walk, leg); + state_from_ros = AREA2_LRFWALK_STATE; + break; + case TRUNLEFT_STATE: + SetWalk(walk, TRUNLEFT); + for (int i = 0; i < 5; i++) + MoveOneCycle(walk, leg); + state_from_ros = AREA2_LRFWALK_STATE; break; } +#ifdef USE_ROS nh_mbed.spinOnce(); +#endif +#ifdef VSCODE + ++loopcount; + if (loopcount > 1) + break; //VSCODEでデバックする際は一ループで抜ける +#endif } -#endif printf("program end\r\n"); #ifdef VSCODE fclose(fp); @@ -379,8 +410,8 @@ #ifdef USE_ROS void callback(const geometry_msgs::Vector3 &cmd_vel) { - if (state_from_ros == 0) - state_from_ros = 1; + if (state_from_ros == STOP) + state_from_ros = AREA1_LRFWALK_STATE; stride_m[LEFT_F] = cmd_vel.x; stride_m[LEFT_B] = cmd_vel.x; stride_m[RIGHT_F] = cmd_vel.y;