Richell Booyink / Mbed 2 deprecated EMG_Controlling

EMG controlling calibration + filters biorobotics group 13 BMT

Dependencies:   HIDScope biquadFilter mbed

main.cpp@1:16165e207e70, 2015-10-15 (annotated)

Committer:
RichellBooyink
Date:
Thu Oct 15 14:55:16 2015 +0000
Revision:
1:16165e207e70
Parent:
0:2ded24549b70
Child:
2:e3259334299e
Alle drie de filters. Deze zijn achter elkaar aangesloten en zouden moeten werken. ;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RichellBooyink 0:2ded24549b70 1 #include "mbed.h"
RichellBooyink 0:2ded24549b70 2 #include "HIDScope.h"
RichellBooyink 0:2ded24549b70 3
RichellBooyink 1:16165e207e70 4 AnalogIn EMG(A0);
RichellBooyink 1:16165e207e70 5 HIDScope scope (3);
RichellBooyink 1:16165e207e70 6
RichellBooyink 0:2ded24549b70 7 // Standaard formule voor het biquad filter
RichellBooyink 0:2ded24549b70 8 double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2)
RichellBooyink 0:2ded24549b70 9
RichellBooyink 0:2ded24549b70 10 {
RichellBooyink 1:16165e207e70 11 u = EMG.read();
RichellBooyink 0:2ded24549b70 12 double v = u - a1*v1 - a2*v2;
RichellBooyink 0:2ded24549b70 13 double y = b0*v + b1*v1 + b2*v2;
RichellBooyink 0:2ded24549b70 14 v2=v1;
RichellBooyink 0:2ded24549b70 15 v1=v;
RichellBooyink 0:2ded24549b70 16 return y;
RichellBooyink 0:2ded24549b70 17 }
RichellBooyink 0:2ded24549b70 18 // Filter1 = High pass filter tot 20 Hz
RichellBooyink 0:2ded24549b70 19
RichellBooyink 0:2ded24549b70 20 double fh1_v1=0, fh1_v2=0, fh2_v1=0, fh2_v2=0;
RichellBooyink 0:2ded24549b70 21 const double fh1_a1=-0.84909054461, fh1_a2=0.00000000000, fh1_b0= 1, fh1_b1=-1, fh1_b2=0;
RichellBooyink 0:2ded24549b70 22 const double fh2_a1=-1.82553264091, fh2_a2=0.85001416809, fh2_b0= 1, fh2_b1=-2, fh2_b2=1;
RichellBooyink 0:2ded24549b70 23 Ticker FHIGH;
RichellBooyink 0:2ded24549b70 24 void Highpass_filter()
RichellBooyink 0:2ded24549b70 25 {
RichellBooyink 0:2ded24549b70 26 biquad (u, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0, fh1_b1, fh1_b2);
RichellBooyink 0:2ded24549b70 27 biquad (u, fh2_v1, fh2_v2, fh2_a1, fh2_a2, fh2_b0, fh2_b1, fh2_b2);
RichellBooyink 0:2ded24549b70 28 }
RichellBooyink 0:2ded24549b70 29
RichellBooyink 0:2ded24549b70 30 // Filter2 = Low pass filter na 60 Hz
RichellBooyink 0:2ded24549b70 31 double fl1_v1=0, fl1_v2=0, fl2_v1=0, fl2_v2=0;
RichellBooyink 0:2ded24549b70 32 const double fl1_a1=-0.66979455390, fl1_a2=0.00000000000, fl1_b0= 1, fl1_b1=1, fl1_b2=0;
RichellBooyink 0:2ded24549b70 33 const double fl2_a1=-1.55376616139, fl2_a2=0.68023470431, fl2_b0= 1, fl2_b1=2, fl2_b2=1;
RichellBooyink 0:2ded24549b70 34 Ticker FLOW;
RichellBooyink 0:2ded24549b70 35 void Lowpass_filter()
RichellBooyink 0:2ded24549b70 36 {
RichellBooyink 0:2ded24549b70 37 biquad (u, fl1_v1, fl1_v2, fl1_a1, fl1_a2, fl1_b0, fl1_b1, fl1_b2);
RichellBooyink 0:2ded24549b70 38 biquad (u, fl2_v1, fl2_v2, fl2_a1, fl2_a2, fl2_b0, fl2_b1, fl2_b2);
RichellBooyink 0:2ded24549b70 39 }
RichellBooyink 0:2ded24549b70 40
RichellBooyink 0:2ded24549b70 41 // Filter3 = Notch filter at 50 Hz
RichellBooyink 0:2ded24549b70 42 double fno1_v1=0, fno1_v2=0, fno2_v1=0, fno2_v2=0, fno3_v1=0, fno3_v2=0;
RichellBooyink 0:2ded24549b70 43 const double fno1_a1 = -1.87934916386, fno1_a2= 0.97731851355, fno1_b0= 1, fno1_b1= -1.90090686046, fno1_b2= 1;
RichellBooyink 0:2ded24549b70 44 const double fno2_a1 = -1.88341028603, fno2_a2= 0.98825147717, fno2_b0= 1, fno2_b1= -1.90090686046, fno2_b2= 1;
RichellBooyink 0:2ded24549b70 45 const double fno3_a1 = -1.89635403726, fno3_a2= 0.98894004849, fno3_b0= 1, fno3_b1= -1.90090686046, fno3_b2= 1;
RichellBooyink 0:2ded24549b70 46 Ticker FNOTCH;
RichellBooyink 0:2ded24549b70 47 void Notch_filter ()
RichellBooyink 0:2ded24549b70 48 {
RichellBooyink 0:2ded24549b70 49 biquad (u, fno1_v1, fno1_v2, fno1_a1, fno1_a2, fno1_b0, fno1_b1, fno1_b2);
RichellBooyink 0:2ded24549b70 50 biquad (u, fno2_v1, fno2_v2, fno2_a1, fno2_a2, fno2_b0, fno2_b1, fno2_b2);
RichellBooyink 0:2ded24549b70 51 biquad (u, fno3_v1, fno3_v2, fno3_a1, fno3_a2, fno3_b0, fno3_b1, fno3_b2);
RichellBooyink 0:2ded24549b70 52 }
RichellBooyink 0:2ded24549b70 53
RichellBooyink 1:16165e207e70 54 // HIDScope
RichellBooyink 1:16165e207e70 55 Ticker timer;
RichellBooyink 1:16165e207e70 56 void signal (){
RichellBooyink 1:16165e207e70 57 scope.set (0, Highpass_filter);
RichellBooyink 1:16165e207e70 58 scope.set (0, Lowpass_filter);
RichellBooyink 1:16165e207e70 59 scope.set (0, Notch_filter);
RichellBooyink 1:16165e207e70 60 scope.send ();
RichellBooyink 1:16165e207e70 61 }
RichellBooyink 1:16165e207e70 62
RichellBooyink 0:2ded24549b70 63 int main ()
RichellBooyink 0:2ded24549b70 64 {
RichellBooyink 0:2ded24549b70 65 FHIGH.attach_us(Highpass_filter, 1e4);
RichellBooyink 0:2ded24549b70 66 FLOW.attach_us(Lowpass_filter, 1e4);
RichellBooyink 0:2ded24549b70 67 FNOTCH.attach_us (Notch_filter, 1e4);
RichellBooyink 1:16165e207e70 68 timer.attach_us (signal, 1e4);
RichellBooyink 0:2ded24549b70 69 }
RichellBooyink 0:2ded24549b70 70
RichellBooyink 0:2ded24549b70 71
RichellBooyink 0:2ded24549b70 72