Diff: main.cpp

Revision:

9:905f93247688

Parent:

8:21b932c4e6c5

Child:

10:7a340c52e270

--- a/main.cpp	Sun Feb 10 12:38:16 2019 +0000
+++ b/main.cpp	Mon Feb 11 01:56:49 2019 +0000
@@ -1,289 +1,115 @@
-//mbed間の同期を、master-slaveで行いたい。
-//masetrで4つの脚の動きをすべて決定,
-//その動きをslaveに送る。
-
+//NHK2019MR2 馬型機構プログラム.main.cppでは4足の協調に関わるプログラムを主に書く。
 #include "mbed.h"
-#include "KondoServo.h"
-#define Mastar;
-//#define Slave;
-#define DebugMode;
+#include "pinnames.h"
+#include "ToePosi.h"        //個々の足先の位置を決めるプログラム
+#include "MoveServoOfLeg.h" //サーボを実際に動かすプログラム
 
-//各値
-namespace Parameters
-{
-    //パラメータ
-const float Pi = 3.14;
-const float kRadToDegree = 180.0 / Pi;
-const float tickerTime = 0.007f;//Ticker間隔
-float OffsetY = 0.15f;
-float Stride_time_s = 0.8f;//床についている時間 1.0f
-float Stride_length_m = 0.05f;//歩幅
-float Rising_time_s = 0.2f;//脚を上げている時間 0.2f
-float Width_m = 0.03f;//脚を上げる高さ
-static const int straight = 0;
-static const int turnleft = 1;
-static const int turnright = 2;
-static const int climb = 3;
-static const int overcoming = 4;
-}
-//定義　
-namespace Quadruped//4脚ロボット
-{
-using namespace Parameters;
-//USERが直接制御に使う関数
-void SetOffsetY(float offsetY){OffsetY=offsetY;}
-void SetStrideMotion(float stride_time_s, float stride_length_m){Stride_time_s=stride_time_s;Stride_length_m=stride_length_m;}///////////////////////////////////設定
-void SetRisiongMotion(float rising_time_s, float width_m){Rising_time_s=rising_time_s;Width_m =width_m;}////////////////////////////////////////設定
-//ROSから送られてきたLRFのデータをもとに現在すべき動作を定義する。
-int WalkMode = 0;
-void SetWalkMode(int mode){WalkMode = mode;}
-//4つの脚のtargetを保存する　Legクラスで後で使えるようにしておく
-float TargetX[4];
-float TargetY[4];
-//脚の定義　位置を入力するとその方向にサーボが動く
-class Leg;
-//脚ごとのモーション　これを組み合わせる
-namespace Motion{}
-//モーションを組み合わせて歩行パターンを形成する
-//前進、旋回など
-namespace WalkingPattern{}
-//パターン関数を呼び出す　一つの関数でいいようにswitchする
-void CalcWalk(int legNum, Leg leg);
-} // namespace Quadruped
+////////////調整すべきパラメータ.全てここに集めた。
+const float kCycleTime_s = 0.02f;              //計算周期
+const float kStride_m = 0.05f;                 //歩幅
+const float kHeight_m = 0.03f;                 //足を上げる高さ
+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 kVelocity_m_s = 0.01f; //進む速度(適当)
+const float kDist_m = 10;          //歩行中に進む距離(適当)
+///////////////
+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),
+};
+DigitalOut led(LED1);
 
