Diff: main.cpp

Revision:

31:481ad25a40c3

Parent:

30:8191e8541a0a

Child:

32:137d0f27e5a8

--- a/main.cpp	Thu Nov 01 12:46:20 2018 +0000
+++ b/main.cpp	Thu Nov 01 14:14:03 2018 +0000
@@ -14,11 +14,6 @@
 AnalogIn potmeter1          (A0);                   //First raw EMG signal input
 AnalogIn potmeter2          (A1);                   //Second raw EMG signal input
 
-InterruptIn encoderA1       (D9);
-InterruptIn encoderB1       (D8);
-InterruptIn encoderA2       (D12);
-InterruptIn encoderB2       (D13);
-
 InterruptIn button1         (D10);                  
 InterruptIn button2         (D11);
 
@@ -34,13 +29,14 @@
 
 
 MODSERIAL pc(USBTX, USBRX);                       //Serial communication to see if the code works step by step, turn on if hidscope is off
-QEI encoder1 (D9, D8, NC, 8400 , QEI::X4_ENCODING);
-QEI encoder2 (D12, D13, NC, 8400 , QEI::X4_ENCODING);
+QEI encoder2 (D9, D8, NC, 8400,QEI::X4_ENCODING);
+QEI encoder1 (D12, D13, NC, 8400,QEI::X4_ENCODING);
 
 //HIDScope    scope( 6 );                             //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered
 
 //Tickers
-Ticker      func_tick;  
+Ticker      func_tick; 
+Ticker      printTicker;
                      
 
 //Global variables
@@ -74,18 +70,23 @@
 double PI = 3.14159;
 double Kp1 = 17.5;                                  //Motor 1           eerst 17.5 , nu 1
 double Ki1 = 1.02;
-double Kd1 = 23.2;
+double Kd1 = 15.5;
 double encoder_radians1=0;
 double err_integral1 = 0;
-double err_prev1 = 0;
+double err_prev1, err_prev2;
+double err1, err2;
+
+BiQuad LowPassFilterDer1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+BiQuad LowPassFilterDer2(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
 
 
 double Kp2 = 17.5;                                  //Motor 2            eerst 17.5, nu 1
 double Ki2 = 1.02;
-double Kd2 = 23.2;
+double Kd2 = 15.5;
 double encoder_radians2=0;
 double err_integral2 = 0;
-double err_prev2 = 0;
+double u1, u2;
+
 
 int start_control = 0;
 double emg_cal = 1;
@@ -98,100 +99,117 @@
 
 
 //---------PID controller motor 1 + motor control 1 & 2-----------------------------------------------------------//
-double PID_control(double err, const double Kp, const double Ki, const double Kd, double &err_integral, double &err_prev)
+void PID_control1()
 {
-    pc.printf("ik doe het, PDI \n\r");
-
-    err_integral = 0;
-    err_prev = err; // initialization with this value only done once!
-  
-    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+    //pc.printf("ik doe het, PDI \n\r");
 
     // Proportional part:
-    double u_k = Kp * err;
+    double u_k1 = Kp1 * err1;
 
     //Integral part  
-      err_integral = err_integral + err * T;
-      double u_i = Ki * err_integral;
+      err_integral1 = err_integral1 + err1 * T;
+      double u_i1 = Ki1 * err_integral1;
     
     // Derivative part
-      double err_derivative = (err - err_prev)/T;
-      double filtered_err_derivative = LowPassFilter.step(err_derivative);
-      double u_d = Kd * filtered_err_derivative;
-      err_prev = err;
+      double err_derivative1 = (err1 - err_prev1)/T;
+      double filtered_err_derivative1 = LowPassFilterDer1.step(err_derivative1);
+      double u_d1 = Kd1 * filtered_err_derivative1;
+      err_prev1 = err1;
       
     
       // Sum all parts and return it
-      return u_k + u_i + u_d;  
-    }
+      u1 = u_k1 + u_i1 + u_d1;  
+}
+
+void PID_control2()
+{
+    //pc.printf("ik doe het, PDI \n\r");
+
+    // Proportional part:
+    double u_k2 = Kp2 * err2;
+
+    //Integral part  
+      err_integral2 = err_integral2 + err2 * T;
+      double u_i2 = Ki2 * err_integral2;
+    
+    // Derivative part
+      double err_derivative2 = (err2 - err_prev2)/T;
+      double filtered_err_derivative2 = LowPassFilterDer2.step(err_derivative2);
+      double u_d2 = Kd2 * filtered_err_derivative2;
+      err_prev2 = err2;
+      
+    
+      // Sum all parts and return it
+      u2 = u_k2 + u_i2 + u_d2;  
+}
 
 void engine_control1()                                           //Engine 1 is translational engine, connected with left side pins
 {
-        //pc.printf("ik doe het, engine control 1\n\r");
-        encoder_radians1 = encoder1.getPulses()*(2*PI)/8400.0;
-        pc.printf("encoder1 %d \n\r",encoder1.getPulses());
-        pc.printf("encoder_radians1 %f \n\r",encoder_radians1);
-        double err1 = q1ref - encoder_radians1;
-        pc.printf("err1 = %f\n\r",err1);
-        double u1 = PID_control(err1, Kp1, Ki1, Kd1, err_integral1, err_prev1);                               //PID controller function call
+            //pc.printf("ik doe het, engine control 1\n\r");
+        encoder_radians1 = (double)encoder1.getPulses()*(2.0*PI)/8400.0;
+            //pc.printf("encoder1 %f \n\r", (float)encoder1.getPulses());
+             //pc.printf("encoder_radians1 %f \n\r",(float) encoder_radians1);
+        err1 = q1ref - encoder_radians1;
+            //pc.printf("err1 = %f\n\r",err1);
+        PID_control1();                               //PID controller function call
         
-        pc.printf("u1 = %f\n\r",u1);
+            //pc.printf("u1 = %f\n\r",u1);
         
-        if(encoder1.getPulses()<12000 && encoder1.getPulses()>-1)                              //limits translation in counts, eerst 12600
-            {
+        //if(encoder1.getPulses()<12000 && encoder1.getPulses()>-1)                              //limits translation in counts, eerst 12600
+            //{
                  pwmpin1 = fabs(u1);                                         //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
                  directionpin1.write(u1<0);
-            }
-        else
-            {
-                pwmpin1 = 0;
-            }  
+            //}
+        //else
+           // {
+               // pwmpin1 = 0;
+           // }  
 }
 
 void engine_control2()                                             //Engine 2 is rotational engine, connected with right side wires
 {
-        encoder_radians2 = encoder2.getPulses()*(2*PI)/8400.0;
-        pc.printf("encoder2 %f \n\r",encoder2.getPulses());
-        pc.printf("encoder_radians2 %d \n\r",encoder_radians2);
-        double err2 = q2ref - encoder_radians2;
-        pc.printf("err2 = %f\n\r",err2);
-        double u2 = PID_control(err2, Kp2, Ki2, Kd2, err_integral2, err_prev2);                             //PID controller function call
-        pc.printf("u2 = %f\n\r",u2);
+        encoder_radians2 = (float)encoder2.getPulses()*(2.0*PI)/8400.0;
+            //pc.printf("encoder2 %f \n\r",(float)encoder2.getPulses());
+            //pc.printf("encoder_radians2 %f \n\r",(float)encoder_radians2);
+        err2 = q2ref - encoder_radians2;
+            //pc.printf("err2 = %f\n\r",err2);
+        PID_control2();                            //PID controller function call
+             //pc.printf("u2 = %f\n\r",u2);
 
-        if(encoder2.getPulses()<5250 && encoder2.getPulses()>-5250)                              //limits rotation, in counts                
-         {
+        //if(encoder2.getPulses()<-5250 && encoder2.getPulses()>5250)                              //limits rotation, in counts                
+        // {
             pwmpin2 = fabs(u2);                                       //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
             directionpin2.write(u2>0);
-         }
-        else
-         {
-            pwmpin2 = 0;
-         }
+        // }
+        //else
+        // {
+        //    pwmpin2 = 0;
+        // }
 }
 
 
 //------------------ Inversed Kinematics --------------------------------//
 
 
