Teun van der Molen / Mbed 2 deprecated frdm_EMG

Emg filter function script for a uni project. Made by Teun van der Molen

Dependencies:   HIDScope MODSERIAL mbed

main.cpp@10:b11eacb391ea, 2015-09-25 (annotated)

Committer:
teunman
Date:
Fri Sep 25 08:53:19 2015 +0000
Revision:
10:b11eacb391ea
Parent:
9:2b9240084724 
added a 50Hz notch filter and did a clean up. Also added more documentation and did some rubber ducking

Who changed what in which revision?

UserRevisionLine numberNew contents of line
teunman 0:674026fdd982 1 #include "mbed.h"
teunman 0:674026fdd982 2 #include "HIDScope.h"
teunman 0:674026fdd982 3 #include "math.h"
teunman 0:674026fdd982 4 // Define the HIDScope and Ticker object
teunman 4:1baefd1397d6 5 HIDScope scope(3);
teunman 10:b11eacb391ea 6
teunman 0:674026fdd982 7 Ticker scopeTimer;
teunman 1:75f61e111ed0 8 Ticker biquadTicker;
teunman 10:b11eacb391ea 9
teunman 10:b11eacb391ea 10
teunman 8:54f0a76d35f4 11 DigitalOut light(LED2);
teunman 8:54f0a76d35f4 12 DigitalIn knop(SW3);
teunman 10:b11eacb391ea 13 AnalogIn an_in(A0); //The input from the emg board, make sure that the jumper pin on the EMG board corresponds with the value in the brackets here!!! (see bb pptx for details on jumper placement, default is A0)
teunman 10:b11eacb391ea 14
teunman 10:b11eacb391ea 15
teunman 10:b11eacb391ea 16
teunman 10:b11eacb391ea 17
teunman 10:b11eacb391ea 18 light = 1; //Light is off by default
teunman 10:b11eacb391ea 19 double c = 5; //calibration value (in volts)
teunman 8:54f0a76d35f4 20 double n = 0; // counter for calibration
teunman 8:54f0a76d35f4 21 bool calib = false; //changes into false when calibration is done (reset program for new calibration);
teunman 10:b11eacb391ea 22 double v1=0, v2=0, u=0, y=0, fy=0, fv1=0, fv2=0, fu=0, ny=0, nv1=0, nv2=0, nu=0;;
teunman 10:b11eacb391ea 23
teunman 10:b11eacb391ea 24
teunman 10:b11eacb391ea 25 //Notch filter (n stand for notch filter (this is in fact the very first filter but is rarely changed) to alter this filter add "n" to all coefficents example: nb0,nb1 etc.)
teunman 10:b11eacb391ea 26 const double nb0 = 0.8206743576788857,nb1 = -1.5610153912536877,nb2 = 0.8206743576788857,na1 = -1.5610153912536877,na2 = 0.6413487153577715; //Notch filter Fc = 50 Hz Fs 1000Hz
teunman 10:b11eacb391ea 27
teunman 10:b11eacb391ea 28
teunman 10:b11eacb391ea 29 // 100 Hz filters (obsolete)
teunman 1:75f61e111ed0 30
teunman 5:56725d9362ee 31 //const double b0 = 0.9999999999999999,b1 = 1.9999999999999998,b2 = 0.9999999999999999, a1 = 1.9999999999999998 ,a2 = 0.9999999999999998; //low-pass Fc = 50hz fs = 100hz
teunman 5:56725d9362ee 32 //const double fb0 = 0.02008333102602092 ,fb1 = 0.04016666205204184 ,fb2 = 0.02008333102602092, fa1 = -1.5610153912536877 ,fa2 = 0.6413487153577715; //low-pass Fc = 5hz fs = 100hz
teunman 5:56725d9362ee 33 //const double b0 = 0.8005910266528649,b1 = -1.6011820533057297,b2 = 0.8005910266528649,a1 = -1.5610153912536877,a2 = 0.6413487153577715; //high-pass Fc = 5hz fs = 100hz
teunman 5:56725d9362ee 34 //const double b0 = 0.007820199259120319,b1 = 0.015640398518240638,b2 = 0.007820199259120319,a1 = -1.7347238224240125,a2 = 0.7660046194604936; //low-pass Fc = 3hz fs = 100hz
teunman 5:56725d9362ee 35 //const double b0 = 0.0009446914586925257,b1 = 0.0018893829173850514,b2 = 0.0009446914586925257,a1 = -1.911196288237583,a2 = 0.914975054072353; //low-pass Fc = 1hz fs = 100hz
teunman 5:56725d9362ee 36 //const double b0 = 0.956542835577484, b1 = -1.913085671154968, b2 = 0.956542835577484, a1 = -1.911196288237583, a2 = 0.914975054072353; //high-pass Fc = 1hz fs = 100hz
teunman 3:499c71ca30a0 37
teunman 10:b11eacb391ea 38
teunman 10:b11eacb391ea 39
teunman 10:b11eacb391ea 40 // 1000 Hz filters (f stands for the seccond filter, so make sure the first coefiecients are b0,b1 etc. and the seccond filter is fb0,fb1,fb2 etc.)
teunman 10:b11eacb391ea 41
teunman 10:b11eacb391ea 42 //const double b0 = 0.00008765553769759188,b1 = 0.00017531107539518376,b2 = 0.00008765553769759188,a1 = -1.9733440008737442,a2 = 0.9736946230245347;//low-pass Fc = 3Hz fs = 1000hz
teunman 10:b11eacb391ea 43 const double fb0 = 0.00034604125149151127,fb1 = 0.0006920825029830225, fb2 = 0.00034604125149151127 ,fa1 = -1.9466970561224466 ,fa2 = 0.9480812211284125; //low-pass Fc = 6Hz fs = 1000hz
teunman 7:43f2f7039841 44 //const double fb0 = 0.9149684297741606, fb1 = -1.8299368595483212, fb2 = 0.9149684297741606 ,fa1 = -1.8226935021735358, fa2 = 0.8371802169231065; // High-pass Fc = 20Hz fs = 1000hz
teunman 5:56725d9362ee 45 //const double fb0 = 0.8948577513857248 ,fb1 = -1.7897155027714495, fb2 = 0.8948577513857248,fa1 = -1.7786300789392977,fa2 = 0.8008009266036016; // High-pass Fc = 25Hz fs = 1000Hz
teunman 6:4cbf5c66e2fb 46 //const double b0 = 0.005542711916075981,b1 = 0.011085423832151962,b2 = 0.005542711916075981,a1 = -1.7786300789392977,a2 = 0.8008009266036016; //Low-pass Fc = 25Hz fs=1000Hz
teunman 6:4cbf5c66e2fb 47 //const double fb0 = 0.9780302754084559,fb1 = -1.9560605508169118,fb2 = 0.9780302754084559,fa1 = -1.9555778328194147,fa2 = 0.9565432688144089; //high-pass Fc = 6hz fs = 1000hz
teunman 10:b11eacb391ea 48 const double b0 = 0.9911535113858849,b1 = -1.9823070227717698,b2 = 0.9911535113858849,a1 = -1.9822287623675816,a2 = 0.982385283175958; //High-pass Fc = 2Hz fs = 1000Hz
teunman 6:4cbf5c66e2fb 49 //const double fb0 = 0.0036216786873927774,fb1 = 0.007243357374785555,fb2 = 0.0036216786873927774,fa1 = -1.8226935021735358,fa2 = 0.8371802169231065; ///Low-pass Fc = 20 Hz Fs = 1000Hz
teunman 6:4cbf5c66e2fb 50
teunman 3:499c71ca30a0 51
teunman 3:499c71ca30a0 52
teunman 10:b11eacb391ea 53
teunman 3:499c71ca30a0 54
teunman 10:b11eacb391ea 55
teunman 10:b11eacb391ea 56 // The data read and send function for HIDscope (can send any value to the HIDscope for read out)
teunman 3:499c71ca30a0 57 void scopeSend()
teunman 3:499c71ca30a0 58 {
teunman 8:54f0a76d35f4 59 scope.set(0,c);
teunman 4:1baefd1397d6 60 scope.set(1,fy);
teunman 10:b11eacb391ea 61 scope.set(2,an_in);
teunman 3:499c71ca30a0 62
teunman 3:499c71ca30a0 63 scope.send();
teunman 8:54f0a76d35f4 64
teunman 10:b11eacb391ea 65
teunman 8:54f0a76d35f4 66
teunman 8:54f0a76d35f4 67
teunman 3:499c71ca30a0 68 }
teunman 3:499c71ca30a0 69
teunman 10:b11eacb391ea 70 void computeBiquad(){ //The filter function (is called at the same frequency of the scope function (not sure if timing is fully correct here) and does the filter calculations)
teunman 7:43f2f7039841 71
teunman 10:b11eacb391ea 72 double nv = an_in - na1*nv1 - na2*nv2; //notch filter at 50Hz for laptop adapter ny is the filtered output
teunman 10:b11eacb391ea 73 ny= nb0*nv + nb1*nv1 +nb2*nv2;
teunman 10:b11eacb391ea 74 nv2=nv1;
teunman 10:b11eacb391ea 75 nv1=nv;
teunman 7:43f2f7039841 76
teunman 10:b11eacb391ea 77 double v = ny - a1*v1 - a2*v2; //First filter (see filters to check values) y is the filtered output
teunman 1:75f61e111ed0 78 y= b0*v + b1*v1 +b2*v2;
teunman 1:75f61e111ed0 79 v2=v1;
teunman 1:75f61e111ed0 80 v1=v;
teunman 4:1baefd1397d6 81
teunman 10:b11eacb391ea 82 y = abs(y); //Rectifier y is the rectified output
teunman 7:43f2f7039841 83
teunman 6:4cbf5c66e2fb 84
teunman 10:b11eacb391ea 85 double fv = y - fa1*fv1 - fa2*fv2; //Second Filter (see filters to check values) fy is the filtered output
teunman 4:1baefd1397d6 86 fy= fb0*fv + fb1*fv1 + fb2*fv2;
teunman 4:1baefd1397d6 87 fv2=fv1;
teunman 4:1baefd1397d6 88 fv1=fv;
teunman 7:43f2f7039841 89
teunman 10:b11eacb391ea 90 //fy = abs(fy); //Optional final rectifier (default should be off)
teunman 1:75f61e111ed0 91 }
teunman 3:499c71ca30a0 92
teunman 0:674026fdd982 93
teunman 8:54f0a76d35f4 94
teunman 8:54f0a76d35f4 95
teunman 8:54f0a76d35f4 96
teunman 8:54f0a76d35f4 97
teunman 8:54f0a76d35f4 98
teunman 8:54f0a76d35f4 99
teunman 0:674026fdd982 100 int main()
teunman 0:674026fdd982 101 {
teunman 8:54f0a76d35f4 102
teunman 5:56725d9362ee 103 // Attach the data read and send function at 1000 Hz
teunman 5:56725d9362ee 104 scopeTimer.attach_us(&scopeSend, 1e5);
teunman 10:b11eacb391ea 105 // Attach the filter calculation function at 1000 Hz
teunman 5:56725d9362ee 106 biquadTicker.attach(&computeBiquad,0.0001f);
teunman 10:b11eacb391ea 107
teunman 10:b11eacb391ea 108
teunman 10:b11eacb391ea 109
teunman 10:b11eacb391ea 110 while(1) { //the everlasting while loop
teunman 10:b11eacb391ea 111
teunman 10:b11eacb391ea 112
teunman 10:b11eacb391ea 113 if (fy >= c and calib == false){ //if there is no calibration going on and the filtered emg is above calibration the light goes on
teunman 10:b11eacb391ea 114 light = 0;
teunman 10:b11eacb391ea 115 }
teunman 8:54f0a76d35f4 116
teunman 8:54f0a76d35f4 117
teunman 10:b11eacb391ea 118 else if (knop == 0){ //pressing the button starts calibration
teunman 10:b11eacb391ea 119
teunman 10:b11eacb391ea 120 c = 0;
teunman 10:b11eacb391ea 121 n = 0;
teunman 10:b11eacb391ea 122 light = 0; //light is on during calibration
teunman 10:b11eacb391ea 123 calib = true;
teunman 10:b11eacb391ea 124
teunman 10:b11eacb391ea 125 for(;calib == true and n <= 20.00; n = n + 1) { //this loop ads the calibration value from the output of the filter, this also decides the time of calibration (see wait in combination with the n maximum)
teunman 9:2b9240084724 126
teunman 10:b11eacb391ea 127 c = c + fy;
teunman 10:b11eacb391ea 128 wait(0.1);
teunman 9:2b9240084724 129
teunman 10:b11eacb391ea 130 }//end for
teunman 9:2b9240084724 131
teunman 9:2b9240084724 132
teunman 9:2b9240084724 133
teunman 10:b11eacb391ea 134 light = 1; // light is off for a while after calibration
teunman 10:b11eacb391ea 135 calib = false; // shuts of calibration
teunman 10:b11eacb391ea 136 c = c/n; //take the average of the emg output during calibration and set the calibration value
teunman 10:b11eacb391ea 137
teunman 9:2b9240084724 138
teunman 10:b11eacb391ea 139 wait(5); // you can only calibrate every this amount of seconds
teunman 10:b11eacb391ea 140 } //end else if
teunman 10:b11eacb391ea 141
teunman 8:54f0a76d35f4 142 else{
teunman 10:b11eacb391ea 143 light = 1; //light stays off by default
teunman 6:4cbf5c66e2fb 144 }
teunman 10:b11eacb391ea 145
teunman 10:b11eacb391ea 146
teunman 10:b11eacb391ea 147 } //END while
teunman 10:b11eacb391ea 148 }//END main