Diff: main.cpp

Revision:

14:d7cb429946f4

Parent:

13:e7ecdb20665a

Child:

16:0069a56f11a3

diff -r e7ecdb20665a -r d7cb429946f4 main.cpp
--- 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);
 }