Diff: main.cpp

Revision:

6:43708adf2e5d

Parent:

5:556d5a5e9d24

Child:

7:72c80a7c20d6

--- a/main.cpp	Sun Feb 10 06:07:02 2019 +0000
+++ b/main.cpp	Sun Feb 10 09:14:15 2019 +0000
@@ -4,37 +4,39 @@
 
 #include "mbed.h"
 #include "KondoServo.h"
-#define Mastar ;
+#define Mastar;
 //#define Slave;
 
-//定義　
-namespace Quadruped
+//各値
+namespace Parameters
 {
 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 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 Stride_length_m = 0.05f;//歩幅
 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);
+}
+//定義　
+namespace Quadruped//4脚ロボット
+{
+using namespace Parameters;
+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];
+float TargetX[4];
+float TargetY[4];
 //脚の定義　位置を入力するとその方向にサーボが動く
 class Leg;
 //脚ごとのモーション　これを組み合わせる
@@ -48,8 +50,9 @@
 
 namespace CanConnector
 {
-//class Mastar;
-//class Slave;
+//Maserとslaveの定義　同じ関数名で動くようにしたい
+class MastarConnector;
+class SlaveConnector;
 }
 
 //実装
@@ -76,8 +79,9 @@
 
 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); //引数に上げる高さ
+void Stride(float time_s,float &targetX, float &targetY); 
+void Rising(float time_s,float &targetX, float &targetY); 
+void Stop(float &targetX, float &targetY);
 } // namespace Motion
 
 namespace WalkingPattern
@@ -92,7 +96,6 @@
 
 Leg::Leg(PinName pin_serial_tx, PinName pin_serial_rx) : servo_(pin_serial_tx, pin_serial_rx)
 {
-    
     rad_[0] = 0;
     rad_[1] = Pi;
 };
@@ -131,50 +134,52 @@
 }
 
 
-void Motion::Stride(float time_s,float stride_length_m,float &targetX, float &targetY)
+void Motion::Stride(float time_s,float &targetX, float &targetY)
 {
-    
+    targetX = -((Stride_length_m*time_s/Stride_time_s) - (Stride_length_m/2.0f));
+    targetY = 0.0f;
 }
 
-void Motion::Rising(float time_s,float width_m,float &targetX, float &targetY)
+void Motion::Rising(float time_s,float &targetX, float &targetY)
 {
-    
+    //targetX = ((Stride_length_m*time_s/Rising_time_s) - (Stride_length_m/2.0f));
+    //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)
+    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)//4脚それぞれのtargetPoseを設定する
     {
-        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]);
+        float thisLegTime = time_s + offsetTime[i];//脚に合わせたそれぞれの時間
+        if(thisLegTime <= Stride_time_s) Motion::Stride(thisLegTime, TargetX[i],TargetY[i]);
+        else if(thisLegTime <= walkPeriod) Motion::Rising(thisLegTime-Stride_time_s,TargetX[i],TargetY[i]);
+        else Motion::Stride(thisLegTime-walkPeriod,TargetX[i],TargetY[i]);
     }
     time_s += tickerTime*2.0f;
-    if(time_s >= walkPeriod) time_s == 0.0f;
+    if(time_s >= walkPeriod) time_s = 0.0f;
     
 }
 void WalkingPattern::TurnLeft()
 {
-    static float time_s = 0.0f;
+    //static float time_s = 0.0f;
 
 }
 void WalkingPattern::TurnRight()
 {
-    static float time_s = 0.0f;
+    //static float time_s = 0.0f;
 
 }
 void WalkingPattern::Climb()
 {
-    static float time_s = 0.0f;
+    //static float time_s = 0.0f;
     
 }
 void WalkingPattern::Overcoming()
 {
-    static float time_s = 0.0f;
+    //static float time_s = 0.0f;
     
 }
 
@@ -201,6 +206,35 @@
 }
 } // namespace Quadruped
 
+namespace CanConnector
+{
+    class MastarConnector
+    {
+        public:
+            MastarConnector(PinName rd, PinName td);
+            CAN can_;
+
+    };
+    MastarConnector::MastarConnector(PinName rd, PinName td): can_(rd, td)
+    {
+        
+    }
+
+
+    class SlaveConnector
+    {
+        public:
+            SlaveConnector(PinName rd, PinName td);
+            CAN can_;
+
+    };
+    SlaveConnector::SlaveConnector(PinName rd, PinName td): can_(rd, td)
+    {
+        
+    }
+
+}
+
 //////////////////////////////////////////////////////////////以上が4脚ロボットに関する記述
 using namespace Quadruped;
 Leg leg1(p9, p10);
@@ -209,27 +243,10 @@
 Ticker fliper;
 
 
-
-void F_t(float t, float &tarX, float &tarY)
-{
-    float theta = -omega * t + offsetDegree;
-    if (sin(theta) <= 0.0f)
-    {
-        tarX = 0.1f * cos(theta);  //0.1
-        tarY = 0.03f * sin(theta); //0.03
-    }
-    else
-    {
-        tarX = 0.1f * cos(theta);
-        tarY = 0.0f * sin(theta);
-    }
-}
 void cb_timer()
 {
     static int servo_num = 0;
     static float time_s = 0.0f;
-    float target_x_m = 0;
-    float target_y_m = 0;
     //出力。片サーボずつ
     leg1.MoveServo(servo_num);
     leg2.MoveServo(servo_num);
@@ -238,9 +255,10 @@
     if (servo_num > 1) //両サーボ出力したら
     {
         //目標角の更新
-        F_t(time_s, target_x_m, target_y_m);
-        leg1.CalServoRad(target_x_m, target_y_m + offsetY);
-        leg2.CalServoRad(target_x_m, target_y_m + offsetY);
+        Quadruped::CalcWalk();
+        //このマイコンで使う脚だけ計算
+        leg1.CalServoRad(TargetX[0], TargetY[0] + OffsetY);
+        leg2.CalServoRad(TargetX[1], TargetY[1] + OffsetY);
         servo_num = 0;
     }
     time_s += tickerTime;
@@ -248,10 +266,8 @@
 }
 int main()
 {
-    float target_x_m = 0;
-    float target_y_m = 0;
-    F_t(0, target_x_m, target_y_m);
-    leg1.CalServoRad(target_x_m, target_y_m + offsetY);
-    leg2.CalServoRad(target_x_m, target_y_m + offsetY);
+    Quadruped::CalcWalk();
+    leg1.CalServoRad(TargetX[0], TargetY[0] + OffsetY);
+    leg2.CalServoRad(TargetX[1], TargetY[1] + OffsetY);
     fliper.attach(&cb_timer, tickerTime);
 }