Biorobotics / Mbed 2 deprecated EMG_test

Script to plot both a raw EMG signal and the filtered signal in HIDscope

Dependencies:   HIDScope mbed

main.cpp@4:d32946ab9c54, 2015-10-26 (annotated)

Committer:
roosbulthuis
Date:
Mon Oct 26 16:10:02 2015 +0000
Revision:
4:d32946ab9c54
Parent:
3:7cb317c00afb
Child:
5:cf85007ce40a
Subtracted mean of input signal, rectifier works, but filters seem not to work

Who changed what in which revision?

UserRevisionLine numberNew contents of line
roosbulthuis 0:c85b764527f4 1 #include "mbed.h"
roosbulthuis 0:c85b764527f4 2 //#include "read_filter_emg.h"
roosbulthuis 0:c85b764527f4 3 //included for fabs() function
roosbulthuis 0:c85b764527f4 4 #include <math.h>
roosbulthuis 0:c85b764527f4 5 #include "HIDScope.h"
roosbulthuis 4:d32946ab9c54 6 #include <iostream>
roosbulthuis 0:c85b764527f4 7
roosbulthuis 0:c85b764527f4 8 Ticker sample_timer;
roosbulthuis 3:7cb317c00afb 9 HIDScope scope(2);
roosbulthuis 0:c85b764527f4 10
roosbulthuis 0:c85b764527f4 11 AnalogIn analog_emg_left(A0);
roosbulthuis 3:7cb317c00afb 12 //AnalogIn analog_emg_right(A1);
roosbulthuis 3:7cb317c00afb 13 double input = 0;
roosbulthuis 3:7cb317c00afb 14 double filter_signal;
roosbulthuis 3:7cb317c00afb 15 //double input_right = 0;
roosbulthuis 0:c85b764527f4 16
roosbulthuis 3:7cb317c00afb 17 double v1=0;
roosbulthuis 3:7cb317c00afb 18 double v2=0;
roosbulthuis 3:7cb317c00afb 19 //double v1_right=0;
roosbulthuis 3:7cb317c00afb 20 //double v2_right=0;
roosbulthuis 0:c85b764527f4 21
roosbulthuis 0:c85b764527f4 22 double filter_left;
roosbulthuis 0:c85b764527f4 23 double filter_right;
roosbulthuis 0:c85b764527f4 24
roosbulthuis 0:c85b764527f4 25 //general biquad filter that can be called in all the filter functions
roosbulthuis 2:1d29b91bc46a 26 double biquad(double u, double v1, double v2, const double a1,
roosbulthuis 0:c85b764527f4 27 const double a2, const double b0, const double b1, const double b2)
roosbulthuis 0:c85b764527f4 28 {
roosbulthuis 1:f32f8eac8af1 29 double v = u - a1*v1 - a2*v2;
roosbulthuis 0:c85b764527f4 30 double y = b0*v + b1*v1 + b2*v2;
roosbulthuis 0:c85b764527f4 31 //values of v2 and v1 are updated, as they are passed by reference
roosbulthuis 0:c85b764527f4 32 //they update globally
roosbulthuis 0:c85b764527f4 33 v2 = v1;
roosbulthuis 0:c85b764527f4 34 v1 = v;
roosbulthuis 0:c85b764527f4 35 return y;
roosbulthuis 0:c85b764527f4 36 }
roosbulthuis 0:c85b764527f4 37
roosbulthuis 0:c85b764527f4 38 //Specifying filter coefficients highpass
roosbulthuis 0:c85b764527f4 39
roosbulthuis 3:7cb317c00afb 40 /* notch filter with 3 cascaded biquads*/
roosbulthuis 3:7cb317c00afb 41 //first notch biquad
roosbulthuis 3:7cb317c00afb 42 const double notch1_a1 = -1.55951422433;
roosbulthuis 3:7cb317c00afb 43 const double notch1_a2 = 0.92705680308;
roosbulthuis 3:7cb317c00afb 44 const double notch1_b0 = 1.00000000000;
roosbulthuis 3:7cb317c00afb 45 const double notch1_b1 = -1.61854515325;
roosbulthuis 3:7cb317c00afb 46 const double notch1_b2 = 1.00000000000;
roosbulthuis 3:7cb317c00afb 47
roosbulthuis 3:7cb317c00afb 48 //second notch biquad
roosbulthuis 3:7cb317c00afb 49 const double notch2_a1 = -1.54767435801;
roosbulthuis 3:7cb317c00afb 50 const double notch2_a2 = 0.96124842048;
roosbulthuis 3:7cb317c00afb 51 const double notch2_b0 = 1.00000000000;
roosbulthuis 3:7cb317c00afb 52 const double notch2_b1 = -1.61854515325;
roosbulthuis 3:7cb317c00afb 53 const double notch2_b2 = 1.00000000000;
roosbulthuis 3:7cb317c00afb 54
roosbulthuis 3:7cb317c00afb 55 //third notch biquad
roosbulthuis 3:7cb317c00afb 56 const double notch3_a1 = -1.62600366964;
roosbulthuis 3:7cb317c00afb 57 const double notch3_a2 = 0.96453460373;
roosbulthuis 3:7cb317c00afb 58 const double notch3_b0 = 1.00000000000;
roosbulthuis 3:7cb317c00afb 59 const double notch3_b1 = -1.61854515325;
roosbulthuis 3:7cb317c00afb 60 const double notch3_b2 = 1.00000000000;
roosbulthuis 3:7cb317c00afb 61
roosbulthuis 0:c85b764527f4 62 /* high pass filter consists of three cascaded biquads
roosbulthuis 0:c85b764527f4 63 blow coefficients for those three biquads */
roosbulthuis 0:c85b764527f4 64 //first high pass biquad
roosbulthuis 0:c85b764527f4 65 const double highp1_a1 = -0.67538034389;
roosbulthuis 0:c85b764527f4 66 const double highp1_a2 = 0.12769255668;
roosbulthuis 0:c85b764527f4 67 const double highp1_b0 = 1.00000000000;
roosbulthuis 0:c85b764527f4 68 const double highp1_b1 = -2.00000000000;
roosbulthuis 0:c85b764527f4 69 const double highp1_b2 = 1.00000000000;
roosbulthuis 0:c85b764527f4 70
roosbulthuis 0:c85b764527f4 71 //second high pass biquad
roosbulthuis 0:c85b764527f4 72 const double highp2_a1 = -0.76475499450;
roosbulthuis 0:c85b764527f4 73 const double highp2_a2 = 0.27692273367;
roosbulthuis 0:c85b764527f4 74 const double highp2_b0 = 1.00000000000;
roosbulthuis 0:c85b764527f4 75 const double highp2_b1 = -2.00000000000;
roosbulthuis 0:c85b764527f4 76 const double highp2_b2 = 1.00000000000;
roosbulthuis 0:c85b764527f4 77
roosbulthuis 0:c85b764527f4 78 //third high pass biquad
roosbulthuis 0:c85b764527f4 79 const double highp3_a1 = -0.99216561242;
roosbulthuis 0:c85b764527f4 80 const double highp3_a2 = 0.65663360837;
roosbulthuis 0:c85b764527f4 81 const double highp3_b0 = 1.00000000000;
roosbulthuis 0:c85b764527f4 82 const double highp3_b1 = -2.00000000000;
roosbulthuis 0:c85b764527f4 83 const double highp3_b2 = 1.00000000000;
roosbulthuis 0:c85b764527f4 84
roosbulthuis 0:c85b764527f4 85 //Specifying filter coefficients lowpass
roosbulthuis 0:c85b764527f4 86
roosbulthuis 0:c85b764527f4 87 /* lowpass filter consists of three cascaded biquads
roosbulthuis 0:c85b764527f4 88 below the coefficients for those three biquads */
roosbulthuis 0:c85b764527f4 89 //first high pass biquad
roosbulthuis 0:c85b764527f4 90 const double lowp1_a1 = -1.05207469728;
roosbulthuis 0:c85b764527f4 91 const double lowp1_a2 = 0.28586907478;
roosbulthuis 0:c85b764527f4 92 const double lowp1_b0 = 1.00000000000;
roosbulthuis 0:c85b764527f4 93 const double lowp1_b1 = 2.00000000000;
roosbulthuis 0:c85b764527f4 94 const double lowp1_b2 = 1.00000000000;
roosbulthuis 0:c85b764527f4 95
roosbulthuis 0:c85b764527f4 96 //second high pass biquad
roosbulthuis 0:c85b764527f4 97 const double lowp2_a1 = -1.16338171052;
roosbulthuis 0:c85b764527f4 98 const double lowp2_a2 = 0.42191097989;
roosbulthuis 0:c85b764527f4 99 const double lowp2_b0 = 1.00000000000;
roosbulthuis 0:c85b764527f4 100 const double lowp2_b1 = 2.00000000000;
roosbulthuis 0:c85b764527f4 101 const double lowp2_b2 = 1.00000000000;
roosbulthuis 0:c85b764527f4 102
roosbulthuis 0:c85b764527f4 103 //third high pass biquad
roosbulthuis 0:c85b764527f4 104 const double lowp3_a1 = -1.42439823874;
roosbulthuis 0:c85b764527f4 105 const double lowp3_a2 = 0.74093118112;
roosbulthuis 0:c85b764527f4 106 const double lowp3_b0 = 1.00000000000;
roosbulthuis 0:c85b764527f4 107 const double lowp3_b1 = 2.00000000000;
roosbulthuis 0:c85b764527f4 108 const double lowp3_b2 = 1.00000000000;
roosbulthuis 0:c85b764527f4 109
roosbulthuis 0:c85b764527f4 110 //input to each filter is output of the filter before(excl. first which uses input_sample)
roosbulthuis 0:c85b764527f4 111 /* NOT SURE IF PASSING V1 AND V2 IS CORRECT,
roosbulthuis 0:c85b764527f4 112 WILL IT UPDATE IN THE MEMORY POSITION SO THAT
roosbulthuis 0:c85b764527f4 113 V1 IS CHANGED GLOBALLY */
roosbulthuis 0:c85b764527f4 114
roosbulthuis 3:7cb317c00afb 115 //notch
roosbulthuis 0:c85b764527f4 116
roosbulthuis 3:7cb317c00afb 117 double notch_filter(double input, double v1, double v2)
roosbulthuis 0:c85b764527f4 118 {
roosbulthuis 3:7cb317c00afb 119 double y1 = biquad(input, v1, v2, notch1_a1, notch1_a2, notch1_b0, notch1_b1, notch1_b2);
roosbulthuis 3:7cb317c00afb 120 double y2 = biquad(y1, v1, v2, notch2_a1, notch2_a2, notch2_b0, notch2_b1, notch2_b2);
roosbulthuis 3:7cb317c00afb 121 double y3 = biquad(y2, v1, v2, notch3_a1, notch3_a2, notch3_b0, notch3_b1, notch3_b2);
roosbulthuis 0:c85b764527f4 122
roosbulthuis 0:c85b764527f4 123 return y3;
roosbulthuis 0:c85b764527f4 124 }
roosbulthuis 0:c85b764527f4 125
roosbulthuis 0:c85b764527f4 126
roosbulthuis 3:7cb317c00afb 127 //highpass
roosbulthuis 3:7cb317c00afb 128
roosbulthuis 3:7cb317c00afb 129 double highpass_filter(double y3, double v1, double v2)
roosbulthuis 0:c85b764527f4 130 {
roosbulthuis 3:7cb317c00afb 131 double y4 = biquad(y3, v1, v2, highp1_a1, highp1_a2, highp1_b0, highp1_b1, highp1_b2);
roosbulthuis 3:7cb317c00afb 132 double y5 = biquad(y4, v1, v2, highp2_a1, highp2_a2, highp2_b0, highp2_b1, highp2_b2);
roosbulthuis 3:7cb317c00afb 133 double y6 = biquad(y5, v1, v2, highp3_a1, highp3_a2, highp3_b0, highp3_b1, highp3_b2);
roosbulthuis 3:7cb317c00afb 134
roosbulthuis 3:7cb317c00afb 135 return y6;
roosbulthuis 3:7cb317c00afb 136 }
roosbulthuis 3:7cb317c00afb 137
roosbulthuis 3:7cb317c00afb 138
roosbulthuis 3:7cb317c00afb 139 //rectifier
roosbulthuis 3:7cb317c00afb 140 double rectify(double y6)
roosbulthuis 3:7cb317c00afb 141 {
roosbulthuis 3:7cb317c00afb 142 y6 = fabs(y6);
roosbulthuis 3:7cb317c00afb 143 return y6;
roosbulthuis 0:c85b764527f4 144 }
roosbulthuis 0:c85b764527f4 145
roosbulthuis 0:c85b764527f4 146 //lowpass
roosbulthuis 0:c85b764527f4 147
roosbulthuis 3:7cb317c00afb 148 double lowpass_filter(double y6, double v1, double v2)
roosbulthuis 0:c85b764527f4 149 {
roosbulthuis 3:7cb317c00afb 150 double y7 = biquad(y6, v1, v2, lowp1_a1, lowp1_a2, lowp1_b0, lowp1_b1, lowp1_b2);
roosbulthuis 3:7cb317c00afb 151 double y8 = biquad(y7, v1, v2, lowp2_a1, lowp2_a2, lowp2_b0, lowp2_b1, lowp2_b2);
roosbulthuis 3:7cb317c00afb 152 double filtered_signal = biquad(y8, v1, v2, lowp3_a1, lowp3_a2, lowp3_b0, lowp3_b1, lowp3_b2);
roosbulthuis 0:c85b764527f4 153
roosbulthuis 0:c85b764527f4 154 return filtered_signal;
roosbulthuis 0:c85b764527f4 155 }
roosbulthuis 0:c85b764527f4 156
roosbulthuis 2:1d29b91bc46a 157 double filter(double input, double v1, double v2)
roosbulthuis 0:c85b764527f4 158 {
roosbulthuis 0:c85b764527f4 159 /* function passes the input through the three filters
roosbulthuis 0:c85b764527f4 160 returns the final output value as filtered sample
roosbulthuis 0:c85b764527f4 161 this is used in check_state() function to determine state of system
roosbulthuis 0:c85b764527f4 162 */
roosbulthuis 3:7cb317c00afb 163 double y0 = notch_filter(input, v1, v2);
roosbulthuis 3:7cb317c00afb 164 double y1 = highpass_filter(y0, v1, v2);
roosbulthuis 0:c85b764527f4 165 double y2 = rectify(y1);
roosbulthuis 0:c85b764527f4 166 double filtered_signal = lowpass_filter(y2, v1, v2);
roosbulthuis 0:c85b764527f4 167
roosbulthuis 0:c85b764527f4 168 return filtered_signal;
roosbulthuis 0:c85b764527f4 169 }
roosbulthuis 0:c85b764527f4 170
roosbulthuis 0:c85b764527f4 171 double test=1;
roosbulthuis 0:c85b764527f4 172
roosbulthuis 0:c85b764527f4 173 void sample()
roosbulthuis 0:c85b764527f4 174 {
roosbulthuis 3:7cb317c00afb 175 input = analog_emg_left.read();
roosbulthuis 4:d32946ab9c54 176 input = input-0.45; //FIRST SUBTRACT MEAN THEN FILTER
roosbulthuis 3:7cb317c00afb 177 //input_right = analog_emg_right.read();
roosbulthuis 3:7cb317c00afb 178 filter_signal = filter(input, v1, v2);
roosbulthuis 3:7cb317c00afb 179 //filter_right = filter(input_right, v1_right, v2_right);
roosbulthuis 0:c85b764527f4 180
roosbulthuis 3:7cb317c00afb 181 scope.set(0, input);
roosbulthuis 3:7cb317c00afb 182 scope.set(1, filter_signal);
roosbulthuis 0:c85b764527f4 183 scope.send();
roosbulthuis 0:c85b764527f4 184 }
roosbulthuis 0:c85b764527f4 185
roosbulthuis 0:c85b764527f4 186
roosbulthuis 0:c85b764527f4 187 int main()
roosbulthuis 0:c85b764527f4 188 {
roosbulthuis 0:c85b764527f4 189 sample_timer.attach(&sample, 0.002);
roosbulthuis 0:c85b764527f4 190 while(1){}
roosbulthuis 0:c85b764527f4 191 }