Revision 71:a79a88ca4b48, committed 2015-10-19

Comitter:

bjornnijhuis

Date:

Mon Oct 19 12:34:11 2015 +0000

Parent:

70:20e1a73ab134

Child:

72:507d6e76988a

Commit message:

Included EMG processing

Changed in this revision

--- a/EMG.cpp	Mon Oct 19 12:16:35 2015 +0000
+++ b/EMG.cpp	Mon Oct 19 12:34:11 2015 +0000
@@ -1,7 +1,374 @@
-#include "EMG.h"
 #include "mbed.h"
-// functions for reading and writing EMG data
+#include "HIDScope.h"
+#include "MODSERIAL.h"
+
+// Define objects
+AnalogIn    emg1(A0);                   // Analog input 1
+AnalogIn    emg2(A1);                   // Analog input 2
+DigitalOut  ledred(LED_RED);            // Red led
+DigitalOut  ledgreen(LED_GREEN);        // Green led
+DigitalOut  ledblue(LED_BLUE);          // Blue led
+Ticker      sample_tick;                // Ticker for sampling
+Ticker      output;                     // Ticker for PC output
+HIDScope    scope(6);                   // Number of scopes
+Timer       normalizing_timer;          // Timer for normalizing
+Timer       EMG_timer;                  // Timer for switch statement
+MODSERIAL   pc(USBTX, USBRX);           // Ports for communication to pc
+
+// Define program constants
+const int     on = 0;                   // On-constant for LEDs for program readability
+const int     off = 1;                  // Off-constant for LEDs for program readability
+const int     sample = 0;               // Constant for mode switching for program readability
+const int     normalize = 1;            // Constant for mode switching for program readability
+volatile bool emg_go = false;
+const int baudrate = 115200;            // Baudrate for serial connection
+
+
+//**********************************************************************************************
+bool mode               = normalize;         // Set program mode
+//**********************************************************************************************
+
+// Define sampling constants
+const double sample_frequency = 200;         // Sample frequency [Hz]
+double emg_val1 = 0, emg_val2 = 0, emg_filt_val1 = 0, emg_filt_val2 = 0;
+
+// Define normalizing parameters
+const int     normalize_time    = 2000;      // Normalizing time [ms]
+const int     normalize_wait    = 4000;      // Normalizing wait time [ms]
+double        max_vol_cont1     = 0;         // Maximum voluntary contraction for scaling EMG1
+double        max_vol_cont2     = 0;         // Maximum voluntary contraction for scaling EMG2
+int           channel           = 1;         // Channel for normalizing (EMG1 or EMG2)
+
+// Define movement parameters
+int         DOF = 1;                         // Switch variable for controlled DOF: 1=x 2=y 3=z
+bool        pump = false;                    // Pump switch
+bool        thr_pass1 = false;               // Processing threshold passed for signal 1?
+bool        thr_pass2 = false;               // Processing threshold passed for signal 2?
+double      velocity = 0;                    // Forward velocity
+double      x_velocity = 0;                  // x component for velocity
+double      y_velocity = 0;                  // y component for velocity
+double      z_velocity = 0;                  // z component for velocity
+
+// Define thresholds
+const double UPT = 0.35;                    // Upper Treshold Value     (0-1)
+const double LPT = 0.2;                     // Lower Treshold Vvalue     (0-1)
+const double LST = 100;                     // Lower Switch Time        [ms]
+const double UST = 500;                     // Upper Switch Time        [ms]
+
+// Define filter constants
+double f11_v1=0, f11_v2=0, f12_v1=0, f12_v2=0, f13_v1=0, f13_v2=0;
+double f11_v1b=0, f11_v2b=0, f12_v1b=0, f12_v2b=0, f13_v1b=0, f13_v2b=0;
+const double f11_gain = 0.907616, f11_a1 = 0.00000000267,  f11_a2 = 0.77693864001, f11_b0 = 1, f11_b1 = 0.00000000301, f11_b2 = 1; // Constants for pre-filter BiQuad: Bandstop 48-52Hz     #1
+const double f12_gain = 0.857718, f12_a1 = -0.20392089403, f12_a2 = 0.88318656585, f12_b0 = 1, f12_b1 = 0.00000000301, f12_b2 = 1; // Constants for pre-filter BiQuad: Bandstop 48-52Hz     #2
+const double f13_gain = 1       , f13_a1 = 0.20392089964,  f13_a2 = 0.88318656589, f13_b0 = 1, f13_b1 = 0.00000000301, f13_b2 = 1; // Constants for pre-filter BiQuad: Bandstop 48-52Hz     #3
+
+double f21_v1=0, f21_v2=0, f22_v1=0, f22_v2=0, f23_v1=0, f23_v2=0;
+double f21_v1b=0, f21_v2b=0, f22_v1b=0, f22_v2b=0, f23_v1b=0, f23_v2b=0;
+const double f21_gain = 0.807131, f21_a1 = -1.58896195896, f21_a2 = 0.63360956141, f21_b0 = 1, f21_b1 = -2, f21_b2 =  1; // Constants for pre-filter BiQuad: Highpass 6 Hz        #1
+const double f22_gain = 0.847233, f22_a1 = -1.67098565006, f22_a2 = 0.71793800319, f22_b0 = 1, f22_b1 = -2, f22_b2 =  1; // Constants for pre-filter BiQuad: Highpass 6 Hz        #2
+const double f23_gain = 0.928702, f23_a1 = -1.83505834569, f23_a2 = 0.88662091145, f23_b0 = 1, f23_b1 = -2, f23_b2 =  1; // Constants for pre-filter BiQuad: Highpass 6 Hz        #3
+
+double f31_v1=0, f31_v2=0, f32_v1=0, f32_v2=0, f33_v1=0, f33_v2=0;
+double f31_v1b=0, f31_v2b=0, f32_v1b=0, f32_v2b=0, f33_v1b=0, f33_v2b=0;
+const double f31_gain = 0.130049, f31_a1 = -0.57789344562, f31_a2 = 0.09809080329, f31_b0 = 1, f31_b1 = 2, f31_b2 = 1; // Constants for pre-filter BiQuad: Lowpass  30 Hz       #1
+const double f32_gain = 0.147923, f32_a1 = -0.65731925030, f32_a2 = 0.24901264940, f32_b0 = 1, f32_b1 = 2, f32_b2 = 1; // Constants for pre-filter BiQuad: Lowpass  30 Hz       #2
+const double f33_gain = 0.194139, f33_a1 = -0.86268420183, f33_a2 = 0.63923919773, f33_b0 = 1, f33_b1 = 2, f33_b2 = 1; // Constants for pre-filter BiQuad: Lowpass  30 Hz       #3
+
+double a11_v1=0, a11_v2=0, a12_v1=0, a12_v2=0, a13_v1=0, a13_v2=0;
+double a11_v1b=0, a11_v2b=0, a12_v1b=0, a12_v2b=0, a13_v1b=0, a13_v2b=0;
+const double a11_gain = 0.002932, a11_a1 = -1.79028908066, a11_a2 = 0.80198751695, a11_b0 = 1, a11_b1 = 2, a11_b2 = 1; // Constants for averaging BiQuad:   Lowpass  4 Hz       #1
+const double a12_gain = 0.003004, a12_a1 = -1.83907730494, a12_a2 = 0.85109454222, a12_b0 = 1, a12_b1 = 2, a12_b2 = 1; // Constants for averaging BiQuad:   Lowpass  4 Hz       #2
+const double a13_gain = 0.003145, a13_a1 = -1.93018419700, a13_a2 = 0.94279676171, a13_b0 = 1, a13_b1 = 2, a13_b2 = 1; // Constants for averaging BiQuad:   Lowpass  4 Hz       #2
+
+// Reusable BiQuad filter
+double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2, const double gain)
+{
+    double v = u - a1*v1-a2*v2;
+    double y = gain*(b0*v+b1*v1+b2*v2);
+    v2 = v1;
+    v1=v;
+    return y;
+}
+
+// Apply filters: applies all necesary filters and averaging methods to input signal value
+double filtera(double emg_val)
+{
+    // Filtering signal
+    double emg_filt_val;
+    emg_filt_val = biquad(emg_val,f11_v1,f11_v2,f11_a1,f11_a2,f11_b0,f11_b1,f11_b2,f11_gain);      // Apply bandstop
+    emg_filt_val = biquad(emg_filt_val,f12_v1,f12_v2,f12_a1,f12_a2,f12_b0,f12_b1,f12_b2,f12_gain);
+    emg_filt_val = biquad(emg_filt_val,f13_v1,f13_v2,f13_a1,f13_a2,f13_b0,f13_b1,f13_b2,f13_gain);
+    emg_filt_val = biquad(emg_filt_val,f21_v1,f21_v2,f21_a1,f21_a2,f21_b0,f21_b1,f21_b2,f21_gain); // Apply highpass
+    emg_filt_val = biquad(emg_filt_val,f22_v1,f22_v2,f22_a1,f22_a2,f22_b0,f22_b1,f22_b2,f22_gain);
+    emg_filt_val = biquad(emg_filt_val,f23_v1,f23_v2,f23_a1,f23_a2,f23_b0,f23_b1,f23_b2,f23_gain);
+    emg_filt_val = biquad(emg_filt_val,f31_v1,f31_v2,f31_a1,f31_a2,f31_b0,f31_b1,f31_b2,f31_gain); // Apply lowpass
+    emg_filt_val = biquad(emg_filt_val,f32_v1,f32_v2,f32_a1,f32_a2,f32_b0,f32_b1,f32_b2,f32_gain);
+    emg_filt_val = biquad(emg_filt_val,f33_v1,f33_v2,f33_a1,f33_a2,f33_b0,f33_b1,f33_b2,f33_gain);
+
+    // Rectify signal
+    emg_filt_val = fabs(emg_filt_val);
+
+    // Averaging signal
+    emg_filt_val = biquad(emg_filt_val,a11_v1,a11_v2,a11_a1,a11_a2,a11_b0,a11_b1,a11_b2,a11_gain); // Apply avg. lowpass
+    emg_filt_val = biquad(emg_filt_val,a12_v1,a12_v2,a12_a1,a12_a2,a12_b0,a12_b1,a12_b2,a12_gain);
+    emg_filt_val = biquad(emg_filt_val,a13_v1,a13_v2,a13_a1,a13_a2,a13_b0,a13_b1,a13_b2,a13_gain);
+
+    return emg_filt_val;
+}
+double filterb(double emg_val)
+{
+    // Filtering signal
+    double emg_filt_val;
+    emg_filt_val = biquad(emg_val,f11_v1b,f11_v2b,f11_a1,f11_a2,f11_b0,f11_b1,f11_b2,f11_gain);      // Apply bandstop
+    emg_filt_val = biquad(emg_filt_val,f12_v1b,f12_v2b,f12_a1,f12_a2,f12_b0,f12_b1,f12_b2,f12_gain);
+    emg_filt_val = biquad(emg_filt_val,f13_v1b,f13_v2b,f13_a1,f13_a2,f13_b0,f13_b1,f13_b2,f13_gain);
+    emg_filt_val = biquad(emg_filt_val,f21_v1b,f21_v2b,f21_a1,f21_a2,f21_b0,f21_b1,f21_b2,f21_gain); // Apply highpass
+    emg_filt_val = biquad(emg_filt_val,f22_v1b,f22_v2b,f22_a1,f22_a2,f22_b0,f22_b1,f22_b2,f22_gain);
+    emg_filt_val = biquad(emg_filt_val,f23_v1b,f23_v2b,f23_a1,f23_a2,f23_b0,f23_b1,f23_b2,f23_gain);
+    emg_filt_val = biquad(emg_filt_val,f31_v1b,f31_v2b,f31_a1,f31_a2,f31_b0,f31_b1,f31_b2,f31_gain); // Apply lowpass
+    emg_filt_val = biquad(emg_filt_val,f32_v1b,f32_v2b,f32_a1,f32_a2,f32_b0,f32_b1,f32_b2,f32_gain);
+    emg_filt_val = biquad(emg_filt_val,f33_v1b,f33_v2b,f33_a1,f33_a2,f33_b0,f33_b1,f33_b2,f33_gain);
+
+    // Rectify signal
+    emg_filt_val = fabs(emg_filt_val);
+
+    // Averaging signal
+    emg_filt_val = biquad(emg_filt_val,a11_v1b,a11_v2b,a11_a1,a11_a2,a11_b0,a11_b1,a11_b2,a11_gain); // Apply avg. lowpass
+    emg_filt_val = biquad(emg_filt_val,a12_v1b,a12_v2b,a12_a1,a12_a2,a12_b0,a12_b1,a12_b2,a12_gain);
+    emg_filt_val = biquad(emg_filt_val,a13_v1b,a13_v2b,a13_a1,a13_a2,a13_b0,a13_b1,a13_b2,a13_gain);
+
+    return emg_filt_val;
+}
+
+// Create velocity steps: Converts continious velocity input signal to stepped output
+double velocity_step(double velocity)
+{
+    if (velocity <= 0.33) {
+        velocity=0.33;
+    } else if(velocity <= 0.66) {
+        velocity=0.66;
+    } else if(velocity > 0.66) {
+        velocity=1;
+    }
+    return velocity;
+}
+
+// Output velocity components
+void EMG_velocity(double &x_velocity, double &y_velocity, double &z_velocity, double emg_val, bool thr_pass1, int &DOF, int emg_time)
+{
+    if (emg_time == 0) {
+        EMG_timer.start();
+    }
+
+    if (emg_val > LPT) {
+        if (emg_time > LST) {
+            if(emg_time > UST) {
+                // Output velocities -------------------------------------------------------------------
+                velocity = (emg_val - LPT)*(1/(1-LPT));             
+                
+                if(thr_pass1) {
+                    velocity = velocity_step(velocity);
+                } else {
+                    velocity = - velocity_step(velocity);
+                }
 
-void readEMG(){
-    
-    }
\ No newline at end of file
+                switch(DOF) {
+                    case 1 :
+                        x_velocity = velocity;
+                        y_velocity = 0;
+                        z_velocity = 0;
+                        break;
+                    case 2 :
+                        x_velocity = 0;
+                        y_velocity = velocity;
+                        z_velocity = 0;
+                        break;
+                    case 3 :
+                        x_velocity = 0;
+                        y_velocity = 0;
+                        
+                        // z-velocity is not controlled continiously, but set to fixed value +/- 1
+                        if(velocity > 0) {
+                            z_velocity = 1;
+                        } else if( velocity < 0) {
+                            z_velocity = -1;
+                        } else {
+                            z_velocity = 0;
+                        }
+                        break;
+                }
+            }
+        }
+    } else {
+        if((emg_time > LST)&&(emg_time < UST)) {
+            if(thr_pass1) {
+                // Switch channel  -----------------------------------------------------------------------
+                DOF = DOF + 1;
+                if (DOF == 4) {
+                    DOF = 1;
+                }
+            } else {
+                // Switch pump ---------------------------------------------------------------------------
+                pump = !pump;
+            }
+        }
+        // No input: set all value to zero ---------------------------------------------------------------
+        x_velocity = 0;
+        y_velocity = 0;
+        z_velocity = 0;
+        thr_pass1 = false;
+        thr_pass2 = false;
+        EMG_timer.stop();
+        EMG_timer.reset();
+    }
+}
+
+void EMG_check(bool &thr_pass1, bool &thr_pass2, double &x_velocity, double &y_velocity, double &z_velocity, double emg_filt_val1, double emg_filt_val2, int emg_time)
+{
+    if (emg_filt_val1 > UPT) {
+        thr_pass1 = true;
+    }
+    if (emg_filt_val2 > UPT) {
+        thr_pass2 = true;
+    }
+
+    if ((thr_pass1 == true) && (thr_pass2 == true)) {
+        // If both true terminate
+        thr_pass1 = false;
+        thr_pass2 = false;
+        EMG_timer.stop();
+        EMG_timer.reset();
+        x_velocity = 0;
+        y_velocity = 0;
+        z_velocity = 0;
+        
+    } else {
+        if(thr_pass1) {
+            EMG_velocity(x_velocity, y_velocity,  z_velocity, emg_filt_val1, thr_pass1, DOF, emg_time);
+        } else if(thr_pass2) {
+            EMG_velocity(x_velocity, y_velocity,  z_velocity, emg_filt_val2, thr_pass1, DOF, emg_time);
+        }
+    }
+}
+
+
+// Go_flag for EMG_sampling
+void emg_activate()
+{
+    emg_go=true;
+}
+
+int main()
+{
+    ledgreen.write(off);
+    ledblue.write(off);
+    ledred.write(on);
+    wait_ms(5000);
+    sample_tick.attach(&emg_activate, 1/sample_frequency);      // Call sample function with ticker
+    pc.baud(baudrate);                                          // Connect to pc for debugging output
+
+    if(mode==normalize) {                                       // Start normalizing timer
+        normalizing_timer.reset();
+        normalizing_timer.start();
+    }
+
+    while(1) {
+
+        if(emg_go && mode ==normalize) {
+            emg_go = false;
+
+            emg_val1 = emg1.read();               // Sample EMG value 1 from AnalogIn
+            emg_val2 = emg2.read();               // Sample EMG value 2 from AnalogIn
+
+            emg_filt_val1 = filtera(emg_val1);    // Filter and average signal
+            emg_filt_val2 = filterb(emg_val2);    // Filter and average signal
+
+
+            /* Determining normalizing constants
+            - Read value from EMG and average as above
+            - If mode "normalizing" and normalizing timer below ending time:
+                * store averaged EMG value 1 in max_vol_cont1 while overwriting previous value when current value > previous value
+            - If normalizing time exceeded, wait for given time
+            - Switch channels: redo above procedure for EMG value 2
+            - Switch to sampling mode
+            */
+
+            // First normalizing step: channel 1
+            if (normalizing_timer.read_ms() <= normalize_time && channel == 1) {
+                ledred.write(off);
+                ledgreen.write(on);
+                if (emg_filt_val1 > max_vol_cont1) {
+                    max_vol_cont1 = emg_filt_val1;
+                }
+                // Second normalizing step: wait time, switch channel
+            } else if (normalizing_timer.read_ms() > normalize_time && channel == 1) {
+                channel = 2;
+                ledgreen.write(off);
+                ledred.write(on);
+                // Third normalizing step: channel 2
+            } else if (normalizing_timer.read_ms() >= (normalize_time + normalize_wait) && normalizing_timer.read_ms() <= (2*normalize_time + normalize_wait) && channel == 2) {
+                ledred.write(off);
+                ledgreen.write(on);
+                if (emg_filt_val2 > max_vol_cont2) {
+                    max_vol_cont2 = emg_filt_val2;
+                }
+                // Final normalizing step: stop normalizing process, start outputting velocities
+            } else if (normalizing_timer.read_ms() > (2*normalize_time + normalize_wait)) {
+                normalizing_timer.stop();
+                normalizing_timer.reset();
+                ledgreen.write(off);
+                ledred.write(off);
+                ledblue.write(on);
+                mode = sample;
+            }
+        }
+
+
+        if(emg_go && mode ==sample) {
+            emg_go = false;
+
+            emg_val1 = emg1.read();                             // Sample EMG value 1 from AnalogIn
+            emg_val2 = emg2.read();                             // Sample EMG value 2 from AnalogIn
+
+            // Normalize EMG values using pre-determined coefficients
+            if(max_vol_cont1 != 0 && max_vol_cont2 != 0) {      // Safety check: normalizing coefficients have to be set first
+                emg_val1 = emg_val1 / max_vol_cont1;            // Normalize EMG-value 1
+                emg_val2 = emg_val2 / max_vol_cont2;            // Normalize EMG-value 2
+
+                emg_filt_val1 = filtera(emg_val1);              // Filter and average signal 1
+                emg_filt_val2 = filterb(emg_val2);              // Filter and average signal 2
+
+                if (emg_filt_val1 > 1) {                        // Safety-function: set max. output to 1, even if muscle contraction exceeds max. voluntary contraction
+                    emg_filt_val1 = 1;
+                }
+                if (emg_filt_val2 > 1) {                        // Safety-function: set max. output to 1, even if muscle contraction exceeds max. voluntary contraction
+                    emg_filt_val2 = 1;
+                }
+
+                /* Checking EMG input:
+                -   Make sure only one DOF is actuated at a time
+                -   Distinct between switching function and motion from one EMG-signal
+                */
+                
+                EMG_check(thr_pass1, thr_pass2, x_velocity, y_velocity, z_velocity, emg_filt_val1, emg_filt_val2, EMG_timer.read_ms());
+
+            } else {
+                ledgreen.write(off);
+                ledred.write(on);
+                ledblue.write(off);
+            }
+        }
+
+        // Graphical output to HIDScope for debugging/ program check
+        scope.set(0,emg_filt_val2);
+        scope.set(1,x_velocity);
+        scope.set(2,y_velocity);
+        scope.set(3,pump);
+        scope.set(4,DOF);
+        scope.set(5,DOF);
+        scope.send();
+        //pc.printf("EMG timer: %i, EMG1 %.4f, EMG2 %.4f, DOF: %i, pump: %i \n", EMG_timer.read_ms(),emg_filt_val1,emg_filt_val2,DOF,pump);
+        //pc.printf("Normalizing timer: %i, max_vol_cont1 = %.4f,max_vol_cont2 = %.4f \n",normalizing_timer.read_ms(),max_vol_cont1,max_vol_cont2);
+    }
+}
+