岡本 陸
test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: main.cpp
- Revision:
- 42:982064594ba6
- Parent:
- 41:38d79b6513c0
- Child:
- 43:2ed84f3558c1
--- 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