岡本 陸
test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: main.cpp
- Revision:
- 34:89d701e15cdf
- Parent:
- 33:945d634d4c9b
- Child:
- 35:b4e1b8f25cd7
--- a/main.cpp Thu Feb 28 09:39:02 2019 +0000
+++ b/main.cpp Fri Mar 01 12:07:23 2019 +0000
@@ -12,12 +12,14 @@
#include "can.h"
#define USE_CAN //can通信するならdefine.しないなら切らないとエラー出る
//#define USE_ROS
+#include <ros.h>
+#include <geometry_msgs/Vector3.h>
#endif
//#define DEBUG_ON //デバッグ用。使わないときはコメントアウト
#include "debug.h"
#include "OneLeg.h" ///足先の座標を保存するクラス。x,yやサーボの角度の保存、サーボの駆動も行う。他の足を考慮した処理は別のクラスに任せる。
#include "Walk.h" //歩き方に関するファイル
-
+#define USE_OVERCOME
////////////あまり変化させないパラメータ。この他は全てmainの上(ParamSetup())にある。
const int kServoSpan_ms = 6; //サーボの送信間隔
const float kBetweenServoHalf_m = 0.03 * 0.5; //サーボ間の距離の半分
@@ -47,8 +49,6 @@
DigitalOut led[4] = {DigitalOut(LED1), DigitalOut(LED2), DigitalOut(LED3), DigitalOut(LED4)};
#endif
#ifdef USE_ROS
-#include <ros.h>
-#include <geometry_msgs/Vector3.h>
ros::NodeHandle nh_mbed;
//ROSからのコールバック関数
void callback(const geometry_msgs::Vector3 &cmd_vel);
@@ -57,13 +57,7 @@
ros::Subscriber<geometry_msgs::Vector3> sub_vel("/cmd_vel", &callback);
#endif
OneLeg leg[4]; //各足の位置
-enum LegNum //足とシリアルサーボの番号
-{
- RIGHT_B,
- RIGHT_F,
- LEFT_B,
- LEFT_F,
-};
+
FILE *fp;
const float kRadToDegree = 180.0 / M_PI;
int MoveOneCycle(Walk walkway, OneLeg leg[4]);
@@ -79,18 +73,11 @@
enum WalkWay
{
STANDUP,
- STRAIGHT,
- TURNLEFT,
- TURNRIGHT,
- UP, //足を伸ばして立つ
- OVERCOME, //前足を乗せる
- SANDDUNE, //前足だけがsandduneに乗った状態で進む
- OVERCOME_BACK, //後ろ足を乗せる
- STRAIGHT_W, //両足がsandduneに乗った状態で進む
- ROPE_F, //ropeをまたぐ（前足)
- ROPE_B, //ropeをまたぐ（後ろ足)
+ // STRAIGHT,
+ // TURNLEFT,
+ // TURNRIGHT,
+ UP, //足を伸ばして立つ
END,
- STRAIGHT_W2, //後足がsandduneに乗った状態で進む
};
void ParamsSetup(Walk walks[END], OneLeg leg[4]) //各パラメータの設定。減らしていく必要あり
{
@@ -99,10 +86,10 @@
for (int i = 0; i < 4; i++)
leg[i] = OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2);
walks[STANDUP].SetAllLegStandParam(0, 0.2, 0.5); //x,y,time_s.一括で設定できる
-
+ /*
//STRAIGT 論文通りのとき、offset_y_mは <=0.32
float offset_x_m = -0.05, offset_y_m = 0.20, stride_m = 0.12, height_m = 0.05, buffer_height_m = 0.01,
- stridetime_s = 0.3, toptime_s = 0.07, buffer_time_s = 0.03;
+ stridetime_s = 1, toptime_s = 0.1, buffer_time_s = 0.03;
walks[STRAIGHT].SetAllLegTriangleParam // 4足を取り敢えず一括で設定してから
(offset_x_m, offset_y_m, stride_m, height_m, buffer_height_m, stridetime_s, toptime_s, buffer_time_s);
walks[STRAIGHT].ChangeOneParam(LEFT_F, OFFSET_X_M, 0);
@@ -114,7 +101,7 @@
//TURNLEFTのparam
float stride_short_m = 0.05, stride_long_m = 0.25;
stridetime_s = 0.3, toptime_s = 0.07, buffer_time_s = 0.03,
- offset_x_m=0, offset_y_m = 0.2, height_m = 0.03, buffer_height_m = 0.01;
+ offset_x_m = 0, offset_y_m = 0.2, height_m = 0.03, buffer_height_m = 0.01;
walks[TURNLEFT].SetAllLegTriangleParam // 4足を取り敢えず一括で設定してから
(offset_x_m, offset_y_m, stride_long_m, height_m, buffer_height_m, stridetime_s, toptime_s, buffer_time_s);
walks[TURNLEFT].ChangeOneParam(LEFT_F, STRIDE_M, stride_short_m); //一部のパラメータを変更
@@ -123,140 +110,70 @@
//TURNRIGHT1のparam
walks[TURNRIGHT].SetAllLegTriangleParam // 4足を取り敢えず一括で設定してから
- (offset_x_m, offset_y_m, stride_long_m, height_m, buffer_height_m, stridetime_s, toptime_s, buffer_time_s);
+ (offset_x_m, offset_y_m, stride_long_m, height_m, buffer_height_m, stridetime_s, toptime_s, buffer_time_s);
walks[TURNRIGHT].ChangeOneParam(RIGHT_F, STRIDE_M, stride_short_m); //一部のパラメータを変更
walks[TURNRIGHT].ChangeOneParam(RIGHT_B, STRIDE_M, stride_short_m);
walks[TURNRIGHT].SetOffsetTime(0, 0.5, 0.5, 0); //位相ずれの程度(値域[0..1])
-
+ */
// UP, //足を伸ばして立つ
- LineParam up[] = {
- {.time_s = 1, .x_m = 0, .y_m = 0.3},
- {.time_s = 0, .x_m = 0, .y_m = 0.3}};
- walks[UP].SetAllLegStandParam(0, 0.3, 1.2); //x,y,time_s.一括で設定できる
-
- // OVERCOME 前足を乗せる
-
- LineParam over_come_front_r[] = {
- {.time_s = 0.2, .x_m = 0, .y_m = 0.32},
- {.time_s = 0.2, .x_m = -0.13, .y_m = 0.15},
- {.time_s = 0.2, .x_m = 0.12, .y_m = 0.15},
- {.time_s = 0.6, .x_m = 0.12, .y_m = 0.2},
- {.time_s = 0,
- .x_m = 0.05, .y_m = 0.2}};
- LineParam over_come_front_l[] = {
- {.time_s = 0.6, .x_m = -0, .y_m = 0.32},
- {.time_s = 0.2, .x_m = -0, .y_m = 0.32},
- {.time_s = 0.2, .x_m = -0.13, .y_m = 0.15},
- {.time_s = 0.2, .x_m = 0.12, .y_m = 0.15},
- {.time_s = 0, .x_m = 0.12, .y_m = 0.2}};
- LineParam over_come_backleg[] = {
- {.time_s = 0.6, .x_m = 0, .y_m = 0.3},
- {.time_s = 0.2, .x_m = 0, .y_m = 0.3},
- {.time_s = 0.4, .x_m = -0.1,.y_m = 0.3},
- {.time_s = 0, .x_m = -0.1,.y_m = 0.3},
- };
- walks[OVERCOME].SetOneLegFreeLinesParam(RIGHT_F, over_come_front_r, sizeof(over_come_front_r)/ sizeof(over_come_front_r[0]));
- walks[OVERCOME].SetOneLegFreeLinesParam(LEFT_F, over_come_front_l, sizeof(over_come_front_l)/sizeof(over_come_front_l[0]));
- walks[OVERCOME].SetOneLegFreeLinesParam(LEFT_B, over_come_backleg, sizeof(over_come_backleg)/sizeof(over_come_backleg[0]));
- walks[OVERCOME].SetOneLegFreeLinesParam(RIGHT_B, over_come_backleg, sizeof(over_come_backleg)/sizeof(over_come_backleg[0]));
-
-
- LineParam over_come_front_r[] = {
- {.time_s = 0.2, .x_m = 0, .y_m = 0.32},
- {.time_s = 0.2, .x_m = -0.13, .y_m = 0.15},
- {.time_s = 0.2, .x_m = 0.12, .y_m = 0.15},
- {.time_s = 0.6, .x_m = 0.12, .y_m = 0.2},
- {.time_s = 0,
- .x_m = 0.05, .y_m = 0.2}};
- LineParam over_come_front_l[] = {
- {.time_s = 0.6, .x_m = -0, .y_m = 0.32},
- {.time_s = 0.2, .x_m = -0, .y_m = 0.32},
- {.time_s = 0.2, .x_m = -0.13, .y_m = 0.15},
- {.time_s = 0.2, .x_m = 0.12, .y_m = 0.15},
- {.time_s = 0, .x_m = 0.12, .y_m = 0.2}};
- LineParam over_come_backleg[] = {
- {.time_s = 0.6, .x_m = 0, .y_m = 0.3},
- {.time_s = 0.2, .x_m = 0, .y_m = 0.3},
- {.time_s = 0.4, .x_m = -0.1,.y_m = 0.3},
- {.time_s = 0, .x_m = -0.1,.y_m = 0.3},
- };
- walks[OVERCOME].SetOneLegFreeLinesParam(RIGHT_F, over_come_front_r, sizeof(over_come_front_r)/ sizeof(over_come_front_r[0]));
- walks[OVERCOME].SetOneLegFreeLinesParam(LEFT_F, over_come_front_l, sizeof(over_come_front_l)/sizeof(over_come_front_l[0]));
- walks[OVERCOME].SetOneLegFreeLinesParam(LEFT_B, over_come_backleg, sizeof(over_come_backleg)/sizeof(over_come_backleg[0]));
- walks[OVERCOME].SetOneLegFreeLinesParam(RIGHT_B, over_come_backleg, sizeof(over_come_backleg)/sizeof(over_come_backleg[0]));
-
-
- // SANDDUNE, //前足だけがsandduneに乗った状態で進む
- offset_x_m = -0.03, offset_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足一括で設定
- (offset_x_m, offset_y_m, stride_m, height_m, buffer_height_m, stridetime_s, toptime_s, buffer_time_s);
- walks[SANDDUNE].ChangeOneParam(LEFT_F, OFFSET_X_M, 0);
- walks[SANDDUNE].ChangeOneParam(RIGHT_F, OFFSET_X_M, 0);
- walks[SANDDUNE].ChangeOneParam(LEFT_F, OFFSET_Y_M, offset_y_m - 0.1);
- walks[SANDDUNE].ChangeOneParam(RIGHT_F, OFFSET_Y_M, offset_y_m - 0.1);
- walks[SANDDUNE].SetOffsetTime(0, 0.5, 0.5, 0); //位相ずれの程度(値域[0..1])
-
- // OVERCOME_BACK, //後ろ足を乗せる
- float offset_x_back = -0.05;
- LineParam overcomebackParams[] = {
- {.time_s = 0.8, .x_m = -0.05f + offset_x_back, .y_m = 0.3},
- {.time_s = 0.2, .x_m = -0.05f + offset_x_back, .y_m = 0.3},
- {.time_s = 0.2, .x_m = -0.1f + offset_x_back, .y_m = 0.16},
- {.time_s = 0.2, .x_m = 0.1f + offset_x_back, .y_m = 0.16},
- {.time_s = 0.2, .x_m = 0.1f + offset_x_back, .y_m = 0.2},
- {.time_s = 0.8, .x_m = 0.05f + offset_x_back, .y_m = 0.2},
- {.time_s = 0, .x_m = 0.05f + offset_x_back, .y_m = 0.2},
- };
- LineParam standbackParams[] = {
- {.time_s = 0.6, .x_m = 0.0f + offset_x_back, .y_m = 0.2},
- {.time_s = 0.2, .x_m = 0.0f + offset_x_back, .y_m = 0.2},
- {.time_s = 0.6, .x_m = -0.05f + offset_x_back, .y_m = 0.2},
- {.time_s = 0, .x_m = -0.05f + 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に乗った状態で進む
- offset_x_m = -0.09, offset_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足一括で設定
- (offset_x_m, offset_y_m, stride_m, height_m, buffer_height_m, stridetime_s, toptime_s, buffer_time_s);
- walks[STRAIGHT_W].ChangeOneParam(LEFT_F, OFFSET_X_M, 0);
- walks[STRAIGHT_W].ChangeOneParam(RIGHT_F, OFFSET_X_M, 0);
- walks[STRAIGHT_W].SetOffsetTime(0, 0.5, 0.5, 0);
-
- // ROPE_F, //ropeをまたぐ（前足)
- offset_x_m = 0, offset_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足一括で設定
- (offset_x_m, offset_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をまたぐ（後ろ足)
- offset_x_m = -0.05, offset_y_m = 0.3, stride_m = .1, 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足一括で設定
- (offset_x_m, offset_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, OFFSET_X_M, -0.07);
- walks[ROPE_B].ChangeOneParam(RIGHT_B, OFFSET_X_M, -0.07);
- walks[ROPE_B].SetOffsetTime(0, 0.5, 0.5, 0); //位相ずれの程度(値域[0..1])
+ walks[UP].SetAllLegStandParam(0, 0.30, 2); //x,y,time_s.一括で設定できる
+ // walks[UP].SetOneLegStandParam(RIGHT_B, 0, 0.30, 2); //x,y,time_s
+ // walks[UP].SetOneLegStandParam(RIGHT_F, 0, 0.30, 2); //x,y,time_s
+ // walks[UP].SetOneLegStandParam(LEFT_B, 0, 0.30, 2); //x,y,time_s
+ // walks[UP].SetOneLegStandParam(RIGHT_B, 0, 0.30, 2); //x,y,time_s
}
Walk walks[END]; //歩行法
int main()
{
+
+ //足とシリアルサーボの番号
+ // RIGHT_B, RIGHT_F, LEFT_B, LEFT_F,};
+ //前足かける
+ float start_x_m[4] = {-0, 0, -0, 0}; //足のoffset
+ float start_y_m[4] = {.3, .3, .31, .3};
+ float d_x_m = 0.15;
+ float goal_y_m[4] = {.3, 0.2, .31, 0.2};
+ float height_m[] = {0.03, 0.13, 0.03, 0.13};
+ float gravity_dist[] = {0.075, 0, 0.075, 0};
+ OverCome overcome(start_x_m, start_y_m, d_x_m, goal_y_m, height_m, gravity_dist);
+ //そのまま進む
+ float d_x_m2 = 0.075;
+ float height1_m[] = {0.05, 0.05, 0.05, 0.05};
+ OverCome overcome1(start_x_m, goal_y_m, d_x_m2, goal_y_m, height1_m, gravity_dist);
+ //後ろ脚載せる
+ float height2_m[] = {0.08, 0.08, 0.08, 0.08};
+ float start2_y_m[] = {.25, .3, .25, .3};
+ float goal2_y_m[] = {.2, .3, .2, .3};
+ OverCome overcome2(start_x_m, start2_y_m, d_x_m2, goal2_y_m, height2_m, gravity_dist);
+ //
+ float height3_m[] = {0.05, 0.05, 0.05, 0.05};
+ float start3_x_m[] = {0, 0, 0, 0.05};
+ OverCome overcome3(start3_x_m, goal2_y_m, d_x_m2, goal2_y_m, height3_m, gravity_dist);
+ //後ろ脚を落とす
+ float d_x_m4 = 0.1;
+ float goal4_y_m[] = {.3, .3, .3, .3};
+ OverCome overcome4(start_x_m, goal2_y_m, d_x_m4, goal4_y_m, height3_m, gravity_dist);
+ OverCome overcome5(start_x_m, goal4_y_m, d_x_m4, goal4_y_m, height3_m, gravity_dist);
+
+/*ROPEを超えた
+ //足とシリアルサーボの番号
+ // RIGHT_B, RIGHT_F, LEFT_B, LEFT_F,};
+ //上手く行った
+ float start_x_m[4] = {-0, 0, -0, 0};
+ float start_y_m[4] = {.3, .28, .3, .28};
+ float d_x_m = 0.1;
+ float goal_y_m[4] = {.3, .28, .3, .28};
+ float height_m = 0.13;
+ float gravity_dist[] = {0.075, -0.03, 0.105, 0};
+ OverCome rope[] = {
+ OverCome(start_x_m, start_y_m, d_x_m, goal_y_m, height_m, gravity_dist),
+ };
+*/
#ifdef VSCODE
if (FileOpen()) //csv fileに書き込み
return 1; //異常終了したら強制終了
#endif
- DEBUG("param set start");
+ printf("param set start\r\n");
ParamsSetup(walks, leg); //各動きの際の足の軌道を設定。パラメータはこの関数内に打ち込む
for (int i = 0; i < END; i++) //軌道のチェック
{
@@ -266,6 +183,49 @@
return 1; //強制終了.errorは内部の関数からprintfで知らせる
}
}
+ /*ROPEのチェック
+ for (int i = 0; i < sizeof(rope) / sizeof(rope[0]); i++)
+ {
+ if (rope[i].walk.CheckOrbit(leg[0]) == 1) //軌道が定義外なら
+ {
+ printf("error: rope pattern %d\r\n", i);
+ return 1; //強制終了.errorは内部の関数からprintfで知らせる
+ }
+ }
+*/
+ if (overcome.walk.CheckOrbit(leg[0]) == 1) //軌道が定義外なら
+ {
+ printf("error: move pattern overcome0\r\n");
+ return 1; //強制終了.errorは内部の関数からprintfで知らせる
+ }
+ if (overcome1.walk.CheckOrbit(leg[0]) == 1) //軌道が定義外なら
+ {
+ printf("error: move pattern overcome1\r\n");
+ return 1; //強制終了.errorは内部の関数からprintfで知らせる
+ }
+
+ if (overcome2.walk.CheckOrbit(leg[0]) == 1) //軌道が定義外なら
+ {
+ printf("error: move pattern overcome 2\r\n");
+ return 1; //強制終了.errorは内部の関数からprintfで知らせる
+ }
+ if (overcome3.walk.CheckOrbit(leg[0]) == 1) //軌道が定義外なら
+ {
+ printf("error: move pattern overcome 3\r\n");
+ return 1; //強制終了.errorは内部の関数からprintfで知らせる
+ }
+
+ if (overcome4.walk.CheckOrbit(leg[0]) == 1) //軌道が定義外なら
+ {
+ printf("error: move pattern overcome 4\r\n");
+ return 1; //強制終了.errorは内部の関数からprintfで知らせる
+ }
+
+ if (overcome5.walk.CheckOrbit(leg[0]) == 1) //軌道が定義外なら
+ {
+ printf("error: move pattern overcome 5\r\n");
+ return 1; //強制終了.errorは内部の関数からprintfで知らせる
+ }
printf("Stand up?\r\n");
WaitStdin('y'); // ボタンを押したら立つ
MoveOneCycle(walks[STANDUP], leg);
@@ -278,29 +238,27 @@
while (1)
nh_mbed.spinOnce();
#else
-
- for(int k=0;k<5;k++)MoveOneCycle(walks[STRAIGHT], leg);
- for(int k=0;k<1;k++)MoveOneCycle(walks[UP], leg);
- for(int k=0;k<1;k++)MoveOneCycle(walks[OVERCOME],leg);
- for(int k=0;k<5;k++) MoveOneCycle(walks[SANDDUNE],leg);
-// for(int k=0;k<1;k++)MoveOneCycle(walks[OVERCOME_BACK],leg);
-// for(int k=0;k<3;k++)MoveOneCycle(walks[STRAIGHT_W], leg);
-// for(int k=0;k<5;k++)Move(*straight_w2, leg,(*straight_w2).GetTheta(0.1));
-// for(int k=0;k<3;k++)Move(*straight, leg);
-/*
- for (int i = 1; i < END; i++) //ENDになるまでWalkWayの順に動作
- {
- printf("Move %d\r\n", i);
- for (int j = 0; j < 20; j++) //debug用に2歩進む
- {
- if(MoveOneCycle(walks[i], leg) == 1)
- printf("error:In MoveOneCycle. WalkWay %d\r\n",i);
- }
- }
-
- MoveOneCycle(walks[STANDUP], leg); //最後はLRFを保護するためSTANDUPの状態で終わる
-
- */ printf("program end\r\n");
+
+ // for (int k = 0; k < 5; k++)//壁あて.機体保護のため抜いておく
+ // MoveOneCycle(walks[STRAIGHT], leg);
+ MoveOneCycle(walks[UP], leg); //立つ
+ for (int k = 0; k < 1; k++) //前足かける
+ MoveOneCycle(overcome.walk, leg);
+ for (int k = 0; k < 5; k++) //そのまま進む
+ MoveOneCycle(overcome1.walk, leg);
+ for (int k = 0; k < 1; k++) //後ろ足載せる
+ MoveOneCycle(overcome2.walk, leg);
+ for (int k = 0; k < 3; k++) //そのまま進む
+ MoveOneCycle(overcome3.walk, leg);
+ for (int k = 0; k < 1; k++) //後ろ足を落とす
+ MoveOneCycle(overcome4.walk, leg);
+ for (int k = 0; k < 5; k++) //足上げたまま動く
+ MoveOneCycle(overcome5.walk, leg);
+ /*rope用
+ for (int k = 0; k < 10; k++) //前足かける
+ MoveOneCycle(rope[0].walk, leg);
+*/
+ printf("program end\r\n");
#ifdef VSCODE
fclose(fp);
#endif
@@ -392,6 +350,7 @@
{
float left_vel = cmd_vel.x;
float right_vel = cmd_vel.y;
+ int state = cmd_vel.z;
//閾値は要検討
if (right_vel < left_vel)
MoveOneCycle(walks[TURNRIGHT], leg);
@@ -401,10 +360,12 @@
MoveOneCycle(walks[STRAIGHT], leg);
}
#endif
+/*
//param back_height_on_step:段差の高さ。後ろにあるとする。前にあるときはマイナスを入れる
float GetSteepBodyRad(float back_height_on_step)
{
float offset_hight = back_height_on_step + leg[LEFT_B].GetY_m() - leg[LEFT_B].GetY_m();
float theta = atan2(offset_hight, (float)(sqrt(0.09 - offset_hight * offset_hight)));
return theta;
-}
\ No newline at end of file
+}
+*/
\ No newline at end of file