Diff: main.cpp

Revision:

11:e81425872740

Parent:

10:7a340c52e270

Child:

12:2ac37fe6c3bb

diff -r 7a340c52e270 -r e81425872740 main.cpp
--- a/main.cpp	Mon Feb 11 04:07:51 2019 +0000
+++ b/main.cpp	Mon Feb 11 12:56:00 2019 +0000
@@ -1,77 +1,65 @@
-//NHK2019MR2 馬型機構プログラム.main.cppでは4足の協調に関わるプログラムを主に書く。
+//NHK2019MR2 馬型機構プログラム.
 #include "mbed.h"
 #include "pinnames.h"
-#include "ToePosi.h"        //個々の足先の位置を決めるプログラム
-#include "MoveServoOfLeg.h" //サーボを実際に動かすプログラム
-#define DEBUG
+#include "KondoServo.h"
+#include "OneLeg.h" ///足先の座標を保存するクラス。x,yやサーボの角度の保存、サーボの駆動も行う。他の足を考慮した処理は別のクラスに任せる。
+#include "Walk.h"   //歩き方に関するファイル
 
 ////////////調整すべきパラメータ.全てここに集めた。
-const float kCycleTime_s = 0.02f;              //計算周期
-const float kStride_m = 0.05f;                 //歩幅
-const float kHeight_m = 0.03f;                 //足の上げ幅
-const float kOffsetY_m = 0.02f;                //足のデフォルト高さ
+const float kCycleTime_s = 0.01f;              //計算周期
 const float kBetweenServoHalf_m = 0.06f * 0.5; //サーボ間の距離の半分
 const float kLegLength1 = 0.1f;
 const float kLegLength2 = 0.2f;
-const float kServoSpan_ms = 0;     //サーボの送信間隔
-const float kVelocity_m_s = 0.01f; //進む速度(適当)
-const float kDist_m = 10;          //歩行中に進む距離(適当)
+const float kServoSpan_ms = 5; //サーボの送信間隔
 ///////////////
 Timer timer;
-MoveServoOfLeg moveleg[2] = {
-    MoveServoOfLeg(pin_serial_servo_tx[0], pin_serial_servo_rx[0],
-                   kBetweenServoHalf_m, kLegLength1, kLegLength2),
-    MoveServoOfLeg(pin_serial_servo_tx[1], pin_serial_servo_rx[1],
-                   kBetweenServoHalf_m, kLegLength1, kLegLength2),
+KondoServo servo[2] = {
+    KondoServo(pin_serial_servo_tx[0], pin_serial_servo_rx[0]),
+    KondoServo(pin_serial_servo_tx[1], pin_serial_servo_rx[1]),
+};
+OneLeg leg[4] = {
+    OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2),
+    OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2),
+    OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2),
+    OneLeg(kBetweenServoHalf_m, kLegLength1, kLegLength2),
 };
 DigitalOut led(LED1);
 Serial pc(USBTX, USBRX);
-
 const float M_PI = 3.141592;
-//歩き方のパターン
-enum WalkingPattern
-{
-    STRAIGHT_TROT,
-};
-//歩き方: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);
-
+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("program start\r\n");
-    //現在のそれぞれの足の位置を保存するもの
-    ToePosi now[4] = {
-        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("When you push any key, this robot starts.\r\n");
+    while (pc.readable() == 0) //キーボード押したらスタート
         ;
     printf("move start\r\n");
-    Move(STRAIGHT_TROT, now, kVelocity_m_s, kDist_m);
+    //各足の軌道の設定.今回は全部同じにすることで直線を描く。
+    float stridetime_s = 1, risetime_s = 0.3, stride_m = 0.2f, height_m = 0.03f, ground_m = 0.14f;
+    Orbit orbit[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),
+    };
+    //4足の軌道と位相ずれOffsetTime_sをまとめる
+    float offset_time_s[4] = {
+        0,
+        orbit[0].GetOneWalkTime() * 0.5,
+        0,
+        orbit[0].GetOneWalkTime() * 0.5,
+    };
+    Walk straight(orbit, offset_time_s, kCycleTime_s);
+    //実際にWalkの指示通りに動かす
+    float dist_m = 0.5f;
+    Move(straight, leg, dist_m);
+
     printf("program end\r\n");
 }
 
-void Move(WalkingPattern pattern, ToePosi (&now)[4], float maxvelocity_m_s, float dist_m)
+void Move(Walk WalkWay, OneLeg (&leg)[4], float dist_m)
 {
-    //patternを見て、使う関数を決定.関数ポインタを利用
-    void (*Cal4LegsPhase)(ToePosi, ToePosi(&target)[4], float);
-    switch (pattern)
-    {
-    case STRAIGHT_TROT:
-        Cal4LegsPhase = CalPhaseStraight;
-        //収束判定関数もここでpatternによって変える必要あるかも。
-        break;
-    default:
-        printf("error:there is no func of the pattern");
-    }
-
-    ToePosi target[4] = {now[0], now[1], now[2], now[3]}; //目標位置を取り敢えず現在位置に。
     timer.reset();
     timer.start();
     int is_arrived = 0;
@@ -81,24 +69,19 @@
         //注:未実装。到着したかの判定.LRFからのデータが必要？
         // is_arrived = IsArrived();
 
-        //4本の足それぞれの位相更新.二次曲線のどこにいるべきかが決まる
-        Cal4LegsPhase(now[0], target, maxvelocity_m_s);
+        //4本の足それぞれの足先サーボ角度更新
+        WalkWay.Cal4LegsPosi(leg);
         //注:未実装。slave_mbed分の足の目標位置を送信
 
-        //自身が動かす足のサーボ角度更新
-        moveleg[0].CalServoRad(target[0].GetX_m(), target[0].GetY_m());
-        moveleg[1].CalServoRad(target[1].GetX_m(), target[1].GetY_m());
         //自身が動かす足のサーボを動かす
-        moveleg[0].MoveServo(0);
-        moveleg[1].MoveServo(0);
+        MoveServo(leg[0], 0, 0);
+        MoveServo(leg[1], 1, 0);
         wait_ms(kServoSpan_ms);
-        moveleg[0].MoveServo(1);
-        moveleg[1].MoveServo(1);
-        //現在位置更新
-        for (int i = 0; i < 4; i++)
-            now[i] = target[i];
-        //計算周期をkCycleTime_sにする
-        float rest_time_s = kCycleTime_s - (timer.read() - time_s);
+        MoveServo(leg[0], 0, 1);
+        MoveServo(leg[1], 1, 1);
+        printf("%f,%f\r\n", leg[0].GetRad(0), leg[1].GetRad(0));
+        //計算周期が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で知らせるだけ。動きはする。
@@ -106,15 +89,13 @@
     }
 }
 
-void CalPhaseStraight(ToePosi now_base, ToePosi (&target)[4], float ave_v_m_s)
+void MoveServo(OneLeg leg, int legnum, int servo_id)
 {
-    //指定速度で進むための周期 = 平均速度＊時間/歩幅 * pi(半周期で歩幅分進む)
-    float phase_step = ave_v_m_s * kCycleTime_s / kStride_m * M_PI;
-    //対角線上の足の位相は同じで、反対側は半周期先。
-    float target_phase0 = now_base.GetPhase() + phase_step;
-    float target_phase1 = target_phase0 + M_PI;
-    target[0].SetPhase(target_phase0);
-    target[1].SetPhase(target_phase1);
-    target[2].SetPhase(target_phase0);
-    target[3].SetPhase(target_phase1);
-}
+    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);
+}
\ No newline at end of file