Revision 28:5e54cd4525de, committed 2018-11-01

Comitter:

MarijkeZondag

Date:

Thu Nov 01 17:29:06 2018 +0000

Parent:

27:fa493551be99

Child:

29:6cd4f5ac57c4

Commit message:

Werkende PID controller, Inversed kinematics zijn we nog niet helemaal zeker van.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Thu Nov 01 17:29:06 2018 +0000
@@ -0,0 +1,1 @@
+http://os.mbed.com/users/aberk/code/QEI/#5c2ad81551aa

--- a/main.cpp	Wed Oct 31 12:38:00 2018 +0000
+++ b/main.cpp	Thu Nov 01 17:29:06 2018 +0000
@@ -3,6 +3,7 @@
 #include "BiQuad.h"
 #include "HIDScope.h"
 #include <math.h>
+#include "QEI.h"
 
 //ATTENTION:    set mBed to version 151
 //              set QEI to version 0, (gebruiken wij (nog) niet, is voor encoder)
@@ -14,19 +15,14 @@
 AnalogIn emg1_in            (A1);                   //Second raw EMG signal input
 AnalogIn emg2_in            (A2);                   //Third raw EMG signal input
 
-InterruptIn encoderA1       (D9);
-InterruptIn encoderB1       (D8);
-InterruptIn encoderA2       (D12);
-InterruptIn encoderB2       (D13);
-
 InterruptIn button1         (D10);                  
 InterruptIn button2         (D11);
 
-DigitalOut directionpin1    (D7);
-DigitalOut directionpin2    (D4);
+DigitalOut directionpin1    (D4);
+DigitalOut directionpin2    (D7);
 
-PwmOut pwmpin1              (D6);
-PwmOut pwmpin2              (D5);
+PwmOut pwmpin1              (D5);
+PwmOut pwmpin2              (D6);
 
 DigitalOut ledr             (LED_RED);
 DigitalOut ledb             (LED_BLUE);
@@ -34,11 +30,15 @@
 
 
 MODSERIAL pc(USBTX, USBRX);                       //Serial communication to see if the code works step by step, turn on if hidscope is off
+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      ticker;                          
+Ticker      func_tick; 
+Ticker      movag_tick;
+Ticker      emg_tick;                        
 
 //Global variables
 const float T   = 0.002f;                           //Ticker period             Deze wordt ook gebruikt in de PID, moet die niet anders???
@@ -79,224 +79,66 @@
 BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
 BiQuad notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );                //Notch filter
 
+//Global variables
+const float T   = 0.002f;                           //Ticker period             Deze wordt ook gebruikt in de PID, moet die niet anders???
+const float T2  = 0.002f;
+
 // Inverse Kinematica variables
 const double L1 = 0.208;                                  // Hoogte van tafel tot joint 1
-const double L2 = 0.288;                                  // Hoogte van tafel tot joint 2
+//const double L2 = 0.288;                                  // Hoogte van tafel tot joint 2
 const double L3 = 0.212;                                  // Lengte van de arm
 const double L4 = 0.089;                                  // Afstand van achterkant base tot joint 1
-const double L5 = 0.030;                                  // Afstand van joint 1 naar joint 2
+//const double L5 = 0.030;                                  // Afstand van joint 1 naar joint 2
 const double r_trans = 0.035;                             // Kan gebruikt worden om om te rekenen van translation naar shaft rotation 
 
 // Variërende variabelen inverse kinematics: 
-double q1ref = 0;                                   // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is
-double q2ref = 0;                                   // Huidige motorhoek van joint 2 zoals bepaald uit referentiesignaal --> checken of het goede type is
+double q1ref = 0.0;                                   // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is
+double q2ref = 0.0;                                   // Huidige motorhoek van joint 2 zoals bepaald uit referentiesignaal --> checken of het goede type is
 double v_x;                                         // Desired velocity end effector in x direction --> Determined by EMG signals
 double v_y;                                         // Desired velocity end effector in y direction --> Determined by EMG signals
 
 double Lq1;                                         // Translatieafstand als gevolg van motor rotation joint 1
 double Cq2;                                         // Joint angle of the system (corrected for gear ratio 1:5)
 
-double q1_dot;                                      // Benodigde hoeksnelheid van motor 1 om v_des te bereiken
-double q2_dot;                                      // Benodigde hoeksnelheid van motor 2 om v_des te bereiken 
+double q1_dot=0.0;                                      // Benodigde hoeksnelheid van motor 1 om v_des te bereiken
+double q2_dot=0.0;                                      // Benodigde hoeksnelheid van motor 2 om v_des te bereiken 
 
