Diff: main.cpp

Revision:

10:7a340c52e270

Parent:

9:905f93247688

Child:

11:e81425872740

--- a/main.cpp	Mon Feb 11 01:56:49 2019 +0000
+++ b/main.cpp	Mon Feb 11 04:07:51 2019 +0000
@@ -3,15 +3,17 @@
 #include "pinnames.h"
 #include "ToePosi.h"        //個々の足先の位置を決めるプログラム
 #include "MoveServoOfLeg.h" //サーボを実際に動かすプログラム
+#define DEBUG
 
 ////////////調整すべきパラメータ.全てここに集めた。
 const float kCycleTime_s = 0.02f;              //計算周期
 const float kStride_m = 0.05f;                 //歩幅
-const float kHeight_m = 0.03f;                 //足を上げる高さ
+const float kHeight_m = 0.03f;                 //足の上げ幅
+const float kOffsetY_m = 0.02f;                //足のデフォルト高さ
 const float kBetweenServoHalf_m = 0.06f * 0.5; //サーボ間の距離の半分
 const float kLegLength1 = 0.1f;
 const float kLegLength2 = 0.2f;
-const float kServoSpan_ms = 50;    //サーボの送信感覚
+const float kServoSpan_ms = 0;     //サーボの送信間隔
 const float kVelocity_m_s = 0.01f; //進む速度(適当)
 const float kDist_m = 10;          //歩行中に進む距離(適当)
 ///////////////
@@ -23,6 +25,7 @@
                    kBetweenServoHalf_m, kLegLength1, kLegLength2),
 };
 DigitalOut led(LED1);
+Serial pc(USBTX, USBRX);
 
 const float M_PI = 3.141592;
 //歩き方のパターン
@@ -32,19 +35,26 @@
 };
 //歩き方:patternでdist_mだけ進む。
 void Move(WalkingPattern pattern, ToePosi (&now)[4], float maxvelocity_m_s, float dist_m);
+//直進時の歩き方
 void CalPhaseStraight(ToePosi now_base, ToePosi (&target)[4], float ave_v_m_s);
 
 int main()
 {
+    printf("program start\r\n");
     //現在のそれぞれの足の位置を保存するもの
     ToePosi now[4] = {
-        ToePosi(kStride_m, kHeight_m),
-        ToePosi(kStride_m, kHeight_m),
-        ToePosi(kStride_m, kHeight_m),
-        ToePosi(kStride_m, kHeight_m),
+        ToePosi(kStride_m, kHeight_m, kOffsetY_m),
+        ToePosi(kStride_m, kHeight_m, kOffsetY_m),
+        ToePosi(kStride_m, kHeight_m, kOffsetY_m),
+        ToePosi(kStride_m, kHeight_m, kOffsetY_m),
     };
     //直進
+    printf("please put some key\r\n");
+    while (pc.readable() == 0)
+        ;
+    printf("move start\r\n");
     Move(STRAIGHT_TROT, now, kVelocity_m_s, kDist_m);
+    printf("program end\r\n");
 }
 
 void Move(WalkingPattern pattern, ToePosi (&now)[4], float maxvelocity_m_s, float dist_m)
@@ -69,7 +79,7 @@
     {
         float time_s = timer.read();
         //注:未実装。到着したかの判定.LRFからのデータが必要？
-        //is_arrived = IsArrived();
+        // is_arrived = IsArrived();
 
         //4本の足それぞれの位相更新.二次曲線のどこにいるべきかが決まる
         Cal4LegsPhase(now[0], target, maxvelocity_m_s);
@@ -84,20 +94,15 @@
         wait_ms(kServoSpan_ms);
         moveleg[0].MoveServo(1);
         moveleg[1].MoveServo(1);
-        
         //現在位置更新
-        for(int i = 0; i < 4;i++)
+        for (int i = 0; i < 4; i++)
             now[i] = target[i];
-        
         //計算周期をkCycleTime_sにする
         float rest_time_s = kCycleTime_s - (timer.read() - time_s);
         if (rest_time_s > 0)
             wait(rest_time_s);
-        else //計算周期が達成できないときは
-        {
-            led = 1; //LED1を光らせて知らせる。
+        else //計算周期が達成できないときはprintfで知らせるだけ。動きはする。
             printf("error: rest_time_s = %f in Move()\r\n", rest_time_s);
-        }
     }
 }