Group 9 BioRobotics / Mbed 2 deprecated EMG_verwerking

Het EMG verwerkingsdeel

Dependencies:   mbed

main.cpp@1:3cc91fc2fb1c, 2018-10-26 (annotated)

Committer:
keeswieriks
Date:
Fri Oct 26 10:19:17 2018 +0000
Revision:
1:3cc91fc2fb1c
Parent:
0:3db1f2a9d605
Child:
2:dc387d9de7c4
variabelen toefevoegd

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 1:3cc91fc2fb1c 9 const double a1 =0.9565;
keeswieriks 1:3cc91fc2fb1c 10 const double a2 =-1.9131;
keeswieriks 1:3cc91fc2fb1c 11 const double b0 =0.9565;
keeswieriks 1:3cc91fc2fb1c 12 const double b1 =-1.9112;
keeswieriks 1:3cc91fc2fb1c 13 const double b2 =0.9150;
keeswieriks 1:3cc91fc2fb1c 14 const double c1 =0.0675;
keeswieriks 1:3cc91fc2fb1c 15 const double c2 =0.1349;
keeswieriks 1:3cc91fc2fb1c 16 const double d0 =0.0675;
keeswieriks 1:3cc91fc2fb1c 17 const double d1 =-1.1430;
keeswieriks 1:3cc91fc2fb1c 18 const double d2 =0.4128;
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()
keeswieriks 1:3cc91fc2fb1c 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 }