Bouke Scheltinga / Mbed 2 deprecated Werk_college_23sept

Werkcollege opgave 23 september BMT K9

Dependencies:   Encoder HIDScope MODSERIAL mbed QEI biquadFilter

main.cpp@27:85e5d36bb6c5, 2015-10-13 (annotated)

Committer:
ThomasBNL
Date:
Tue Oct 13 12:31:55 2015 +0000
Revision:
27:85e5d36bb6c5
Parent:
26:1090acf98efc
Child:
28:44cb644233eb
Child:
32:97cf6cb8d054
EMG filter script werkend

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bscheltinga 0:fe3896c6eeb0 1 #include "mbed.h"
bscheltinga 12:0a079e86348e 2 #include "HIDScope.h"
bscheltinga 0:fe3896c6eeb0 3 #include "MODSERIAL.h"
bscheltinga 13:04e10692e239 4 #include "biquadFilter.h" //Filter direct form II
bscheltinga 0:fe3896c6eeb0 5
bscheltinga 26:1090acf98efc 6 // [DEFINE INPUTS] //
ThomasBNL 27:85e5d36bb6c5 7 //AnalogIn emgL(A0); //Analog input left arm
bscheltinga 18:68067ffd169e 8 //AnalogIn emgR(PTB1); //Analog input right arm
bscheltinga 19:bd453bee03f6 9 DigitalOut led1(LED_GREEN);
bscheltinga 21:594915ba2bf9 10
bscheltinga 26:1090acf98efc 11 // [DEFINE CONSTANTS] //
bscheltinga 26:1090acf98efc 12 float emgL_L, emgL_LH, emgLeft, emgL_H, emgL_Notch;
bscheltinga 26:1090acf98efc 13 double B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, MOVAVG;
ThomasBNL 27:85e5d36bb6c5 14 HIDScope scope(3); // Aantal HIDScope kanalen
bscheltinga 18:68067ffd169e 15 MODSERIAL pc(USBTX,USBRX);
bscheltinga 20:d5f5c60adc43 16 volatile bool control_go = false;
bscheltinga 20:d5f5c60adc43 17 Ticker control_tick;
ThomasBNL 27:85e5d36bb6c5 18 Ticker T1;
bscheltinga 15:7870f7912904 19
bscheltinga 26:1090acf98efc 20 // [BIQUAD FILTERS] //
ThomasBNL 27:85e5d36bb6c5 21 int Fs = 512; // sampling frequency
ThomasBNL 27:85e5d36bb6c5 22 const double low_b1 = 1.480219865318266e-04; //filter coefficients
ThomasBNL 27:85e5d36bb6c5 23 const double low_b2 = 2.960439730636533e-04;
ThomasBNL 27:85e5d36bb6c5 24 const double low_b3 = 1.480219865318266e-04;
ThomasBNL 27:85e5d36bb6c5 25 const double low_a2 = -1.965293372622690e+00; // a1 is normalized to 1
ThomasBNL 27:85e5d36bb6c5 26 const double low_a3 = 9.658854605688177e-01;
ThomasBNL 27:85e5d36bb6c5 27 const double high_b1 = 8.047897937631126e-01;
ThomasBNL 27:85e5d36bb6c5 28 const double high_b2 = -1.609579587526225e+00;
ThomasBNL 27:85e5d36bb6c5 29 const double high_b3 = 8.047897937631126e-01;
ThomasBNL 27:85e5d36bb6c5 30 const double high_a2 = -1.571102440190402e+00; // a1 is normalized to 1
ThomasBNL 27:85e5d36bb6c5 31 const double high_a3 = 6.480567348620491e-01;
bscheltinga 15:7870f7912904 32
ThomasBNL 27:85e5d36bb6c5 33 biquadFilter highpass1(high_a2, high_a3, high_b1, high_b2, high_b3);
ThomasBNL 27:85e5d36bb6c5 34 biquadFilter lowpass1(low_a2, low_a3, low_b1, low_b2, low_b3);
ThomasBNL 27:85e5d36bb6c5 35
ThomasBNL 27:85e5d36bb6c5 36 AnalogIn input1(A0); // declaring the 4 inputs
ThomasBNL 27:85e5d36bb6c5 37
ThomasBNL 27:85e5d36bb6c5 38 double u1; double y1; // declaring the input variables
bscheltinga 22:14abcfdd1554 39
bscheltinga 26:1090acf98efc 40 // [FUNCTIONS] //
bscheltinga 21:594915ba2bf9 41 void ControlGo() //Control flag
bscheltinga 21:594915ba2bf9 42 {
bscheltinga 21:594915ba2bf9 43 control_go = true;
bscheltinga 21:594915ba2bf9 44 led1 = 0;
bscheltinga 21:594915ba2bf9 45 }
bscheltinga 0:fe3896c6eeb0 46
ThomasBNL 27:85e5d36bb6c5 47 void sample_filter()
bscheltinga 13:04e10692e239 48 {
ThomasBNL 27:85e5d36bb6c5 49 u1 = input1;
ThomasBNL 27:85e5d36bb6c5 50 y1 = highpass1.step(u1); // filter order is: high-pass --> rectify --> low-pass
ThomasBNL 27:85e5d36bb6c5 51 y1 = fabs(y1);
ThomasBNL 27:85e5d36bb6c5 52 y1 = lowpass1.step(y1);// roughly normalize to a scale of 0 - 1, where 0 is minimum and 1 is roughly the maximum output of dennis.
ThomasBNL 27:85e5d36bb6c5 53
ThomasBNL 27:85e5d36bb6c5 54 B0 = y1;
bscheltinga 25:38ab6dd19d9b 55 MOVAVG=B0*0.1+B1*0.1+B2*0.1+B3*0.1+B4*0.1+B5*0.1+B6*0.1+B7*0.1+B8*0.1+B9*0.1;
bscheltinga 24:a1ce6a87103c 56 B9=B8;
bscheltinga 24:a1ce6a87103c 57 B8=B7;
bscheltinga 24:a1ce6a87103c 58 B7=B6;
bscheltinga 24:a1ce6a87103c 59 B6=B5;
bscheltinga 24:a1ce6a87103c 60 B5=B4;
bscheltinga 24:a1ce6a87103c 61 B4=B3;
bscheltinga 24:a1ce6a87103c 62 B3=B2;
bscheltinga 24:a1ce6a87103c 63 B2=B1;
bscheltinga 24:a1ce6a87103c 64 B1=B0;
bscheltinga 23:c9c9c1d7864a 65
bscheltinga 21:594915ba2bf9 66 led1 = 1; //De led gaat flikkeren wanneer deze loop uitgevoerd wordt
ThomasBNL 27:85e5d36bb6c5 67 }
bscheltinga 20:d5f5c60adc43 68
ThomasBNL 27:85e5d36bb6c5 69
ThomasBNL 27:85e5d36bb6c5 70 // [MAIN FUNCTION] //
ThomasBNL 27:85e5d36bb6c5 71 int main()
ThomasBNL 27:85e5d36bb6c5 72 {
ThomasBNL 27:85e5d36bb6c5 73 control_tick.attach(&ControlGo, (float)1/Fs);
ThomasBNL 27:85e5d36bb6c5 74 pc.baud(9600);
ThomasBNL 27:85e5d36bb6c5 75 while(1)
ThomasBNL 27:85e5d36bb6c5 76 {
ThomasBNL 27:85e5d36bb6c5 77 led1=0;
ThomasBNL 27:85e5d36bb6c5 78 if(control_go)
ThomasBNL 27:85e5d36bb6c5 79 {
ThomasBNL 27:85e5d36bb6c5 80 sample_filter();
ThomasBNL 27:85e5d36bb6c5 81 scope.set(0,u1);
ThomasBNL 27:85e5d36bb6c5 82 scope.set(1,y1);
ThomasBNL 27:85e5d36bb6c5 83 scope.set(2,MOVAVG);
ThomasBNL 27:85e5d36bb6c5 84 scope.send();
ThomasBNL 27:85e5d36bb6c5 85 control_go = 0;
ThomasBNL 27:85e5d36bb6c5 86 }
ThomasBNL 27:85e5d36bb6c5 87 } // while end
ThomasBNL 27:85e5d36bb6c5 88 } // main end
ThomasBNL 27:85e5d36bb6c5 89