Numero Uno / Mbed 2 deprecated EMG_Filter

Basic emg filter setup

Dependencies:   HIDScope mbed biquadFilter

Fork of EMG_Filter by Maarten Ganseij

main.cpp@2:85984e2aee8d, 2015-09-25 (annotated)

Committer:
mganseij
Date:
Fri Sep 25 08:19:05 2015 +0000
Revision:
2:85984e2aee8d
Parent:
1:55620051895b 
CHanging some filter code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mganseij 0:cf9353d63f27 1 #include "mbed.h"
mganseij 0:cf9353d63f27 2 #include "HIDScope.h"
mganseij 0:cf9353d63f27 3 #include <cstring>
mganseij 0:cf9353d63f27 4 #include <cmath>
mganseij 2:85984e2aee8d 5 #include "biquadFilter.h"
mganseij 0:cf9353d63f27 6
mganseij 0:cf9353d63f27 7 AnalogIn input1(A0); // first EMG input
mganseij 0:cf9353d63f27 8 AnalogIn input2(A1); // second EMG input
mganseij 0:cf9353d63f27 9 AnalogIn input3(A2); // third EMG input
mganseij 0:cf9353d63f27 10 AnalogIn input4(A3); // fourth EMG input
mganseij 0:cf9353d63f27 11
mganseij 0:cf9353d63f27 12 HIDScope scope(2); // HIDScope declared
mganseij 0:cf9353d63f27 13
mganseij 0:cf9353d63f27 14 // note that this filter is specified for a Fs of 512;
mganseij 0:cf9353d63f27 15 // Butterworth low pass filter, second order at 2 hertz
mganseij 0:cf9353d63f27 16 // Butterworth high pass filter, second order at 25 hertz
mganseij 0:cf9353d63f27 17 int Fs = 512;
mganseij 0:cf9353d63f27 18 const double low_b1 = 1.480219865318266e-04; //filter coefficients
mganseij 0:cf9353d63f27 19 const double low_b2 = 2.960439730636533e-04;
mganseij 0:cf9353d63f27 20 const double low_b3 = 1.480219865318266e-04;
mganseij 0:cf9353d63f27 21 const double low_a2 = -1.965293372622690e+00; // a1 is normalized to 1
mganseij 0:cf9353d63f27 22 const double low_a3 = 9.658854605688177e-01;
mganseij 0:cf9353d63f27 23 const double high_b1 = 8.047897937631126e-01;
mganseij 0:cf9353d63f27 24 const double high_b2 = -1.609579587526225e+00;
mganseij 0:cf9353d63f27 25 const double high_b3 = 8.047897937631126e-01;
mganseij 0:cf9353d63f27 26 const double high_a2 = -1.571102440190402e+00; // a1 is normalized to 1
mganseij 0:cf9353d63f27 27 const double high_a3 = 6.480567348620491e-01;
mganseij 0:cf9353d63f27 28
mganseij 0:cf9353d63f27 29 double inputs_current [4] = {}; // declaring an array for the inputs
mganseij 0:cf9353d63f27 30 double inputs_previous [4] = {}; // previous input
mganseij 0:cf9353d63f27 31 double inputs_previous2 [4] = {}; // second previous input
mganseij 0:cf9353d63f27 32
mganseij 0:cf9353d63f27 33 double outputs_current [4] = {}; // same for outputs
mganseij 0:cf9353d63f27 34 double outputs_previous [4] = {};
mganseij 0:cf9353d63f27 35 double outputs_previous2 [4] = {};
mganseij 0:cf9353d63f27 36
mganseij 0:cf9353d63f27 37 Ticker T1;
mganseij 0:cf9353d63f27 38
mganseij 0:cf9353d63f27 39 // this function will be called by a ticker sample and filter
mganseij 0:cf9353d63f27 40 volatile bool sample_go;
mganseij 0:cf9353d63f27 41
mganseij 0:cf9353d63f27 42 void samplego()
mganseij 0:cf9353d63f27 43 {
mganseij 0:cf9353d63f27 44 sample_go = 1;
mganseij 0:cf9353d63f27 45 }
mganseij 0:cf9353d63f27 46
mganseij 0:cf9353d63f27 47
mganseij 0:cf9353d63f27 48 void sample() // arrays are passed by reference, I think?
mganseij 0:cf9353d63f27 49 {
mganseij 0:cf9353d63f27 50 memmove(inputs_previous2, inputs_previous, sizeof(inputs_previous2)); // I hope this black magic works, equaling the array of previous2 to previous
mganseij 0:cf9353d63f27 51 memmove(inputs_previous, inputs_current, sizeof(inputs_previous)); // black magic again, this time previous to current
mganseij 0:cf9353d63f27 52 inputs_current[0] = input1;
mganseij 0:cf9353d63f27 53 inputs_current[1] = input2;
mganseij 0:cf9353d63f27 54 inputs_current[2] = input3;
mganseij 0:cf9353d63f27 55 inputs_current[3] = input4;
mganseij 0:cf9353d63f27 56 }
mganseij 0:cf9353d63f27 57
mganseij 0:cf9353d63f27 58 void filter() // filter: high --> full rect --> low
mganseij 0:cf9353d63f27 59 {
mganseij 0:cf9353d63f27 60 memmove(outputs_previous2, outputs_previous, sizeof(outputs_previous2)); // moving output one array back
mganseij 0:cf9353d63f27 61 memmove(outputs_previous, outputs_current, sizeof(outputs_previous)); // same for this output
mganseij 0:cf9353d63f27 62 for(int n = 0; n <= 3; n++)
mganseij 0:cf9353d63f27 63 {
mganseij 2:85984e2aee8d 64 outputs_current[n] = high_b1*inputs_current[n] + high_b2*inputs_previous[n] + high_b3*inputs_previous2[n] - high_a2*outputs_previous[n] - high_a3*outputs_previous2[n];
mganseij 2:85984e2aee8d 65 outputs_current[n] = fabs(outputs_current[n]);
mganseij 2:85984e2aee8d 66 outputs_current[n] = (low_b1*inputs_current[n] + low_b2*inputs_previous[n] + low_b3*inputs_previous2[n] - low_a2*outputs_previous[n] - low_a3*outputs_previous2[n]);
mganseij 2:85984e2aee8d 67 outputs_current[n] = outputs_current[n];
mganseij 0:cf9353d63f27 68 }
mganseij 0:cf9353d63f27 69 }
mganseij 0:cf9353d63f27 70
mganseij 0:cf9353d63f27 71 int main()
mganseij 0:cf9353d63f27 72 {
mganseij 0:cf9353d63f27 73 T1.attach(&samplego, (float)1/Fs);
mganseij 0:cf9353d63f27 74 while("pigs" != "fly")
mganseij 0:cf9353d63f27 75 {
mganseij 0:cf9353d63f27 76 if(sample_go)
mganseij 0:cf9353d63f27 77 {
mganseij 0:cf9353d63f27 78 sample();
mganseij 0:cf9353d63f27 79 filter();
mganseij 0:cf9353d63f27 80 scope.set(0,inputs_current[0]);
mganseij 0:cf9353d63f27 81 scope.set(1,outputs_current[0]);
mganseij 1:55620051895b 82 scope.send();
mganseij 0:cf9353d63f27 83 sample_go = 0;
mganseij 0:cf9353d63f27 84
mganseij 0:cf9353d63f27 85 }
mganseij 0:cf9353d63f27 86 }
mganseij 0:cf9353d63f27 87 } // main end