Revision 1:15dd461fbc2a, committed 2018-12-21

Comitter:

OscarLiao

Date:

Fri Dec 21 06:27:26 2018 +0000

Parent:

0:94cf69f1f327

Commit message:

balance

Changed in this revision

diff -r 94cf69f1f327 -r 15dd461fbc2a main.cpp
--- a/main.cpp	Thu Dec 13 14:27:35 2018 +0000
+++ b/main.cpp	Fri Dec 21 06:27:26 2018 +0000
@@ -21,7 +21,7 @@
 
 //Initial Position
 #define init_leg_Angle1 0
-#define init_leg_length 0.22f
+#define init_leg_length 0.23f
 #define init_leg_Angle2 0
 
 #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
@@ -136,13 +136,9 @@
 //╔═════════════════╗
 //║     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;
 
 //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Varible registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//
 
@@ -312,8 +308,7 @@
 //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Button funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡//
 void    pressed(void)
 {
-    Ki = 0.005;
-    Kd = 0.001;
+
 }
 
 void    released(void)
@@ -326,24 +321,33 @@
 
 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.5*(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", Til_phy, rollU);
 }
 
 //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Balance funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//