岡本 陸
test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: main.cpp
- Revision:
- 44:4aac39b8670b
- Parent:
- 43:2ed84f3558c1
- Child:
- 45:0654364226c9
diff -r 2ed84f3558c1 -r 4aac39b8670b main.cpp
--- a/main.cpp Wed Mar 06 12:13:46 2019 +0000
+++ b/main.cpp Thu Mar 07 06:36:34 2019 +0000
@@ -1,4 +1,13 @@
-//NHK2019MR2 馬型機構プログラム.
+/* NHK2019MR2 馬型機構プログラム.
+//基本プログラム
+SetWalk(walk, 歩行パターン);//歩行パターン設定
+MoveOneCycle(walk, leg);//1回呼び出すと一サイクル分進む。2歩以上進みたいときはforで回して回数分呼び出す
+
+で動く。
+SetWalkの引数はenum WalkWayの定数。SetWalk内でswitchしている。SetWalk内ではchange_walk.hの関数で軌道を設定。
+ROSありだと、ROSからのstateに従いswitchで切り替えている。
+ */
+
#include "pi.h"
#include <math.h>
#include <stdio.h>
@@ -6,7 +15,7 @@
#include "mbed.h"
#include "pinnames.h"
#include "can.h"
-//#define USE_ROS //ROSを使うときはコメントアウトを外す
+#define USE_ROS //ROSを使うときはコメントアウトを外す
#include "ros.h"
#include "geometry_msgs/Vector3.h"
#include "std_msgs/Int16.h"
@@ -30,8 +39,10 @@
ros::NodeHandle nh_mbed;
void callback(const geometry_msgs::Vector3 &cmd_vel);
void callback_state(const std_msgs::Int16 &cmd_vel);
+geometry_msgs::Vector3 back_vel;
ros::Subscriber<geometry_msgs::Vector3> sub_vel("/cmd_vel", &callback);
ros::Subscriber<std_msgs::Int16> sub_state("/state", &callback_state);
+ros::Publisher pub_vel("/back_vel", &back_vel);
#endif
////////////あまり変化させないパラメーター
const float kBetweenServoHalf_m = 0.034 * 0.5; //サーボ間の距離の半分
@@ -39,7 +50,7 @@
float kLegLength2[2] = {0.33, 0.34};
///////////////
////////調整するべきパラメータ
-enum WalkWay
+enum WalkWay //歩行軌道
{
STANDUP, //受け渡し用に待つ
LRFPOSTURE, //LRF用
@@ -49,6 +60,7 @@
OVERCOME, //前足が乗った状態で進む
BACKLEG_ON_SANDDUNE, //後ろ脚を段差に載せる
OVERCOME2, //後ろ脚が乗った状態で進む
+ AFTER_OVERCOME, //段差を終了したら座って指示まち
ROPE, //rope前足超える
ROPE_BACK, //rope後ろ足超える
ROPE_DOWN, //rope機体高さをLRF用に落とす
@@ -61,7 +73,9 @@
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;
-int SetWalk(Walk &walk, WalkWay way) //歩行パラメータの事前セット
+//各歩行軌道のパラメータ設定。switchで羅列している。新規に歩行パターンを増やすときはcaseを増やす。
+//param walk:結果を入れる箱。way:歩行軌道を指定するWalkWayの定数。
+int SetWalk(Walk &walk, WalkWay way)
{
float overcome_start_y_m[] = {0.41, 0.41, 0.41, 0.41};
switch (way)
@@ -90,8 +104,8 @@
stride_short_m[i], height_m[i], buffer_height_m,
stridetime_s, toptime_s, buffer_time_s);
walk.SetOffsetTime(0, 0.5, 0.5, 0);
+ break;
}
- break;
case STANDUP_SANDDUNE:
{
float start_y_m[4] = {0.41, 0.41, 0.41, 0.41};
@@ -105,8 +119,8 @@
SetOneLegFreeLinesParam(walk, i, lines, sizeof(lines) / sizeof(lines[0]));
}
walk.SetOffsetTime(0, 0, 0, 0);
+ break;
}
- break;
case FRONTLEG_ON_SANDDUNE:
{ //前足を段差にかける
float d_x_m = 0.1;
@@ -161,31 +175,15 @@
walk.SetOffsetTime(0, 0.5, 0.5, 0);
break;
}
- case ROPE:
+ case AFTER_OVERCOME:
{
- /*
- float offset_x_m[4] = {-0.10, -0.10, -0.10, -0.10},
- offset_y_m[4] = {0.45, 0.45, 0.45, 0.45},
- stride_m[4] = {0.1, 0.1, 0.1, 0.1},
- height_m[4] = {0.17, 0.17, 0.17, 0.17}, buffer_height_m = 0.15,
- 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);*/
- /*
- LineParam over_come_front[] = {
- {.time_s = 0, .x_m = -0.05, .y_m = 0.45, .is_point_to_point = 0},
- {.time_s = 0.5, .x_m = -0.15, .y_m = 0.45, .is_point_to_point = 0},
- {.time_s = 1, .x_m = -0.2366, .y_m = 0.3, .is_point_to_point = 0},
- {.time_s = 1.5, .x_m = -0.037, .y_m = 0.3, .is_point_to_point = 1},
- {.time_s = 2, .x_m = -0.05, .y_m = 0.45, .is_point_to_point = 0},
- };
- for (int i = 0; i < 4; i++)
- SetOneLegFreeLinesParam(walk, i, over_come_front, sizeof(over_come_front) / sizeof(over_come_front[0]));
- walk.SetOffsetTime(0, 0.5, 0.5, 0);
- */
+ for (int i = 0; i < 4; i++)
+ SetOneLegStandParam(walk, i, offset_x_m[i], offset_y_m[i], 0.5);
+ walk.SetOffsetTime(0, 0, 0, 0);
+ break;
+ }
+ case ROPE:
+ {
float offset_x_m[4] = {-0.05, 0, -0.05, 0};
float d_x_m = 0.2;
float start_y_m[4] = {0.41, 0.41, 0.41, 0.41};
@@ -196,11 +194,10 @@
OverCome overcome(offset_x_m, start_y_m, d_x_m, goal_y_m,
overcome_height_m, gravity_dist, walk.leg, raise_offset_x_m);
walk.Copy(overcome.walk);
+ break;
}
- break;
case ROPE_BACK:
{
-
float offset_x_m[4] = {};
float d_x_m = 0.15;
float start_y_m[4] = {0.41, 0.41, 0.41, 0.41};
@@ -211,8 +208,8 @@
OverCome overcome(offset_x_m, start_y_m, d_x_m, goal_y_m,
overcome_height_m, gravity_dist, walk.leg, raise_offset_x_m);
walk.Copy(overcome.walk);
+ break;
}
- break;
case ROPE_DOWN:
{
for (int i = 0; i < 4; i++)
@@ -224,8 +221,8 @@
SetOneLegFreeLinesParam(walk, i, lines, sizeof(lines) / sizeof(lines[0]));
}
walk.SetOffsetTime(0, 0, 0, 0);
+ break;
}
- break;
case SLOPE:
{
float offset_x_m[4] = {-0.15, 0.15, -0.15, 0.15},
@@ -238,89 +235,73 @@
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;
}
- break;
default:
printf("error: there is no WalkWay\r\n");
- return 1;
+ return 1; //以上終了
}
- return walk.CheckOrbit();
+ walk.ResetPhase();
+ return 0; //正常終了
}
int main()
{
+ printf("program start\r\n");
#ifdef VSCODE
if (FileOpen()) //csv fileに書き込み
- return 1; //異常終了したら強制終了
+ return 1; //異常なら強制終了
#endif
+ /////足の長さ、計算周期の設定
OneLeg leg[4]; //各足の位置
for (int i = 0; i < 4; i++)
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)
+ SetWalk(walk, (WalkWay)i);
+ if (walk.CheckOrbit() == 1)
{
printf("error: move %d\r\n", i);
return 1; //強制終了.errorは内部の関数からprintfで知らせる
}
}
- // SetWalk(walk, STANDUP);
- SetWalk(walk, STANDUP_SANDDUNE);
#ifndef USE_ROS
printf("Stand up?\r\n");
- WaitStdin('y'); // ボタンを押したら立つ
+ WaitStdin('y'); // キーボード入力されるまでまで待つ
#endif
+ /////立つ
+ // SetWalk(walk, STANDUP);
+ SetWalk(walk, AFTER_OVERCOME);//段差越え後調整用に一時的に入れた
MoveOneCycle(walk, leg);
#ifndef USE_ROS
printf("Move?\r\n");
- WaitStdin('y'); // ボタンを押したら立つ
+ WaitStdin('y'); //キーボード入力されるまでまで待つ
#endif
-/*
-
- WaitStdin('y'); // ボタンを押したらスタート
- if (SetWalk(walk, SLOPE) == 1)
- {
- printf("error: LRFPOSTURE\r\n");
- return 1; //強制終了.
- }
+/*
+///SLOPE
+ SetWalk(walk, SLOPE);
for (int i = 0; i < 30; i++)
MoveOneCycle(walk, leg);
-*/
-/*
- /////////////ROPEを試す
- if (SetWalk(walk, ROPE) == 1)
- {
- printf("error: ROPE\r\n");
- return 1; //強制終了.
- }
- MoveOneCycle(walk, leg);
- if (SetWalk(walk, ROPE_DOWN) == 1)
- {
- printf("error: ROPE_DOWN\r\n");
- return 1; //強制終了.
- }
+
+ /////////////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);
- if (SetWalk(walk, LRFPOSTURE) == 1)
- {
- printf("error: LRFPOSTURE\r\n");
- return 1; //強制終了.
- }
- for (int i = 0; i < 3; i++)
- MoveOneCycle(walk, leg);
-
- if (SetWalk(walk, ROPE_BACK) == 1)
- {
- printf("error:ROPE_BACK \r\n");
- return 1; //強制終了.
- }
- MoveOneCycle(walk, leg);
- */
+ SetWalk(walk, ROPE_BACK);
+ MoveOneCycle(walk, leg);
+*/
#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(); //一度受信
SetWalk(walk, LRFPOSTURE);
while (1)
@@ -341,36 +322,22 @@
//2歩進んで当て処理
// for (int i = 0; i < 2; i++)
// MoveOneCycle(walk, leg);
+ SetWalk(walk, STANDUP_SANDDUNE);
+ MoveOneCycle(walk, leg);
//前足を段差にかける
- if (SetWalk(walk, FRONTLEG_ON_SANDDUNE) == 1)
- {
- printf("error: triangle\r\n");
- return 1; //強制終了.
- }
+ SetWalk(walk, FRONTLEG_ON_SANDDUNE);
for (int i = 0; i < 1; i++)
MoveOneCycle(walk, leg);
//前足が段差に乗った状態で進む
- if (SetWalk(walk, OVERCOME) == 1)
- {
- printf("error: triangle\r\n");
- return 1; //強制終了.
- }
+ SetWalk(walk, OVERCOME);
for (int i = 0; i < 5; i++)
MoveOneCycle(walk, leg);
//後ろ脚載せる
- if (SetWalk(walk, BACKLEG_ON_SANDDUNE) == 1)
- {
- printf("error: triangle\r\n");
- return 1; //強制終了.
- }
+ SetWalk(walk, BACKLEG_ON_SANDDUNE);
for (int i = 0; i < 1; i++)
MoveOneCycle(walk, leg);
//後ろ脚乗った状態で進む
- if (SetWalk(walk, OVERCOME2) == 1)
- {
- printf("error: triangle\r\n");
- return 1; //強制終了.
- }
+ SetWalk(walk, OVERCOME2);
for (int i = 0; i < 3; i++)
MoveOneCycle(walk, leg);
#ifdef USE_ROS
@@ -387,13 +354,14 @@
#ifdef USE_ROS
void callback(const geometry_msgs::Vector3 &cmd_vel)
{
- // pub_vel.publish(&cmd_vel);
if (state_from_ros == 0)
state_from_ros = 1;
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;
+ back_vel = cmd_vel;
+ pub_vel.publish(&back_vel);
}
void callback_state(const std_msgs::Int16 &cmd)
{