Diff: main.cpp

Revision:

7:72c80a7c20d6

Parent:

6:43708adf2e5d

Child:

8:21b932c4e6c5

--- a/main.cpp	Sun Feb 10 09:14:15 2019 +0000
+++ b/main.cpp	Sun Feb 10 10:37:02 2019 +0000
@@ -6,17 +6,19 @@
 #include "KondoServo.h"
 #define Mastar;
 //#define Slave;
+#define DebugMode;
 
 //各値
 namespace Parameters
 {
-const float Pi = 3.141592;
+    //パラメータ
+const float Pi = 3.14;
 const float kRadToDegree = 180.0 / Pi;
-const float tickerTime = 0.006f;    //Ticker間隔
+const float tickerTime = 0.02f;    //Ticker間隔
 float OffsetY = 0.15f;
-float Stride_time_s = 1.0f;//床についている時間
+float Stride_time_s = 1.0f*10.0f;//床についている時間 1.0f
 float Stride_length_m = 0.05f;//歩幅
-float Rising_time_s = 0.2f;//脚を上げている時間
+float Rising_time_s = 0.2f*10.0f;//脚を上げている時間 0.2f
 float Width_m = 0.03f;//脚を上げる高さ
 static const int straight = 0;
 static const int turnleft = 1;
@@ -28,6 +30,7 @@
 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;}////////////////////////////////////////設定
@@ -51,8 +54,8 @@
 namespace CanConnector
 {
 //Maserとslaveの定義　同じ関数名で動くようにしたい
-class MastarConnector;
-class SlaveConnector;
+class MastarConnector;//cansendするマイコンの場合
+class SlaveConnector;//canreadするマイコンの場合
 }
 
 //実装
@@ -79,9 +82,9 @@
 
 namespace Motion
 {
-void Stride(float time_s,float &targetX, float &targetY); 
-void Rising(float time_s,float &targetX, float &targetY); 
-void Stop(float &targetX, float &targetY);
+void Stride(float time_s,int legnum); 
+void Rising(float time_s,int legnum); 
+void Stop(int legnum);
 } // namespace Motion
 
 namespace WalkingPattern
@@ -134,30 +137,37 @@
 }
 
 
-void Motion::Stride(float time_s,float &targetX, float &targetY)
+void Motion::Stride(float time_s,int legnum)
 {
-    targetX = -((Stride_length_m*time_s/Stride_time_s) - (Stride_length_m/2.0f));
-    targetY = 0.0f;
+    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,float &targetX, float &targetY)
+void Motion::Rising(float time_s,int legnum)
 {
-    //targetX = ((Stride_length_m*time_s/Rising_time_s) - (Stride_length_m/2.0f));
-    //targetY = 
+    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()
 {
     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, 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]);
+        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);
     }
+    #ifdef DebugMode
+    printf("time_s: %.3f  targetX0: %.3f  targetY0: %.3f\r\n", time_s, TargetX[0],TargetY[0]);
+    #endif
     time_s += tickerTime*2.0f;
     if(time_s >= walkPeriod) time_s = 0.0f;
     
@@ -213,12 +223,10 @@
         public:
             MastarConnector(PinName rd, PinName td);
             CAN can_;
+            void Connect();
 
     };
-    MastarConnector::MastarConnector(PinName rd, PinName td): can_(rd, td)
-    {
-        
-    }
+    MastarConnector::MastarConnector(PinName rd, PinName td): can_(rd, td){}
 
 
     class SlaveConnector
@@ -226,12 +234,10 @@
         public:
             SlaveConnector(PinName rd, PinName td);
             CAN can_;
+            void Connect();
 
     };
-    SlaveConnector::SlaveConnector(PinName rd, PinName td): can_(rd, td)
-    {
-        
-    }
+    SlaveConnector::SlaveConnector(PinName rd, PinName td): can_(rd, td){}
 
 }
 
@@ -239,9 +245,13 @@
 using namespace Quadruped;
 Leg leg1(p9, p10);
 Leg leg2(p28, p27);
-CAN can1(p30, p29);
+//CAN can1(p30, p29);
 Ticker fliper;
-
+#ifdef Mastar
+CanConnector::MastarConnector connector(p30,p29);
+#elif Slave
+CanConnector::SlaveConnector connector(p30,p29);
+#endif
 
 void cb_timer()
 {
@@ -259,6 +269,7 @@
         //このマイコンで使う脚だけ計算
         leg1.CalServoRad(TargetX[0], TargetY[0] + OffsetY);
         leg2.CalServoRad(TargetX[1], TargetY[1] + OffsetY);
+        
         servo_num = 0;
     }
     time_s += tickerTime;