Diff: main.cpp

Revision:

8:c7d21f5f87d8

Parent:

7:db050a878cff

Child:

9:4f594927cff3

--- a/main.cpp	Tue Oct 30 14:15:57 2018 +0000
+++ b/main.cpp	Wed Oct 31 09:47:09 2018 +0000
@@ -38,8 +38,10 @@
 
     float kp1 = 0.1;
     float kp2 = 0.1;
-    float ki1 = 0.2;
-    float ki2 = 0.2;
+    float ki1 = 0.01;
+    float ki2 = 0.01;
+    float kd1 = 0.0005;
+    float kd2 = 0.0005;
     float int_u1 = 0;
     float int_u2 = 0;
     float u1 = 0;
@@ -53,6 +55,12 @@
     
     float ref_v1;
     float ref_v2;
+    float teraterm1;
+    float teraterm2;
+    float error1;
+    float error2;
+    float filtered_d_error1;
+    float filtered_d_error2;
     
 enum States {failure, waiting, calib_motor, homing ,calib_emg, operational, demo};
 enum Operations {rest, forward, backward, up, down};
@@ -90,6 +98,7 @@
 static BiQuad HighPass2(0.95653708,  -1.91307417, 0.95653708, -1.91118480, 0.91496354);
 static BiQuad LowPass1(0.00362164,  0.00724327, 0.00362164, -1.82267251, 0.83715906);
 static BiQuad LowPass2(0.00362164,  0.00724327, 0.00362164, -1.82267251, 0.83715906);
+static BiQuad LowpassFilter(0.0640,0.1279,0.0640,-1.1683,0.4241);
 
 void filterall()
 {
@@ -147,13 +156,13 @@
  float twistf[2] = {0,0};
  float abs_sig;  
  
-    int gain = 1;
+    int gain = 3;
     
 void do_forward(){
     
     
-    twistf[0] = 0;
-    twistf[1] = -1;
+    twistf[0] = 1;
+    twistf[1] = 0;
     
     if (filteredsignal2 > 0.15*max2){
         abs_sig = (filteredsignal2 - (0.15*max2))/(0.85*max2);       
@@ -411,9 +420,9 @@
     float inv_jacobian[4];
     
     jacobian[0] = L1;
-    jacobian[1] = L2*sin(deg_m1)+L1;
+    jacobian[1] = L2*sin(deg_m2)+L1;
     jacobian[2] = -L0;
-    jacobian[3] = -L0 - L2*cos(deg_m1);
+    jacobian[3] = -L0 - L2*cos(deg_m2);
     
     float det = 1/(jacobian[0]*jacobian[3]-jacobian[1]*jacobian[2]);
     
@@ -431,25 +440,52 @@
     ref_q1 = ref_q1 + 0.002*ref_v1;
     ref_q2 = ref_q2 + 0.002*ref_v2;
     
-    float error1 = deg_m1 - ref_q1;
-    float error2 = deg_m2 - ref_q2;
+    error1 = deg_m1 - ref_q1;
+    error2 = deg_m2 - ref_q2;
+    float olderror1;
+    float olderror2;
+    float d_error1;
+    float d_error2;
+
+
+    
     
    if(error1 > 5){
         u1 = 1;        }
     if(error1 < -5){
         u1 = -1;        }
     else{
+        d_error1 = (error1 - olderror1)/Ts;
+        filtered_d_error1 = LowpassFilter.step(d_error1);
         int_u1 = int_u1 + error1 * Ts;
-        u1 = kp1*error1 + int_u1*ki1;   
+        u1 = kp1*error1 + int_u1*ki1 + kd1*filtered_d_error1;
+        teraterm1 = u1;   // to see how big u1 and u2 actually get, they should lie between -1 and 1 for the right gains.
+        if(u1>1){
+            u1 =1;
+            }
+        if(u1<-1){
+            u1 = -1;
+            }
+        
     }
+    olderror1 = error1;
     
     if(error2 > 5){
         u2 = 1;}
     if(error2 < -5){
         u2 = -1;}
     else{
+         d_error2 = (error2 - olderror2)/Ts;
+        filtered_d_error2 = LowpassFilter.step(d_error2);       
         int_u2 = int_u2 + error2 * Ts;
-        u2 = (kp2*error2 + int_u2*ki2);
+        u2 = kp2*error2 + int_u2*ki2 + kd2*filtered_d_error2;
+        teraterm2 = u2;
+         if(u2>1){
+            u2 =1;
+            }
+        if(u2<-1){
+            u2 = -1;
+            }       
         }
   
        
@@ -501,6 +537,6 @@
     sample_timer2.attach(&filterall, Ts);
 
     while (true) { 
-            printf("%f %f \n\r",ref_q1,ref_q2);
+            printf("%f %f %f %f %f \n\r",ref_q1,ref_q2,error1,int_u1,filtered_d_error1);
     }
 }
\ No newline at end of file