Biorobotics / Mbed 2 deprecated frdm_EMG

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

Dependencies:   HIDScope MODSERIAL mbed

Fork of frdm_EMG by Teun van der Molen

main.cpp@11:d8dd024f9784, 2015-10-19 (annotated)

Committer:
teunman
Date:
Mon Oct 19 09:58:49 2015 +0000
Revision:
11:d8dd024f9784
Parent:
10:b11eacb391ea 
latest version. Cleaned up some documentation not much else;

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 11:d8dd024f9784 5 HIDScope scope(4);
teunman 10:b11eacb391ea 6
teunman 0:674026fdd982 7 Ticker scopeTimer;
teunman 11:d8dd024f9784 8 Ticker FilterTicker;
teunman 10:b11eacb391ea 9
teunman 10:b11eacb391ea 10
teunman 11:d8dd024f9784 11 ///////////////////////////////////////////////////MBED PORTS////////////////////////////////////////////////////////////////////
teunman 11:d8dd024f9784 12
teunman 8:54f0a76d35f4 13 DigitalOut light(LED2);
teunman 8:54f0a76d35f4 14 DigitalIn knop(SW3);
teunman 10:b11eacb391ea 15 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 16
teunman 10:b11eacb391ea 17
teunman 11:d8dd024f9784 18 /////////////////////////////////////////FILTER VALUES///////////////////////////////////////////////////////////////////////////////////
teunman 10:b11eacb391ea 19
teunman 11:d8dd024f9784 20 const float lag = 2500; //NO. of data points for moving average filter
teunman 11:d8dd024f9784 21 float values [2500] = { }; // The array that stores the values for Movavg !!!!! the value between "[]" MUST BE THE SAME AS LAG!!!!!!!!! (needs to be a numerical value so can't fill in lag here :(
teunman 11:d8dd024f9784 22 int n_avg = 0; // Counter for movavg
teunman 11:d8dd024f9784 23 float y_avg = 0; // value for movavg
teunman 11:d8dd024f9784 24
teunman 11:d8dd024f9784 25 double ffy = 0; //movavg filterd output
teunman 11:d8dd024f9784 26
teunman 10:b11eacb391ea 27 double c = 5; //calibration value (in volts)
teunman 8:54f0a76d35f4 28 double n = 0; // counter for calibration
teunman 8:54f0a76d35f4 29 bool calib = false; //changes into false when calibration is done (reset program for new calibration);
teunman 10:b11eacb391ea 30 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 31
teunman 10:b11eacb391ea 32
teunman 10:b11eacb391ea 33 //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 34 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 35
teunman 10:b11eacb391ea 36
teunman 10:b11eacb391ea 37 // 100 Hz filters (obsolete)
teunman 1:75f61e111ed0 38
teunman 5:56725d9362ee 39 //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 40 //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 41 //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 42 //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 43 //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 44 //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 45
teunman 10:b11eacb391ea 46
teunman 10:b11eacb391ea 47
teunman 10:b11eacb391ea 48 // 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 49
teunman 10:b11eacb391ea 50 //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 51 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 52 //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 53 //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 54 //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 55 //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 56 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 57 //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 58
teunman 3:499c71ca30a0 59
teunman 11:d8dd024f9784 60 ////////////////////////////////////////////////////////////////////HID SCOPE FUNCTION//////////////////////////////////////////////////////////////////////////////
teunman 10:b11eacb391ea 61
teunman 3:499c71ca30a0 62
teunman 10:b11eacb391ea 63
teunman 10:b11eacb391ea 64 // The data read and send function for HIDscope (can send any value to the HIDscope for read out)
teunman 3:499c71ca30a0 65 void scopeSend()
teunman 3:499c71ca30a0 66 {
teunman 11:d8dd024f9784 67 scope.set(0,an_in);
teunman 4:1baefd1397d6 68 scope.set(1,fy);
teunman 11:d8dd024f9784 69 scope.set(2,ffy);
teunman 11:d8dd024f9784 70 scope.set(3,c);
teunman 3:499c71ca30a0 71 scope.send();
teunman 8:54f0a76d35f4 72
teunman 10:b11eacb391ea 73
teunman 8:54f0a76d35f4 74
teunman 8:54f0a76d35f4 75
teunman 3:499c71ca30a0 76 }
teunman 3:499c71ca30a0 77
teunman 11:d8dd024f9784 78
teunman 11:d8dd024f9784 79 ///////////////////////////////////////////////////////////////////////FILTER CASCADE////////////////////////////////////////////////////////////////////////
teunman 11:d8dd024f9784 80
teunman 10:b11eacb391ea 81 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 82
teunman 10:b11eacb391ea 83 double nv = an_in - na1*nv1 - na2*nv2; //notch filter at 50Hz for laptop adapter ny is the filtered output
teunman 10:b11eacb391ea 84 ny= nb0*nv + nb1*nv1 +nb2*nv2;
teunman 10:b11eacb391ea 85 nv2=nv1;
teunman 10:b11eacb391ea 86 nv1=nv;
teunman 7:43f2f7039841 87
teunman 10:b11eacb391ea 88 double v = ny - a1*v1 - a2*v2; //First filter (see filters to check values) y is the filtered output
teunman 1:75f61e111ed0 89 y= b0*v + b1*v1 +b2*v2;
teunman 1:75f61e111ed0 90 v2=v1;
teunman 1:75f61e111ed0 91 v1=v;
teunman 4:1baefd1397d6 92
teunman 10:b11eacb391ea 93 y = abs(y); //Rectifier y is the rectified output
teunman 7:43f2f7039841 94
teunman 6:4cbf5c66e2fb 95
teunman 10:b11eacb391ea 96 double fv = y - fa1*fv1 - fa2*fv2; //Second Filter (see filters to check values) fy is the filtered output
teunman 4:1baefd1397d6 97 fy= fb0*fv + fb1*fv1 + fb2*fv2;
teunman 4:1baefd1397d6 98 fv2=fv1;
teunman 4:1baefd1397d6 99 fv1=fv;
teunman 7:43f2f7039841 100
teunman 10:b11eacb391ea 101 //fy = abs(fy); //Optional final rectifier (default should be off)
teunman 11:d8dd024f9784 102
teunman 11:d8dd024f9784 103 values [n_avg] = fy; //Moving Average filter (see filters to check lag etc.)
teunman 11:d8dd024f9784 104 n_avg = n_avg + 1;
teunman 11:d8dd024f9784 105
teunman 11:d8dd024f9784 106 if (n_avg == lag){
teunman 11:d8dd024f9784 107
teunman 11:d8dd024f9784 108 for (int n_divide = 0 ;n_divide <= lag; n_divide = n_divide + 1){
teunman 11:d8dd024f9784 109 y_avg = y_avg + values [n_divide];
teunman 11:d8dd024f9784 110
teunman 11:d8dd024f9784 111 }
teunman 11:d8dd024f9784 112
teunman 11:d8dd024f9784 113 n_avg = 0;
teunman 11:d8dd024f9784 114 ffy = y_avg/lag;
teunman 11:d8dd024f9784 115 y_avg = 0;
teunman 11:d8dd024f9784 116 }
teunman 11:d8dd024f9784 117
teunman 11:d8dd024f9784 118
teunman 11:d8dd024f9784 119
teunman 11:d8dd024f9784 120 } //end compute biquad
teunman 3:499c71ca30a0 121
teunman 0:674026fdd982 122
teunman 8:54f0a76d35f4 123
teunman 8:54f0a76d35f4 124
teunman 8:54f0a76d35f4 125
teunman 11:d8dd024f9784 126 ////////////////////////////////////////////////////////////////////////MAIN FUNCTION/////////////////////////////////////////////////////////////
teunman 8:54f0a76d35f4 127
teunman 8:54f0a76d35f4 128
teunman 0:674026fdd982 129 int main()
teunman 0:674026fdd982 130 {
teunman 11:d8dd024f9784 131 light = 1; //Light is off by default
teunman 11:d8dd024f9784 132
teunman 11:d8dd024f9784 133
teunman 11:d8dd024f9784 134 /////////////////////////////////////////////////////////////////TICKERS///////////////////////////////////////////////////////////////////////
teunman 8:54f0a76d35f4 135
teunman 5:56725d9362ee 136 // Attach the data read and send function at 1000 Hz
teunman 5:56725d9362ee 137 scopeTimer.attach_us(&scopeSend, 1e5);
teunman 10:b11eacb391ea 138 // Attach the filter calculation function at 1000 Hz
teunman 11:d8dd024f9784 139 FilterTicker.attach(&computeBiquad,0.0001f);
teunman 10:b11eacb391ea 140
teunman 10:b11eacb391ea 141
teunman 10:b11eacb391ea 142
teunman 10:b11eacb391ea 143 while(1) { //the everlasting while loop
teunman 10:b11eacb391ea 144
teunman 10:b11eacb391ea 145
teunman 11:d8dd024f9784 146 /////////////////////////////////////////////////////CALIBRATION AND TRIGGER ////////////////////////////////////////////
teunman 11:d8dd024f9784 147
teunman 11:d8dd024f9784 148 if (ffy >= 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 149 light = 0;
teunman 10:b11eacb391ea 150 }
teunman 8:54f0a76d35f4 151
teunman 8:54f0a76d35f4 152
teunman 10:b11eacb391ea 153 else if (knop == 0){ //pressing the button starts calibration
teunman 10:b11eacb391ea 154
teunman 10:b11eacb391ea 155 c = 0;
teunman 10:b11eacb391ea 156 n = 0;
teunman 10:b11eacb391ea 157 light = 0; //light is on during calibration
teunman 10:b11eacb391ea 158 calib = true;
teunman 10:b11eacb391ea 159
teunman 10:b11eacb391ea 160 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 161
teunman 11:d8dd024f9784 162 c = (c + ffy);
teunman 10:b11eacb391ea 163 wait(0.1);
teunman 9:2b9240084724 164
teunman 10:b11eacb391ea 165 }//end for
teunman 9:2b9240084724 166
teunman 9:2b9240084724 167
teunman 9:2b9240084724 168
teunman 10:b11eacb391ea 169 light = 1; // light is off for a while after calibration
teunman 10:b11eacb391ea 170 calib = false; // shuts of calibration
teunman 11:d8dd024f9784 171 c = (c/n) * 0.3; //take the average of the emg output during calibration and set the calibration value
teunman 10:b11eacb391ea 172
teunman 9:2b9240084724 173
teunman 10:b11eacb391ea 174 wait(5); // you can only calibrate every this amount of seconds
teunman 10:b11eacb391ea 175 } //end else if
teunman 10:b11eacb391ea 176
teunman 8:54f0a76d35f4 177 else{
teunman 10:b11eacb391ea 178 light = 1; //light stays off by default
teunman 6:4cbf5c66e2fb 179 }
teunman 10:b11eacb391ea 180
teunman 10:b11eacb391ea 181
teunman 10:b11eacb391ea 182 } //END while
teunman 10:b11eacb391ea 183 }//END main