Diff: main.cpp

Revision:

8:21b932c4e6c5

Parent:

7:72c80a7c20d6

Child:

9:905f93247688

diff -r 72c80a7c20d6 -r 21b932c4e6c5 main.cpp
--- a/main.cpp	Sun Feb 10 10:37:02 2019 +0000
+++ b/main.cpp	Sun Feb 10 12:38:16 2019 +0000
@@ -14,11 +14,11 @@
     //パラメータ
 const float Pi = 3.14;
 const float kRadToDegree = 180.0 / Pi;
-const float tickerTime = 0.02f;    //Ticker間隔
+const float tickerTime = 0.007f;//Ticker間隔
 float OffsetY = 0.15f;
-float Stride_time_s = 1.0f*10.0f;//床についている時間 1.0f
+float Stride_time_s = 0.8f;//床についている時間 1.0f
 float Stride_length_m = 0.05f;//歩幅
-float Rising_time_s = 0.2f*10.0f;//脚を上げている時間 0.2f
+float Rising_time_s = 0.2f;//脚を上げている時間 0.2f
 float Width_m = 0.03f;//脚を上げる高さ
 static const int straight = 0;
 static const int turnleft = 1;
@@ -90,11 +90,11 @@
 namespace WalkingPattern
 {
 using namespace Motion;
-void Straight();
-void TurnLeft();
-void TurnRight();
-void Climb();
-void Overcoming(); //段差、紐乗り越え動作
+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)
@@ -155,10 +155,11 @@
 float offsetTime[4] = {0.0f, walkPeriod/2.0f, 0.0f, walkPeriod/2.0f};//位相をずれすための時間の初期誤差
 
 
-void WalkingPattern::Straight()
+void WalkingPattern::Straight(int servo_num)
 {
+    int servo_state = 2*servo_num;
     static float time_s = 0.0f;//共通時間
-    for(int i=0;i<4;++i)//4脚それぞれのtargetPoseを設定する
+    for(int i=0+servo_state;i<2+servo_state;++i)//2脚ずつそれぞれのtargetPoseを設定する
     {
         float thisLegTime = time_s + offsetTime[i];//脚に合わせたそれぞれの時間
         if(thisLegTime <= Stride_time_s) Motion::Stride(thisLegTime, i);
@@ -166,51 +167,54 @@
         else Motion::Stride(thisLegTime-walkPeriod,i);
     }
     #ifdef DebugMode
-    printf("time_s: %.3f  targetX0: %.3f  targetY0: %.3f\r\n", time_s, TargetX[0],TargetY[0]);
+    if(servo_num == 0)
+    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;
-    
+    time_s += tickerTime;
+    if(time_s >= walkPeriod) 
+    {
+        time_s = 0.0f;
+    }
 }
-void WalkingPattern::TurnLeft()
+void WalkingPattern::TurnLeft(int servo_num)
 {
     //static float time_s = 0.0f;
 
 }
-void WalkingPattern::TurnRight()
+void WalkingPattern::TurnRight(int servo_num)
 {
     //static float time_s = 0.0f;
 
 }
-void WalkingPattern::Climb()
+void WalkingPattern::Climb(int servo_num)
 {
     //static float time_s = 0.0f;
     
 }
-void WalkingPattern::Overcoming()
+void WalkingPattern::Overcoming(int servo_num)
 {
     //static float time_s = 0.0f;
     
 }
 
-void CalcWalk()
+void CalcWalk(int servo_num)
 {
     switch(WalkMode)
     {
         case straight:
-            WalkingPattern::Straight();
+            WalkingPattern::Straight(servo_num);
         break;
         case turnleft:
-            WalkingPattern::TurnLeft();
+            WalkingPattern::TurnLeft(servo_num);
         break;
         case turnright:
-            WalkingPattern::TurnRight(); 
+            WalkingPattern::TurnRight(servo_num); 
         break;
         case climb:
-            WalkingPattern::Climb();
+            WalkingPattern::Climb(servo_num);
         break;
         case overcoming:
-            WalkingPattern::Overcoming();
+            WalkingPattern::Overcoming(servo_num);
         break;
     }
 }
@@ -256,28 +260,29 @@
 void cb_timer()
 {
     static int servo_num = 0;
-    static float time_s = 0.0f;
     //出力。片サーボずつ
     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) //両サーボ出力したら
+    /*if (servo_num > 1) //両サーボ出力したら
     {
         //目標角の更新
-        Quadruped::CalcWalk();
+        Quadruped::CalcWalk(servo_num);
         //このマイコンで使う脚だけ計算
         leg1.CalServoRad(TargetX[0], TargetY[0] + OffsetY);
         leg2.CalServoRad(TargetX[1], TargetY[1] + OffsetY);
         
         servo_num = 0;
-    }
-    time_s += tickerTime;
+    }*/
     
 }
 int main()
 {
-    Quadruped::CalcWalk();
+    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);