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@5:cf85007ce40a, 2015-10-27 (annotated)

Committer:
yohoo15
Date:
Tue Oct 27 08:54:37 2015 +0000
Revision:
5:cf85007ce40a
Parent:
4:d32946ab9c54 
this time with go flag;

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