-namespace CanConnector
-{
-//Maserとslaveの定義　同じ関数名で動くようにしたい
-class MastarConnector;//cansendするマイコンの場合
-class SlaveConnector;//canreadするマイコンの場合
-}
-
-//実装
-namespace Quadruped
-{
-//脚の定義
-class Leg
+const float M_PI = 3.141592;
+//歩き方のパターン
+enum WalkingPattern
 {
-    float rad_[2];
-    int id_[2];
-    //パラメータ。実際の機体に合わせていじる
-    static const float dist_between_servo_half_m_ = 0.06f * 0.5;
-    static const float LegLength1 = 0.1f;
-    static const float LegLength2 = 0.2f;
-    KondoServo servo_;
-
-  public:
-    Leg(PinName pin_serial_tx, PinName pin_serial_rx);
-    void MoveServo(int servo_num);
-    void CalServoRad(float x_m, float y_m);
-    void SetRad(float rad, int servo_num);
-    float GetRad(int servo_num);
+    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);
 
-namespace Motion
-{
-void Stride(float time_s,int legnum); 
-void Rising(float time_s,int legnum); 
-void Stop(int legnum);
-} // namespace Motion
-
-namespace WalkingPattern
-{
-using namespace Motion;
-void Straight(int servo_num);
-void TurnLeft(int servo_num);
-void TurnRight(int servo_num);
-void Climb(int servo_num);
-void Overcoming(int servo_num); //段差、紐乗り越え動作
-} // namespace WalkingPattern
-
-Leg::Leg(PinName pin_serial_tx, PinName pin_serial_rx) : servo_(pin_serial_tx, pin_serial_rx)
+int main()
 {
-    rad_[0] = 0;
-    rad_[1] = Pi;
-};
-void Leg::MoveServo(int servo_num)
-{
-    float degree = GetRad(servo_num) * kRadToDegree;
-    //servo1は反転させる
-    if (servo_num == 0)
-        degree += 90;
-    else
-        degree = 270 - degree;
-    servo_.set_degree(servo_num, degree);
-}
-void Leg::CalServoRad(float x_m, float y_m)
-{
-    //処理を軽くするために共通部分は先に計算
-    float temp_x[] = {x_m + dist_between_servo_half_m_,
-                      x_m - dist_between_servo_half_m_};
-    float temp_y2 = y_m * y_m;
-    float temp_L = LegLength1 * LegLength1 - LegLength2 * LegLength2;
-
-    float r1 = sqrt((temp_x[1]) * (temp_x[1]) + temp_y2);
-    float r2 = sqrt((temp_x[0]) * (temp_x[0]) + temp_y2);
-    float targetTheta[] = {atan2(y_m, temp_x[1]) - acos((temp_L + r1 * r1) / (2.0f * r1 * LegLength1)),
-                           atan2(y_m, temp_x[0]) + acos((temp_L + r2 * r2) / (2.0f * r2 * LegLength1))};
-    for (size_t i = 0; i < 2; i++)
-        SetRad(targetTheta[i], i);
-}
-void Leg::SetRad(float rad, int servo_num)
-{
-    rad_[servo_num] = rad;
-}
-float Leg::GetRad(int servo_num)
-{
-    return rad_[servo_num];
+    //現在のそれぞれの足の位置を保存するもの
+    ToePosi now[4] = {
+        ToePosi(kStride_m, kHeight_m),
+        ToePosi(kStride_m, kHeight_m),
+        ToePosi(kStride_m, kHeight_m),
+        ToePosi(kStride_m, kHeight_m),
+    };
+    //直進
+    Move(STRAIGHT_TROT, now, kVelocity_m_s, kDist_m);
 }
 
-
-void Motion::Stride(float time_s,int legnum)
-{
-    TargetX[legnum] = (Stride_length_m*time_s/Stride_time_s) - (Stride_length_m/2.0f);
-    TargetY[legnum] = 0.0f;
-    
-}
-
-void Motion::Rising(float time_s,int legnum)
+void Move(WalkingPattern pattern, ToePosi (&now)[4], float maxvelocity_m_s, float dist_m)
 {
-    TargetX[legnum] = (Stride_length_m/2.0f)*cos(-Pi*time_s/Rising_time_s);
-    TargetY[legnum] = Width_m*sin(-Pi*time_s/Rising_time_s);
-    
-}
-
-float walkPeriod = Stride_time_s+Rising_time_s;//周期
-float offsetTime[4] = {0.0f, walkPeriod/2.0f, 0.0f, walkPeriod/2.0f};//位相をずれすための時間の初期誤差
-
-
-void WalkingPattern::Straight(int servo_num)
-{
-    int servo_state = 2*servo_num;
-    static float time_s = 0.0f;//共通時間
-    for(int i=0+servo_state;i<2+servo_state;++i)//2脚ずつそれぞれのtargetPoseを設定する
+    //patternを見て、使う関数を決定.関数ポインタを利用
+    void (*Cal4LegsPhase)(ToePosi, ToePosi(&target)[4], float);
+    switch (pattern)
     {
-        float thisLegTime = time_s + offsetTime[i];//脚に合わせたそれぞれの時間
-        if(thisLegTime <= Stride_time_s) Motion::Stride(thisLegTime, i);
-        else if(thisLegTime <= walkPeriod) Motion::Rising(thisLegTime-Stride_time_s,i);
-        else Motion::Stride(thisLegTime-walkPeriod,i);
+    case STRAIGHT_TROT:
+        Cal4LegsPhase = CalPhaseStraight;
+        //収束判定関数もここでpatternによって変える必要あるかも。
+        break;
+    default:
+        printf("error:there is no func of the pattern");
     }
-    #ifdef DebugMode
-    if(servo_num == 0)
-    printf("time_s: %.3f  targetX0: %.3f  targetY0: %.3f\r\n", time_s, TargetX[0], TargetY[0]);
-    #endif
-    time_s += tickerTime;
-    if(time_s >= walkPeriod) 
+
+    ToePosi target[4] = {now[0], now[1], now[2], now[3]}; //目標位置を取り敢えず現在位置に。
+    timer.reset();
+    timer.start();
+    int is_arrived = 0;
+    while (is_arrived == 0)
     {
-        time_s = 0.0f;
-    }
-}
-void WalkingPattern::TurnLeft(int servo_num)
-{
-    //static float time_s = 0.0f;
+        float time_s = timer.read();
+        //注:未実装。到着したかの判定.LRFからのデータが必要？
+        //is_arrived = IsArrived();
+
+        //4本の足それぞれの位相更新.二次曲線のどこにいるべきかが決まる
+        Cal4LegsPhase(now[0], target, maxvelocity_m_s);
+        //注:未実装。slave_mbed分の足の目標位置を送信
 
-}
-void WalkingPattern::TurnRight(int servo_num)
-{
-    //static float time_s = 0.0f;
-
-}
-void WalkingPattern::Climb(int servo_num)
-{
-    //static float time_s = 0.0f;
-    
-}
-void WalkingPattern::Overcoming(int servo_num)
-{
-    //static float time_s = 0.0f;
-    
+        //自身が動かす足のサーボ角度更新
+        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);
+        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);
+        if (rest_time_s > 0)
+            wait(rest_time_s);
+        else //計算周期が達成できないときは
+        {
+            led = 1; //LED1を光らせて知らせる。
+            printf("error: rest_time_s = %f in Move()\r\n", rest_time_s);
+        }
+    }
 }
 
