Diff: main.cpp

Revision:

27:3392f03bfada

Parent:

26:cd1db85359aa

Child:

28:a40884792e0a

--- a/main.cpp	Mon Oct 05 16:58:58 2015 +0000
+++ b/main.cpp	Thu Oct 08 18:10:26 2015 +0000
@@ -28,12 +28,14 @@
     DigitalOut motor2direction(D4);         // Motor 2, Direction & Speed
     PwmOut motor2speed(D5);
 
+//EMG
+    AnalogIn    EMG_left(A0);               //Analog input
+    AnalogIn    EMG_right(A1);   
 // Tickers
     Ticker ScopeTime;
     Ticker myControllerTicker2;
     Ticker myControllerTicker1;
-    Ticker myEMG1;
-    Ticker myEMG2;
+    Ticker SampleEMG;
 
 // Constants
     double reference2, reference1;
@@ -42,7 +44,19 @@
     int count = 0;
     double Grens2 = 90, Grens1 = 90;
     double Stapgrootte = 5;
-    double Threshold;
+    
+    double EMG_L_f_v1 = 0, EMG_L_f_v2 = 0;
+    double EMG_L_fh=0;
+    double EMG_left_value;
+    double EMG_left_f1;
+    double EMG_left_f2;
+    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; 
 
 //Sample time (motor-step)
     const double m2_Ts = 0.01, m1_Ts = 0.01;
@@ -57,10 +71,19 @@
     const double BiGain2 = 0.012, BiGain1 = 0.016955;
     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
 
 // 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
 
 //--------------------------------------------------------------------------------------------------------------------------//
 // General Functions
@@ -105,7 +128,19 @@
 //EMG functions
 //--------------------------------------------------------------------------------------------------------------------------//
 
-// Hier komen functies die de EMG signalen uitlezen en filteren
+// EMG filtering function
+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_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);
+}
 
 //--------------------------------------------------------------------------------------------------------------------------//
 // Motor control functions
@@ -174,7 +209,7 @@
         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
-        // 2 Tickers die de waarden van de EMG-signalen uitlezen
+        SampleEMG.attach(&EMGfilter, 0.01f);                    // 100Hz, EMG signalen
     
 //--------------------------------------------------------------------------------------------------------------------------//
 // Control Program
@@ -183,7 +218,7 @@
     {
         char c = pc.getc();
     // 1 Program UP
-        if(c == 'e') // If ((EMG1 => Threshold) && (EMG2 => Threshold))
+        if ((EMG_right_abs >= Threshold1) && (EMG_left_abs >= Threshold1)) //if(c == 'e') 
         {
             count = count + 1;
             if(count > 2)
@@ -207,7 +242,7 @@
                 
                 LedR = LedB = 1;
                 LedG = 0;
-                if(c == 'r') // if ((EMG1 => Threshold) && (EMG2 =< Threshold))
+                if ((EMG_right_abs >= Threshold1) && (EMG_left_abs <= Threshold1)) //if(c == 'r')
                 {
                     m2_ref = m2_ref + Stapgrootte;
                     m1_ref = m1_ref - Stapgrootte;
@@ -217,7 +252,7 @@
                         m1_ref = -1*Grens1;
                     }
                 }
-                if(c == 'f') // if ((EMG2 => Threshold) && (EMG1 =< Threshold))
+                if ((EMG_left_abs > Threshold1) && (EMG_right_abs < Threshold1)) //if(c == 'f') 
                 {
                     m2_ref = m2_ref - Stapgrootte;
                     m1_ref = m1_ref + Stapgrootte;
@@ -233,7 +268,7 @@
         {
                 LedG = LedB = 1;
                 LedR = 0;
-                if(c == 't') // if ((EMG1 => Threshold) && (EMG2 =< Threshold))
+                if ((EMG_right_abs >= Threshold1) && (EMG_left_abs <= Threshold1)) //if(c == 't') 
                 {
                     m1_ref = m1_ref + Stapgrootte;
                     if (m1_ref > Grens1)
@@ -241,7 +276,7 @@
                         m1_ref = Grens1;
                     }
                 }
-                if(c == 'g') // if ((EMG1 => Threshold) && (EMG2 =< Threshold))
+                if ((EMG_left_abs > Threshold1) && (EMG_right_abs < Threshold1)) //if(c == 'g')
                 {
                     m1_ref = m1_ref - Stapgrootte;
                     if (m1_ref < -1*Grens1)