Diff: main.cpp

Revision:

28:a40884792e0a

Parent:

27:3392f03bfada

Child:

29:c34c2c3d30a9

--- a/main.cpp	Thu Oct 08 18:10:26 2015 +0000
+++ b/main.cpp	Mon Oct 12 16:53:25 2015 +0000
@@ -15,8 +15,9 @@
     MODSERIAL pc(USBTX, USBRX);             // To/From PC
     QEI Encoder2(D3, D2, NC, 32);           // Encoder Motor 2
     QEI Encoder1(D13,D12,NC, 32);           // Encoder Motor 1
-    HIDScope scope(4);                      // Scope, 4 channels
-
+    HIDScope scope(5);                      // Scope, 4 channels
+    DigitalIn Button1(SW3);
+    DigitalIn Button2(SW2);
 // LEDs
     DigitalOut LedR(LED_RED);
     DigitalOut LedG(LED_GREEN);
@@ -31,11 +32,15 @@
 //EMG
     AnalogIn    EMG_left(A0);               //Analog input
     AnalogIn    EMG_right(A1);   
+
 // Tickers
-    Ticker ScopeTime;
-    Ticker myControllerTicker2;
-    Ticker myControllerTicker1;
-    Ticker SampleEMG;
+    Ticker      ScopeTime;
+    Ticker      myControllerTicker2;
+    Ticker      myControllerTicker1;
+    Ticker      SampleEMG;
+    Ticker      ScopeTimer;
+    Ticker      serial;
+    Ticker      MovingAverage;
 
 // Constants
     double reference2, reference1;
@@ -50,13 +55,21 @@
     double EMG_left_value;
     double EMG_left_f1;
     double EMG_left_f2;
+    double EMG_left_f3;
+    double EMG_left_f4;
+    double EMG_left_f5;
+    double EMG_left_f6;
     double EMG_left_abs;
     double EMG_right_value;
     double EMG_right_f1;
     double EMG_right_f2;
     double EMG_right_abs;
-    double Threshold1 = 0.08;
-    double Threshold2 = 0.06; 
+    double Threshold1 = 0.02;
+    double Threshold2 = 0.06;
+    
+    int N = 50;
+    double MAF_left[50];
+    double EMG_left_MAF;
 
 //Sample time (motor-step)
     const double m2_Ts = 0.01, m1_Ts = 0.01;
@@ -72,19 +85,31 @@
     const double m2_f_a1 = -0.96608908283*BiGain2, m2_f_a2 = 0.0*BiGain2, m2_f_b0 = 1.0*BiGain2, m2_f_b1 = 1.0*BiGain2, m2_f_b2 = 0.0*BiGain2;
     const double m1_f_a1 = -0.96608908283*BiGain1, m1_f_a2 = 0.0*BiGain1, m1_f_b0 = 1.0*BiGain1, m1_f_b1 = 1.0*BiGain1, m1_f_b2 = 0.0*BiGain1;
     
