Revision 2:e8c86014468c, committed 2018-12-27

Comitter:

OscarLiao

Date:

Thu Dec 27 15:53:37 2018 +0000

Parent:

1:c6c07cb81c1a

Commit message:

SEA_DEMO

Changed in this revision

--- a/main.cpp	Fri Dec 21 06:18:28 2018 +0000
+++ b/main.cpp	Thu Dec 27 15:53:37 2018 +0000
@@ -62,9 +62,9 @@
 #define Kpi_Hip     0
 #define Kpi_Thigh   2000
 #define Kpi_Shin    8
-#define Kd_Hip      0.0
-#define Kd_Thigh    0.0
-#define Kd_Shin     0.0
+#define Kd_Hip      0
+#define Kd_Thigh    0
+#define Kd_Shin     0
 #define KN          20
 #define L1          0.120f          //Tigh length
 #define L2          0.1480f          //Shing lenth
@@ -74,7 +74,7 @@
 #define L0          0.2f           //Length of the leg
 
 //Structure
-#define PWM_Hip     1460            //900~1550~2200 2050
+#define PWM_Hip     1460            //900~1550~2200 1800
 #define PWM_Thigh   1330            //900~1550~2200 1300
 #define PWM_Shin    1780            //900~1550~2200 1450 1740
 
@@ -219,6 +219,8 @@
 
 float q_0_E_1_gain = 0.2f/255.0f;
 float q_0_E_2_gain = (pi/4)/128.0f;
+float K_c_gain = 4000.0f/255.0f;
+float B_c_gain = 6.0f/255.0f;
 
 float T_0[3] = {                    //"Controller Input" from upper controller in joint level
     0, 0, 0                         //{T_0_Hip, T_0_Tigh, F_0_Stretch} force feed foward
@@ -405,12 +407,12 @@
                 T_0[2] = 1.0f * (Buff[2] - 128);
             } else if(Buff[3] == 'K') {
                 K_c[0] = 0.02f * Buff[0];
-                K_c[1] = 0.02f * Buff[1];
-                K_c[2] = 0.02f * Buff[2];
+                K_c[1] = K_c_gain * Buff[1];
+                K_c[2] = Buff[2];
             } else if (Buff[3] == 'B') {
                 B_c[0] = 0.02f * (Buff[0] - 128);
-                B_c[1] = 0.02f * (Buff[1] - 128);
-                B_c[2] = 0.02f * (Buff[2] - 128);
+                B_c[1] = (B_c_gain * Buff[1] - 1.0f)*KN;
+                B_c[2] = (B_c_gain * Buff[2] - 1.0f)*KN;
             } else {
             //Show error in communication
                 led2 = 1;
@@ -419,6 +421,7 @@
             Buff[3] = 0x00;           //clear end signals
             Cmd_Flag = 0;
             led = 0;
+            //Debug.printf("%.f, %.f, %.f, %.f, %.f, %.f\r", K_c[0], K_c[1], K_c[2], B_c[0], B_c[1], B_c[2]);
             //Send out message threw Cmd
 //            Cmd.printf("%c %c %c\r", Dummy[0], Dummy[1], Dummy[2]);
         }