test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: main.cpp
- Revision:
- 42:982064594ba6
- Parent:
- 41:38d79b6513c0
- Child:
- 43:2ed84f3558c1
diff -r 38d79b6513c0 -r 982064594ba6 main.cpp --- a/main.cpp Tue Mar 05 01:45:03 2019 +0000 +++ b/main.cpp Tue Mar 05 09:08:40 2019 +0000 @@ -10,9 +10,10 @@ #include "pi.h" #include "can.h" #define USE_CAN //can通信するならdefine.しないなら切らないとエラー出る -//#define USE_ROS +#define USE_ROS #include <ros.h> #include <geometry_msgs/Vector3.h> +#include <std_msgs/Int16.h> #endif //#define DEBUG_ON //デバッグ用。使わないときはコメントアウト #include "debug.h" @@ -31,12 +32,12 @@ const double kServoValToDegree = 270.0 / (11500 - 3500); //ICSの値を度に変換 const double kOriginDegree[2][2] = { { - (7300 - 3500) * kServoValToDegree, - (6995 - 3500) * kServoValToDegree + 180, + (7268 - 3500) * kServoValToDegree, + (7768 - 3500) * kServoValToDegree + 180, }, { - (7619 - 3500) * kServoValToDegree, - (7085 - 3500) * kServoValToDegree + 180, + (7914 - 3500) * kServoValToDegree, + (7543 - 3500) * kServoValToDegree + 180, }, }; /////////////// @@ -52,12 +53,15 @@ ros::NodeHandle nh_mbed; //ROSからのコールバック関数 void callback(const geometry_msgs::Vector3 &cmd_vel); +void callback_state(const std_msgs::Int16 &cmd_vel); //LPからの左右速度比を受けとり、それをもとに歩行パターンを決定する //1サイクルの間、通信は遮断され、サイクル終了後に通信を受け付ける ros::Subscriber<geometry_msgs::Vector3> sub_vel("/cmd_vel", &callback); +ros::Subscriber<std_msgs::Int16> sub_state("/state", &callback_state); +// ros::Publisher<geometry_msgs::Vector3> pub_vel("/callback_vel"); #endif -FILE *fp; + FILE *fp; const float kRadToDegree = 180.0 / M_PI; int MoveOneCycle(Walk walkway, OneLeg leg[4]); void MoveServo(OneLeg leg, int legnum, int servo_id); @@ -67,56 +71,72 @@ enum WalkWay { STANDUP, - LRFPOSTURE, - OVERCOME, - FRONTLEG_ON_SANDDUNE, //前足かける + LRFPOSTURE, //LRF用 + FRONTLEG_ON_SANDDUNE, //前足を段差にかける + OVERCOME, //前足が乗った状態で進む + BACKLEG_ON_SANDDUNE, //後ろ脚を段差に載せる + OVERCOME2, //後ろ脚が乗った状態で進む + CHECK, //check用に最後に置いておく }; - -float offset_x_m[4] = {-0.1, -0.1, -0.1, -0.1}, +//LRFPOSTUREだけspinOnceで変更するためグローバルで +float offset_x_m[4] = {-0.15, 0.15, -0.15, 0.15}, offset_y_m[4] = {0.35, 0.35, 0.35, 0.35}; -//strideはLRFから変更するようにする float stride_m[4] = {0.2, 0.2, 0.2, 0.2}, - height_m[4] = {0.02, 0.02, 0.02, 0.02}, buffer_height_m = 0.05, - stridetime_s = 1, toptime_s = 0.4, buffer_time_s = 0.2; -/* - LineParam over_come_front[]{ - {.time_s = 0, .x_m = -0.075, .y_m = 0.41}, - {.time_s = 0.2, .x_m = -0.175, .y_m = 0.26}, - {.time_s = 0.4, .x_m = 0.075, .y_m = 0.26}, - {.time_s = 0.6, .x_m = 0.075, .y_m = 0.41}, - {.time_s = 1.6, .x_m = -0.075, .y_m = 0.41}, - }; -*/ -float d_x_m = 0.1; -float start_y_m[4] = {0.41, 0.41, 0.41, 0.41}; -float goal_y_m[4] = {0.41, 0.29, 0.41, 0.29}; -float overcome_height_m[] = {0.1, 0.2, 0.1, 0.2}; -float gravity_dist[] = {0.01, -0.01, 0.01, -0.01}; + 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; + +int state_from_ros = 0; // +enum ROS_STATE +{ + RUN, + SANDDUNE, + ROPE +}; int SetWalk(Walk &walk, WalkWay way) { switch (way) { - case STANDUP: + case STANDUP: //LRF用に変数はグローバルにある 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], offset_y_m[i], 10); break; - case LRFPOSTURE: + case LRFPOSTURE: //LRF用に変数はグローバルにある 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); 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: //前足を段差にかける + { + float d_x_m = 0.1; + float start_y_m[4] = {0.41, 0.41, 0.41, 0.41}; + float goal_y_m[4] = {0.41, 0.29, 0.41, 0.29}; + float overcome_height_m[] = {0.1, 0.2, 0.1, 0.2}; + float gravity_dist[] = {0.05, 0, 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); walk.Copy(overcome.walk); + break; } - break; - case OVERCOME: //4点歩行でトロットし続ける + case BACKLEG_ON_SANDDUNE: //後ろ脚を乗せる + { + float d_x_m = 0.1; + float start_x_m[4] = {0, -0.08, 0, -0.08}; + 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 overcome_height_m[] = {0.2, 0.1, 0.2, 0.1}; + float gravity_dist[] = {0.15, 0, 0.05, -0.05}; + OverCome overcome(start_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: //前足が乗った状態で進む + { + float offset_x_m[4] = {-0.15, 0.15, -0.15, 0.15}, + offset_y_m[4] = {0.35, 0.35, 0.35, 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, @@ -124,6 +144,25 @@ 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 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; + } + default: + printf("error: there is no WalkWay\r\n"); + return 1; + } + return walk.CheckOrbit(); } @@ -138,6 +177,14 @@ leg[i] = OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2); Walk walk(leg); //歩行法はここに入れていく Walk::calctime_s_ = 0.03; + for (int i = 0; i < CHECK; i++) + { + if (SetWalk(walk, (WalkWay)i) == 1) + { + printf("error: move %d\r\n", i); + return 1; //強制終了.errorは内部の関数からprintfで知らせる + } + } printf("Stand up?\r\n"); WaitStdin('y'); // ボタンを押したら立つ if (SetWalk(walk, STANDUP) == 1) @@ -152,57 +199,57 @@ nh_mbed.getHardware()->setBaud(115200); nh_mbed.initNode(); nh_mbed.subscribe(sub_vel); + nh_mbed.subscribe(sub_state); + nh_mbed.spinOnce(); //一度受信 while (1) { + SetWalk(walk, LRFPOSTURE); MoveOneCycle(walk, leg); nh_mbed.spinOnce(); - } -#else - /*if (SetWalk(walk, OVERCOME) == 1) - { - printf("error: OVERCOME\r\n"); - return 1; //強制終了 - } - for (int i = 0; i < 2; i++) //歩数 - MoveOneCycle(walk, leg);*/ - stride_m[LEFT_F] = 0.25; - stride_m[RIGHT_F] = 0.25; - stride_m[LEFT_B] = 0.25; - stride_m[RIGHT_B] = 0.25; - if (SetWalk(walk, LRFPOSTURE) == 1) - { - printf("error: OVERCOME\r\n"); - return 1; //強制終了 + if (state_from_ros = SANDDUNE) + { +#endif + //前足を段差にかける + if (SetWalk(walk, FRONTLEG_ON_SANDDUNE) == 1) + { + printf("error: triangle\r\n"); + return 1; //強制終了. + } + for (int i = 0; i < 1; i++) + MoveOneCycle(walk, leg); + //前足が段差に乗った状態で進む + if (SetWalk(walk, OVERCOME) == 1) + { + printf("error: triangle\r\n"); + return 1; //強制終了. + } + for (int i = 0; i < 5; i++) + MoveOneCycle(walk, leg); + //後ろ脚載せる + if (SetWalk(walk, BACKLEG_ON_SANDDUNE) == 1) + { + printf("error: triangle\r\n"); + return 1; //強制終了. + } + for (int i = 0; i < 1; i++) + MoveOneCycle(walk, leg); + //後ろ脚乗った状態で進む + if (SetWalk(walk, OVERCOME2) == 1) + { + printf("error: triangle\r\n"); + return 1; //強制終了. + } + for (int i = 0; i < 5; i++) + MoveOneCycle(walk, leg); +#ifdef USE_ROS + } } - while(1) 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++) - MoveOneCycle(walk, leg); - - offset_y_m[LEFT_F] = 0.29; - offset_y_m[RIGHT_F] = 0.29; - */ - /* - if (SetWalk(walk, LRFPOSTURE) == 1) - { - printf("error: triangle\r\n"); - return 1; //強制終了.errorは内部の関数からprintfで知らせる - } - for (int i = 0; i < 2; i++) - MoveOneCycle(walk, leg); - */ - +#endif printf("program end\r\n"); #ifdef VSCODE fclose(fp); #endif -#endif } //一サイクル分進む.return 1:異常終了 int MoveOneCycle(Walk walkway, OneLeg leg[4]) @@ -288,23 +335,13 @@ #ifdef USE_ROS void callback(const geometry_msgs::Vector3 &cmd_vel) { - float left_vel = cmd_vel.x; - float right_vel = cmd_vel.y; -// int state = cmd_vel.z; - //閾値は要検討 - if (right_vel < left_vel) - { - stride_m[LEFT_F] = 0.2; - stride_m[RIGHT_F] = 0.2; - stride_m[LEFT_B] = 0.1; - stride_m[RIGHT_B] = 0.1; - } - else - { - stride_m[LEFT_F] = 0.1; - stride_m[RIGHT_F] = 0.1; - stride_m[LEFT_B] = 0.2; - stride_m[RIGHT_B] = 0.2; - } + stride_m[LEFT_F] = cmd_vel.x; + stride_m[LEFT_B] = cmd_vel.x; + stride_m[RIGHT_F] = cmd_vel.y; + stride_m[RIGHT_B] = cmd_vel.y; +} +void callback_state(const std_msgs::Int16 &cmd_vel) +{ + state_from_ros = cmd_vel.data; } #endif \ No newline at end of file