Het EMG verwerkingsdeel
Dependencies: mbed
main.cpp@0:3db1f2a9d605, 2018-10-26 (annotated)
- Committer:
- s1574396
- Date:
- Fri Oct 26 07:47:32 2018 +0000
- Revision:
- 0:3db1f2a9d605
- Child:
- 1:3cc91fc2fb1c
EMG verwerking
Who changed what in which revision?
User | Revision | Line number | New 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 |
s1574396 | 0:3db1f2a9d605 | 9 | const double a1 = |
s1574396 | 0:3db1f2a9d605 | 10 | const double a2 = |
s1574396 | 0:3db1f2a9d605 | 11 | const double b0 = |
s1574396 | 0:3db1f2a9d605 | 12 | const double b1 = |
s1574396 | 0:3db1f2a9d605 | 13 | const double b2 = |
s1574396 | 0:3db1f2a9d605 | 14 | const double c1 = |
s1574396 | 0:3db1f2a9d605 | 15 | const double c2 = |
s1574396 | 0:3db1f2a9d605 | 16 | const double d0 = |
s1574396 | 0:3db1f2a9d605 | 17 | const double d1 = |
s1574396 | 0:3db1f2a9d605 | 18 | const double d2 = |
s1574396 | 0:3db1f2a9d605 | 19 | |
s1574396 | 0:3db1f2a9d605 | 20 | double highpassFilter1 = 0; |
s1574396 | 0:3db1f2a9d605 | 21 | double lowpassFilter1 = 0; |
s1574396 | 0:3db1f2a9d605 | 22 | double highpassFilter2 = 0; |
s1574396 | 0:3db1f2a9d605 | 23 | double lowpassFilter2 = 0; |
s1574396 | 0:3db1f2a9d605 | 24 | |
s1574396 | 0:3db1f2a9d605 | 25 | // Filteren |
s1574396 | 0:3db1f2a9d605 | 26 | void filterEMG() |
s1574396 | 0:3db1f2a9d605 | 27 | { |
s1574396 | 0:3db1f2a9d605 | 28 | highpassFilter1 = fabs(biquad(emg0.read(), a1, a2, b0, b1, b2)); |
s1574396 | 0:3db1f2a9d605 | 29 | lowpassFilter1 = biquad(highpassFilter1, c1, c2, d0, d1, d2); |
s1574396 | 0:3db1f2a9d605 | 30 | highpassFilter2 = fabs(biquad(emg1.read(), a1, a2, b0, b1, b2)); |
s1574396 | 0:3db1f2a9d605 | 31 | lowpassFilter2 = biquad(highpassFilter2, c1, c2, d0, d1, d2); |
s1574396 | 0:3db1f2a9d605 | 32 | highpassFilter3 = fabs(biquad(emg2.read(), a1, a2, b0, b1, b2)); |
s1574396 | 0:3db1f2a9d605 | 33 | lowpassFilter3 = biquad(highpassFilter3, c1, c2, d0, d1, d2); |
s1574396 | 0:3db1f2a9d605 | 34 | } |
s1574396 | 0:3db1f2a9d605 | 35 | |
s1574396 | 0:3db1f2a9d605 | 36 | //calibrate (hier moet iets met maximale waarden enzo |
s1574396 | 0:3db1f2a9d605 | 37 | dempelwaarde = int 0.5 * max_lowpassFilter3 |
s1574396 | 0:3db1f2a9d605 | 38 | |
s1574396 | 0:3db1f2a9d605 | 39 | //Direction control |
s1574396 | 0:3db1f2a9d605 | 40 | enum direction {Pos_RB, Pos_LB, Pos_RO, Pos_LO}; |
s1574396 | 0:3db1f2a9d605 | 41 | direction currentdirection = Pos_RB; |
s1574396 | 0:3db1f2a9d605 | 42 | bool directionchanged = true; |
s1574396 | 0:3db1f2a9d605 | 43 | |
s1574396 | 0:3db1f2a9d605 | 44 | int main() |
s1574396 | 0:3db1f2a9d605 | 45 | { |
s1574396 | 0:3db1f2a9d605 | 46 | while () |
s1574396 | 0:3db1f2a9d605 | 47 | { |
s1574396 | 0:3db1f2a9d605 | 48 | switch (currentdirection) |
s1574396 | 0:3db1f2a9d605 | 49 | { |
s1574396 | 0:3db1f2a9d605 | 50 | case Pos_RB: |
s1574396 | 0:3db1f2a9d605 | 51 | |
s1574396 | 0:3db1f2a9d605 | 52 | lowpassFilter1 = lowpassFilter1; |
s1574396 | 0:3db1f2a9d605 | 53 | lowpassFilter2 = lowpassFilter2; |
s1574396 | 0:3db1f2a9d605 | 54 | |
s1574396 | 0:3db1f2a9d605 | 55 | if (lowpassFilter3 > drempelwaarde) |
s1574396 | 0:3db1f2a9d605 | 56 | { |
s1574396 | 0:3db1f2a9d605 | 57 | currentdirection == Pos_LB; |
s1574396 | 0:3db1f2a9d605 | 58 | cirectionchanged = true; |
s1574396 | 0:3db1f2a9d605 | 59 | } |
s1574396 | 0:3db1f2a9d605 | 60 | break; |
s1574396 | 0:3db1f2a9d605 | 61 | |
s1574396 | 0:3db1f2a9d605 | 62 | case Pos_LB: |
s1574396 | 0:3db1f2a9d605 | 63 | |
s1574396 | 0:3db1f2a9d605 | 64 | lowpassFilter1 = lowpassFilter1 * int -1; |
s1574396 | 0:3db1f2a9d605 | 65 | lowpassFilter2 = lowpassFilter2; |
s1574396 | 0:3db1f2a9d605 | 66 | |
s1574396 | 0:3db1f2a9d605 | 67 | if (lowpassFilter3 > drempelwaarde) |
s1574396 | 0:3db1f2a9d605 | 68 | |
s1574396 | 0:3db1f2a9d605 | 69 | { |
s1574396 | 0:3db1f2a9d605 | 70 | currentdirection == Pos_RO; |
s1574396 | 0:3db1f2a9d605 | 71 | cirectionchanged = true; |
s1574396 | 0:3db1f2a9d605 | 72 | } |
s1574396 | 0:3db1f2a9d605 | 73 | break; |
s1574396 | 0:3db1f2a9d605 | 74 | |
s1574396 | 0:3db1f2a9d605 | 75 | case Pos_RO: |
s1574396 | 0:3db1f2a9d605 | 76 | |
s1574396 | 0:3db1f2a9d605 | 77 | lowpassFilter1 = lowpassFilter1; |
s1574396 | 0:3db1f2a9d605 | 78 | lowpassFilter2 = lowpassFilter2 * int -1; |
s1574396 | 0:3db1f2a9d605 | 79 | |
s1574396 | 0:3db1f2a9d605 | 80 | if (lowpassFilter3 > drempelwaarde) |
s1574396 | 0:3db1f2a9d605 | 81 | { |
s1574396 | 0:3db1f2a9d605 | 82 | currentdirection == Pos_LO; |
s1574396 | 0:3db1f2a9d605 | 83 | cirectionchanged = true; |
s1574396 | 0:3db1f2a9d605 | 84 | } |
s1574396 | 0:3db1f2a9d605 | 85 | break; |
s1574396 | 0:3db1f2a9d605 | 86 | |
s1574396 | 0:3db1f2a9d605 | 87 | case Pos_LO: |
s1574396 | 0:3db1f2a9d605 | 88 | |
s1574396 | 0:3db1f2a9d605 | 89 | lowpassFilter1 = lowpassFilter1 * int -1; |
s1574396 | 0:3db1f2a9d605 | 90 | lowpassFilter2 = lowpassFilter2 * int -1; |
s1574396 | 0:3db1f2a9d605 | 91 | |
s1574396 | 0:3db1f2a9d605 | 92 | if (lowpassFilter3 > drempelwaarde) |
s1574396 | 0:3db1f2a9d605 | 93 | { |
s1574396 | 0:3db1f2a9d605 | 94 | currentdirection == Pos_RB; |
s1574396 | 0:3db1f2a9d605 | 95 | cirectionchanged = true; |
s1574396 | 0:3db1f2a9d605 | 96 | } |
s1574396 | 0:3db1f2a9d605 | 97 | break; |
s1574396 | 0:3db1f2a9d605 | 98 | } |