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@2:1d29b91bc46a, 2015-10-26 (annotated)

Committer:
roosbulthuis
Date:
Mon Oct 26 15:00:52 2015 +0000
Revision:
2:1d29b91bc46a
Parent:
1:f32f8eac8af1
Child:
3:7cb317c00afb
Got rid of pass by reference for v1 and v2, now we get a signal if we plot the filtered signal in channel 0.

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 0:c85b764527f4 6
roosbulthuis 0:c85b764527f4 7 Ticker sample_timer;
roosbulthuis 0:c85b764527f4 8 HIDScope scope(1);
roosbulthuis 0:c85b764527f4 9
roosbulthuis 0:c85b764527f4 10 AnalogIn analog_emg_left(A0);
roosbulthuis 0:c85b764527f4 11 AnalogIn analog_emg_right(A1);
roosbulthuis 0:c85b764527f4 12 double input_left = 0;
roosbulthuis 0:c85b764527f4 13 double input_right = 0;
roosbulthuis 0:c85b764527f4 14
roosbulthuis 0:c85b764527f4 15 double v1_left=0;
roosbulthuis 0:c85b764527f4 16 double v2_left=0;
roosbulthuis 0:c85b764527f4 17 double v1_right=0;
roosbulthuis 0:c85b764527f4 18 double v2_right=0;
roosbulthuis 0:c85b764527f4 19
roosbulthuis 0:c85b764527f4 20 double filter_left;
roosbulthuis 0:c85b764527f4 21 double filter_right;
roosbulthuis 0:c85b764527f4 22
roosbulthuis 0:c85b764527f4 23 //general biquad filter that can be called in all the filter functions
roosbulthuis 2:1d29b91bc46a 24 double biquad(double u, double v1, double v2, const double a1,
roosbulthuis 0:c85b764527f4 25 const double a2, const double b0, const double b1, const double b2)
roosbulthuis 0:c85b764527f4 26 {
roosbulthuis 1:f32f8eac8af1 27 double v = u - a1*v1 - a2*v2;
roosbulthuis 0:c85b764527f4 28 double y = b0*v + b1*v1 + b2*v2;
roosbulthuis 0:c85b764527f4 29 //values of v2 and v1 are updated, as they are passed by reference
roosbulthuis 0:c85b764527f4 30 //they update globally
roosbulthuis 0:c85b764527f4 31 v2 = v1;
roosbulthuis 0:c85b764527f4 32 v1 = v;
roosbulthuis 0:c85b764527f4 33 return y;
roosbulthuis 0:c85b764527f4 34 }
roosbulthuis 0:c85b764527f4 35
roosbulthuis 0:c85b764527f4 36 //Specifying filter coefficients highpass
roosbulthuis 0:c85b764527f4 37
roosbulthuis 0:c85b764527f4 38 /* high pass filter consists of three cascaded biquads
roosbulthuis 0:c85b764527f4 39 blow coefficients for those three biquads */
roosbulthuis 0:c85b764527f4 40 //first high pass biquad
roosbulthuis 0:c85b764527f4 41 const double highp1_a1 = -0.67538034389;
roosbulthuis 0:c85b764527f4 42 const double highp1_a2 = 0.12769255668;
roosbulthuis 0:c85b764527f4 43 const double highp1_b0 = 1.00000000000;
roosbulthuis 0:c85b764527f4 44 const double highp1_b1 = -2.00000000000;
roosbulthuis 0:c85b764527f4 45 const double highp1_b2 = 1.00000000000;
roosbulthuis 0:c85b764527f4 46
roosbulthuis 0:c85b764527f4 47 //second high pass biquad
roosbulthuis 0:c85b764527f4 48 const double highp2_a1 = -0.76475499450;
roosbulthuis 0:c85b764527f4 49 const double highp2_a2 = 0.27692273367;
roosbulthuis 0:c85b764527f4 50 const double highp2_b0 = 1.00000000000;
roosbulthuis 0:c85b764527f4 51 const double highp2_b1 = -2.00000000000;
roosbulthuis 0:c85b764527f4 52 const double highp2_b2 = 1.00000000000;
roosbulthuis 0:c85b764527f4 53
roosbulthuis 0:c85b764527f4 54 //third high pass biquad
roosbulthuis 0:c85b764527f4 55 const double highp3_a1 = -0.99216561242;
roosbulthuis 0:c85b764527f4 56 const double highp3_a2 = 0.65663360837;
roosbulthuis 0:c85b764527f4 57 const double highp3_b0 = 1.00000000000;
roosbulthuis 0:c85b764527f4 58 const double highp3_b1 = -2.00000000000;
roosbulthuis 0:c85b764527f4 59 const double highp3_b2 = 1.00000000000;
roosbulthuis 0:c85b764527f4 60
roosbulthuis 0:c85b764527f4 61 //Specifying filter coefficients lowpass
roosbulthuis 0:c85b764527f4 62
roosbulthuis 0:c85b764527f4 63 /* lowpass filter consists of three cascaded biquads
roosbulthuis 0:c85b764527f4 64 below the coefficients for those three biquads */
roosbulthuis 0:c85b764527f4 65 //first high pass biquad
roosbulthuis 0:c85b764527f4 66 const double lowp1_a1 = -1.05207469728;
roosbulthuis 0:c85b764527f4 67 const double lowp1_a2 = 0.28586907478;
roosbulthuis 0:c85b764527f4 68 const double lowp1_b0 = 1.00000000000;
roosbulthuis 0:c85b764527f4 69 const double lowp1_b1 = 2.00000000000;
roosbulthuis 0:c85b764527f4 70 const double lowp1_b2 = 1.00000000000;
roosbulthuis 0:c85b764527f4 71
roosbulthuis 0:c85b764527f4 72 //second high pass biquad
roosbulthuis 0:c85b764527f4 73 const double lowp2_a1 = -1.16338171052;
roosbulthuis 0:c85b764527f4 74 const double lowp2_a2 = 0.42191097989;
roosbulthuis 0:c85b764527f4 75 const double lowp2_b0 = 1.00000000000;
roosbulthuis 0:c85b764527f4 76 const double lowp2_b1 = 2.00000000000;
roosbulthuis 0:c85b764527f4 77 const double lowp2_b2 = 1.00000000000;
roosbulthuis 0:c85b764527f4 78
roosbulthuis 0:c85b764527f4 79 //third high pass biquad
roosbulthuis 0:c85b764527f4 80 const double lowp3_a1 = -1.42439823874;
roosbulthuis 0:c85b764527f4 81 const double lowp3_a2 = 0.74093118112;
roosbulthuis 0:c85b764527f4 82 const double lowp3_b0 = 1.00000000000;
roosbulthuis 0:c85b764527f4 83 const double lowp3_b1 = 2.00000000000;
roosbulthuis 0:c85b764527f4 84 const double lowp3_b2 = 1.00000000000;
roosbulthuis 0:c85b764527f4 85
roosbulthuis 0:c85b764527f4 86 //input to each filter is output of the filter before(excl. first which uses input_sample)
roosbulthuis 0:c85b764527f4 87 /* NOT SURE IF PASSING V1 AND V2 IS CORRECT,
roosbulthuis 0:c85b764527f4 88 WILL IT UPDATE IN THE MEMORY POSITION SO THAT
roosbulthuis 0:c85b764527f4 89 V1 IS CHANGED GLOBALLY */
roosbulthuis 0:c85b764527f4 90
roosbulthuis 0:c85b764527f4 91 //highpass
roosbulthuis 0:c85b764527f4 92
roosbulthuis 2:1d29b91bc46a 93 double highpass_filter(double input, double v1, double v2)
roosbulthuis 0:c85b764527f4 94 {
roosbulthuis 0:c85b764527f4 95 double y1 = biquad(input, v1, v2, highp1_a1, highp1_a2, highp1_b0, highp1_b1, highp1_b2);
roosbulthuis 0:c85b764527f4 96 double y2 = biquad(y1, v1, v2, highp2_a1, highp2_a2, highp2_b0, highp2_b1, highp2_b2);
roosbulthuis 0:c85b764527f4 97 double y3 = biquad(y2, v1, v2, highp3_a1, highp3_a2, highp3_b0, highp3_b1, highp3_b2);
roosbulthuis 0:c85b764527f4 98
roosbulthuis 0:c85b764527f4 99 return y3;
roosbulthuis 0:c85b764527f4 100 }
roosbulthuis 0:c85b764527f4 101
roosbulthuis 0:c85b764527f4 102
roosbulthuis 0:c85b764527f4 103 //rectifier
roosbulthuis 0:c85b764527f4 104 double rectify(double y3)
roosbulthuis 0:c85b764527f4 105 {
roosbulthuis 0:c85b764527f4 106 y3 = fabs(y3);
roosbulthuis 0:c85b764527f4 107 return y3;
roosbulthuis 0:c85b764527f4 108 }
roosbulthuis 0:c85b764527f4 109
roosbulthuis 0:c85b764527f4 110 //lowpass
roosbulthuis 0:c85b764527f4 111
roosbulthuis 2:1d29b91bc46a 112 double lowpass_filter(double y3, double v1, double v2)
roosbulthuis 0:c85b764527f4 113 {
roosbulthuis 0:c85b764527f4 114 double y4 = biquad(y3, v1, v2, lowp1_a1, lowp1_a2, lowp1_b0, lowp1_b1, lowp1_b2);
roosbulthuis 0:c85b764527f4 115 double y5 = biquad(y4, v1, v2, lowp2_a1, lowp2_a2, lowp2_b0, lowp2_b1, lowp2_b2);
roosbulthuis 0:c85b764527f4 116 double filtered_signal = biquad(y5, v1, v2, lowp3_a1, lowp3_a2, lowp3_b0, lowp3_b1, lowp3_b2);
roosbulthuis 0:c85b764527f4 117
roosbulthuis 0:c85b764527f4 118 return filtered_signal;
roosbulthuis 0:c85b764527f4 119 }
roosbulthuis 0:c85b764527f4 120
roosbulthuis 2:1d29b91bc46a 121 double filter(double input, double v1, double v2)
roosbulthuis 0:c85b764527f4 122 {
roosbulthuis 0:c85b764527f4 123 /* function passes the input through the three filters
roosbulthuis 0:c85b764527f4 124 returns the final output value as filtered sample
roosbulthuis 0:c85b764527f4 125 this is used in check_state() function to determine state of system
roosbulthuis 0:c85b764527f4 126 */
roosbulthuis 0:c85b764527f4 127 double y1 = highpass_filter(input, v1, v2);
roosbulthuis 0:c85b764527f4 128 double y2 = rectify(y1);
roosbulthuis 0:c85b764527f4 129 double filtered_signal = lowpass_filter(y2, v1, v2);
roosbulthuis 0:c85b764527f4 130
roosbulthuis 0:c85b764527f4 131 return filtered_signal;
roosbulthuis 0:c85b764527f4 132 }
roosbulthuis 0:c85b764527f4 133
roosbulthuis 0:c85b764527f4 134 double test=1;
roosbulthuis 0:c85b764527f4 135
roosbulthuis 0:c85b764527f4 136 void sample()
roosbulthuis 0:c85b764527f4 137 {
roosbulthuis 0:c85b764527f4 138 input_left = analog_emg_left.read();
roosbulthuis 0:c85b764527f4 139 input_right = analog_emg_right.read();
roosbulthuis 0:c85b764527f4 140 filter_left = filter(input_left, v1_left, v2_left);
roosbulthuis 0:c85b764527f4 141 filter_right = filter(input_right, v1_right, v2_right);
roosbulthuis 0:c85b764527f4 142
roosbulthuis 1:f32f8eac8af1 143 scope.set(1, input_left);
roosbulthuis 1:f32f8eac8af1 144 scope.set(0, filter_left);
roosbulthuis 0:c85b764527f4 145 scope.send();
roosbulthuis 0:c85b764527f4 146 }
roosbulthuis 0:c85b764527f4 147
roosbulthuis 0:c85b764527f4 148
roosbulthuis 0:c85b764527f4 149 int main()
roosbulthuis 0:c85b764527f4 150 {
roosbulthuis 0:c85b764527f4 151 sample_timer.attach(&sample, 0.002);
roosbulthuis 0:c85b764527f4 152 while(1){}
roosbulthuis 0:c85b764527f4 153 }