Group 9 BioRobotics / Mbed 2 deprecated EMG_verwerking

Het EMG verwerkingsdeel

Dependencies:   mbed

main.cpp@2:dc387d9de7c4, 2018-10-26 (annotated)

Committer:
keeswieriks
Date:
Fri Oct 26 10:28:22 2018 +0000
Revision:
2:dc387d9de7c4
Parent:
1:3cc91fc2fb1c 
notch erbij;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1574396 0:3db1f2a9d605 1 #include "mbed.h"
s1574396 0:3db1f2a9d605 2
s1574396 0:3db1f2a9d605 3 // inputs EMG
s1574396 0:3db1f2a9d605 4 AnalogIn emg0( A0 );
s1574396 0:3db1f2a9d605 5 AnalogIn emg1( A1 );
s1574396 0:3db1f2a9d605 6 AnalogIn emg3( A2 );
s1574396 0:3db1f2a9d605 7
s1574396 0:3db1f2a9d605 8 // Variabelen EMG
keeswieriks 2:dc387d9de7c4 9 const double m1 =0.5000;
keeswieriks 2:dc387d9de7c4 10 const double m2 =-0.8090;
keeswieriks 2:dc387d9de7c4 11 const double n0 =0.5000;
keeswieriks 2:dc387d9de7c4 12 const double n1 =-0.8090;
keeswieriks 2:dc387d9de7c4 13 const double n2 =0;
keeswieriks 1:3cc91fc2fb1c 14 const double a1 =0.9565;
keeswieriks 1:3cc91fc2fb1c 15 const double a2 =-1.9131;
keeswieriks 1:3cc91fc2fb1c 16 const double b0 =0.9565;
keeswieriks 1:3cc91fc2fb1c 17 const double b1 =-1.9112;
keeswieriks 1:3cc91fc2fb1c 18 const double b2 =0.9150;
keeswieriks 1:3cc91fc2fb1c 19 const double c1 =0.0675;
keeswieriks 1:3cc91fc2fb1c 20 const double c2 =0.1349;
keeswieriks 1:3cc91fc2fb1c 21 const double d0 =0.0675;
keeswieriks 1:3cc91fc2fb1c 22 const double d1 =-1.1430;
keeswieriks 1:3cc91fc2fb1c 23 const double d2 =0.4128;
s1574396 0:3db1f2a9d605 24
s1574396 0:3db1f2a9d605 25 double highpassFilter1 = 0;
s1574396 0:3db1f2a9d605 26 double lowpassFilter1 = 0;
s1574396 0:3db1f2a9d605 27 double highpassFilter2 = 0;
s1574396 0:3db1f2a9d605 28 double lowpassFilter2 = 0;
s1574396 0:3db1f2a9d605 29
s1574396 0:3db1f2a9d605 30 // Filteren
s1574396 0:3db1f2a9d605 31 void filterEMG()
keeswieriks 1:3cc91fc2fb1c 32 {
keeswieriks 2:dc387d9de7c4 33 notchFilter1 = biquad(emg0.read(), m1, m2, n0, n1, n2);
keeswieriks 2:dc387d9de7c4 34 highpassFilter1 = fabs(biquad(notchFilter1, a1, a2, b0, b1, b2));
s1574396 0:3db1f2a9d605 35 lowpassFilter1 = biquad(highpassFilter1, c1, c2, d0, d1, d2);
keeswieriks 2:dc387d9de7c4 36 notchFilter2 = biquad(emg1.read(), m1, m2, n0, n1, n2);
keeswieriks 2:dc387d9de7c4 37 highpassFilter2 = fabs(biquad(notchFilter2, a1, a2, b0, b1, b2));
s1574396 0:3db1f2a9d605 38 lowpassFilter2 = biquad(highpassFilter2, c1, c2, d0, d1, d2);
keeswieriks 2:dc387d9de7c4 39 notchFilter3 = biquad(emg2.read(), m1, m2, n0, n1, n2);
keeswieriks 2:dc387d9de7c4 40 highpassFilter3 = fabs(biquad(notchFilter2, a1, a2, b0, b1, b2));
s1574396 0:3db1f2a9d605 41 lowpassFilter3 = biquad(highpassFilter3, c1, c2, d0, d1, d2);
s1574396 0:3db1f2a9d605 42 }
s1574396 0:3db1f2a9d605 43
s1574396 0:3db1f2a9d605 44 //calibrate (hier moet iets met maximale waarden enzo
s1574396 0:3db1f2a9d605 45 dempelwaarde = int 0.5 * max_lowpassFilter3
s1574396 0:3db1f2a9d605 46
s1574396 0:3db1f2a9d605 47 //Direction control
s1574396 0:3db1f2a9d605 48 enum direction {Pos_RB, Pos_LB, Pos_RO, Pos_LO};
s1574396 0:3db1f2a9d605 49 direction currentdirection = Pos_RB;
s1574396 0:3db1f2a9d605 50 bool directionchanged = true;
s1574396 0:3db1f2a9d605 51
s1574396 0:3db1f2a9d605 52 int main()
s1574396 0:3db1f2a9d605 53 {
s1574396 0:3db1f2a9d605 54 while ()
s1574396 0:3db1f2a9d605 55 {
s1574396 0:3db1f2a9d605 56 switch (currentdirection)
s1574396 0:3db1f2a9d605 57 {
s1574396 0:3db1f2a9d605 58 case Pos_RB:
s1574396 0:3db1f2a9d605 59
s1574396 0:3db1f2a9d605 60 lowpassFilter1 = lowpassFilter1;
s1574396 0:3db1f2a9d605 61 lowpassFilter2 = lowpassFilter2;
s1574396 0:3db1f2a9d605 62
s1574396 0:3db1f2a9d605 63 if (lowpassFilter3 > drempelwaarde)
s1574396 0:3db1f2a9d605 64 {
s1574396 0:3db1f2a9d605 65 currentdirection == Pos_LB;
s1574396 0:3db1f2a9d605 66 cirectionchanged = true;
s1574396 0:3db1f2a9d605 67 }
s1574396 0:3db1f2a9d605 68 break;
s1574396 0:3db1f2a9d605 69
s1574396 0:3db1f2a9d605 70 case Pos_LB:
s1574396 0:3db1f2a9d605 71
s1574396 0:3db1f2a9d605 72 lowpassFilter1 = lowpassFilter1 * int -1;
s1574396 0:3db1f2a9d605 73 lowpassFilter2 = lowpassFilter2;
s1574396 0:3db1f2a9d605 74
s1574396 0:3db1f2a9d605 75 if (lowpassFilter3 > drempelwaarde)
s1574396 0:3db1f2a9d605 76
s1574396 0:3db1f2a9d605 77 {
s1574396 0:3db1f2a9d605 78 currentdirection == Pos_RO;
s1574396 0:3db1f2a9d605 79 cirectionchanged = true;
s1574396 0:3db1f2a9d605 80 }
s1574396 0:3db1f2a9d605 81 break;
s1574396 0:3db1f2a9d605 82
s1574396 0:3db1f2a9d605 83 case Pos_RO:
s1574396 0:3db1f2a9d605 84
s1574396 0:3db1f2a9d605 85 lowpassFilter1 = lowpassFilter1;
s1574396 0:3db1f2a9d605 86 lowpassFilter2 = lowpassFilter2 * int -1;
s1574396 0:3db1f2a9d605 87
s1574396 0:3db1f2a9d605 88 if (lowpassFilter3 > drempelwaarde)
s1574396 0:3db1f2a9d605 89 {
s1574396 0:3db1f2a9d605 90 currentdirection == Pos_LO;
s1574396 0:3db1f2a9d605 91 cirectionchanged = true;
s1574396 0:3db1f2a9d605 92 }
s1574396 0:3db1f2a9d605 93 break;
s1574396 0:3db1f2a9d605 94
s1574396 0:3db1f2a9d605 95 case Pos_LO:
s1574396 0:3db1f2a9d605 96
s1574396 0:3db1f2a9d605 97 lowpassFilter1 = lowpassFilter1 * int -1;
s1574396 0:3db1f2a9d605 98 lowpassFilter2 = lowpassFilter2 * int -1;
s1574396 0:3db1f2a9d605 99
s1574396 0:3db1f2a9d605 100 if (lowpassFilter3 > drempelwaarde)
s1574396 0:3db1f2a9d605 101 {
s1574396 0:3db1f2a9d605 102 currentdirection == Pos_RB;
s1574396 0:3db1f2a9d605 103 cirectionchanged = true;
s1574396 0:3db1f2a9d605 104 }
s1574396 0:3db1f2a9d605 105 break;
s1574396 0:3db1f2a9d605 106 }