-//Filter coeffs for EMG
-    const double BiGainEMG_Lh = 0.723601, BiGainEMG_Ll=0.959332;
-    const double EMGh_a1 = -1.74355513773*BiGainEMG_Lh, EMGh_a2 = 0.80079826172*BiGainEMG_Lh, EMGh_b0 = 1.0*BiGainEMG_Lh, EMGh_b1 = 1.99999965990*BiGainEMG_Lh, EMGh_b2 = 1.0*BiGainEMG_Lh; //coefficients for high-pass filter
-    const double EMGl_a1 = 1.91721405106*BiGainEMG_Ll, EMGl_a2 = 0.92055427516*BiGainEMG_Ll, EMGl_b0 = 1.0*BiGainEMG_Ll, EMGl_b1 = 1.99999993582*BiGainEMG_Ll, EMGl_b2 = 1.0*BiGainEMG_Ll; // coefficients for low-pass filter
+// coëfficiënten
+    const double BiGainEMG_H1 = 0.795375, BiGainEMG_H2 = 0.895763;
+    const double EMGH1_a1 = -1.56308931068*BiGainEMG_H1, EMGH1_a2 = 0.61765749583*BiGainEMG_H1, EMGH1_b0 = 1.0*BiGainEMG_H1, EMGH1_b1 = -1.99909075151*BiGainEMG_H1, EMGH1_b2 = 1.0*BiGainEMG_H1; //coefficients for high-pass filter
+    const double EMGH2_a1 = -1.75651417587*BiGainEMG_H2, EMGH2_a2 = 0.82183182692*BiGainEMG_H2, EMGH2_b0 = 1.0*BiGainEMG_H2, EMGH2_b1 = -1.99470632157*BiGainEMG_H2, EMGH2_b2 = 1.0*BiGainEMG_H2; //coefficients for high-pass filter
+    
+    const double BiGainEMG_L1=0.959332, BiGainEMG_L2 = 0.223396;
+    const double EMGL1_a1 = -1.55576653052*BiGainEMG_L1, EMGL1_a2 = 0.61374320375*BiGainEMG_L1, EMGL1_b0 = 1.0*BiGainEMG_L1, EMGL1_b1 = -0.90928276835*BiGainEMG_L1, EMGL1_b2 = 1.0*BiGainEMG_L1; // coefficients for low-pass filter
+    const double EMGL2_a1 = -1.79696141922*BiGainEMG_L2, EMGL2_a2 = 0.85096669383*BiGainEMG_L2, EMGL2_b0 = 1.0*BiGainEMG_L2, EMGL2_b1 = -1.75825311060*BiGainEMG_L2, EMGL2_b2 = 1.0*BiGainEMG_L2; // coefficients for low-pass filter
 
+    const double BiGainEMG_N1 = 1.0, BiGainEMG_N2 = 0.965081;
+    const double EMGN1_a1 = -1.56858163035*BiGainEMG_N1, EMGN1_a2 = 0.96424138362*BiGainEMG_N1, EMGN1_b0 = 1.0*BiGainEMG_N1, EMGN1_b1 = -1.61854514265*BiGainEMG_N1, EMGN1_b2 = 1.0*BiGainEMG_N1; //coefficients for high-pass filter
+    const double EMGN2_a1 = -1.61100357722*BiGainEMG_N2, EMGN2_a2 = 0.96592170538*BiGainEMG_N2, EMGN2_b0 = 1.0*BiGainEMG_N2, EMGN2_b1 = -1.61854514265*BiGainEMG_N2, EMGN2_b2 = 1.0*BiGainEMG_N2; //coefficients for high-pass filter
+   
 // Filter variables
     double m2_f_v1 = 0, m2_f_v2 = 0;
     double m1_f_v1 = 0, m1_f_v2 = 0;
     
 // Creating the filters
-    biquadFilter EMG_highpass (EMGh_a1, EMGh_a2, EMGh_b0, EMGh_b1, EMGh_b2);        // creates the high pass filter for EMG
-    biquadFilter EMG_lowpass (EMGl_a1, EMGl_a2, EMGl_b0, EMGl_b1, EMGl_b2);         // creates the low pass filter for EMG
-
+    biquadFilter EMG_highpass1 (EMGH1_a1, EMGH1_a2, EMGH1_b0, EMGH1_b1, EMGH1_b2);        // creates the high pass filter
+    biquadFilter EMG_highpass2 (EMGH2_a1, EMGH2_a2, EMGH2_b0, EMGH2_b1, EMGH2_b2);
+    biquadFilter EMG_lowpass1 (EMGL1_a1, EMGL1_a2, EMGL1_b0, EMGL1_b1, EMGL1_b2);         // creates the low pass filter
+    biquadFilter EMG_lowpass2 (EMGL2_a1, EMGL2_a2, EMGL2_b0, EMGL2_b1, EMGL2_b2);  
+    biquadFilter EMG_notch1 (EMGN1_a1, EMGN1_a2, EMGN1_b0, EMGN1_b1, EMGN1_b2);           // creates the notch filter
+    biquadFilter EMG_notch2 (EMGN2_a1, EMGN2_a2, EMGN2_b0, EMGN2_b1, EMGN2_b2);
+    
 //--------------------------------------------------------------------------------------------------------------------------//
 // General Functions
 //--------------------------------------------------------------------------------------------------------------------------//
@@ -96,6 +121,7 @@
         scope.set(1, position2);
         scope.set(2, reference1 - position1);    
         scope.set(3, position1);
+        scope.set(4, EMG_left_MAF);
         scope.send();
     
     }
