Revision 1:ddca0613a53e, committed 2018-12-21

Comitter:

OscarLiao

Date:

Fri Dec 21 09:31:08 2018 +0000

Parent:

0:94cf69f1f327

Commit message:

trot

Changed in this revision

--- a/main.cpp	Thu Dec 13 14:27:35 2018 +0000
+++ b/main.cpp	Fri Dec 21 09:31:08 2018 +0000
@@ -21,9 +21,14 @@
 
 //Initial Position
 #define init_leg_Angle1 0
-#define init_leg_length 0.22f
+#define init_leg_length 0.23f
 #define init_leg_Angle2 0
 
+//Trot gait
+#define trot_step_height 0.02   //step height in meters
+#define airborne_pct 30 //leg airborne time percentage 0~100
+#define ground_pct 20   //leg stay on ground time percentage 0~100
+
 #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
 
 //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡GPIO registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
@@ -89,13 +94,13 @@
     init_leg_Angle1, init_leg_length, init_leg_Angle2
 };
 float   LH_q_0_E[3] = {            
-    init_leg_Angle1, init_leg_length, init_leg_Angle2
+    init_leg_Angle1, init_leg_length, init_leg_Angle2-0.349066
 };
 float   RF_q_0_E[3] = {            
     init_leg_Angle1, init_leg_length, init_leg_Angle2
 };
 float   RH_q_0_E[3] = {            
-    init_leg_Angle1, init_leg_length, init_leg_Angle2
+    init_leg_Angle1, init_leg_length, init_leg_Angle2-0.349066
 };
 
 uint8_t LF_Cmd_Hex[3] = {0x00, 0x00, 0x00};
@@ -136,13 +141,39 @@
 //╔═════════════════╗
 //║     Balance     ║
 //╚═════════════════╝
-float Kp = 0.04;
-float Ki = 0.0;
-float Kd = 0.0;
-float Pu = 0.0;
-float Iu = 0.0;
-float Du = 0.0;
-float Ele_phy_old = 0.0;
+float pitchU[3] = {0, 0, 0};
+float pitchE[3] = {0, 0, 0};
+float rollU = 0;
+
+//╔═════════════════╗
+//║       Trot      ║
+//╚═════════════════╝
+bool trotingFlag = false;
+
+float period = 0;
+float move_pct = 0;
+float liftTime = 0;
+float dropTime = 0;
+float airborneTime = 0;
+float groundTime = 0;
+float liftSpeed = 0;
+float dropSpeed = 0;
+
+int liftCounter = 0;
+int dropCounter = 0;
+int airCounter = 0;
+int groundCounter = 0;
+
+bool liftBool = false;
+bool dropBool = false;
+bool airborneBool = false;
+bool groundBool = false;
+
+enum legs{LF, RF, RH, LH}; 
+bool trotDiagonal = false;
+int trotLeg[2] = {LF, RH};
+
+bool press = false;
 
 //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Varible registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//
 
@@ -161,6 +192,9 @@
 void    pressed();
 void    Balance(); 
 
+void    trot(float freq);
+void    move_leg(int leg, int type, float speed);
+
 void    Rx_irq();
 //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Function registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//
 
@@ -201,18 +235,36 @@
         //Balance
         if(Flag_1 == 1) {             
             
-            //Initial Position
-            LF_q_0_E[1] = init_leg_length;
-            LH_q_0_E[1] = init_leg_length;
-            RF_q_0_E[1] = init_leg_length;
-            RH_q_0_E[1] = init_leg_length;
-
-            Balance();
+//            //Initial Position
+//            LF_q_0_E[1] = init_leg_length;
+//            LH_q_0_E[1] = init_leg_length;
+//            RF_q_0_E[1] = init_leg_length;
+//            RH_q_0_E[1] = init_leg_length;
+            if(press)
+            {
+               trot(0.5);
+               Balance();
+            }
+            else
+            {
+                LF_q_0_E[1] = init_leg_length;
+                LH_q_0_E[1] = init_leg_length;
+                RF_q_0_E[1] = init_leg_length;
+                RH_q_0_E[1] = init_leg_length;
+            }
+            
+            
+            Debug.printf("%.3f, %.3f\r", LF_q_0_E[1], RF_q_0_E[1]);
             Flag_1 = 0;                 
         } 
         
         //Send Command
