test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: main.cpp
- Revision:
- 18:0033ef1814ba
- Parent:
- 17:ca18c5980a34
- Child:
- 19:1adc7302cfd9
diff -r ca18c5980a34 -r 0033ef1814ba main.cpp --- a/main.cpp Wed Feb 13 07:49:24 2019 +0000 +++ b/main.cpp Thu Feb 14 09:04:25 2019 +0000 @@ -2,18 +2,19 @@ #include "mbed.h" #include "pinnames.h" #include "KondoServo.h" +//#define DEBUG_ON//デバッグ用。使わないときはコメントアウト #include "debug.h" #include "pi.h" #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.2f; +const float kLegLength2 = 0.2452f+0.0025f;//0.22529+0.0025 //サーボの設定 const int kServoSpan_ms = 10; //サーボの送信間隔 const double kServoValToDegree = 270.0 / (11500 - 3500); @@ -23,18 +24,18 @@ -1, }, { + -1, 1, - -1, }}; //欲しい座標系0度でのサーボのICSマネージャーの値 const double kOriginDegree[2][2] = { { - (7374 - 3500) * kServoValToDegree, - (7458 - 3500) * kServoValToDegree + 180, + (7199 - 3500) * kServoValToDegree, + (6600 - 3500) * kServoValToDegree + 180, }, { - (6629 - 3500) * kServoValToDegree , - (7092 - 3500) * kServoValToDegree+180, + (7523 - 3500) * kServoValToDegree , + (6657 - 3500) * kServoValToDegree-180,//180度の時6657シータ負の方向に動かすと値は減る }, }; /////////////// @@ -43,6 +44,7 @@ KondoServo(pin_serial_servo_tx[0], pin_serial_servo_rx[0]), KondoServo(pin_serial_servo_tx[1], pin_serial_servo_rx[1]), }; +//足の作成、サイズデータのみの足の骨組 OneLeg leg[4] = { OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2), OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2), @@ -55,33 +57,74 @@ 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) + return 1; + else + return 0; +} + int main() { - DEBUG("When you push any key, this robot starts.\r\n"); + printf("When you push any key, this robot starts.\r\n"); while (pc.readable() == 0) //キーボード押したらスタート ; - DEBUG("move start\r\n"); + 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; + + //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,}; + //4つの足のorbit, 位相を代入してstraightという歩行パターンを作成している + //このインスタンスはlegそれぞれの計算を行う役割を担う + //orbitはここでしか使わないあくまで鍵のような扱い + Walk stand(stand_orbits, stand_offset_time_s, kCycleTime_s); + //収束判定用関数。現在は回すループの数 + float dist_m = 10; + //Walkの指示通りに動作 + Move(stand, leg, dist_m); + + wait(1); + DEBUG("move start\r\n"); //各足の軌道の設定.今回は全部同じにすることで直線を描く。 - float stridetime_s = 1, risetime_s = 0.5, stride_m = 0.2f, height_m = 0.05f, ground_m = 0.2f; + //直進したパラメータ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; + //軌道の作成 + //代入用の軌道SetStraightParam関数を用いると真っ直ぐ進む前提となる + temp_orbit.SetStraightParam(stridetime_s, risetime_s, stride_m, height_m, ground_m); Orbit straightOrbit[4]; - Orbit default_orbit(ELLIPSE); - default_orbit.SetStraightParam(stridetime_s, risetime_s, stride_m, height_m, ground_m); - for (int i = 0; i < 4; i++) - straightOrbit[i] = default_orbit; + 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; + //4足の軌道と位相ずれOffsetTime_sをまとめる float offset_time_s[4] = { 0, + straightOrbit[0].GetOneWalkTime() * 0.25, straightOrbit[0].GetOneWalkTime() * 0.5, - 0, - straightOrbit[0].GetOneWalkTime() * 0.5, + straightOrbit[0].GetOneWalkTime() * 0.75, }; //4つの足のorbit, 位相を代入してstraightという歩行パターンを作成している //このインスタンスはlegそれぞれの計算を行う役割を担う //orbitはここでしか使わないあくまで鍵のような扱い - Walk straight(straightOrbit, offset_time_s, kCycleTime_s); + Walk straight2(straightOrbit, offset_time_s, kCycleTime_s); //実際にWalkの指示通りに動かす - float dist_m = 0.5f; - Move(straight, leg, dist_m); + dist_m = 500; + Move(straight2, leg, dist_m); DEBUG("program end\r\n"); } @@ -91,11 +134,13 @@ timer.reset(); timer.start(); int is_arrived = 0; + int count = 0; while (is_arrived == 0) { float time_s = timer.read(); //注:未実装。到着したかの判定.LRFからのデータが必要? - //is_arrived = IsArrived(); + is_arrived = IsArrived(count, (int)dist_m); + ++count; //4本の足それぞれの足先サーボ角度更新 WalkWay.Cal4LegsPosi(leg); #ifdef USE_CAN @@ -125,6 +170,6 @@ 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); }