@@ -124,6 +150,61 @@
         return Kp * e + Ki*e_int + Kd*e_der;
     }
     
+// Movingaverage Filter
+    void MovingAverageFilter()
+    {
+        EMG_left_MAF = (MAF_left[0]+MAF_left[1]+MAF_left[2]+MAF_left[3]+MAF_left[4]+MAF_left[5]+MAF_left[6]+MAF_left[7]+MAF_left[8]+MAF_left[9]+MAF_left[10]+MAF_left[11]+MAF_left[12]+MAF_left[13]+MAF_left[14]+MAF_left[15]+MAF_left[16]+MAF_left[17]+MAF_left[18]+MAF_left[19]+MAF_left[20]+MAF_left[21]+MAF_left[22]+MAF_left[23]+MAF_left[24]+MAF_left[25]+MAF_left[26]+MAF_left[27]+MAF_left[28]+MAF_left[29]+MAF_left[30]+MAF_left[31]+MAF_left[32]+MAF_left[33]+MAF_left[34]+MAF_left[35]+MAF_left[36]+MAF_left[37]+MAF_left[38]+MAF_left[39]+MAF_left[40]+MAF_left[41]+MAF_left[42]+MAF_left[43]+MAF_left[44]+MAF_left[45]+MAF_left[46]+MAF_left[47]+MAF_left[48]+MAF_left[49])/N;
+        MAF_left[49] = MAF_left[48];
+        MAF_left[48] = MAF_left[47];
+        MAF_left[47] = MAF_left[46];
+        MAF_left[46] = MAF_left[45];
+        MAF_left[45] = MAF_left[44];
+        MAF_left[44] = MAF_left[43];
+        MAF_left[43] = MAF_left[42];
+        MAF_left[42] = MAF_left[41];
+        MAF_left[41] = MAF_left[40];
+        MAF_left[40] = MAF_left[39];
+        MAF_left[39] = MAF_left[38];
+        MAF_left[38] = MAF_left[37];
+        MAF_left[37] = MAF_left[36];
+        MAF_left[36] = MAF_left[35];
+        MAF_left[35] = MAF_left[34];
+        MAF_left[34] = MAF_left[33];
+        MAF_left[33] = MAF_left[32];
+        MAF_left[32] = MAF_left[31];
+        MAF_left[31] = MAF_left[30];
+        MAF_left[30] = MAF_left[29];
+        MAF_left[29] = MAF_left[28];
+        MAF_left[28] = MAF_left[27];
+        MAF_left[27] = MAF_left[26];
+        MAF_left[26] = MAF_left[25];
+        MAF_left[25] = MAF_left[24];
+        MAF_left[24] = MAF_left[23];
+        MAF_left[23] = MAF_left[22];
+        MAF_left[22] = MAF_left[21];
+        MAF_left[21] = MAF_left[20];
+        MAF_left[20] = MAF_left[19];
+        MAF_left[19] = MAF_left[18];
+        MAF_left[18] = MAF_left[17];
+        MAF_left[17] = MAF_left[16];
+        MAF_left[16] = MAF_left[15];
+        MAF_left[15] = MAF_left[14];
+        MAF_left[14] = MAF_left[13];
+        MAF_left[13] = MAF_left[12];
+        MAF_left[12] = MAF_left[11];
+        MAF_left[11] = MAF_left[10];
+        MAF_left[10] = MAF_left[9];
+        MAF_left[9] = MAF_left[8];
+        MAF_left[8] = MAF_left[7];
+        MAF_left[7] = MAF_left[6];
+        MAF_left[6] = MAF_left[5];
+        MAF_left[5] = MAF_left[4];
+        MAF_left[4] = MAF_left[3];
+        MAF_left[3] = MAF_left[2];
+        MAF_left[2] = MAF_left[1];
+        MAF_left[1] = MAF_left[0];
+        MAF_left[0] = EMG_left_f6;
+    }
 //--------------------------------------------------------------------------------------------------------------------------//
 //EMG functions
 //--------------------------------------------------------------------------------------------------------------------------//