-        if(Flag_4 == 1) {             
+        if(Flag_4 == 1) {    
+                 
+            LF_q_0_E[1] =  constrain(LF_q_0_E[1],0.1 ,0.3);//safty constrain
+            LH_q_0_E[1] =  constrain(LH_q_0_E[1],0.1 ,0.3);//safty constrain
+            RF_q_0_E[1] =  constrain(RF_q_0_E[1],0.1 ,0.3);//safty constrain
+            RH_q_0_E[1] =  constrain(RH_q_0_E[1],0.1 ,0.3);//safty constrain
             
             LF_Cmd_Hex[0] = (LF_q_0_E[0]+pi/4)/(pi/2)*255;
             LF_Cmd_Hex[1] = ((LF_q_0_E[1]-0.1)/0.2)*255;
@@ -312,8 +364,7 @@
 //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Button funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
 void    pressed(void)
 {
-    Ki = 0.005;
-    Kd = 0.001;
+    press = true;
 }
 
 void    released(void)
@@ -326,28 +377,151 @@
 
 void Balance(void)
 {
-    Pu = Kp*Ele_phy;
-    Iu += Ki*Ele_phy;
-    Du = Kd*(Ele_phy-Ele_phy_old)/(dt*(Task_1_count+1));
+    //update pitch controller
+    pitchE[0] = Ele_phy;
+    pitchU[0] = 0.2*(0.0663*pitchU[1]+ 0.9337*pitchU[2]- 0.13635*(pitchE[0]+0.3535*pitchE[1]-0.6465*pitchE[2]));
+    
+    pitchE[2] = pitchE[1];
+    pitchE[1] = pitchE[0];
+    pitchU[2] = pitchU[1];
+    pitchU[1] = pitchU[0];
     
-    LF_q_0_E[1] = LF_q_0_E[1] - (Pu+Iu+Du);
-    RF_q_0_E[1] = RF_q_0_E[1] - (Pu+Iu+Du);
-    LH_q_0_E[1] = LH_q_0_E[1] + (Pu+Iu+Du);
-    RH_q_0_E[1] = RH_q_0_E[1] + (Pu+Iu+Du);
+    rollU = 0.05*Til_phy;
+    
+//    LF_q_0_E[0] =  Til_phy;
+//    RF_q_0_E[0] =  -Til_phy;
+//    LH_q_0_E[0] =  Til_phy;
+//    RH_q_0_E[0] =  -Til_phy;
+    
+    LF_q_0_E[1] = LF_q_0_E[1] + pitchU[0] + rollU;
+    RF_q_0_E[1] = RF_q_0_E[1] + pitchU[0] - rollU;
+    LH_q_0_E[1] = LH_q_0_E[1] - pitchU[0] + rollU;
+    RH_q_0_E[1] = RH_q_0_E[1] - pitchU[0] - rollU;
     
     LF_q_0_E[2] =  - Ele_phy;
     RF_q_0_E[2] =  - Ele_phy;
     LH_q_0_E[2] =  - Ele_phy;
     RH_q_0_E[2] =  - Ele_phy;
-        
-    //Update
-    Ele_phy_old = Ele_phy;
     
-    Debug.printf("%.3f, %.3f, %.3f\r", Pu, Iu, Du);
+    //Debug.printf("%.3f, %.3f\r", Ele_phy, pitchU[0]);
 }
 
 //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Balance funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//
 
+//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Trot funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
+
+void trot(float freq)
+{ 
+    if(!trotingFlag)
+    {
+        period = 1/freq;
+        move_pct = 100.0f - airborne_pct - ground_pct;
+        liftTime = period*((move_pct/2)/100.0f);
+        dropTime = liftTime;
+        airborneTime = period*(airborne_pct/100.0f);
+        groundTime = period*(ground_pct/100.0f);
+        liftSpeed = trot_step_height/liftTime;
+        dropSpeed = trot_step_height/dropTime;
+        
+        liftCounter = liftTime/(dt*(Task_1_NN+1));
+        dropCounter = liftCounter;
+        airCounter = airborneTime/(dt*(Task_1_NN+1));    
+        groundCounter = groundTime/(dt*(Task_1_NN+1));
+        
+        //switch legs
+        if(trotDiagonal)
+        {
+            trotLeg[0] = LF;
+            trotLeg[1] = RH;
+        }
+        else
+        {
+            trotLeg[0] = RF;
+            trotLeg[1] = LH;
+        }
+        
+        //start troting
+        liftBool = true;
+        trotingFlag = true;
+    }
+    //Debug.printf("%d, %d, %d, %d\r", liftCounter, airCounter, dropCounter, groundCounter);
+    //lift leg
+    if(liftBool)
+    {     
+        move_leg(trotLeg[0], 1, -liftSpeed); 
+        move_leg(trotLeg[1], 1, -liftSpeed);
+        liftCounter--;
+        if(liftCounter == 0)
+        {
+            liftBool = false;    
+            airborneBool = true;
+        }
+    }
+    
+    //stay airborne
+    if(airborneBool)
+    {
+        airCounter--;
+        if(airCounter == 0)
+        {
+            airborneBool = false;    
+            dropBool = true;
+        }
+    }
+    
+    //drop leg
+    if(dropBool)
+    {
+        move_leg(trotLeg[0], 1, dropSpeed); 
+        move_leg(trotLeg[1], 1, dropSpeed);
+        dropCounter--;
+        if(dropCounter == 0)
+        {
+            dropBool = false;    
+            groundBool = true;
+        }
+    }
+    
+    //stay on ground
+    if(groundBool)
+    {
+        groundCounter--;
+        if(groundCounter == 0)
+        {
+            groundBool = false;    
+            trotingFlag = false;
+            trotDiagonal = !trotDiagonal; //switch legs
+        }
+    }
+}
+
+//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Trot funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//
+
+//≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Move leg funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
+
+void move_leg(int leg, int type, float speed)
+{ 
+    switch(leg)
+    {
+        case LF:
+            LF_q_0_E[type] += speed*(dt*(Task_1_NN+1));
+            break;
+        case RF:
+            RF_q_0_E[type] += speed*(dt*(Task_1_NN+1));
+            break;
+        case LH:
+            LH_q_0_E[type] += speed*(dt*(Task_1_NN+1));
+            break;
+        case RH:
+            RH_q_0_E[type] += speed*(dt*(Task_1_NN+1));
+            break; 
+    }
+    
+    //Debug.printf("%.3f, %.3f\r", Ele_phy, u[0]);
+}
+
+//■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Move leg funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//
+
 
 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
 void init_IMU(void)                 //initialize