-void CalcWalk(int servo_num)
-{
-    switch(WalkMode)
-    {
-        case straight:
-            WalkingPattern::Straight(servo_num);
-        break;
-        case turnleft:
-            WalkingPattern::TurnLeft(servo_num);
-        break;
-        case turnright:
-            WalkingPattern::TurnRight(servo_num); 
-        break;
-        case climb:
-            WalkingPattern::Climb(servo_num);
-        break;
-        case overcoming:
-            WalkingPattern::Overcoming(servo_num);
-        break;
-    }
-}
-} // namespace Quadruped
-
-namespace CanConnector
+void CalPhaseStraight(ToePosi now_base, ToePosi (&target)[4], float ave_v_m_s)
 {
-    class MastarConnector
-    {
-        public:
-            MastarConnector(PinName rd, PinName td);
-            CAN can_;
-            void Connect();
-
-    };
-    MastarConnector::MastarConnector(PinName rd, PinName td): can_(rd, td){}
-
-
-    class SlaveConnector
-    {
-        public:
-            SlaveConnector(PinName rd, PinName td);
-            CAN can_;
-            void Connect();
-
-    };
-    SlaveConnector::SlaveConnector(PinName rd, PinName td): can_(rd, td){}
-
+    //指定速度で進むための周期 = 平均速度＊時間/歩幅 * 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);
 }
-
-//////////////////////////////////////////////////////////////以上が4脚ロボットに関する記述
-using namespace Quadruped;
-Leg leg1(p9, p10);
-Leg leg2(p28, p27);
-//CAN can1(p30, p29);
-Ticker fliper;
-#ifdef Mastar
-CanConnector::MastarConnector connector(p30,p29);
-#elif Slave
-CanConnector::SlaveConnector connector(p30,p29);
-#endif
-
-void cb_timer()
-{
-    static int servo_num = 0;
-    //出力。片サーボずつ
-    leg1.MoveServo(servo_num);
-    leg2.MoveServo(servo_num);
-    Quadruped::CalcWalk(servo_num);
-    ++servo_num;
-    if(servo_num > 1)servo_num = 0;
-    
-    /*if (servo_num > 1) //両サーボ出力したら
-    {
-        //目標角の更新
-        Quadruped::CalcWalk(servo_num);
-        //このマイコンで使う脚だけ計算
-        leg1.CalServoRad(TargetX[0], TargetY[0] + OffsetY);
-        leg2.CalServoRad(TargetX[1], TargetY[1] + OffsetY);
-        
-        servo_num = 0;
-    }*/
-    
-}
-int main()
-{
-    Quadruped::CalcWalk(0);
-    Quadruped::CalcWalk(1);
-    leg1.CalServoRad(TargetX[0], TargetY[0] + OffsetY);
-    leg2.CalServoRad(TargetX[1], TargetY[1] + OffsetY);
-    fliper.attach(&cb_timer, tickerTime);
-}