@@ -132,14 +213,18 @@
 void EMGfilter() // Both EMG signals are filtered in one function and with the same filters
 {
     EMG_left_value = EMG_left.read();
-    EMG_left_f1 = EMG_highpass.step(EMG_left_value);
-    EMG_left_f2 = EMG_lowpass.step(EMG_left_f1);
+    EMG_left_f1 = EMG_highpass1.step(EMG_left_value);
+    EMG_left_f2 = EMG_highpass2.step(EMG_left_f1);
     EMG_left_abs = fabs(EMG_left_f2);
-    
-    EMG_right_value = EMG_right.read();
-    EMG_right_f1 = EMG_highpass.step(EMG_right_value);
-    EMG_right_f1 = EMG_lowpass.step(EMG_right_f1);
-    EMG_right_abs = fabs(EMG_right_f2);
+    EMG_left_f3 = EMG_lowpass1.step(EMG_left_abs);
+    EMG_left_f4 = EMG_lowpass2.step(EMG_left_f3);
+    EMG_left_f5 = EMG_notch1.step(EMG_left_f4);
+    EMG_left_f6 = EMG_notch1.step(EMG_left_f5);
+        
+//    EMG_right_value = EMG_right.read();
+//    EMG_right_f1 = EMG_highpass.step(EMG_right_value);
+//    EMG_right_f1 = EMG_lowpass.step(EMG_right_f1);
+//    EMG_right_abs = fabs(EMG_right_f2);
 }
 
 //--------------------------------------------------------------------------------------------------------------------------//
@@ -209,8 +294,8 @@
         ScopeTime.attach(&ScopeSend, 0.01f);                    // 100 Hz, Scope
         myControllerTicker2.attach(&motor2_Controller, 0.01f ); // 100 Hz, Motor 2
         myControllerTicker1.attach(&motor1_Controller, 0.01f ); // 100 Hz, Motor 1
-        SampleEMG.attach(&EMGfilter, 0.01f);                    // 100Hz, EMG signalen
-    
+        SampleEMG.attach(&EMGfilter, 0.01f);
+        MovingAverage.attach(&MovingAverageFilter, 0.01f);
 //--------------------------------------------------------------------------------------------------------------------------//
 // Control Program
 //--------------------------------------------------------------------------------------------------------------------------//
@@ -218,7 +303,7 @@
     {
         char c = pc.getc();
     // 1 Program UP
-        if ((EMG_right_abs >= Threshold1) && (EMG_left_abs >= Threshold1)) //if(c == 'e') 
+       if(c == 'e') //if ((EMG_right_abs >= Threshold1) && (EMG_left_abs >= Threshold1)) 
         {
             count = count + 1;
             if(count > 2)
@@ -242,7 +327,7 @@
                 
                 LedR = LedB = 1;
                 LedG = 0;
-                if ((EMG_right_abs >= Threshold1) && (EMG_left_abs <= Threshold1)) //if(c == 'r')
+                if(c == 'r') //if ((EMG_right_abs >= Threshold1) && (EMG_left_abs <= Threshold1)) //
                 {
                     m2_ref = m2_ref + Stapgrootte;
                     m1_ref = m1_ref - Stapgrootte;
@@ -252,7 +337,7 @@
                         m1_ref = -1*Grens1;
                     }
                 }
-                if ((EMG_left_abs > Threshold1) && (EMG_right_abs < Threshold1)) //if(c == 'f') 
+                if (c == 'f') //&& (EMG_right_abs < Threshold1))  
                 {
                     m2_ref = m2_ref - Stapgrootte;
                     m1_ref = m1_ref + Stapgrootte;
@@ -268,7 +353,7 @@
         {
                 LedG = LedB = 1;
                 LedR = 0;
-                if ((EMG_right_abs >= Threshold1) && (EMG_left_abs <= Threshold1)) //if(c == 't') 
+                if(c == 't') //if ((EMG_right_abs >= Threshold1) && (EMG_left_abs <= Threshold1)) // 
                 {
                     m1_ref = m1_ref + Stapgrootte;
                     if (m1_ref > Grens1)
@@ -276,7 +361,7 @@
                         m1_ref = Grens1;
                     }
                 }
-                if ((EMG_left_abs > Threshold1) && (EMG_right_abs < Threshold1)) //if(c == 'g')
+                if(c == 'g') //if ((EMG_left_abs > Threshold1) && (EMG_right_abs < Threshold1)) 
                 {
                     m1_ref = m1_ref - Stapgrootte;
                     if (m1_ref < -1*Grens1)