Diff: main.cpp

Revision:

16:0069a56f11a3

Parent:

14:d7cb429946f4

Child:

17:ca18c5980a34

diff -r 3fd1d5698d10 -r 0069a56f11a3 main.cpp
--- 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);
 }