Diff: main.cpp

Revision:

5:556d5a5e9d24

Parent:

4:fffdb273836e

Child:

6:43708adf2e5d

diff -r fffdb273836e -r 556d5a5e9d24 main.cpp
--- a/main.cpp	Sat Feb 09 14:18:15 2019 +0000
+++ b/main.cpp	Sun Feb 10 06:07:02 2019 +0000
@@ -4,126 +4,211 @@
 
 #include "mbed.h"
 #include "KondoServo.h"
-#define Mastar;
+#define Mastar ;
 //#define Slave;
 
-const float Pi = 3.141592;
-const float kRadToDegree = 180.0 / Pi;
-
 //定義　
 namespace Quadruped
 {
-    class Leg;
-    namespace Motion{}
-    namespace WalkingPattern{}
-}
+const float Pi = 3.141592;
+const float kRadToDegree = 180.0 / Pi;
+const float tickerTime = 0.006f;    //Ticker間隔
+
+const float omega = 3.1415f * 2.5f; //角速度
+const float offsetDegree = 0.0f;
+const float offsetY = 0.15f;
+
+float Stride_time_s = 1.0f;//床についている時間
+float Stride_length_m = 0.1f;//歩幅
+void SetStrideMotion(float stride_time_s, float stride_length_m);
+float Rising_time_s = 0.2f;//脚を上げている時間
+float Width_m = 0.03f;//脚を上げる高さ
+void SetRisiongMotion(float rising_time_s, float width_m);
+//ROSから送られてきたLRFのデータをもとに現在すべき動作を定義する。
+int WalkMode = 0;
+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;
+void SetWalkMode(int 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
 
 namespace CanConnector
 {
-    //class Mastar;
-    //class Slave;
+//class Mastar;
+//class Slave;
 }
 
 //実装
 namespace Quadruped
 {
-    //脚の定義
-    class Leg
-    {
-        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_;
+//脚の定義
+class Leg
+{
+    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);
-    };
-    Leg::Leg(PinName pin_serial_tx, PinName pin_serial_rx) : servo_(pin_serial_tx, pin_serial_rx)
-    {
-        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;
+  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);
+};
+
+namespace Motion
+{
+void Stride(float time_s,float stride_length_m,float &targetX, float &targetY); //引数に歩幅
+void Rising(float time_s,float width_m,float &targetX, float &targetY); //引数に上げる高さ
+} // namespace Motion
+
+namespace WalkingPattern
+{
+using namespace Motion;
+void Straight();
+void TurnLeft();
+void TurnRight();
+void Climb();
+void Overcoming(); //段差、紐乗り越え動作
+} // namespace WalkingPattern
 
-        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];
-    }
+Leg::Leg(PinName pin_serial_tx, PinName pin_serial_rx) : servo_(pin_serial_tx, pin_serial_rx)
+{
+    
+    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;
 
-    namespace Motion
-    {
-        void Slide(Leg leg,float stride_m);//引数に歩幅
-        void Rising(Leg leg,float width_m);//引数に上げる高さ
-    }
-    void Motion::Slide(Leg leg,float stride_m)
-    {
-        static float time_s = 0.0f;
-        
-    }
-    void Motion::Rising(Leg leg,float width_m)
-    {
-        static float time_s = 0.0f;
-
-    }
-
-    namespace WalkingPattern
-    {
-        using namespace Motion;
-        void Straight();
-        void TurnRight();
-        void TurnLeft();
-        void Climb();
-        void Overcoming();//段差、紐乗り越え動作
-    }
+    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];
 }
 
 
+void Motion::Stride(float time_s,float stride_length_m,float &targetX, float &targetY)
+{
+    
+}
+
+void Motion::Rising(float time_s,float width_m,float &targetX, float &targetY)
+{
+    
+}
+
+void WalkingPattern::Straight()
+{
+    static float time_s = 0.0f;
+    float walkPeriod = Stride_time_s+Rising_time_s;
+    float offsetTime[4] = {0.0f, walkPeriod/2.0f, 0.0f, walkPeriod/2.0f};
+    for(int i=0;i<4;++i)
+    {
+        float thisLegTime = time_s + offsetTime[i];
+        if(thisLegTime <= Stride_time_s) Motion::Stride(thisLegTime, Stride_length_m, targetX[i],targetY[i]);
+        else if(thisLegTime <= walkPeriod) Motion::Rising(thisLegTime-Stride_time_s, Width_m,targetX[i],targetY[i]);
+        else Motion::Stride(thisLegTime-walkPeriod, Stride_length_m,targetX[i],targetY[i]);
+    }
+    time_s += tickerTime*2.0f;
+    if(time_s >= walkPeriod) time_s == 0.0f;
+    
+}
+void WalkingPattern::TurnLeft()
+{
+    static float time_s = 0.0f;
+
+}
+void WalkingPattern::TurnRight()
+{
+    static float time_s = 0.0f;
+
+}
+void WalkingPattern::Climb()
+{
+    static float time_s = 0.0f;
+    
+}
+void WalkingPattern::Overcoming()
+{
+    static float time_s = 0.0f;
+    
+}
+
+void CalcWalk()
+{
+    switch(WalkMode)
+    {
+        case straight:
+            WalkingPattern::Straight();
+        break;
+        case turnleft:
+            WalkingPattern::TurnLeft();
+        break;
+        case turnright:
+            WalkingPattern::TurnRight(); 
+        break;
+        case climb:
+            WalkingPattern::Climb();
+        break;
+        case overcoming:
+            WalkingPattern::Overcoming();
+        break;
+    }
+}
+} // namespace Quadruped
 
 //////////////////////////////////////////////////////////////以上が4脚ロボットに関する記述
 using namespace Quadruped;
 Leg leg1(p9, p10);
-Leg leg2(p28,p27);
-CAN can1(p30,p29);
+Leg leg2(p28, p27);
+CAN can1(p30, p29);
 Ticker fliper;
 
-const float tickerTime = 0.006f;    //Ticker間隔
-const float omega = 3.1415f * 2.5f; //角速度
-const float offsetDegree = 0.0f;
-const float offsetY = 0.15f;
+
 
 void F_t(float t, float &tarX, float &tarY)
 {
@@ -149,6 +234,7 @@
     leg1.MoveServo(servo_num);
     leg2.MoveServo(servo_num);
     ++servo_num;
+    
     if (servo_num > 1) //両サーボ出力したら
     {
         //目標角の更新
@@ -158,6 +244,7 @@
         servo_num = 0;
     }
     time_s += tickerTime;
+    
 }
 int main()
 {