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@8:54f0a76d35f4, 2015-09-23 (annotated)

Committer:
teunman
Date:
Wed Sep 23 14:51:13 2015 +0000
Revision:
8:54f0a76d35f4
Parent:
7:43f2f7039841
Child:
9:2b9240084724
signal filter working, included calibration function. which sets a value by taking average over some time and dividing by a counter. The light blinks if the measured value is above the calibartion value.

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 0:674026fdd982 6 Ticker scopeTimer;
teunman 1:75f61e111ed0 7 Ticker biquadTicker;
teunman 8:54f0a76d35f4 8 DigitalOut light(LED2);
teunman 8:54f0a76d35f4 9 DigitalIn knop(SW3);
teunman 1:75f61e111ed0 10
teunman 8:54f0a76d35f4 11 double c=5; //calibration value
teunman 8:54f0a76d35f4 12 double n = 0; // counter for calibration
teunman 8:54f0a76d35f4 13 bool calib = false; //changes into false when calibration is done (reset program for new calibration);
teunman 4:1baefd1397d6 14 double v1=0, v2=0, u=0, y=0, fy=0, fv1=0, fv2=0, fu=0;
teunman 1:75f61e111ed0 15
teunman 5:56725d9362ee 16 //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 17 //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 18 //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 19 //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 20 //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 21 //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 22
teunman 7:43f2f7039841 23 const double b0 = 0.00008765553769759188,b1 = 0.00017531107539518376,b2 = 0.00008765553769759188,a1 = -1.9733440008737442,a2 = 0.9736946230245347;//low-pass Fc = 3Hz fs = 1000hz
teunman 7:43f2f7039841 24 //const double b0 = 0.00034604125149151127,b1 = 0.0006920825029830225, b2 = 0.00034604125149151127 ,a1 = -1.9466970561224466 ,a2 = 0.9480812211284125; //low-pass Fc = 6Hz fs = 1000hz
teunman 7:43f2f7039841 25 //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 26 //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 27 //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 28 //const double fb0 = 0.9780302754084559,fb1 = -1.9560605508169118,fb2 = 0.9780302754084559,fa1 = -1.9555778328194147,fa2 = 0.9565432688144089; //high-pass Fc = 6hz fs = 1000hz
teunman 7:43f2f7039841 29 const double fb0 = 0.9911535113858849,fb1 = -1.9823070227717698,fb2 = 0.9911535113858849,fa1 = -1.9822287623675816,fa2 = 0.982385283175958; //High-pass Fc = 2Hz fs = 1000Hz
teunman 6:4cbf5c66e2fb 30 //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 31
teunman 3:499c71ca30a0 32
teunman 3:499c71ca30a0 33
teunman 3:499c71ca30a0 34 // Read the analog input
teunman 3:499c71ca30a0 35 AnalogIn an_in(A0);
teunman 3:499c71ca30a0 36
teunman 3:499c71ca30a0 37 AnalogOut an_out(DAC0_OUT);
teunman 3:499c71ca30a0 38 // The data read and send function
teunman 3:499c71ca30a0 39 void scopeSend()
teunman 3:499c71ca30a0 40 {
teunman 8:54f0a76d35f4 41 scope.set(0,c);
teunman 4:1baefd1397d6 42 scope.set(1,fy);
teunman 8:54f0a76d35f4 43 scope.set(2,n);
teunman 3:499c71ca30a0 44
teunman 3:499c71ca30a0 45 scope.send();
teunman 8:54f0a76d35f4 46
teunman 8:54f0a76d35f4 47 while(calib == true and n <= 2000.00) {
teunman 8:54f0a76d35f4 48
teunman 8:54f0a76d35f4 49 c = c + fy;
teunman 8:54f0a76d35f4 50
teunman 8:54f0a76d35f4 51 n ++;
teunman 8:54f0a76d35f4 52
teunman 8:54f0a76d35f4 53 }
teunman 8:54f0a76d35f4 54
teunman 8:54f0a76d35f4 55
teunman 3:499c71ca30a0 56 }
teunman 3:499c71ca30a0 57
teunman 3:499c71ca30a0 58 void computeBiquad(){
teunman 7:43f2f7039841 59
teunman 7:43f2f7039841 60
teunman 7:43f2f7039841 61
teunman 3:499c71ca30a0 62 double v = an_in - a1*v1 - a2*v2;
teunman 1:75f61e111ed0 63 y= b0*v + b1*v1 +b2*v2;
teunman 1:75f61e111ed0 64 v2=v1;
teunman 1:75f61e111ed0 65 v1=v;
teunman 4:1baefd1397d6 66
teunman 7:43f2f7039841 67 y = abs(y);
teunman 7:43f2f7039841 68
teunman 6:4cbf5c66e2fb 69
teunman 4:1baefd1397d6 70 double fv = y - fa1*fv1 - fa2*fv2;
teunman 4:1baefd1397d6 71 fy= fb0*fv + fb1*fv1 + fb2*fv2;
teunman 4:1baefd1397d6 72 fv2=fv1;
teunman 4:1baefd1397d6 73 fv1=fv;
teunman 7:43f2f7039841 74
teunman 7:43f2f7039841 75 fy = abs(fy);
teunman 1:75f61e111ed0 76 }
teunman 3:499c71ca30a0 77
teunman 0:674026fdd982 78
teunman 8:54f0a76d35f4 79
teunman 8:54f0a76d35f4 80
teunman 8:54f0a76d35f4 81
teunman 8:54f0a76d35f4 82
teunman 8:54f0a76d35f4 83
teunman 8:54f0a76d35f4 84
teunman 0:674026fdd982 85 int main()
teunman 0:674026fdd982 86 {
teunman 6:4cbf5c66e2fb 87 light = 1;
teunman 8:54f0a76d35f4 88
teunman 5:56725d9362ee 89 // Attach the data read and send function at 1000 Hz
teunman 5:56725d9362ee 90 scopeTimer.attach_us(&scopeSend, 1e5);
teunman 8:54f0a76d35f4 91 //scopeTimer.attach_us(&calibrate, 1e5);
teunman 5:56725d9362ee 92 biquadTicker.attach(&computeBiquad,0.0001f);
teunman 3:499c71ca30a0 93 //float i = 1;
teunman 0:674026fdd982 94 while(1) {
teunman 8:54f0a76d35f4 95 if (fy >= c and calib == false){
teunman 6:4cbf5c66e2fb 96 light = 0;
teunman 6:4cbf5c66e2fb 97 }
teunman 8:54f0a76d35f4 98
teunman 8:54f0a76d35f4 99
teunman 8:54f0a76d35f4 100 else if (knop == 0){
teunman 8:54f0a76d35f4 101 light = 0;
teunman 8:54f0a76d35f4 102 calib = true;
teunman 8:54f0a76d35f4 103 c = 0;
teunman 8:54f0a76d35f4 104 n = 0;
teunman 8:54f0a76d35f4 105 wait(5);
teunman 8:54f0a76d35f4 106 light = 1;
teunman 8:54f0a76d35f4 107 calib = false;
teunman 8:54f0a76d35f4 108 c = c/n;
teunman 8:54f0a76d35f4 109 }
teunman 8:54f0a76d35f4 110
teunman 8:54f0a76d35f4 111
teunman 8:54f0a76d35f4 112 else{
teunman 8:54f0a76d35f4 113 light = 1;
teunman 6:4cbf5c66e2fb 114 }
teunman 0:674026fdd982 115 }
teunman 0:674026fdd982 116 }