岡本 陸
test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: main.cpp
- Revision:
- 49:198030e84936
- Parent:
- 48:1aad3cc386e8
- Child:
- 50:36741e8ab197
--- a/main.cpp Fri Mar 08 06:09:31 2019 +0000
+++ b/main.cpp Mon Mar 11 06:36:56 2019 +0000
@@ -14,7 +14,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"
@@ -59,9 +59,9 @@
AREA1_LRFWALK, //段差までLRFありで歩く.元LRFPOSTURE
AREA2_LRFWALK, //段差後からLRFありで歩く
STANDUP_SANDDUNE, //段差前で立つ
- BEFORE_SANDDUNE, //段差に壁当て
FRONTLEG_ON_SANDDUNE, //前足を段差にかける
OVERCOME, //前足が乗った状態で進む
+ FALL_FRONTLEG, //前足を段差から降ろす
BACKLEG_ON_SANDDUNE, //後ろ脚を段差に載せる
OVERCOME2, //後ろ脚が乗った状態で進む
AFTER_OVERCOME, //段差を終了したら座って指示まち
@@ -81,15 +81,16 @@
{
//複数歩行に共有するパラメータはここに書く。それ以外はcase内に入れる。
//LRFを使って歩行する際のパラメータ.stride_mだけグローバルにある。
- float area1_offset_y_m[4] = {0.27, 0.27, 0.27, 0.27};
+ float area1_offset_y_m[4] = {0.3, 0.3, 0.3, 0.3};
float area2_offset_y_m[4] = {0.27, 0.27, 0.27, 0.27};
- float offset_x_m[4] = {-0.15, 0.15, -0.15, 0.15};
+ float offset_x_m[4] = {0, 0, 0, 0};
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;
+ stridetime_s = 0.45, toptime_s = 0.2, buffer_time_s = 0.1;
//段差
- float overcome_start_y_m[] = {0.41, 0.41, 0.41, 0.41};
-
+ float overcome_start_y_m[] = {0.38, 0.38, 0.38, 0.38};
+ float flontleg_sand_goal_y_m[4] = {0.38, 0.27, 0.38, 0.27};
+ float backleg_sand_goal_y_m[4] = {0.27, 0.38, 0.27, 0.38};
//旋回
float turn_start_x_m = 0, turn_start_y_m = 0.275,
turn_stride_m = 0.05, turn_height_m = 0.05,
@@ -107,9 +108,9 @@
}
case AREA1_LRFWALK: //LRF用に変数はグローバルにある
for (int i = 0; i < 4; 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);
+ SetOneLegFourPointParam(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:
@@ -119,25 +120,14 @@
stridetime_s, toptime_s, buffer_time_s);
walk.SetOffsetTime(0, 0.5, 0.5, 0);
break;
- case BEFORE_SANDDUNE:
- {
- float stride_short_m[] = {0.1, 0.1, 0.1, 0.1};
- for (int i = 0; i < 4; i++)
- SetOneLegTriangleParam(walk, i, offset_x_m[i], overcome_start_y_m[i],
- 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;
- }
case STANDUP_SANDDUNE:
{
- float start_y_m[4] = {0.41, 0.41, 0.41, 0.41};
for (int i = 0; i < 4; i++)
{
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 = start_y_m[i], .is_point_to_point = 0},
- {.time_s = 1.5, .x_m = offset_x_m[i], .y_m = start_y_m[i], .is_point_to_point = 0},
+ {.time_s = 0.5, .x_m = offset_x_m[i], .y_m = overcome_start_y_m[i], .is_point_to_point = 0},
+ {.time_s = 1.5, .x_m = offset_x_m[i], .y_m = overcome_start_y_m[i], .is_point_to_point = 0},
};
SetOneLegFreeLinesParam(walk, i, lines, sizeof(lines) / sizeof(lines[0]));
}
@@ -147,47 +137,54 @@
case FRONTLEG_ON_SANDDUNE:
{ //前足を段差にかける
float d_x_m = 0.1;
- float goal_y_m[4] = {0.41, 0.29, 0.41, 0.29};
float raise_offset_x_m[4] = {0, -0.02, 0, 0};
- float overcome_height_m[] = {0.1, 0.2, 0.1, 0.2};
+ float overcome_height_m[] = {0.05, 0.15, 0.05, 0.15};
float gravity_dist[] = {0.05, 0, 0.05, -0.05};
- OverCome overcome(offset_x_m, overcome_start_y_m, d_x_m, goal_y_m,
+ OverCome overcome(offset_x_m, overcome_start_y_m, d_x_m, flontleg_sand_goal_y_m,
+ overcome_height_m, gravity_dist, walk.leg, raise_offset_x_m);
+ walk.Copy(overcome.walk);
+ break;
+ }
+ case OVERCOME:
+ { //前足が乗った状態で進む
+ float stride_m[4] = {0.07, 0.07, 0.07, 0.07};
+ for (int i = 0; i < 4; i++)
+ SetOneLegFourPointParam(walk, i, offset_x_m[i], flontleg_sand_goal_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 FALL_FRONTLEG: //前足を段差から降ろす
+ {
+ float d_x_m = 0.15;
+ float start_x_m[4] = {0, 0, 0, 0};
+ float raise_offset_x_m[4] = {};
+ float overcome_height_m[] = {0.05, 0.05, 0.05, 0.05};
+ float gravity_dist[] = {0.1, 0, 0.1, 0};
+ OverCome overcome(start_x_m, flontleg_sand_goal_y_m, d_x_m, overcome_start_y_m,
overcome_height_m, gravity_dist, walk.leg, raise_offset_x_m);
walk.Copy(overcome.walk);
break;
}
case BACKLEG_ON_SANDDUNE:
{ //後ろ脚を乗せる
- float d_x_m = 0.1;
+ float d_x_m = 0.15;
float start_x_m[4] = {0, 0, 0, 0};
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 raise_offset_x_m[4] = {};
float overcome_height_m[] = {0.2, 0.1, 0.2, 0.1};
float gravity_dist[] = {0.1, 0, 0.1, 0};
- OverCome overcome(start_x_m, start_y_m, d_x_m, goal_y_m,
+ OverCome overcome(start_x_m, start_y_m, d_x_m, backleg_sand_goal_y_m,
overcome_height_m, gravity_dist, walk.leg, raise_offset_x_m);
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.38, 0.35, 0.38, 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;
- }
case OVERCOME2:
{ //後ろ脚が乗った状態で進む
- float offset_x_m[4] = {},
- offset_y_m[4] = {0.35, 0.35, 0.35, 0.35};
+ float offset_x_m[4] = {0.1,0,0.1,0};
+ float offset_y_m[4] = {0.27, 0.33, 0.27, 0.33};
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;
@@ -252,7 +249,7 @@
offset_y_m[4] = {0.32, 0.22, 0.32, 0.22};
float stride_m[4] = {0.18, 0.18, 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;
+ stridetime_s = 1, toptime_s = 0.15, buffer_time_s = 0.05;
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,
@@ -308,106 +305,114 @@
nh_mbed.subscribe(sub_state);
nh_mbed.advertise(pub_vel);
nh_mbed.spinOnce(); //一度受信
-#else
- printf("Stand up?\r\n");
- WaitStdin('y'); // キーボード入力されるまでまで待つ
#endif
+ ROS_STATE old_state = STOP;
/////立つ
- SetWalk(walk, STANDUP);
- // SetWalk(walk, AFTER_OVERCOME); //段差越え後調整用に一時的に入れた
- MoveOneCycle(walk, leg);
#ifndef USE_ROS
- printf("Move?\r\n");
- WaitStdin('y'); //キーボード入力されるまでまで待つ
state_from_ros = SANDDUNE; //遠してやらないならやりたいSTATEに変更
#endif
- int loopcount = 0;
- ROS_STATE old_state = state_from_ros;
//プログラムの最初のほうにあるstate_from_rosで動かすものを切り替える.caseはenum ROS_STATEで分ける
- while (1)
+ // while (1)
+ // {
+ switch (state_from_ros)
{
- switch (state_from_ros)
- {
- case STOP:
- break;
- case AREA1_LRFWALK_STATE:
- if (old_state != state_from_ros)
- SmoothChange(walk, leg, AREA1_LRFWALK, 0.5); //切り替え
- else
- SetWalk(walk, AREA1_LRFWALK);
+ case STOP:
+ break;
+ case AREA1_LRFWALK_STATE:
+ printf("Stand up?\r\n");
+ WaitStdin('y'); // キーボード入力されるまでまで待つ
+ SetWalk(walk, STANDUP);
+ MoveOneCycle(walk, leg);
+ printf("Move?\r\n");
+ WaitStdin('y'); //キーボード入力されるまでまで待つ
+ SmoothChange(walk, leg, AREA1_LRFWALK, 0.5); //切り替え
+ for (int i = 0; i < 20; i++)
MoveOneCycle(walk, leg);
- break;
- case AREA2_LRFWALK_STATE:
- if (old_state != state_from_ros)
- SmoothChange(walk, leg, AREA2_LRFWALK, 0.5); //切り替え
- else
- SetWalk(walk, AREA2_LRFWALK);
- MoveOneCycle(walk, leg);
- break;
- case SANDDUNE:
- //前足を段差にかける
- SmoothChange(walk, leg, FRONTLEG_ON_SANDDUNE, 0.5); //切り替えスムーズ
+ state_from_ros = STOP;
+ break;
+ case AREA2_LRFWALK_STATE:
+ if (old_state != state_from_ros)
+ SmoothChange(walk, leg, AREA2_LRFWALK, 0.5); //切り替え
+ else
+ SetWalk(walk, AREA2_LRFWALK);
+ MoveOneCycle(walk, leg);
+ break;
+ case SANDDUNE:
+ //前足を段差にかける
+ printf("Stand up?\r\n");
+ WaitStdin('y'); // キーボード入力されるまでまで待つ
+ SetWalk(walk, STANDUP_SANDDUNE);
+ MoveOneCycle(walk, leg);
+ printf("Move?\r\n");
+ WaitStdin('y'); //キーボード入力されるまでまで待つ
+ SmoothChange(walk, leg, FRONTLEG_ON_SANDDUNE, 0.5); //切り替えスムーズ
+ MoveOneCycle(walk, leg);
+ //前足が段差に乗った状態で進む
+ SmoothChange(walk, leg, OVERCOME, 0.5); //切り替えスムーズ
+ for (int i = 0; i < 2; i++)
MoveOneCycle(walk, leg);
- //前足が段差に乗った状態で進む
- SmoothChange(walk, leg, OVERCOME, 0.5); //切り替えスムーズ
- for (int i = 0; i < 5; i++)
- MoveOneCycle(walk, leg);
- //後ろ脚載せる
- SmoothChange(walk, leg, BACKLEG_ON_SANDDUNE, 0.5); //切り替えスムーズ
- for (int i = 0; i < 1; i++)
- MoveOneCycle(walk, leg);
- //後ろ脚乗った状態で進む
- SmoothChange(walk ,leg, OVERCOME2, 0.5); //切り替えスムーズ
- for (int i = 0; i < 3; i++)
- MoveOneCycle(walk, leg);
- //数歩左に曲がる
- SmoothChange(walk, leg, TRUNLEFT, 0.5); //切り替えスムーズ
- for (int i = 0; i < 2; i++)
- MoveOneCycle(walk, leg);
- state_from_ros = AREA2_LRFWALK_STATE;
- break;
- case ROPE_STATE:
- //前足だけ紐越え
- SmoothChange(walk, leg, ROPE, 0.5);
+ //前足を降ろす
+ SmoothChange(walk, leg, FALL_FRONTLEG, 0.5);
+ MoveOneCycle(walk, leg);
+ //またいだ状態で歩く
+ SmoothChange(walk, leg, AREA1_LRFWALK, 0.5);
+ MoveOneCycle(walk, leg);
+
+ //後ろ脚載せる
+ SmoothChange(walk, leg, BACKLEG_ON_SANDDUNE, 0.5); //切り替えスムーズ
+ MoveOneCycle(walk, leg);
+ //後ろ脚乗った状態で進む
+ // SmoothChange(walk, leg, OVERCOME2, 0.5); //切り替えスムーズ
+ SetWalk(walk, OVERCOME2);
+ for (int i = 0; i < 3; i++)
MoveOneCycle(walk, leg);
- //LRFが取れる高さで歩行
- SmoothChange(walk, leg, AREA1_LRFWALK, 0.5);
- for (int i = 0; i < 3; i++)
- MoveOneCycle(walk, leg);
- //後ろ足越え
- SmoothChange(walk,leg, ROPE_BACK, 0.5);
+ //普通に進む
+ SetWalk(walk, AREA1_LRFWALK);
+ for (int i = 0; i < 4; i++)
+ MoveOneCycle(walk, leg);
+ /* //数歩左に曲がる
+ SmoothChange(walk, leg, TRUNLEFT, 0.5); //切り替えスムーズ
+ for (int i = 0; i < 2; i++)
MoveOneCycle(walk, leg);
- state_from_ros = AREA2_LRFWALK_STATE;
- break;
- case SLOPE_STATE:
- SmoothChange(walk, leg, SLOPE, 0.5);
- for (int i = 0; i < 30; i++)
- MoveOneCycle(walk, leg);
- state_from_ros = STOP;
- break;
- case TURNRIGHT_STATE:
- SmoothChange(walk, leg, TRUNRIGHT, 0.5);
- for (int i = 0; i < 5; i++)
- MoveOneCycle(walk, leg);
- state_from_ros = AREA2_LRFWALK_STATE;
- break;
- case TRUNLEFT_STATE:
- SmoothChange(walk, leg, TRUNLEFT, 0.5);
- for (int i = 0; i < 5; i++)
- MoveOneCycle(walk, leg);
- state_from_ros = AREA2_LRFWALK_STATE;
- break;
- }
- old_state = state_from_ros;
+ */
+ state_from_ros = AREA2_LRFWALK_STATE;
+ break;
+ case ROPE_STATE:
+ //前足だけ紐越え
+ SmoothChange(walk, leg, ROPE, 0.5);
+ MoveOneCycle(walk, leg);
+ //LRFが取れる高さで歩行
+ SmoothChange(walk, leg, AREA1_LRFWALK, 0.5);
+ for (int i = 0; i < 3; i++)
+ MoveOneCycle(walk, leg);
+ //後ろ足越え
+ SmoothChange(walk, leg, ROPE_BACK, 0.5);
+ MoveOneCycle(walk, leg);
+ state_from_ros = AREA2_LRFWALK_STATE;
+ break;
+ case SLOPE_STATE:
+ SmoothChange(walk, leg, SLOPE, 0.5);
+ for (int i = 0; i < 30; i++)
+ MoveOneCycle(walk, leg);
+ state_from_ros = STOP;
+ break;
+ case TURNRIGHT_STATE:
+ SmoothChange(walk, leg, TRUNRIGHT, 0.5);
+ for (int i = 0; i < 5; i++)
+ MoveOneCycle(walk, leg);
+ state_from_ros = AREA2_LRFWALK_STATE;
+ break;
+ case TRUNLEFT_STATE:
+ SmoothChange(walk, leg, TRUNLEFT, 0.5);
+ for (int i = 0; i < 5; i++)
+ MoveOneCycle(walk, leg);
+ state_from_ros = AREA2_LRFWALK_STATE;
+ break;
+ }
+ // old_state = state_from_ros;
#ifdef USE_ROS
- nh_mbed.spinOnce();
+ nh_mbed.spinOnce();
#endif
-#ifdef VSCODE
- ++loopcount;
- if (loopcount > 1)
- break; //VSCODEでデバックする際は一ループで抜ける
-#endif
- }
printf("program end\r\n");
#ifdef VSCODE
fclose(fp);