-void inverse_kinematics(double v_x, double v_y)
+void inverse_kinematics()
 {
     
-    pc.printf("v_x is %f en v_y is %f\n\r",v_x, v_y);
+    //pc.printf("v_x is %f en v_y is %f\n\r",v_x, v_y);
     
     Lq1 = q1ref*r_trans;                            
     Cq2 = q2ref/5.0;                               
 
-    q1_dot = v_x + (v_y*(L1 + L3*sin(Cq2)))/(L4 + Lq1 + L3*cos(Cq2));     
-    q2_dot = v_y/(L4 + Lq1 + L3*cos(Cq2));                                       
+    q1_dot = v_x + (v_y*(L1 + L3*sin(Cq2)))/(L4 + Lq1 + L3*cos(Cq2));               //RKI systeem  
+    q2_dot = v_y/(L4 + Lq1 + L3*cos(Cq2));                                          //
 
-    q1_ii = q1ref + (q1_dot/r_trans)*T;                       
+    q1_ii = q1ref + (q1_dot/r_trans)*T;                                             //Omgezet naar motorhoeken
     q2_ii = q2ref + (q2_dot*5.0)*T; 
         
     q1ref = q1_ii;
     q2ref = q2_ii; 
     
     
-    pc.printf("q1ref is %f en q2ref is %f\n\r",q1ref, q2ref);
+    //pc.printf("q1ref is %f en q2ref is %f\n\r",q1ref, q2ref);
 
     
     //start_control = 1;
@@ -252,13 +270,19 @@
                     //pwmpin2 = 0;
                 }
       
-        inverse_kinematics(v_x, v_y);                           //Call inverse kinematics function
+        inverse_kinematics();                           //Call inverse kinematics function
        
 }
 
+void printFunctie()
+{
+    pc.printf("%f, %f\n\r", u1, u2);    
+}
+
 //------------------ Start main function --------------------------//
 
 
+
 int main()
 {        
         pc.baud(115200);
@@ -266,6 +290,7 @@
         pwmpin1.period_us(60);                                    //60 microseconds PWM period, 16.7 kHz 
         
         func_tick.attach(&v_des_calculate_qref,T2);                 //v_des determined every T
+        printTicker.attach(&printFunctie, 0.01);
 
         
     while(true)
@@ -275,8 +300,9 @@
         
            // HIDScope_tick.attach(&HIDScope_sample,T);             //EMG signals raw + filtered to HIDScope every T sec.
            
-        pc.printf("Encoder engine 1 %d\n\r",encoder2.getPulses());
-        pc.printf("Encoder engine 2 %d\n\r",encoder1.getPulses());
-        wait(0.1f);
+        //pc.printf("Encoder engine 1 %d\n\r",encoder2.getPulses());
+        //pc.printf("Encoder engine 2 %d\n\r",encoder1.getPulses());
+        //wait(0.1f);
+        ;
     }      
 }