test
Dependencies: mbed ros_lib_kinetic nhk19mr2_can_info splitData SerialHalfDuplex_HM
Diff: main.cpp
- Revision:
- 16:0069a56f11a3
- Parent:
- 14:d7cb429946f4
- Child:
- 17:ca18c5980a34
--- a/main.cpp Wed Feb 13 01:57:31 2019 +0000 +++ b/main.cpp Wed Feb 13 06:43:05 2019 +0000 @@ -4,17 +4,18 @@ #include "KondoServo.h" #include "debug.h" #include "pi.h" +#include "can.h" #include "OneLeg.h" ///足先の座標を保存するクラス。x,yやサーボの角度の保存、サーボの駆動も行う。他の足を考慮した処理は別のクラスに任せる。 #include "Walk.h" //歩き方に関するファイル +#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 int kServoSpan_ms = 10; //サーボの送信間隔 - -//サーボの設定 const double kServoValToDegree = 270.0 / (11500 - 3500); //サーボの正負と座標系の正負の補正.足で一セット。 const int kServoSign[2][2] = {{ @@ -97,15 +98,17 @@ //is_arrived = IsArrived(); //4本の足それぞれの足先サーボ角度更新 WalkWay.Cal4LegsPosi(leg); - //注:未実装。slave_mbed分の足の目標位置を送信 - + #ifdef USE_CAN + //slave_mbed分の足の目標位置を送信 + SendRad(leg[2],leg[3]); + #endif //自身が動かす足のサーボを動かす - //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()); + 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) @@ -120,10 +123,8 @@ 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); }