group14 - k9 / Mbed 2 deprecated Biquad

jongens kijk eens wat een mooie biquad

Dependencies:   HIDScope mbed

main.cpp@0:d8312eea7c50, 2015-10-12 (annotated)

Committer:
Paashaas
Date:
Mon Oct 12 14:00:57 2015 +0000
Revision:
0:d8312eea7c50
biquad om emg signalen te filteren

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Paashaas 0:d8312eea7c50 1 #include "mbed.h"
Paashaas 0:d8312eea7c50 2 #include "HIDScope.h"
Paashaas 0:d8312eea7c50 3
Paashaas 0:d8312eea7c50 4 AnalogIn emg1(A0); //onderste board
Paashaas 0:d8312eea7c50 5 AnalogIn emg2(A1); // 2e board
Paashaas 0:d8312eea7c50 6 AnalogIn emg3(A2); // 3e board
Paashaas 0:d8312eea7c50 7 AnalogIn emg4(A3); // bovenste board
Paashaas 0:d8312eea7c50 8
Paashaas 0:d8312eea7c50 9 Ticker sample_ticker; // naam van de emg-ticker
Paashaas 0:d8312eea7c50 10
Paashaas 0:d8312eea7c50 11 HIDScope scope(4); // aantal kanalen voor je HIDScope
Paashaas 0:d8312eea7c50 12
Paashaas 0:d8312eea7c50 13 //highpass filter 20 Hz
Paashaas 0:d8312eea7c50 14 const double numhigh_1 = 0.956543225556877;
Paashaas 0:d8312eea7c50 15 const double numhigh_2 = -1.91308645113754;
Paashaas 0:d8312eea7c50 16 const double numhigh_3 = 0.956543225556877;
Paashaas 0:d8312eea7c50 17 const double denhigh_2 = -1.91197067426073;
Paashaas 0:d8312eea7c50 18 const double denhigh_3 = 0.9149758348014341;
Paashaas 0:d8312eea7c50 19
Paashaas 0:d8312eea7c50 20 //notchfilter 50Hz
Paashaas 0:d8312eea7c50 21 //biquad 1
Paashaas 0:d8312eea7c50 22 const double numnotch_1_1 = 1;
Paashaas 0:d8312eea7c50 23 const double numnotch_1_2 = -1.97538661427765;
Paashaas 0:d8312eea7c50 24 const double numnotch_1_3 = 1.00000018882502;
Paashaas 0:d8312eea7c50 25 const double dennotch_1_2 = -1.97024348390877;
Paashaas 0:d8312eea7c50 26 const double dennotch_1_3 = 0.995504923090779;
Paashaas 0:d8312eea7c50 27
Paashaas 0:d8312eea7c50 28 //biquad 2
Paashaas 0:d8312eea7c50 29 const double numnotch_2_1 = 1;
Paashaas 0:d8312eea7c50 30 const double numnotch_2_2 = -1.97538624436946;
Paashaas 0:d8312eea7c50 31 const double numnotch_2_3 = 1.00000018882502;
Paashaas 0:d8312eea7c50 32 const double dennotch_2_2 = -1.97175302143845;
Paashaas 0:d8312eea7c50 33 const double dennotch_2_3 = 0.995629024908953;
Paashaas 0:d8312eea7c50 34
Paashaas 0:d8312eea7c50 35 //lowpass filter 7 Hz - envelop
Paashaas 0:d8312eea7c50 36 const double numlow_1 = 0.000119046743110057;
Paashaas 0:d8312eea7c50 37 const double numlow_2 = 0.000238093486220118;
Paashaas 0:d8312eea7c50 38 const double numlow_3 = 0.000119046743110057;
Paashaas 0:d8312eea7c50 39 const double denlow_2 = -1.968902268531908;
Paashaas 0:d8312eea7c50 40 const double denlow_3 = 0.9693784555043481;
Paashaas 0:d8312eea7c50 41
Paashaas 0:d8312eea7c50 42 // storage variables definieren
Paashaas 0:d8312eea7c50 43 double f1_v1 = 0, f1_v2 = 0, f2_v1 = 0, f2_v2 = 0, f3_v1 = 0, f3_v2 = 0, f4_v1 = 0, f4_v2 = 0;
Paashaas 0:d8312eea7c50 44
Paashaas 0:d8312eea7c50 45 double biquad( double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2)
Paashaas 0:d8312eea7c50 46
Paashaas 0:d8312eea7c50 47 {
Paashaas 0:d8312eea7c50 48 double v = u - a1*v1 - a2*v2;
Paashaas 0:d8312eea7c50 49 double y = b0*v + b1*v1 + b2*v2;
Paashaas 0:d8312eea7c50 50 v2=v1;
Paashaas 0:d8312eea7c50 51 v1=v;
Paashaas 0:d8312eea7c50 52 return y;
Paashaas 0:d8312eea7c50 53 }
Paashaas 0:d8312eea7c50 54
Paashaas 0:d8312eea7c50 55 void myController()
Paashaas 0:d8312eea7c50 56 {
Paashaas 0:d8312eea7c50 57 // double u1=..., u2...;
Paashaas 0:d8312eea7c50 58 double u1 = emg1.read();
Paashaas 0:d8312eea7c50 59 double y1 = biquad(u1, f1_v1, f1_v2, denhigh_2, denhigh_3, numhigh_1, numhigh_2, numhigh_3);
Paashaas 0:d8312eea7c50 60 double y2 = abs(y1);
Paashaas 0:d8312eea7c50 61 double y3 = biquad(y2, f2_v1, f2_v2, dennotch_1_2, dennotch_1_3, numnotch_1_1, numnotch_1_2, numnotch_1_3);
Paashaas 0:d8312eea7c50 62 double y4 = biquad(y3, f3_v1, f3_v2, dennotch_2_2, dennotch_2_3, numnotch_2_1, numnotch_2_2, numnotch_2_3);
Paashaas 0:d8312eea7c50 63 double y5 = biquad(y4, f4_v1, f4_v2, denlow_2, denlow_3, nomlow_1, numlow_2, numlow_3);
Paashaas 0:d8312eea7c50 64 scope.set(0,u1);
Paashaas 0:d8312eea7c50 65 scope.set(1,y5);
Paashaas 0:d8312eea7c50 66 scope.send();
Paashaas 0:d8312eea7c50 67 }
Paashaas 0:d8312eea7c50 68
Paashaas 0:d8312eea7c50 69
Paashaas 0:d8312eea7c50 70
Paashaas 0:d8312eea7c50 71 int main()
Paashaas 0:d8312eea7c50 72 {
Paashaas 0:d8312eea7c50 73 sample_ticker.attach(&mycontroller,0.005);
Paashaas 0:d8312eea7c50 74 while (true){
Paashaas 0:d8312eea7c50 75 }
Paashaas 0:d8312eea7c50 76 }