岡本 陸
test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: main.cpp
- Revision:
- 31:86eb746eaed4
- Parent:
- 29:7d8b8011a88d
- Child:
- 32:dc684a0b8448
diff -r 7d8b8011a88d -r 86eb746eaed4 main.cpp
--- a/main.cpp Wed Feb 27 12:16:18 2019 +0000
+++ b/main.cpp Thu Feb 28 03:52:09 2019 +0000
@@ -31,12 +31,12 @@
const double kServoValToDegree = 270.0 / (11500 - 3500); //ICSの値を度に変換
const double kOriginDegree[2][2] = {
{
- (8375 - 3500) * kServoValToDegree,
- (5523 - 3500) * kServoValToDegree + 180,
+ (7639 - 3500) * kServoValToDegree,
+ (7662 - 3500) * kServoValToDegree + 180,
},
{
- (6470 - 3500) * kServoValToDegree,
- (7419 - 3500) * kServoValToDegree - 180, //180度の時6657シータ負の方向に動かすと値は減るので、0度の位置は-180すれば出る
+ (6431 - 3500) * kServoValToDegree,
+ (8135 - 3500) * kServoValToDegree - 180,
},
};
///////////////
@@ -80,7 +80,6 @@
{
STANDUP,
STRAIGHT,
- END,
TURNLEFT,
TURNRIGHT,
UP, //足を伸ばして立つ
@@ -88,13 +87,13 @@
SANDDUNE, //前足だけがsandduneに乗った状態で進む
OVERCOME_BACK, //後ろ足を乗せる
STRAIGHT_W, //両足がsandduneに乗った状態で進む
- STRAIGHT_W2, //後足がsandduneに乗った状態で進む
ROPE_F, //ropeをまたぐ（前足)
ROPE_B, //ropeをまたぐ（後ろ足)
+ END,
+ STRAIGHT_W2, //後足がsandduneに乗った状態で進む
};
void ParamsSetup(Walk walks[END], OneLeg leg[4]) //各パラメータの設定。減らしていく必要あり
{
-
Walk::calctime_s_ = 0.03; //計算周期
//足の作成、サイズデータのみの足の骨組.4足同じにしている
for (int i = 0; i < 4; i++)
@@ -102,11 +101,107 @@
walks[STANDUP].SetAllLegStandParam(0, 0.2, 0.5); //x,y,time_s.一括で設定できる
//STRAIGT 論文通りのとき、start_y_mは <=0.32
- float start_x_m = 0.06, start_y_m = 0.2, stride_m = 0.12, height_m = 0.03, buffer_height_m = 0.01,
- stridetime_s = 0.55, toptime_s = 0.30, buffer_time_s = 0.15;
- walks[STRAIGHT].SetAllLegTriangleParam(start_x_m, start_y_m, stride_m, height_m, buffer_height_m,
- stridetime_s, toptime_s, buffer_time_s);
+ float start_x_m = 0.075, start_y_m = 0.2, stride_m = 0.15, height_m = 0.03, buffer_height_m = 0.01,
+ stridetime_s = 0.3, toptime_s = 0.07, buffer_time_s = 0.03;
+ walks[STRAIGHT].SetAllLegTriangleParam // 4足を取り敢えず一括で設定してから
+ (start_x_m, start_y_m, stride_m, height_m, buffer_height_m, stridetime_s, toptime_s, buffer_time_s);
walks[STRAIGHT].SetOffsetTime(0, 0.5, 0.5, 0);
+/*
+ //TURNLEFTのparam
+ float stride_short_m = 0.2, stride_long_m = 0.1, start_x_short_m = 0.1, start_x_long_m = 0.051;
+ stridetime_s = 0.2, toptime_s = 0.07, buffer_time_s = 0.03,
+ start_y_m = 0.2, height_m = 0.03, buffer_height_m = 0.01;
+ walks[TURNLEFT].SetAllLegTriangleParam // 4足を取り敢えず一括で設定してから
+ (start_x_short_m, start_y_m, stride_short_m, height_m, buffer_height_m, stridetime_s, toptime_s, buffer_time_s);
+ walks[TURNLEFT].ChangeOneParam(LEFT_F, STRIDE_M, stride_long_m); //一部のパラメータを変更
+ walks[TURNLEFT].ChangeOneParam(LEFT_B, STRIDE_M, stride_long_m);
+ walks[TURNLEFT].ChangeOneParam(LEFT_F, STRAT_X_M, start_x_long_m); //一部のパラメータを変更
+ walks[TURNLEFT].ChangeOneParam(LEFT_B, STRAT_X_M, start_x_long_m);
+ walks[TURNLEFT].SetOffsetTime(0, 0.5, 0.5, 0); //位相ずれの程度(値域[0..1])
+
+ //TURNRIGHT1のparam
+ walks[TURNRIGHT].SetAllLegTriangleParam // 4足を取り敢えず一括で設定してから
+ (start_x_short_m, start_y_m, stride_short_m, height_m, buffer_height_m, stridetime_s, toptime_s, buffer_time_s);
+ walks[TURNRIGHT].ChangeOneParam(RIGHT_F, STRIDE_M, stride_long_m); //一部のパラメータを変更
+ walks[TURNRIGHT].ChangeOneParam(RIGHT_B, STRIDE_M, stride_long_m);
+ walks[TURNLEFT].ChangeOneParam(RIGHT_F, STRAT_X_M, start_x_long_m); //一部のパラメータを変更
+ walks[TURNLEFT].ChangeOneParam(RIGHT_B, STRAT_X_M, start_x_long_m);
+ walks[TURNRIGHT].SetOffsetTime(0, 0.5, 0.5, 0); //位相ずれの程度(値域[0..1])
+
+ // UP, //足を伸ばして立つ
+ walks[UP].SetAllLegStandParam(0, 0.3, 1.2); //x,y,time_s.一括で設定できる
+
+ // OVERCOME 前足を乗せる
+ LineParam over_come_front[] = {
+ {.time_s = 0.6, .x_m = -0.1, .y_m = 0.3},
+ {.time_s = 0.2, .x_m = -0.1, .y_m = 0.3},
+ {.time_s = 0.2, .x_m = -0.13, .y_m = 0.16},
+ {.time_s = 0.2, .x_m = 0.08, .y_m = 0.16},
+ {.time_s = 0.6, .x_m = 0.08, .y_m = 0.2},
+ {.time_s = 0, .x_m = 0.08, .y_m = 0.2}};
+ walks[OVERCOME].SetAllLegFreeLinesParam(over_come_front, sizeof(over_come_front)/sizeof(over_come_front[0])); //前足設定
+ walks[OVERCOME].SetOneLegStandParam(LEFT_B, 0, 0.3, frontleg.GetOneWalkTime());
+ walks[OVERCOME].SetOneLegStandParam(RIGHT_B, 0, 0.3, frontleg.GetOneWalkTime());
+ walks[OVERCOME].SetOffsetTime(0, 0.5, 0, 0);
+
+ // SANDDUNE, //前足だけがsandduneに乗った状態で進む
+ start_x_m = 0.02, start_y_m = 0.3, stride_m = 0.1, height_m = 0.03, buffer_height_m = 0.01,
+ stridetime_s = 0.2, toptime_s = 0.07, buffer_time_s = 0.03;
+ walks[SANDDUNE].SetAllLegTriangleParam // 4足一括で設定
+ (start_x_m, start_y_m, stride_m, height_m, buffer_height_m, stridetime_s, toptime_s, buffer_time_s);
+ walks[SANDDUNE].SetOffsetTime(0, 0.5, 0.5, 0); //位相ずれの程度(値域[0..1])
+
+ // OVERCOME_BACK, //後ろ足を乗せる
+ float offset_x_back = -0.05;
+ Lineparam overcomebackParams[6] = {
+ {.time_s = 0.8, .x_m = -0.05 + offset_x_back, .y_m = 0.3},
+ {.time_s = 0.2, .x_m = -0.05 + offset_x_back, .y_m = 0.3},
+ {.time_s = 0.2, .x_m = -0.1 + offset_x_back, .y_m = 0.16},
+ {.time_s = 0.2, .x_m = 0.1 + offset_x_back, .y_m = 0.16},
+ {.time_s = 0.2, .x_m = 0.1 + offset_x_back, .y_m = 0.2},
+ {.time_s = 0.6, .x_m = 0.05 + offset_x_back, .y_m = 0.2},
+ };
+ Lineparam standbackParams[3] = {
+ {.time_s = 0.6, .x_m = 0.0 + offset_x_back, .y_m = 0.2},
+ {.time_s = 0.2, .x_m = 0.0 + offset_x_back, .y_m = 0.2},
+ {.time_s = 0.6, .x_m = -0.05 + offset_x_back, .y_m = 0.2},
+ };
+ walks[OVERCOME_BACK].SetOneLegFreeLinesParam(LEFT_F, standbackParams, sizeof(standbackParams) / sizeof(standbackParams[0]));
+ walks[OVERCOME_BACK].SetOneLegFreeLinesParam(RIGHT_F, standbackParams, sizeof(standbackParams) / sizeof(standbackParams[0]));
+ walks[OVERCOME_BACK].SetOneLegFreeLinesParam(LEFT_B, overcomebackParams, sizeof(overcomebackParams) / sizeof(overcomebackParams[0]));
+ walks[OVERCOME_BACK].SetOneLegFreeLinesParam(RIGHT_B, overcomebackParams, sizeof(overcomebackParams) / sizeof(overcomebackParams[0]));
+ walks[OVERCOME_BACK].SetOffsetTime(0, 0, 0.5, 0);
+
+ // STRAIGHT_W, //両足がsandduneに乗った状態で進む
+ start_x_m = -0.04, start_y_m = 0.2, stride_m = 0.1, height_m = 0.03, buffer_height_m = 0.01,
+ stridetime_s = 0.2, toptime_s = 0.07, buffer_time_s = 0.03;
+ walks[STRAIGHT_W].SetAllLegTriangleParam // 4足一括で設定
+ (start_x_m, start_y_m, stride_m, height_m, buffer_height_m, stridetime_s, toptime_s, buffer_time_s);
+ walks[STRAIGHT_W].ChangeOneParam(LEFT_F, STRAT_X_M, 0.05);
+ walks[STRAIGHT_W].ChangeOneParam(RIGHT_F, STRAT_X_M, 0.05);
+ walks[STRAIGHT_W].SetOffsetTime(0, 0.5, 0.5, 0);
+
+ // ROPE_F, //ropeをまたぐ（前足)
+ float start_x_front_m = 0.05; //前足のstart_x
+ start_x_m = .72, start_y_m = 0.3, stride_m = .15, height_m = 0.03, buffer_height_m = 0.01,
+ stridetime_s = 1, toptime_s = 0.35, buffer_time_s = 0.05;
+ walks[ROPE_F].SetAllLegTriangleParam // 4足一括で設定
+ (start_x_m, start_y_m, stride_m, height_m, buffer_height_m, stridetime_s, toptime_s, buffer_time_s);
+ walks[ROPE_F].ChangeOneParam(LEFT_F, HEIGHT_M, 0.15); //前足だけ足を延ばす
+ walks[ROPE_F].ChangeOneParam(RIGHT_F, HEIGHT_M, 0.15);
+ walks[ROPE_F].SetOffsetTime(0, 0.5, 0.5, 0); //位相ずれの程度(値域[0..1])
+
+ // ROPE_B, //ropeをまたぐ（後ろ足)
+ start_x_m = 0, start_y_m = 0.3, stride_m = .15, height_m = 0.03, buffer_height_m = 0.01,
+ stridetime_s = 1, toptime_s = 0.35, buffer_time_s = 0.05;
+ walks[ROPE_B].SetAllLegTriangleParam // 4足一括で設定
+ (start_x_m, start_y_m, stride_m, height_m, buffer_height_m, stridetime_s, toptime_s, buffer_time_s);
+ walks[ROPE_B].ChangeOneParam(LEFT_B, HEIGHT_M, 0.15); //後ろ足だけ足を延ばす
+ walks[ROPE_B].ChangeOneParam(RIGHT_B, HEIGHT_M, 0.15);
+ walks[ROPE_B].ChangeOneParam(LEFT_B, STRAT_X_M, -0.02);
+ walks[ROPE_B].ChangeOneParam(RIGHT_B, STRAT_X_M, -0.02);
+ walks[ROPE_B].SetOffsetTime(0, 0.5, 0.5, 0); //位相ずれの程度(値域[0..1])
+ */
}
Walk walks[END]; //歩行法
int main()
@@ -140,7 +235,7 @@
for (int i = 1; i < END; i++) //ENDになるまでWalkWayの順に動作
{
DEBUG("Move %d\r\n", i);
- for (int j = 0; j < 2; j++) //debug用に2歩進む
+ for (int j = 0; j < 50; j++) //debug用に2歩進む
MoveOneCycle(walks[i], leg);
}
MoveOneCycle(walks[STANDUP], leg); //最後はLRFを保護するためSTANDUPの状態で終わる