-double q1_ii;                                       // Reference signal for motorangle q1ref 
-double q2_ii;                                       // Reference signal for motorangle q2ref
+double q1_ii=0.0;                                       // Reference signal for motorangle q1ref 
+double q2_ii=0.0;                                       // Reference signal for motorangle q2ref
 
 //Variables PID controller
 double PI = 3.14159;
-double Kp1 = 17.5;                                  //Motor 1
+double Kp1 = 20.0;                                  //Motor 1           eerst 17.5 , nu 1
 double Ki1 = 1.02;
-double Kd1 = 23.2;
-double encoder1 = 0;
+double Kd1 = 1.0;
 double encoder_radians1=0;
+double err_integral1 = 0;
+double err_prev1, err_prev2;
+double err1, err2;
 
-double Kp2 = 17.5;                                  //Motor 2
+//BiQuad LowPassFilterDer1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+//BiQuad LowPassFilterDer2(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+BiQuad LowPassFilterDer1( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 );  //sample frequency 500 Hz, cutoff 20Hz low pass
+BiQuad LowPassFilterDer2( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 );
+
+
+double Kp2 = 20.0;                                  //Motor 2            eerst 17.5, nu 1
 double Ki2 = 1.02;
-double Kd2 = 23.2;
-double encoder2 = 0;
+double Kd2 = 1.0;
 double encoder_radians2=0;
+double err_integral2 = 0;
+double u1, u2;
 
-double start_control = 0;
+
+int start_control = 0;
+double emg_cal = 1;
 
 
 //--------------Functions----------------------------------------------------------------------------------------------------------------------------//
 
 
-//------------------ Encoder motor 1 --------------------------------//
-
-void encoderA1_rise()       
-{
-    if(encoderB1==false)
-    {
-        encoder1++;
-    }
-    else
-    {
-        encoder1--;
-    }
-}
-
-void encoderA1_fall()      
-{
-    if(encoderB1==true)
-    {
-        encoder1++;
-    }
-    else
-    {
-        encoder1--;
-    }
-}
-
-void encoderB1_rise()       
-{
-    if(encoderA1==true)
-    {
-        encoder1++;
-    }
-    else
-    {
-        encoder1--;
-    }
-}
-
-void encoderB1_fall()      
-{
-    if(encoderA1==false)
-    {
-        encoder1++;
-    }
-    else
-    {
-        encoder1--;
-    }
-}
-
-void encoder_count1()
-{
-    encoderA1.rise(&encoderA1_rise);
-    encoderA1.fall(&encoderA1_fall);
-    encoderB1.rise(&encoderB1_rise);
-    encoderB1.fall(&encoderB1_fall);
-}
-
-//------------------ Encoder motor 2 --------------------------------//
-
-void encoderA2_rise()       
-{
-    if(encoderB2==false)
-    {
-        encoder2++;
-    }
-    else
-    {
-        encoder2--;
-    }
-}
-
-void encoderA2_fall()      
-{
-    if(encoderB2==true)
-    {
-        encoder2++;
-    }
-    else
-    {
-        encoder2--;
-    }
-}
-
-void encoderB2_rise()       
-{
-    if(encoderA2==true)
-    {
-        encoder2++;
-    }
-    else
-    {
-        encoder2--;
-    }
-}
-
-void encoderB2_fall()      
-{
-    if(encoderA2==false)
-    {
-        encoder2++;
-    }
-    else
-    {
-        encoder2--;
-    }
-}
-
-void encoder_count2()
-{
-    encoderA2.rise(&encoderA2_rise);
-    encoderA2.fall(&encoderA2_fall);
-    encoderB2.rise(&encoderB2_rise);
-    encoderB2.fall(&encoderB2_fall);
-}
-
 //------------------ Filter EMG + Calibration EMG --------------------------------//
 
