test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: main.cpp
- Revision:
- 14:d7cb429946f4
- Parent:
- 13:e7ecdb20665a
- Child:
- 16:0069a56f11a3
--- a/main.cpp Tue Feb 12 07:51:07 2019 +0000 +++ b/main.cpp Tue Feb 12 12:50:45 2019 +0000 @@ -2,15 +2,40 @@ #include "mbed.h" #include "pinnames.h" #include "KondoServo.h" +#include "debug.h" +#include "pi.h" #include "OneLeg.h" ///足先の座標を保存するクラス。x,yやサーボの角度の保存、サーボの駆動も行う。他の足を考慮した処理は別のクラスに任せる。 #include "Walk.h" //歩き方に関するファイル ////////////調整すべきパラメータ.全てここに集めた。 -const float kCycleTime_s = 0.02f; //計算周期 -const float kBetweenServoHalf_m = 0.06f * 0.5; //サーボ間の距離の半分 +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 kServoSpan_ms = 7.0f; //サーボの送信間隔 +const int kServoSpan_ms = 10; //サーボの送信間隔 + +//サーボの設定 +const double kServoValToDegree = 270.0 / (11500 - 3500); +//サーボの正負と座標系の正負の補正.足で一セット。 +const int kServoSign[2][2] = {{ + 1, + -1, + }, + { + 1, + -1, + }}; +//欲しい座標系0度でのサーボのICSマネージャーの値 +const double kOriginDegree[2][2] = { + { + (4096 - 3500) * kServoValToDegree + 90, + (4724 - 3500) * kServoValToDegree + 270, + }, + { + (7384 - 3500) * kServoValToDegree , + (7523 - 3500) * kServoValToDegree+180, + }, +}; /////////////// Timer timer; KondoServo servo[2] = { @@ -23,28 +48,25 @@ OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2), OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2), }; -DigitalOut led(LED1); -Serial pc(USBTX, USBRX); -const float M_PI = 3.141592; +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 main() { - /*printf("When you push any key, this robot starts.\r\n"); + DEBUG("When you push any key, this robot starts.\r\n"); while (pc.readable() == 0) //キーボード押したらスタート - ;*/ - printf("move start\r\n"); + ; + DEBUG("move start\r\n"); //各足の軌道の設定.今回は全部同じにすることで直線を描く。 - float stridetime_s = 1, risetime_s = 0.3, stride_m = 0.2f, height_m = 0.03f, ground_m = 0.14f; - Orbit straightOrbit[4] = { - - Orbit(stridetime_s, risetime_s, stride_m, height_m, ground_m), - Orbit(stridetime_s, risetime_s, stride_m, height_m, ground_m), - Orbit(stridetime_s, risetime_s, stride_m, height_m, ground_m), - Orbit(stridetime_s, risetime_s, stride_m, height_m, ground_m), - }; + float stridetime_s = 0.5, risetime_s = 0.15, stride_m = 0.2f, height_m = 0.05f, ground_m = 0.2f; + 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; //4足の軌道と位相ずれOffsetTime_sをまとめる float offset_time_s[4] = { 0, @@ -59,8 +81,7 @@ //実際にWalkの指示通りに動かす float dist_m = 0.5f; Move(straight, leg, dist_m); - - printf("program end\r\n"); + DEBUG("program end\r\n"); } //到達判定が来ない間同じ歩行方法でループ @@ -73,33 +94,36 @@ { float time_s = timer.read(); //注:未実装。到着したかの判定.LRFからのデータが必要? - //is_arrived = IsArrived(); + //is_arrived = IsArrived(); //4本の足それぞれの足先サーボ角度更新 WalkWay.Cal4LegsPosi(leg); //注:未実装。slave_mbed分の足の目標位置を送信 //自身が動かす足のサーボを動かす - MoveServo(leg[0], 0, 0); + //MoveServo(leg[0], 0, 0); MoveServo(leg[1], 1, 0); wait_ms(kServoSpan_ms); - MoveServo(leg[0], 0, 1); + //MoveServo(leg[0], 0, 1); MoveServo(leg[1], 1, 1); + DEBUG("x,y %f,%f\r\n",leg[0].GetX_m(),leg[0].GetY_m()); //計算周期が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 //計算周期が達成できないときはprintfで知らせるだけ。動きはする。 + else {//計算周期が達成できないときはDEBUGで知らせるだけ。動きはする。 printf("error: rest_time_s = %f in Move()\r\n", rest_time_s); + led[0] = 1; + } } } -void MoveServo(OneLeg leg, int legnum, int servo_id) +void MoveServo(OneLeg leg, int serial_num, int servo_id) { float degree = leg.GetRad(servo_id) * kRadToDegree; - //servo1は反転させる - if (servo_id == 0) - degree += 90; - else - degree = 270 - degree; - servo[legnum].set_degree(servo_id, degree); + + + //サーボの座標系に変更 + 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); + servo[serial_num].set_degree(servo_id, servo_degree); }