岡本 陸
test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: main.cpp
- Revision:
- 50:36741e8ab197
- Parent:
- 49:198030e84936
diff -r 198030e84936 -r 36741e8ab197 main.cpp
--- a/main.cpp Mon Mar 11 06:36:56 2019 +0000
+++ b/main.cpp Mon Mar 11 10:38:07 2019 +0000
@@ -31,11 +31,8 @@
STOP,
AREA1_LRFWALK_STATE,
SANDDUNE,
- AREA2_LRFWALK_STATE,
ROPE_STATE,
SLOPE_STATE,
- TURNRIGHT_STATE,
- TRUNLEFT_STATE,
};
ROS_STATE state_from_ros = STOP; //ここを変えると動く方法が変わる
#ifdef USE_ROS
@@ -65,9 +62,9 @@
BACKLEG_ON_SANDDUNE, //後ろ脚を段差に載せる
OVERCOME2, //後ろ脚が乗った状態で進む
AFTER_OVERCOME, //段差を終了したら座って指示まち
+ WALK_BEFORE_ROPE, //紐前で歩く
ROPE, //rope前足超える
ROPE_BACK, //rope後ろ足超える
- ROPE_DOWN, //rope機体高さをLRF用に落とす
SLOPE,
TRUNRIGHT,
TRUNLEFT,
@@ -91,10 +88,12 @@
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 d_time = 0.2, d_time_slow = 0.3;
//旋回
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;
+ turn_stride_m = 0.075, turn_height_m = 0.05,
+ turn_stridetime_s = 0.5, turn_risetime_s = 0.2;
+ float d_time_rope = 0.6, d_time_slow_rope = 0.3;
switch (way)
{
case STANDUP:
@@ -141,7 +140,8 @@
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, flontleg_sand_goal_y_m,
- overcome_height_m, gravity_dist, walk.leg, raise_offset_x_m);
+ overcome_height_m, gravity_dist, walk.leg,
+ raise_offset_x_m, d_time, d_time_slow);
walk.Copy(overcome.walk);
break;
}
@@ -163,7 +163,8 @@
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);
+ overcome_height_m, gravity_dist, walk.leg,
+ raise_offset_x_m, d_time, d_time_slow);
walk.Copy(overcome.walk);
break;
}
@@ -177,13 +178,14 @@
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, backleg_sand_goal_y_m,
- overcome_height_m, gravity_dist, walk.leg, raise_offset_x_m);
+ overcome_height_m, gravity_dist, walk.leg,
+ raise_offset_x_m, d_time, d_time_slow);
walk.Copy(overcome.walk);
break;
}
case OVERCOME2:
{ //後ろ脚が乗った状態で進む
- float offset_x_m[4] = {0.1,0,0.1,0};
+ 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,
@@ -202,47 +204,47 @@
walk.SetOffsetTime(0, 0, 0, 0);
break;
}
+ case WALK_BEFORE_ROPE:
+ {
+ // float stride_m[] = {0.2, 0.2, 0.125, 0.125};
+ float stride_m[] = {0.075, 0.075, 0.2, 0.2};
+ for (int i = 0; i < 4; i++)
+ 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 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};
float goal_y_m[4] = {0.41, 0.41, 0.41, 0.41};
- float raise_offset_x_m[] = {0, -0.08, 0, -0.08};
+ float raise_offset_x_m[] = {0, -0.15, 0, -0.15};
float overcome_height_m[] = {0.05, 0.2, 0.05, 0.2};
float gravity_dist[] = {0.05, -0.05, 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, raise_offset_x_m);
+ overcome_height_m, gravity_dist, walk.leg,
+ raise_offset_x_m, d_time_rope, d_time_slow_rope);
walk.Copy(overcome.walk);
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};
- float goal_y_m[4] = {0.41, 0.41, 0.41, 0.41};
- float raise_offset_x_m[] = {-0.03, 0, -0.03, 0};
+ float d_x_m = 0.2;
+ float start_y_m[4] = {0.41, 0.38, 0.41, 0.38};
+ float goal_y_m[4] = {0.41, 0.38, 0.41, 0.38};
+ float raise_offset_x_m[] = {-0.1, 0, -0.1, 0};
float overcome_height_m[] = {0.2, 0.05, 0.2, 0.05};
- float gravity_dist[] = {0.08, 0, 0.08, 0};
+ float gravity_dist[] = {0.25, -0.05, 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, raise_offset_x_m);
+ overcome_height_m, gravity_dist, walk.leg,
+ raise_offset_x_m, d_time_rope, d_time_slow_rope);
walk.Copy(overcome.walk);
break;
}
- case ROPE_DOWN:
- {
- 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 = area1_offset_y_m[i], .is_point_to_point = 0},
- };
- SetOneLegFreeLinesParam(walk, i, lines, sizeof(lines) / sizeof(lines[0]));
- }
- walk.SetOffsetTime(0, 0, 0, 0);
- break;
- }
case SLOPE:
{
float offset_x_m[4] = {-0.15, 0.15, -0.15, 0.15},
@@ -306,63 +308,51 @@
nh_mbed.advertise(pub_vel);
nh_mbed.spinOnce(); //一度受信
#endif
- ROS_STATE old_state = STOP;
+
/////立つ
-#ifndef USE_ROS
- state_from_ros = SANDDUNE; //遠してやらないならやりたいSTATEに変更
-#endif
+ state_from_ros = ROPE_STATE;
+ switch (state_from_ros)
+ {
+ case SANDDUNE:
+ SetWalk(walk, STANDUP_SANDDUNE);
+ break;
+ default:
+ SetWalk(walk, STANDUP);
+ break;
+ }
+ printf("Stand up?\r\n");
+ WaitStdin('y'); // キーボード入力されるまでまで待つ
+ MoveOneCycle(walk, leg);
+ printf("Move?\r\n");
+ WaitStdin('y');
//プログラムの最初のほうにあるstate_from_rosで動かすものを切り替える.caseはenum ROS_STATEで分ける
- // while (1)
- // {
switch (state_from_ros)
{
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'); //キーボード入力されるまでまで待つ
+ case AREA1_LRFWALK_STATE: //キーボード入力されるまでまで待つ
SmoothChange(walk, leg, AREA1_LRFWALK, 0.5); //切り替え
for (int i = 0; i < 20; i++)
MoveOneCycle(walk, leg);
- 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, 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);
@@ -370,46 +360,72 @@
SetWalk(walk, AREA1_LRFWALK);
for (int i = 0; i < 4; i++)
MoveOneCycle(walk, leg);
- /* //数歩左に曲がる
- SmoothChange(walk, leg, TRUNLEFT, 0.5); //切り替えスムーズ
+ break;
+
+ case ROPE_STATE:
+ //紐前歩く
+ SetWalk(walk, WALK_BEFORE_ROPE);
+ for (int i = 0; i < 5; i++)
+ MoveOneCycle(walk, leg);
+ int is_arrived = 0;
+ while (is_arrived == 0)
+ {
+ MoveOneCycle(walk, leg);
+ unsigned int dist_cm = GetDist_cm();
+ if (dist_cm > 90 && dist_cm < 150)
+ ++is_arrived;
+ printf("%d\r\n", dist_cm);
+ }
+ /*
+ SetWalk(walk, AREA1_LRFWALK);
+ for (int i = 0; i < 5; i++)
+ MoveOneCycle(walk, leg);
+ SmoothChange(walk, leg, TRUNRIGHT, 0.5);
for (int i = 0; i < 2; i++)
+ MoveOneCycle(walk, leg);
+ */
+ /*曲がりながら歩く
+ SmoothChange(walk, leg, WALK_BEFORE_ROPE, 0.5);
+ for (int i = 0; i < 7; i++)
MoveOneCycle(walk, leg);
*/
- 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);
+ SmoothChange(walk, leg, AREA1_LRFWALK, 1.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;
+ //紐間歩く
+ SmoothChange(walk, leg, AREA1_LRFWALK, 0.5);
+ for (int i = 0; i < 1; i++)
+ MoveOneCycle(walk, leg);
+ //2本目
+ //前足だけ紐越え
+ SmoothChange(walk, leg, ROPE, 0.5);
+ MoveOneCycle(walk, leg);
+ //LRFが取れる高さで歩行
+ SmoothChange(walk, leg, AREA1_LRFWALK, 1.5);
+ for (int i = 0; i < 3; i++)
+ MoveOneCycle(walk, leg);
+ //後ろ足越え
+ SmoothChange(walk, leg, ROPE_BACK, 0.5);
+ MoveOneCycle(walk, leg);
+ //紐後歩く
+ SmoothChange(walk, leg, AREA1_LRFWALK, 1.5);
+ for (int i = 0; i < 10; i++)
+ MoveOneCycle(walk, leg);
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();
#endif