-void EMGFilter0()
-{   
-    emg0_raw      = emg0_in.read();                      //give name to raw EMG0 data calve
-    emg0_filt_x   = emg0filter.step(emg0_raw);           //Use biquad chain to filter raw EMG data
-    emg0_filt     = abs(emg0_filt_x);                    //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch, stel er komt iets raars uit, dan Notch uit de biquad chain halen en aparte chain voor aanmaken.
-}
-
-void EMGFilter1()
-{
-    emg1_raw      = emg1_in.read();                      //give name to raw EMG1 data bicep 1
-    emg1_filt_x   = emg1filter.step(emg1_raw);           //Use biquad chain to filter raw EMG data
-    emg1_filt     = abs(emg1_filt_x);                    //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch.
-}
-
-void EMGFilter2()
-{
-    emg2_raw      = emg2_in.read();                      //Give name to raw EMG1 data bicep 2
-    emg2_filt_x   = emg2filter.step(emg2_raw);           //Use biquad chain to filter raw EMG data
-    emg2_filt     = abs(emg2_filt_x);                    //Rectifier. LET OP: volgorde filter: band-notch-rectifier.
-}
- 
-void MovAg()                                         //Calculate moving average (MovAg)
-{
-    for (int i = windowsize-1; i>=0; i--)            //Make arrays for the last datapoints of the filtered signals
-    {
-        StoreArray0[i] = StoreArray0[i-1];           //Shifts the i'th element one place to the right, this makes it "rolling or moving" average.
-        StoreArray1[i] = StoreArray1[i-1];
-        StoreArray2[i] = StoreArray2[i-1];
-    }
-    
-    StoreArray0[0] = emg0_filt;                      //Stores the latest datapoint of the filtered signal in the first element of the array
-    StoreArray1[0] = emg1_filt;
-    StoreArray2[0] = emg2_filt;
-    
-    sum1 = 0.0;
-    sum2 = 0.0;
-    sum3 = 0.0;
-    
-    for(int a = 0; a<= windowsize-1; a++)            //Sums the elements in the arrays
-    {
-        sum1 += StoreArray0[a];
-        sum2 += StoreArray1[a];
-        sum3 += StoreArray2[a];
-    }
-    
-    movAg0 = sum1/windowsize;                        //calculates an average in the array
-    movAg1 = sum2/windowsize;
-    movAg2 = sum3/windowsize;
-}
-
-void emg_filtered()             //Call all filter functions
-{
-    EMGFilter0();
-    EMGFilter1();
-    EMGFilter2();
-    MovAg();
-}
 void switch_to_calibrate()
 {
     x++;                        //Every time function gets called, x increases. Every button press --> new calibration state.
@@ -391,6 +233,65 @@
         }
     }
 }
+
+void EMGFilter0()
+{   
+    emg0_raw      = emg0_in.read();                      //give name to raw EMG0 data
+    emg0_filt_x   = emg0filter.step(emg0_raw);           //Use biquad chain to filter raw EMG data
+    emg0_filt     = abs(emg0_filt_x);                    //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch, stel er komt iets raars uit, dan Notch uit de biquad chain halen en aparte chain voor aanmaken.
+}
+
+void EMGFilter1()
+{
+    emg1_raw      = emg1_in.read();                      //give name to raw EMG1 data
+    emg1_filt_x   = emg1filter.step(emg1_raw);           //Use biquad chain to filter raw EMG data
+    emg1_filt     = abs(emg1_filt_x);                    //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch.
+}
+
+void EMGFilter2()
+{
+    emg2_raw      = emg2_in.read();                      //Give name to raw EMG1 data
+    emg2_filt_x   = emg2filter.step(emg2_raw);           //Use biquad chain to filter raw EMG data
+    emg2_filt     = abs(emg2_filt_x);                    //Rectifier. LET OP: volgorde filter: band-notch-rectifier.
+}
+ 
+void MovAg()                                         //Calculate moving average (MovAg), klopt nog niet!!
+{
+    for (int i = windowsize-1; i>=0; i--)            //Make arrays for the last datapoints of the filtered signals
+    {
+        StoreArray0[i] = StoreArray0[i-1];           //Shifts the i'th element one place to the right, this makes it "rolling or moving" average.
+        StoreArray1[i] = StoreArray1[i-1];
+        StoreArray2[i] = StoreArray2[i-1];
+    }
+    
+    StoreArray0[0] = emg0_filt;                      //Stores the latest datapoint of the filtered signal in the first element of the array
+    StoreArray1[0] = emg1_filt;
+    StoreArray2[0] = emg2_filt;
+    
+    sum1 = 0.0;
+    sum2 = 0.0;
+    sum3 = 0.0;
+    
+    for(int a = 0; a<= windowsize-1; a++)            //Sums the elements in the arrays
+    {
+        sum1 += StoreArray0[a];
+        sum2 += StoreArray1[a];
+        sum3 += StoreArray2[a];
+    }
+    
+    movAg0 = sum1/windowsize;                        //calculates an average in the array
+    movAg1 = sum2/windowsize;
+    movAg2 = sum3/windowsize;
+    //serial getallen sturen, als het 1 getal is gaat hier wat fout, als het een reeks is dan gaat er bij de input naar HIDscope wat fout.
+}
+
+void emg_filtered()             //Call all filter functions
+{
+    EMGFilter0();
+    EMGFilter1();
+    EMGFilter2();
+}
+
 /*
 void HIDScope_sample()
 {    
@@ -408,39 +309,142 @@
 }
 */
 
