Revision 9:4f594927cff3, committed 2018-11-01

Comitter:

WouterJS

Date:

Thu Nov 01 08:30:56 2018 +0000

Parent:

8:c7d21f5f87d8

Commit message:

Another one;

Changed in this revision

--- a/main.cpp	Wed Oct 31 09:47:09 2018 +0000
+++ b/main.cpp	Thu Nov 01 08:30:56 2018 +0000
@@ -36,12 +36,7 @@
 PwmOut motor2_speed_control(D6);//aanstuursnelheid motor 2
 DigitalOut motor2_direction(D7);// draairichting motor 2 (0 is CCW )
 
-    float kp1 = 0.1;
-    float kp2 = 0.1;
-    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;
@@ -98,8 +93,8 @@
 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);
-
+static BiQuad LowpassFilter1(0.0640,0.1279,0.0640,-1.1683,0.4241);
+static BiQuad LowpassFilter2(0.0640,0.1279,0.0640,-1.1683,0.4241);
 void filterall()
 {
     //Notch 50 Hz BW 6 Hz
@@ -164,23 +159,19 @@
     twistf[0] = 1;
     twistf[1] = 0;
     
-    if (filteredsignal2 > 0.15*max2){
-        abs_sig = (filteredsignal2 - (0.15*max2))/(0.85*max2);       
-        
+    if (but1 == false){
+        //abs_sig = (filteredsignal2 - (0.3*max2))/(0.7*max2);       
+        abs_sig = 0.4;
         
         }
     else
     {
         abs_sig = 0;
         }
-    //if (but1 == false){
-     //   abs_sig = 0.75;     
-        
-        
-     //   }    
+    
     
-    twist[0] = twistf[0] * abs_sig* gain;
-    twist[1] = twistf[1] * abs_sig* gain;
+    twist[0] = twistf[0] * abs_sig ;
+    twist[1] = twistf[1] * abs_sig ;
              
     motor1_speed_control = fabs(u1);
     
@@ -249,12 +240,23 @@
     }    
 ///////////END MOVEMENT STATES/////////////////////////
 ///////////ROBOT ARM STATES ///////////////////////////
-
+    float kp1 = 0.1;
+    float kp2 = 0.1;
+    float ki1 = 0.05;
+    float ki2 = 0.05;
+    float kd1 = 0.005;
+    float kd2 = 0.005;
+    float olderror1;
+    float olderror2;
+    float d_error1;
+    float d_error2;
 void do_state_failure(){    
      
     }
+    
    int count1 = 0;
    int count2 = 0;  
+   
 void do_state_calib_motor(){
     if (state_changed==true) {
         state_changed = false;        
@@ -291,7 +293,9 @@
         timer.reset();
         state_changed = false;        
     }
-         
+     float kp11 = 0.1;
+     float kp12 = 0.1; 
+        
     float werror1 = deg_m1 - 0;
     float werror2 = deg_m2 - 0;
     
@@ -300,7 +304,7 @@
     if(werror1 < -5){
         wu1 = -1;        }
     else{
-        wu1 = kp1*werror1; //+ (u1 + werror1*0.002)*ki1;   
+        wu1 = kp11*werror1; //+ (u1 + werror1*0.002)*ki1;   
     }
     
     if(werror2 > 5){
@@ -308,7 +312,7 @@
     if(werror2 < -5){
         wu2 = -1;}
     else{
-        wu2 = kp2*werror2; //+ (u2 + werror2*0.002)*ki2);
+        wu2 = kp12*werror2; //+ (u2 + werror2*0.002)*ki2);
         }
     
     motor1_speed_control = fabs(wu1)/8;
@@ -327,16 +331,19 @@
         
     if (  timer.read() > 4) {
          motor1_speed_control = 0;
-         motor2_speed_control = 0;   
-       
+         motor2_speed_control = 0;  
         
+        ref_q1 = 0;
+        ref_q2 = 0;
         deg_m1 = 0;
         deg_m2 = 0;
+        d_error1 = 0;
+        d_error2 = 0;
         u1 = 0;
         u2 = 0;
         int_u1 = 0;
         int_u2 = 0;
-        wait(1);
+         wait(1); 
         current_state = calib_emg;
         timer.reset();
         state_changed = true;
@@ -420,11 +427,11 @@
     float inv_jacobian[4];
     
     jacobian[0] = L1;
-    jacobian[1] = L2*sin(deg_m2)+L1;
+    jacobian[1] = (L2*cos(deg_m2))+L1;
     jacobian[2] = -L0;
-    jacobian[3] = -L0 - L2*cos(deg_m2);
+    jacobian[3] = -L0 - (L2*sin(deg_m2));
     
-    float det = 1/(jacobian[0]*jacobian[3]-jacobian[1]*jacobian[2]);
+    float det = 1/((jacobian[0]*jacobian[3])-(jacobian[1]*jacobian[2]));
     
     inv_jacobian[0] = det*jacobian[3];
     inv_jacobian[1] = -det*jacobian[1]; 
@@ -442,41 +449,39 @@
     
     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{
+   // if(error1 > 5){
+     //   u1 = 1;        }
+    //if(error1 < -5){
+    //    u1 = -1;        }
+    //else{
         d_error1 = (error1 - olderror1)/Ts;
-        filtered_d_error1 = LowpassFilter.step(d_error1);
+        filtered_d_error1 = LowpassFilter1.step(d_error1);
         int_u1 = int_u1 + error1 * Ts;
         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{
+    //if(error2 > 5){
+     //   u2 = 1;}
+   // if(error2 < -5){
+     //   u2 = -1;}
+    //else{
          d_error2 = (error2 - olderror2)/Ts;
-        filtered_d_error2 = LowpassFilter.step(d_error2);       
+        filtered_d_error2 = LowpassFilter2.step(d_error2);       
         int_u2 = int_u2 + error2 * Ts;
         u2 = kp2*error2 + int_u2*ki2 + kd2*filtered_d_error2;
         teraterm2 = u2;
@@ -486,8 +491,9 @@
         if(u2<-1){
             u2 = -1;
             }       
-        }
-  
+     //   }
+    olderror2 = error2;
+    
        
     }
 
@@ -535,8 +541,9 @@
     loop_timer.attach(&loop_function, Ts);    
     sample_timer.attach(&scopedata, Ts);    
     sample_timer2.attach(&filterall, Ts);
-
+    
+    pc.baud(115200);
     while (true) { 
-            printf("%f %f %f %f %f \n\r",ref_q1,ref_q2,error1,int_u1,filtered_d_error1);
+            printf("%f %f %f %f %f %f \n\r",ref_q1,ref_q2,error1,error2,deg_m1, deg_m2);
     }
 }
\ No newline at end of file