Nick van den Berg / Mbed 2 deprecated EMGfilter24

Filter for EMG signals The signal will be filtered using a notch, highpass and lowpass filter. The filtered signal will be compared to a preset threshold and according to the strength of the signal the program will perform an action. In this case it will assign a colour to a led.

Dependencies:   HIDScope MODSERIAL mbed

Fork of EMGfilter24 by Steven Spoolder

main.cpp@6:d2e6404e1cb8, 2016-11-01 (annotated)

Committer:
Nickname
Date:
Tue Nov 01 09:18:44 2016 +0000
Revision:
6:d2e6404e1cb8
Parent:
5:51a28834cd5b 
Filter with extra notch filter 100 Hz

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Nickname 6:d2e6404e1cb8 1 #include "mbed.h"
Iknowright 0:41226c0fd285 2 #include "BiQuad.h"
Iknowright 0:41226c0fd285 3 #include "HIDScope.h"
Iknowright 0:41226c0fd285 4 #include "MODSERIAL.h"
Nickname 3:faed8b7f6542 5 #include <math.h>
Iknowright 0:41226c0fd285 6
Nickname 1:6081dc1ecd1f 7 MODSERIAL pc(USBTX, USBRX);
Iknowright 4:fcada70891c5 8 Ticker sampleTicker;//Ticker that measures the EMG signal every 0,002 seconds and filters it
Iknowright 4:fcada70891c5 9 Ticker goTicker; //Ticker that checks if the signal threshold is reached each time a new filtered EMG signal comes in and determines if the motors are going to rotate
Iknowright 4:fcada70891c5 10 AnalogIn emgl(A1);//Labels are attached to the olimex shields the left tricep should obviously be connected to the should with an L label on it
Iknowright 4:fcada70891c5 11 AnalogIn emgr(A0);
Nickname 3:faed8b7f6542 12 DigitalOut ledG(LED_GREEN);
Nickname 3:faed8b7f6542 13 DigitalOut ledB(LED_BLUE);
Nickname 3:faed8b7f6542 14 DigitalOut ledR(LED_RED);
Iknowright 4:fcada70891c5 15 HIDScope scope(2); //scope has two ports for the two EMG signals
Iknowright 4:fcada70891c5 16
Iknowright 4:fcada70891c5 17 /*coefficients of each filter
Iknowright 4:fcada70891c5 18 lno = left tricep notch filter
Iknowright 4:fcada70891c5 19 lhf = left tricep high pass filter
Iknowright 4:fcada70891c5 20 llf = left tricep lowpass filter
Iknowright 4:fcada70891c5 21 same goes for rno etc.
Iknowright 4:fcada70891c5 22 */
Iknowright 4:fcada70891c5 23 double lno_b0 = 0.9911;
Iknowright 4:fcada70891c5 24 double lno_b1 = -1.6036;
Iknowright 4:fcada70891c5 25 double lno_b2 = 0.9911;
Iknowright 4:fcada70891c5 26 double lno_a1 = -1.603;
Iknowright 4:fcada70891c5 27 double lno_a2 = 0.9822;
Iknowright 4:fcada70891c5 28
Iknowright 4:fcada70891c5 29 double rno_b0 = 0.9911;
Iknowright 4:fcada70891c5 30 double rno_b1 = -1.6036;
Iknowright 4:fcada70891c5 31 double rno_b2 = 0.9911;
Iknowright 4:fcada70891c5 32 double rno_a1 = -1.603;
Nickname 6:d2e6404e1cb8 33 double rno_a2 = 0.9822;
Nickname 6:d2e6404e1cb8 34
Nickname 6:d2e6404e1cb8 35
Nickname 6:d2e6404e1cb8 36 double lno2_b0 = 0.9824;
Nickname 6:d2e6404e1cb8 37 double lno2_b1 = -0.6071;
Nickname 6:d2e6404e1cb8 38 double lno2_b2 = 0.9824;
Nickname 6:d2e6404e1cb8 39 double lno2_a1 = -0.6071;
Nickname 6:d2e6404e1cb8 40 double lno2_a2 = 0.9647;
Nickname 6:d2e6404e1cb8 41
Nickname 6:d2e6404e1cb8 42 double rno2_b0 = 0.9824;
Nickname 6:d2e6404e1cb8 43 double rno2_b1 = -0.6071;
Nickname 6:d2e6404e1cb8 44 double rno2_b2 = 0.9824;
Nickname 6:d2e6404e1cb8 45 double rno2_a1 = -0.6071;
Nickname 6:d2e6404e1cb8 46 double rno2_a2 = 0.9647;
Nickname 3:faed8b7f6542 47
Iknowright 4:fcada70891c5 48
Iknowright 4:fcada70891c5 49 double lhf_b0 = 0.9355;
Iknowright 4:fcada70891c5 50 double lhf_b1 = -1.8711;
Iknowright 4:fcada70891c5 51 double lhf_b2 = 0.9355;
Iknowright 4:fcada70891c5 52 double lhf_a1 = -1.8669;
Iknowright 4:fcada70891c5 53 double lhf_a2 = 0.8752;
Nickname 1:6081dc1ecd1f 54
Iknowright 4:fcada70891c5 55 double rhf_b0 = 0.9355;
Iknowright 4:fcada70891c5 56 double rhf_b1 = -1.8711;
Iknowright 4:fcada70891c5 57 double rhf_b2 = 0.9355;
Iknowright 4:fcada70891c5 58 double rhf_a1 = -1.8669;
Iknowright 4:fcada70891c5 59 double rhf_a2 = 0.8752;
Iknowright 4:fcada70891c5 60
Iknowright 4:fcada70891c5 61
Iknowright 4:fcada70891c5 62 double llf_b0 = 8.7656e-5;
Iknowright 4:fcada70891c5 63 double llf_b1 = 1.17531e-4;
Iknowright 4:fcada70891c5 64 double llf_b2 = 8.7656e-5;
Iknowright 4:fcada70891c5 65 double llf_a1 = -1.9733;
Iknowright 4:fcada70891c5 66 double llf_a2 = 0.9737;
Iknowright 0:41226c0fd285 67
Iknowright 4:fcada70891c5 68 double rlf_b0 = 8.7656e-5;
Iknowright 4:fcada70891c5 69 double rlf_b1 = 1.17531e-4;
Iknowright 4:fcada70891c5 70 double rlf_b2 = 8.7656e-5;
Iknowright 4:fcada70891c5 71 double rlf_a1 = -1.9733;
Iknowright 4:fcada70891c5 72 double rlf_a2 = 0.9737;
Iknowright 4:fcada70891c5 73
Iknowright 4:fcada70891c5 74
Iknowright 4:fcada70891c5 75 //starting values of the biquads of the corresponding filters
Iknowright 4:fcada70891c5 76 double lno_v1 = 0, lno_v2 = 0;
Nickname 6:d2e6404e1cb8 77 double lno2_v1 = 0, lno2_v2 = 0;
Iknowright 4:fcada70891c5 78 double lhf_v1 = 0, lhf_v2 = 0;
Iknowright 4:fcada70891c5 79 double llf_v1 = 0, llf_v2 = 0;
Iknowright 4:fcada70891c5 80
Iknowright 4:fcada70891c5 81 double rno_v1 = 0, rno_v2 = 0;
Nickname 6:d2e6404e1cb8 82 double rno2_v1 = 0, rno2_v2 = 0;
Iknowright 4:fcada70891c5 83 double rhf_v1 = 0, rhf_v2 = 0;
Iknowright 4:fcada70891c5 84 double rlf_v1 = 0, rlf_v2 = 0;
Nickname 1:6081dc1ecd1f 85
Iknowright 4:fcada70891c5 86 /* declaration of the outputs of each biquad.
Iknowright 4:fcada70891c5 87 the output of the previous biquad is the input for the next biquad.
Iknowright 4:fcada70891c5 88 so lno_y goes into lhf_y etc.
Iknowright 4:fcada70891c5 89 */
Iknowright 4:fcada70891c5 90 double lno_y;
Nickname 6:d2e6404e1cb8 91 double lno2_y;
Iknowright 4:fcada70891c5 92 double lhf_y;
Iknowright 4:fcada70891c5 93 double llf_y;
Iknowright 4:fcada70891c5 94 double lrect_y;
Iknowright 4:fcada70891c5 95 double rno_y;
Nickname 6:d2e6404e1cb8 96 double rno2_y;
Iknowright 4:fcada70891c5 97 double rhf_y;
Iknowright 4:fcada70891c5 98 double rlf_y;
Iknowright 4:fcada70891c5 99 double rrect_y;
Iknowright 0:41226c0fd285 100
Iknowright 4:fcada70891c5 101 // set the threshold value for the filtered signal
Iknowright 4:fcada70891c5 102 //if the signal exceeds this value the motors will start to rotate
Iknowright 4:fcada70891c5 103 const double threshold_value = 0.05;
Iknowright 4:fcada70891c5 104
Iknowright 4:fcada70891c5 105 /* declaration of each biquad
Iknowright 4:fcada70891c5 106 The coefficients will be filled in later on in void scopeSend
Iknowright 4:fcada70891c5 107 As said before the input of each biquad is the output of the previous one
Iknowright 4:fcada70891c5 108 The input of the first biquad is the raw EMG signal and the output of the last biquad is the filtered signal.
Iknowright 4:fcada70891c5 109 This is done for both left and right so this makes two chains of 3 biquads */
Iknowright 4:fcada70891c5 110
Iknowright 4:fcada70891c5 111 double biquad_lno(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
Nickname 3:faed8b7f6542 112 const double b1 , const double b2 )
Nickname 1:6081dc1ecd1f 113 {
Nickname 3:faed8b7f6542 114 double v = u - a1*v1 - a2*v2;
Nickname 3:faed8b7f6542 115 double y = b0*v + b1*v1 + b2*v2;
Nickname 3:faed8b7f6542 116 v2 = v1;
Nickname 3:faed8b7f6542 117 v1 = v;
Nickname 3:faed8b7f6542 118 return y;
Nickname 3:faed8b7f6542 119 }
Nickname 3:faed8b7f6542 120
Nickname 6:d2e6404e1cb8 121 double biquad_lno2(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
Nickname 6:d2e6404e1cb8 122 const double b1 , const double b2 )
Nickname 6:d2e6404e1cb8 123 {
Nickname 6:d2e6404e1cb8 124 double v = u - a1*v1 - a2*v2;
Nickname 6:d2e6404e1cb8 125 double y = b0*v + b1*v1 + b2*v2;
Nickname 6:d2e6404e1cb8 126 v2 = v1;
Nickname 6:d2e6404e1cb8 127 v1 = v;
Nickname 6:d2e6404e1cb8 128 return y;
Nickname 6:d2e6404e1cb8 129 }
Nickname 6:d2e6404e1cb8 130
Iknowright 4:fcada70891c5 131 double biquad_lhf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
Nickname 3:faed8b7f6542 132 const double b1 , const double b2 )
Nickname 3:faed8b7f6542 133 {
Nickname 3:faed8b7f6542 134 double v = u - a1*v1 - a2*v2;
Nickname 3:faed8b7f6542 135 double y = b0*v + b1*v1 + b2*v2;
Nickname 3:faed8b7f6542 136 v2 = v1;
Nickname 3:faed8b7f6542 137 v1 = v;
Nickname 3:faed8b7f6542 138 return y;
Nickname 1:6081dc1ecd1f 139 }
Iknowright 0:41226c0fd285 140
Iknowright 4:fcada70891c5 141 double biquad_llf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
Iknowright 4:fcada70891c5 142 const double b1 , const double b2 )
Iknowright 4:fcada70891c5 143 {
Iknowright 4:fcada70891c5 144 double v = u - a1*v1 - a2*v2;
Iknowright 4:fcada70891c5 145 double y = b0*v + b1*v1 + b2*v2;
Iknowright 4:fcada70891c5 146 v2 = v1;
Iknowright 4:fcada70891c5 147 v1 = v;
Iknowright 4:fcada70891c5 148 return y;
Iknowright 4:fcada70891c5 149 }
Iknowright 4:fcada70891c5 150 double biquad_rno(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
Iknowright 4:fcada70891c5 151 const double b1 , const double b2 )
Iknowright 4:fcada70891c5 152 {
Iknowright 4:fcada70891c5 153 double v = u - a1*v1 - a2*v2;
Iknowright 4:fcada70891c5 154 double y = b0*v + b1*v1 + b2*v2;
Iknowright 4:fcada70891c5 155 v2 = v1;
Iknowright 4:fcada70891c5 156 v1 = v;
Iknowright 4:fcada70891c5 157 return y;
Iknowright 4:fcada70891c5 158 }
Iknowright 4:fcada70891c5 159
Nickname 6:d2e6404e1cb8 160 double biquad_rno2(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
Nickname 6:d2e6404e1cb8 161 const double b1 , const double b2 )
Nickname 6:d2e6404e1cb8 162 {
Nickname 6:d2e6404e1cb8 163 double v = u - a1*v1 - a2*v2;
Nickname 6:d2e6404e1cb8 164 double y = b0*v + b1*v1 + b2*v2;
Nickname 6:d2e6404e1cb8 165 v2 = v1;
Nickname 6:d2e6404e1cb8 166 v1 = v;
Nickname 6:d2e6404e1cb8 167 return y;
Nickname 6:d2e6404e1cb8 168 }
Nickname 6:d2e6404e1cb8 169
Iknowright 4:fcada70891c5 170 double biquad_rhf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
Iknowright 4:fcada70891c5 171 const double b1 , const double b2 )
Iknowright 4:fcada70891c5 172 {
Iknowright 4:fcada70891c5 173 double v = u - a1*v1 - a2*v2;
Iknowright 4:fcada70891c5 174 double y = b0*v + b1*v1 + b2*v2;
Iknowright 4:fcada70891c5 175 v2 = v1;
Iknowright 4:fcada70891c5 176 v1 = v;
Iknowright 4:fcada70891c5 177 return y;
Iknowright 4:fcada70891c5 178 }
Iknowright 4:fcada70891c5 179
Iknowright 4:fcada70891c5 180 double biquad_rlf(double u, double&v1 , double&v2 , const double a1 , const double a2 , const double b0 ,
Nickname 3:faed8b7f6542 181 const double b1 , const double b2 )
Nickname 3:faed8b7f6542 182 {
Nickname 3:faed8b7f6542 183 double v = u - a1*v1 - a2*v2;
Nickname 3:faed8b7f6542 184 double y = b0*v + b1*v1 + b2*v2;
Nickname 3:faed8b7f6542 185 v2 = v1;
Nickname 3:faed8b7f6542 186 v1 = v;
Nickname 3:faed8b7f6542 187 return y;
Nickname 3:faed8b7f6542 188 }
Nickname 3:faed8b7f6542 189
Iknowright 4:fcada70891c5 190 /* function that calculates the filtered EMG signal from the raw EMG signal.
Iknowright 4:fcada70891c5 191 So 2 chains of 3 biquads each are calculating the left and the right filtered EMG signal.
Iknowright 4:fcada70891c5 192 After this is calculated, the signals are sent to HIDscope (scope.send) to see what they look like.
Iknowright 4:fcada70891c5 193 The filtered left signal (llf_y) is shown in channel 1, the filtered right signal (rlf_y)is shown in channel 0 (scope.set)*/
Nickname 3:faed8b7f6542 194 void scopeSend(void){
Iknowright 4:fcada70891c5 195 lno_y = biquad_lno(emgl.read(), lno_v1, lno_v2, lno_a1, lno_a2, lno_b0, lno_b1, lno_b2);
Nickname 6:d2e6404e1cb8 196 lno2_y = biquad_lno2(lno_y, lno2_v1, lno2_v2, lno2_a1, lno2_a2, lno2_b0, lno2_b1, lno2_b2);
Nickname 6:d2e6404e1cb8 197 lhf_y = biquad_lhf(lno2_y, lhf_v1, lhf_v2, lhf_a1, lhf_a2, lhf_b0, lhf_b1, lhf_b2);
Iknowright 4:fcada70891c5 198 lrect_y = fabs(lhf_y);
Iknowright 4:fcada70891c5 199 llf_y = biquad_llf(lrect_y, llf_v1, llf_v2, llf_a1, llf_a2, llf_b0, llf_b1, llf_b2)/0.2;
Iknowright 4:fcada70891c5 200 rno_y = biquad_rno(emgr.read(), rno_v1, rno_v2, rno_a1, rno_a2, rno_b0, rno_b1, rno_b2);
Nickname 6:d2e6404e1cb8 201 rno2_y = biquad_rno2(rno_y, rno2_v1, rno2_v2, rno2_a1, rno2_a2, rno2_b0, rno2_b1, rno2_b2);
Nickname 6:d2e6404e1cb8 202 rhf_y = biquad_rhf(rno2_y, rhf_v1, rhf_v2, rhf_a1, rhf_a2, rhf_b0, rhf_b1, rhf_b2);
Iknowright 4:fcada70891c5 203 rrect_y = fabs(rhf_y);
Iknowright 4:fcada70891c5 204 rlf_y = biquad_rlf(rrect_y, rlf_v1, rlf_v2, rlf_a1, rlf_a2, rlf_b0, rlf_b1, rlf_b2)/0.2;
Iknowright 4:fcada70891c5 205 scope.set(1, llf_y);
Iknowright 4:fcada70891c5 206 scope.set(0, rlf_y);
Nickname 3:faed8b7f6542 207 scope.send();
Nickname 3:faed8b7f6542 208
Nickname 3:faed8b7f6542 209 }
Iknowright 4:fcada70891c5 210
Iknowright 4:fcada70891c5 211 //function that compares the filtered EMG signal to the set threshold and determines what colour the led should be
Iknowright 4:fcada70891c5 212 //This led is for feedback purposes only and should obviously be replaced by the motors
Iknowright 4:fcada70891c5 213 void threshold(){
Iknowright 4:fcada70891c5 214 //If the right signal exceeds the threshold, the led should turn blue
Iknowright 4:fcada70891c5 215 if (rlf_y > threshold_value){
Iknowright 4:fcada70891c5 216 ledB = 0;
Iknowright 4:fcada70891c5 217 ledR = 1;
Iknowright 4:fcada70891c5 218 ledG = 1;
Nickname 5:51a28834cd5b 219 //If right already exceeds the threshold, and you also exceed the threshold with your left tricep the motors will be turned off.
Nickname 5:51a28834cd5b 220 //This works both ways.
Nickname 5:51a28834cd5b 221 if (llf_y > threshold_value){
Nickname 5:51a28834cd5b 222 ledB = 1;
Nickname 5:51a28834cd5b 223 ledR = 1;
Nickname 5:51a28834cd5b 224 ledG = 0;
Nickname 5:51a28834cd5b 225 }
Nickname 3:faed8b7f6542 226 }
Iknowright 4:fcada70891c5 227 //If the left signal exceeds the threshold, the led should turn red
Iknowright 4:fcada70891c5 228 else if (llf_y > threshold_value){
Iknowright 4:fcada70891c5 229 ledB = 1;
Iknowright 4:fcada70891c5 230 ledR = 0;
Iknowright 4:fcada70891c5 231 ledG = 1;
Nickname 5:51a28834cd5b 232
Nickname 3:faed8b7f6542 233 }
Nickname 5:51a28834cd5b 234
Nickname 5:51a28834cd5b 235
Iknowright 4:fcada70891c5 236 //If no signal exceeds the threshold, the led should be off
Iknowright 4:fcada70891c5 237 else {
Iknowright 4:fcada70891c5 238 ledB=1;
Iknowright 4:fcada70891c5 239 ledG=1;
Iknowright 4:fcada70891c5 240 ledR=1;
Nickname 3:faed8b7f6542 241 }
Iknowright 4:fcada70891c5 242 }
Iknowright 4:fcada70891c5 243
Iknowright 0:41226c0fd285 244
Iknowright 0:41226c0fd285 245 int main(){
Iknowright 4:fcada70891c5 246
Iknowright 4:fcada70891c5 247
Nickname 1:6081dc1ecd1f 248
Iknowright 4:fcada70891c5 249 /*Attach the 'sample' function to the timer 'sample_timer'.
Iknowright 4:fcada70891c5 250 this ensures that 'sample' is executed every... 0.002 seconds = 500 Hz
Iknowright 4:fcada70891c5 251 emgSampleTicker.attach(&sample, 0.002);
Iknowright 4:fcada70891c5 252 empty loop, sample() is executed periodically
Iknowright 4:fcada70891c5 253 The same goes for the goTicker. It checks if the threshold is reached at the same rate new a new EMG signal comes in*/
Nickname 3:faed8b7f6542 254 sampleTicker.attach(scopeSend,0.002);
Nickname 3:faed8b7f6542 255 goTicker.attach(threshold,0.002);
Nickname 1:6081dc1ecd1f 256
Nickname 3:faed8b7f6542 257 while(1) {
Iknowright 4:fcada70891c5 258
Iknowright 4:fcada70891c5 259 }
Nickname 3:faed8b7f6542 260 }