Numero Uno / Mbed 2 deprecated EMG_Filter

Basic emg filter setup

Dependencies:   HIDScope mbed biquadFilter

Fork of EMG_Filter by Maarten Ganseij

main.cpp@1:55620051895b, 2015-09-24 (annotated)

Committer:
mganseij
Date:
Thu Sep 24 09:45:37 2015 +0000
Revision:
1:55620051895b
Parent:
0:cf9353d63f27
Child:
2:85984e2aee8d
To make victor happy, i commit

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