+
+//---------PID controller 1 + 2 + motor control 1 & 2-----------------------------------------------------------//
+void PID_control1()
+{
+    //pc.printf("ik doe het, PDI \n\r");
+
+    // Proportional part:
+    double u_k1 = Kp1 * err1;
+
+    //Integral part  
+      err_integral1 = err_integral1 + err1 * T;
+      double u_i1 = Ki1 * err_integral1;
+    
+    // Derivative part
+      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
+      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 = (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);
+        
+        //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;
+           // }  
+}
+
+void engine_control2()                                             //Engine 2 is rotational engine, connected with right side wires
+{
+        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                
+        // {
+            pwmpin2 = fabs(u2);                                       //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
+            directionpin2.write(u2>0);
+        // }
+        //else
+        // {
+        //    pwmpin2 = 0;
+        // }
+}
+
 //------------------ Inversed Kinematics --------------------------------//
 
 void inverse_kinematics()
 {
+    
+    //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*T;                       
-    q2_ii = q2ref + q2_dot*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; 
     
-    start_control = 1;
+    
+    //pc.printf("q1ref is %f en q2ref is %f\n\r",q1ref, q2ref);
+
+    
+    //start_control = 1;
+    engine_control1();                           
+    engine_control2();
 }
 
 void v_des_calculate_qref()
 {
-    if(emg_cal==1)                                   //After calibration is finished, emg_cal will be 1. Otherwise 0. 
+    while(emg_cal==1)                                   //After calibration is finished, emg_cal will be 1. Otherwise 0. 
     { 
                 if(movAg1>Threshold1)                   //If the filtered EMG signal of muscle 1 is higher than the threshold, motor 1 will turn
                 {
-                    v_x = 1.0;                          //beweging in +x direction
+                    v_x = 0.5;                          //beweging in +x direction
+                    v_y = 0.0;
+                    
                     ledr = 0;                           //red
                     ledb = 1;
                     ledg = 1;
                 }
                 else if(movAg2>Threshold2)              //If the filtered EMG signal of muscle 2 is higher than the threshold, motor 1 and 2 will turn
                 {
-                    v_y = 1.0;                          //beweging in +y direction
+                    v_y = 0.5;                          //beweging in +y direction
+                    v_x = 0.0;
+                    
                     ledr = 1;                           //green
                     ledb = 1;
                     ledg = 0;
@@ -450,14 +454,17 @@
                 {
                     v_x = -v_x;
                     v_y = -v_y;
+                    
                     ledr = 1;                           //Blue
                     ledb = 0;
                     ledg = 1;  
                 }             
+                
                 else                                    //If not higher than the threshold, motors will not turn at all
                 {                    
                     v_x = 0;
                     v_y = 0;
+                    
                     ledr = 0;                           //white
                     ledb = 0;
                     ledg = 0;
@@ -465,113 +472,11 @@
                 
         inverse_kinematics();                           //Call inverse kinematics function
         
+        break;
         }
 }
 
-//---------PID controller motor 1 + start motor 1 -----------------------------------------------------------//
-double PID_controller1(double err1)
-{
-  static double err_integral1 = 0;
-  static double err_prev1 = err1; // initialization with this value only done once!
-  
-  static BiQuad LowPassFilter1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
 
-  // Proportional part:
-  double u_k1 = Kp1 * err1;
-
-  // Integral part  
-  err_integral1 = err_integral1 + err1 * T;
-  double u_i1 = Ki1 * err_integral1;
-
-  // Derivative part
-  double err_derivative1 = (err1 - err_prev1)/T;
-  double filtered_err_derivative1 = LowPassFilter1.step(err_derivative1);
-  double u_d1 = Kd1 * filtered_err_derivative1;
-  err_prev1 = err1;
-
-  // Sum all parts and return it
-  return u_k1 + u_i1 + u_d1;  
-}
-
-void start_your_engines1(double u1)
-{
-    if(encoder1<5250 && encoder1>-5250)                              //limits rotation, in counts                
-    {
-         pwmpin1 = fabs(u1);                                         //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
-         directionpin1.write(u1 < 0.0f);
-    }
-    else
-    {
-        pwmpin1 = 0;
-    }
-}  
-
-void engine_control1()                                           //Engine 1 is rotational engine, connected with left side pins
-{
-    while(start_control == 1)
-    {
-        encoder_radians1 = encoder1*(2*PI)/8400;
-        double err1 = q1ref - encoder_radians1;
-        double u1 = PID_controller1(err1);                               //PID controller function call
-        start_your_engines1(u1);                                         //Call start_your_engines function
-        
-        break;
-    }
-}
-
-
-
-//---------PID controller motor 1 + start motor 1 -----------------------------------------------------------//
-double PID_controller2(double err2)
-{
-  static double err_integral2 = 0;
-  static double err_prev2 = err2; // initialization with this value only done once!
-  
-  static BiQuad LowPassFilter2(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
-
-  // 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 = LowPassFilter2.step(err_derivative2);
-  double u_d2 = Kd2 * filtered_err_derivative2;
-  err_prev2 = err2;
-
-  // Sum all parts and return it
-  return u_k2 + u_i2 + u_d2;  
-}
-
-void start_your_engines2(double u2)
-{
-     if(encoder2<12600 && encoder2>-1)                              //limits translation in counts
-     {
-        pwmpin2 = fabs(u2);                                       //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
-        directionpin2.write(u2 < 0.0f);
-     }
-    else
-     {
-        pwmpin2 = 0;
-     }
-    
-}  
-
-void engine_control2()                                             //Engine 2 is translational engine, connected with right side wires
-{
-    while(start_control == 1)
-    {
-        encoder_radians2 = encoder2*(2*PI)/8400;
-        double err2 = q2ref - encoder_radians2;
-        double u2 = PID_controller2(err2);                             //PID controller function call
-        start_your_engines2(u2);                                       //Call start_your_engines function
-        
-        break;
-    }
-}
 
 //------------------ Start main function --------------------------//
 
@@ -586,18 +491,21 @@
         emg1filter.add( &emg1band1 ).add( &emg1band2 ).add( &emg1band3 ).add( &notch2 );
         emg2filter.add( &emg2band1 ).add( &emg2band2 ).add( &emg2band3 ).add( &notch3 );
         
+        emg_tick.attach(&emg_filtered,T);                         //EMG signals filtered + moving average every T sec.
+        movag_tick.attach(&MovAg,T);
+        func_tick.attach(&v_des_calculate_qref,T);                 //v_des determined every T
+        engine_control1_tick.attach(&engine_control1,T);
+        engine_control2_tick.attach(&engine_control2,T);
+        
+           // HIDScope_tick.attach(&HIDScope_sample,T);             //EMG signals raw + filtered to HIDScope every T sec.
+        
+        button1.rise(switch_to_calibrate);                      //Switch state of calibration (which muscle)
+        //wait(0.2f);                                             //Wait to avoid bouncing of button
+        button2.rise(calibrate);                                //Calibrate threshold for 3 muscles
+        //wait(0.2f);                                             //Wait to avoid bouncing of button
+        
     while(true)
     {
-        ticker.attach(&emg_filtered,T);                         //EMG signals filtered + moving average every T sec.
-        ticker.attach(&v_des_calculate_qref,T);                 //v_des determined every T
-              
-       // HIDScope_tick.attach(&HIDScope_sample,T);             //EMG signals raw + filtered to HIDScope every T sec.
-        
-        button1.rise(switch_to_calibrate);                      //Switch state of calibration (which muscle)
-        wait(0.2f);                                             //Wait to avoid bouncing of button
-        button2.rise(calibrate);                                //Calibrate threshold for 3 muscles
-        wait(0.2f);                                             //Wait to avoid bouncing of button
-    
         pc.printf("x is %i\n\r",x);
         pc.printf("Movag0 = %f , Movag1 = %f, Movag2 = %f \n\r",movAg0, movAg1, movAg2);
         pc.printf("Thresh0 = %f , Thresh1 = %f, Thresh2 = %f \n\r",Threshold0, Threshold1, Threshold2);