Margreeth de Breij / Mbed 2 deprecated EMG_totaal

Het totale EMG script

Dependencies:   HIDScope MODSERIAL biquadFilter mbed

Fork of EMGV5 by Laura Veenendaal

main.cpp@24:6661dd6a1718, 2015-10-12 (annotated)

Committer:
Margreeth95
Date:
Mon Oct 12 09:24:44 2015 +0000
Revision:
24:6661dd6a1718
Parent:
23:624827baffa6
Child:
25:5e9cc994cd1f
Verschillende filters en shifting average toegevoegd

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:32bb76391d89 1 #include "mbed.h"
vsluiter 11:ce72ec658a95 2 #include "HIDScope.h"
Margreeth95 19:34456a1effc4 3 #include "MODSERIAL.h"
Margreeth95 19:34456a1effc4 4 #include "biquadFilter.h"
Margreeth95 24:6661dd6a1718 5 #include "MAF.h"
vsluiter 0:32bb76391d89 6
vsluiter 4:8b298dfada81 7 //Define objects
Margreeth95 18:12250e88037f 8 AnalogIn EMG_left(A0); //Analog input
Margreeth95 18:12250e88037f 9 AnalogIn EMG_right(A1);
Margreeth95 18:12250e88037f 10 Ticker SampleEMG;
Margreeth95 18:12250e88037f 11 Ticker ScopeTimer;
Margreeth95 19:34456a1effc4 12 Ticker serial;
Margreeth95 24:6661dd6a1718 13 Ticker MovingAverage;
Margreeth95 24:6661dd6a1718 14 HIDScope scope(6);
Margreeth95 19:34456a1effc4 15 DigitalOut led(LED_RED);
Margreeth95 19:34456a1effc4 16 MODSERIAL pc(USBTX, USBRX);
Margreeth95 19:34456a1effc4 17
Margreeth95 18:12250e88037f 18
Margreeth95 19:34456a1effc4 19 // Declaring variables
Margreeth95 18:12250e88037f 20 double EMG_L_f_v1 = 0, EMG_L_f_v2 = 0;
Margreeth95 19:34456a1effc4 21 double EMG_L_fh=0;
Margreeth95 19:34456a1effc4 22 double EMG_left_value;
Margreeth95 22:207d314523ef 23 double EMG_left_f1;
Margreeth95 22:207d314523ef 24 double EMG_left_f2;
Margreeth95 24:6661dd6a1718 25 double EMG_left_f3;
Margreeth95 24:6661dd6a1718 26 double EMG_left_f4;
Margreeth95 24:6661dd6a1718 27 double EMG_left_f5;
Margreeth95 24:6661dd6a1718 28 double EMG_left_f6;
Margreeth95 22:207d314523ef 29 double EMG_left_abs;
Margreeth95 22:207d314523ef 30 double EMG_right_value;
Margreeth95 22:207d314523ef 31 double EMG_right_f1;
Margreeth95 22:207d314523ef 32 double EMG_right_f2;
Margreeth95 22:207d314523ef 33 double EMG_right_abs;
Margreeth95 24:6661dd6a1718 34 double Threshold1 = 0.03;
Margreeth95 24:6661dd6a1718 35 double Threshold2 = 0.06;
Margreeth95 24:6661dd6a1718 36
Margreeth95 24:6661dd6a1718 37 int N = 20;
Margreeth95 24:6661dd6a1718 38 double MAF_left[20];
Margreeth95 24:6661dd6a1718 39 double EMG_left_MAF;
Margreeth95 24:6661dd6a1718 40
Margreeth95 18:12250e88037f 41 // coëfficiënten
Margreeth95 24:6661dd6a1718 42 const double BiGainEMG_H1 = 0.795375, BiGainEMG_H2 = 0.895763;
Margreeth95 24:6661dd6a1718 43 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
Margreeth95 24:6661dd6a1718 44 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
Margreeth95 24:6661dd6a1718 45
Margreeth95 24:6661dd6a1718 46 const double BiGainEMG_L1=0.959332, BiGainEMG_L2 = 0.223396;
Margreeth95 24:6661dd6a1718 47 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
Margreeth95 24:6661dd6a1718 48 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
Margreeth95 24:6661dd6a1718 49
Margreeth95 24:6661dd6a1718 50 const double BiGainEMG_N1 = 1.0, BiGainEMG_N2 = 0.965081;
Margreeth95 24:6661dd6a1718 51 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
Margreeth95 24:6661dd6a1718 52 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
Margreeth95 24:6661dd6a1718 53
Margreeth95 20:a0642e983da9 54 // Filter creation
Margreeth95 24:6661dd6a1718 55 biquadFilter EMG_highpass1 (EMGH1_a1, EMGH1_a2, EMGH1_b0, EMGH1_b1, EMGH1_b2); // creates the high pass filter
Margreeth95 24:6661dd6a1718 56 biquadFilter EMG_highpass2 (EMGH2_a1, EMGH2_a2, EMGH2_b0, EMGH2_b1, EMGH2_b2);
Margreeth95 24:6661dd6a1718 57 biquadFilter EMG_lowpass1 (EMGL1_a1, EMGL1_a2, EMGL1_b0, EMGL1_b1, EMGL1_b2); // creates the low pass filter
Margreeth95 24:6661dd6a1718 58 biquadFilter EMG_lowpass2 (EMGL2_a1, EMGL2_a2, EMGL2_b0, EMGL2_b1, EMGL2_b2);
Margreeth95 24:6661dd6a1718 59 biquadFilter EMG_notch1 (EMGN1_a1, EMGN1_a2, EMGN1_b0, EMGN1_b1, EMGN1_b2); // creates the notch filter
Margreeth95 24:6661dd6a1718 60 biquadFilter EMG_notch2 (EMGN2_a1, EMGN2_a2, EMGN2_b0, EMGN2_b1, EMGN2_b2);
Margreeth95 24:6661dd6a1718 61
Margreeth95 19:34456a1effc4 62 // EMG filtering function
Margreeth95 22:207d314523ef 63 void EMGfilter() // Both EMG signals are filtered in one function and with the same filters
Margreeth95 19:34456a1effc4 64 {
Margreeth95 19:34456a1effc4 65 EMG_left_value = EMG_left.read();
Margreeth95 24:6661dd6a1718 66 EMG_left_f1 = EMG_highpass1.step(EMG_left_value);
Margreeth95 24:6661dd6a1718 67 EMG_left_f2 = EMG_highpass2.step(EMG_left_f1);
Margreeth95 22:207d314523ef 68 EMG_left_abs = fabs(EMG_left_f2);
Margreeth95 24:6661dd6a1718 69 EMG_left_f3 = EMG_lowpass1.step(EMG_left_abs);
Margreeth95 24:6661dd6a1718 70 EMG_left_f4 = EMG_lowpass2.step(EMG_left_f3);
Margreeth95 24:6661dd6a1718 71 EMG_left_f5 = EMG_notch1.step(EMG_left_f4);
Margreeth95 24:6661dd6a1718 72 EMG_left_f6 = EMG_notch1.step(EMG_left_f5);
Margreeth95 24:6661dd6a1718 73
Margreeth95 24:6661dd6a1718 74 // EMG_right_value = EMG_right.read();
Margreeth95 24:6661dd6a1718 75 // EMG_right_f1 = EMG_highpass.step(EMG_right_value);
Margreeth95 24:6661dd6a1718 76 // EMG_right_f1 = EMG_lowpass.step(EMG_right_f1);
Margreeth95 24:6661dd6a1718 77 // EMG_right_abs = fabs(EMG_right_f2);
Margreeth95 19:34456a1effc4 78 }
vsluiter 2:e314bb3b2d99 79
Margreeth95 24:6661dd6a1718 80 // Movingaverage Filter
Margreeth95 24:6661dd6a1718 81 void MovingAverageFilter()
Margreeth95 24:6661dd6a1718 82 {
Margreeth95 24:6661dd6a1718 83 int ArraySum;
Margreeth95 24:6661dd6a1718 84 for (int a = 0; a<N; a++)
Margreeth95 24:6661dd6a1718 85 {
Margreeth95 24:6661dd6a1718 86 ArraySum = ArraySum + MAF_left[a];
Margreeth95 24:6661dd6a1718 87 }
Margreeth95 24:6661dd6a1718 88 EMG_left_MAF = ArraySum/N;
Margreeth95 24:6661dd6a1718 89 // EMG_left_MAF = (MAF_left[0]+MAF_left[1]+MAF_left[2]+MAF_left[3]+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])/20;
Margreeth95 24:6661dd6a1718 90 MAF_left[19] = MAF_left[18];
Margreeth95 24:6661dd6a1718 91 MAF_left[18] = MAF_left[17];
Margreeth95 24:6661dd6a1718 92 MAF_left[17] = MAF_left[16];
Margreeth95 24:6661dd6a1718 93 MAF_left[16] = MAF_left[15];
Margreeth95 24:6661dd6a1718 94 MAF_left[15] = MAF_left[14];
Margreeth95 24:6661dd6a1718 95 MAF_left[14] = MAF_left[13];
Margreeth95 24:6661dd6a1718 96 MAF_left[13] = MAF_left[12];
Margreeth95 24:6661dd6a1718 97 MAF_left[12] = MAF_left[11];
Margreeth95 24:6661dd6a1718 98 MAF_left[11] = MAF_left[10];
Margreeth95 24:6661dd6a1718 99 MAF_left[10] = MAF_left[9];
Margreeth95 24:6661dd6a1718 100 MAF_left[9] = MAF_left[8];
Margreeth95 24:6661dd6a1718 101 MAF_left[8] = MAF_left[7];
Margreeth95 24:6661dd6a1718 102 MAF_left[7] = MAF_left[6];
Margreeth95 24:6661dd6a1718 103 MAF_left[6] = MAF_left[5];
Margreeth95 24:6661dd6a1718 104 MAF_left[5] = MAF_left[4];
Margreeth95 24:6661dd6a1718 105 MAF_left[4] = MAF_left[3];
Margreeth95 24:6661dd6a1718 106 MAF_left[3] = MAF_left[2];
Margreeth95 24:6661dd6a1718 107 MAF_left[2] = MAF_left[1];
Margreeth95 24:6661dd6a1718 108 MAF_left[1] = MAF_left[0];
Margreeth95 24:6661dd6a1718 109 MAF_left[0] = EMG_left_f6;
Margreeth95 24:6661dd6a1718 110 }
Margreeth95 24:6661dd6a1718 111
Margreeth95 18:12250e88037f 112 // HIDScope
Margreeth95 18:12250e88037f 113 void ScopeSend()
Margreeth95 18:12250e88037f 114 {
Margreeth95 19:34456a1effc4 115 scope.set(0, EMG_left_value);
Margreeth95 24:6661dd6a1718 116 scope.set(1, EMG_left_f2);
Margreeth95 24:6661dd6a1718 117 scope.set(2, EMG_left_abs);
Margreeth95 24:6661dd6a1718 118 scope.set(3, EMG_left_f4);
Margreeth95 24:6661dd6a1718 119 scope.set(4, EMG_left_f6);
Margreeth95 24:6661dd6a1718 120 scope.set(5, EMG_left_MAF);
Margreeth95 18:12250e88037f 121 scope.send();
Margreeth95 18:12250e88037f 122 }
Margreeth95 18:12250e88037f 123
vsluiter 0:32bb76391d89 124 int main()
vsluiter 0:32bb76391d89 125 {
Margreeth95 18:12250e88037f 126 SampleEMG.attach(&EMGfilter, 0.002);
Margreeth95 18:12250e88037f 127 ScopeTimer.attach(&ScopeSend, 0.002);
Margreeth95 24:6661dd6a1718 128 MovingAverage.attach(&MovingAverageFilter, 0.002);
Margreeth95 19:34456a1effc4 129 while(1)
Margreeth95 19:34456a1effc4 130 {
Margreeth95 24:6661dd6a1718 131 if (EMG_left_f6 > Threshold1)
Margreeth95 19:34456a1effc4 132 {
Margreeth95 19:34456a1effc4 133 led.write(0);
Margreeth95 19:34456a1effc4 134 }
Margreeth95 19:34456a1effc4 135
Margreeth95 19:34456a1effc4 136 else
Margreeth95 19:34456a1effc4 137 {
Margreeth95 19:34456a1effc4 138 led.write(1);
Margreeth95 19:34456a1effc4 139 }
Margreeth95 19:34456a1effc4 140 }
vsluiter 0:32bb76391d89 141 }