岡本 陸
test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: main.cpp
- Revision:
- 19:1adc7302cfd9
- Parent:
- 18:0033ef1814ba
- Child:
- 20:70cc6083e9c7
--- a/main.cpp Thu Feb 14 09:04:25 2019 +0000
+++ b/main.cpp Fri Feb 15 03:46:52 2019 +0000
@@ -8,13 +8,13 @@
#include "can.h"
#include "OneLeg.h" ///足先の座標を保存するクラス。x,yやサーボの角度の保存、サーボの駆動も行う。他の足を考慮した処理は別のクラスに任せる。
#include "Walk.h" //歩き方に関するファイル
-#define USE_CAN//can通信するならdefine.しないなら切らないとエラー出る
+#define USE_CAN //can通信するならdefine.しないなら切らないとエラー出る
////////////調整すべきパラメータ.全てここに集めた。
-const float kCycleTime_s = 0.03f; //計算周期
-const float kBetweenServoHalf_m = 0.03f * 0.5; //サーボ間の距離の半分
-const float kLegLength1 = 0.1f;
-const float kLegLength2 = 0.2452f+0.0025f;//0.22529+0.0025
+const float kCycleTime_s = 0.03; //計算周期
+const float kBetweenServoHalf_m = 0.03 * 0.5; //サーボ間の距離の半分
+float kLegLength1[2] = {0.1, 0.1};
+float kLegLength2[2] = {0.2452 + 0.0025, 0.22529 + 0.0025};
//サーボの設定
const int kServoSpan_ms = 10; //サーボの送信間隔
const double kServoValToDegree = 270.0 / (11500 - 3500);
@@ -26,7 +26,7 @@
{
-1,
1,
- }};
+ }};
//欲しい座標系0度でのサーボのICSマネージャーの値
const double kOriginDegree[2][2] = {
{
@@ -34,8 +34,8 @@
(6600 - 3500) * kServoValToDegree + 180,
},
{
- (7523 - 3500) * kServoValToDegree ,
- (6657 - 3500) * kServoValToDegree-180,//180度の時6657シータ負の方向に動かすと値は減る
+ (7523 - 3500) * kServoValToDegree,
+ (6657 - 3500) * kServoValToDegree - 180, //180度の時6657シータ負の方向に動かすと値は減るので、0度の位置は-180すれば出る
},
};
///////////////
@@ -51,80 +51,75 @@
OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2),
OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2),
};
-DigitalOut led[4] = {DigitalOut(LED1),DigitalOut(LED2),DigitalOut(LED3),DigitalOut(LED4)};
+DigitalOut led[4] = {DigitalOut(LED1), DigitalOut(LED2), DigitalOut(LED3), DigitalOut(LED4)};
const float kRadToDegree = 180.0 / M_PI;
void Move(Walk WalkWay, OneLeg (&leg)[4], float dist_m);
void MoveServo(OneLeg leg, int legnum, int servo_id);
-
-int IsArrived(int count, int finish){
- if(count > finish)
+int IsArrived(int count, int finish)
+{
+ if (count > finish)
return 1;
else
return 0;
}
-
int main()
{
printf("When you push any key, this robot starts.\r\n");
while (pc.readable() == 0) //キーボード押したらスタート
;
- printf("stand up. 1 sec stand\r\n");
-
- //stand時の足の軌道設定
- Orbit stand_orbits[4];
- Orbit temp_orbit(ELLIPSE);
- float stand_stridetime_s = 1, stand_risetime_s = 0, stand_stride_m = 0, stand_height_m = 0.05f, stand_ground_m = 0.25f;
- temp_orbit.SetStraightParam(stand_stridetime_s, stand_risetime_s, stand_stride_m, stand_height_m, stand_ground_m);
- for (int i = 0; i < 4; i++)
- stand_orbits[i] = temp_orbit;
-
+ printf("stand up for 1 s\r\n");
+
+ //stand時の足の軌道設定.
+ Orbit stand_orbit[4];
+ float stand_offset_y_m = 0.2;
+ stand_orbit[0].SetStandParam(stand_offset_y_m);
+ for (int i = 0; i < 4; i++) //全部足を同じにしてる
+ stand_orbit[i] = stand_orbit[0];
//4足の位相ずれOffsetTime_sをまとめる
float stand_offset_time_s[4] = {
0,
- stand_orbits[0].GetOneWalkTime() * 0.5,
- stand_orbits[0].GetOneWalkTime() * 0,
- stand_orbits[0].GetOneWalkTime() * 0.5,};
+ stand_orbit[0].GetOneWalkTime() * 0.5,
+ stand_orbit[0].GetOneWalkTime() * 0,
+ stand_orbit[0].GetOneWalkTime() * 0.5,
+ };
//4つの足のorbit, 位相を代入してstraightという歩行パターンを作成している
//このインスタンスはlegそれぞれの計算を行う役割を担う
- //orbitはここでしか使わないあくまで鍵のような扱い
- Walk stand(stand_orbits, stand_offset_time_s, kCycleTime_s);
- //収束判定用関数。現在は回すループの数
- float dist_m = 10;
- //Walkの指示通りに動作
+ //orbitはここでしか使わないあくまでパラメータ設定用クラス
+ Walk stand(stand_orbit, stand_offset_time_s, kCycleTime_s);
+ float dist_m = 10; //収束判定用。現在は回すループの数
+ //動作開始
Move(stand, leg, dist_m);
-
wait(1);
- DEBUG("move start\r\n");
- //各足の軌道の設定.今回は全部同じにすることで直線を描く。
- //直進したパラメータstridetime_s = 0.5, risetime_s = 0.3, stride_m = 0.12f, height_m = 0.03f, ground_m = 0.16f;
- float stridetime_s = 3, risetime_s = 0.3, stride_m = 0.10f, height_m = 0.1f, ground_m = stand_ground_m;
+
+ DEBUG("move start\r\n");
+ //取り敢えず歩行したパラメータ
+ //stridetime_s = 0.5, risetime_s = 0.3,stride_m= 0.12f, height_m = 0.03f, offset_x_m = 0, offset_y_m = 0.16f;
+ //曲げてみる
+ float stridetime_s = 2, risetime_s = 0.5,
+ stride_r_m = 0.1, stride_l_m = 0.05, height_m = 0.03,
+ offset_x_m = -0.05, offset_y_m = stand_offset_y_m;
//軌道の作成
//代入用の軌道SetStraightParam関数を用いると真っ直ぐ進む前提となる
- temp_orbit.SetStraightParam(stridetime_s, risetime_s, stride_m, height_m, ground_m);
- Orbit straightOrbit[4];
- straightOrbit[1] = temp_orbit;
- straightOrbit[3]= temp_orbit;
- //stride_m = 0.075f;
- float offset_x_m = 0.1;
- temp_orbit.SetStraightParam(stridetime_s, risetime_s, stride_m, height_m, ground_m, offset_x_m);
- straightOrbit[0] = temp_orbit;
- straightOrbit[2]= temp_orbit;
-
+ Orbit orbit[4];
+ orbit[0].SetStraightParam(stridetime_s, risetime_s, stride_r_m, height_m, offset_x_m, offset_y_m);
+ orbit[1].SetStraightParam(stridetime_s, risetime_s, stride_r_m, height_m, 0, offset_y_m);
+ orbit[2].SetStraightParam(stridetime_s, risetime_s, stride_l_m, height_m, offset_x_m, offset_y_m);
+ orbit[3].SetStraightParam(stridetime_s, risetime_s, stride_l_m, height_m, 0, offset_y_m);
//4足の軌道と位相ずれOffsetTime_sをまとめる
float offset_time_s[4] = {
0,
- straightOrbit[0].GetOneWalkTime() * 0.25,
- straightOrbit[0].GetOneWalkTime() * 0.5,
- straightOrbit[0].GetOneWalkTime() * 0.75,
+ orbit[0].GetOneWalkTime() * 0.5,
+ orbit[0].GetOneWalkTime() * 0.5,
+ orbit[0].GetOneWalkTime() * 0,
};
- //4つの足のorbit, 位相を代入してstraightという歩行パターンを作成している
+ //4つの足のorbit, 位相を代入して歩行パターンを作成している
//このインスタンスはlegそれぞれの計算を行う役割を担う
//orbitはここでしか使わないあくまで鍵のような扱い
- Walk straight2(straightOrbit, offset_time_s, kCycleTime_s);
- //実際にWalkの指示通りに動かす
- dist_m = 500;
- Move(straight2, leg, dist_m);
+ Walk walk(orbit, offset_time_s, kCycleTime_s);
+ dist_m = 500000; //収束判定用。取り敢えず今はループ数。
+ //動く
+ Move(walk, leg, dist_m);
DEBUG("program end\r\n");
}
@@ -138,38 +133,38 @@
while (is_arrived == 0)
{
float time_s = timer.read();
- //注:未実装。到着したかの判定.LRFからのデータが必要？
+ //注:未実装。到着したかの判定.LRFからのデータが必要.今は取り敢えずループ回数で判断
is_arrived = IsArrived(count, (int)dist_m);
++count;
//4本の足それぞれの足先サーボ角度更新
WalkWay.Cal4LegsPosi(leg);
- #ifdef USE_CAN
+#ifdef USE_CAN
//slave_mbed分の足の目標位置を送信
- SendRad(leg[2],leg[3]);
- #endif
+ SendRad(leg[2], leg[3]);
+#endif
//自身が動かす足のサーボを動かす
MoveServo(leg[0], 0, 0);
MoveServo(leg[1], 1, 0);
wait_ms(kServoSpan_ms);
MoveServo(leg[0], 0, 1);
MoveServo(leg[1], 1, 1);
- DEBUG("%f, %f, %f, %f\r\n",leg[2].GetRad(0),leg[2].GetRad(1),leg[3].GetRad(0),leg[3].GetRad(1));
+ DEBUG("%f, %f, %f, %f\r\n", leg[2].GetRad(0), leg[2].GetRad(1), leg[3].GetRad(0), leg[3].GetRad(1));
//計算周期がWalkWay.cycletime_s_になるようwait
float rest_time_s = WalkWay.cycletime_s_ - (timer.read() - time_s);
if (rest_time_s > 0)
wait(rest_time_s);
- else {//計算周期が達成できないときはDEBUGで知らせるだけ。動きはする。
- printf("error: rest_time_s = %f in Move()\r\n", rest_time_s);
+ else
+ { //計算周期が達成できないときはDEBUGで知らせるだけ。動きはする。
+ DEBUG("error: rest_time_s = %f in Move()\r\n", rest_time_s);
led[0] = 1;
}
}
}
-
void MoveServo(OneLeg leg, int serial_num, int servo_id)
{
float degree = leg.GetRad(servo_id) * kRadToDegree;
//サーボの座標系に変更
float servo_degree = kServoSign[serial_num][servo_id] * degree + kOriginDegree[serial_num][servo_id];
- DEBUG("servo_degree[%d][%d],%f\r\n",serial_num,servo_id,servo_degree);
+ DEBUG("servo_degree[%d][%d],%f\r\n", serial_num, servo_id, servo_degree);
servo[serial_num].set_degree(servo_id, servo_degree);
}