Het EMG verwerkingsdeel
Dependencies: mbed
main.cpp
- Committer:
- keeswieriks
- Date:
- 2018-10-26
- Revision:
- 2:dc387d9de7c4
- Parent:
- 1:3cc91fc2fb1c
File content as of revision 2:dc387d9de7c4:
#include "mbed.h" // inputs EMG AnalogIn emg0( A0 ); AnalogIn emg1( A1 ); AnalogIn emg3( A2 ); // Variabelen EMG const double m1 =0.5000; const double m2 =-0.8090; const double n0 =0.5000; const double n1 =-0.8090; const double n2 =0; const double a1 =0.9565; const double a2 =-1.9131; const double b0 =0.9565; const double b1 =-1.9112; const double b2 =0.9150; const double c1 =0.0675; const double c2 =0.1349; const double d0 =0.0675; const double d1 =-1.1430; const double d2 =0.4128; double highpassFilter1 = 0; double lowpassFilter1 = 0; double highpassFilter2 = 0; double lowpassFilter2 = 0; // Filteren void filterEMG() { notchFilter1 = biquad(emg0.read(), m1, m2, n0, n1, n2); highpassFilter1 = fabs(biquad(notchFilter1, a1, a2, b0, b1, b2)); lowpassFilter1 = biquad(highpassFilter1, c1, c2, d0, d1, d2); notchFilter2 = biquad(emg1.read(), m1, m2, n0, n1, n2); highpassFilter2 = fabs(biquad(notchFilter2, a1, a2, b0, b1, b2)); lowpassFilter2 = biquad(highpassFilter2, c1, c2, d0, d1, d2); notchFilter3 = biquad(emg2.read(), m1, m2, n0, n1, n2); highpassFilter3 = fabs(biquad(notchFilter2, a1, a2, b0, b1, b2)); lowpassFilter3 = biquad(highpassFilter3, c1, c2, d0, d1, d2); } //calibrate (hier moet iets met maximale waarden enzo dempelwaarde = int 0.5 * max_lowpassFilter3 //Direction control enum direction {Pos_RB, Pos_LB, Pos_RO, Pos_LO}; direction currentdirection = Pos_RB; bool directionchanged = true; int main() { while () { switch (currentdirection) { case Pos_RB: lowpassFilter1 = lowpassFilter1; lowpassFilter2 = lowpassFilter2; if (lowpassFilter3 > drempelwaarde) { currentdirection == Pos_LB; cirectionchanged = true; } break; case Pos_LB: lowpassFilter1 = lowpassFilter1 * int -1; lowpassFilter2 = lowpassFilter2; if (lowpassFilter3 > drempelwaarde) { currentdirection == Pos_RO; cirectionchanged = true; } break; case Pos_RO: lowpassFilter1 = lowpassFilter1; lowpassFilter2 = lowpassFilter2 * int -1; if (lowpassFilter3 > drempelwaarde) { currentdirection == Pos_LO; cirectionchanged = true; } break; case Pos_LO: lowpassFilter1 = lowpassFilter1 * int -1; lowpassFilter2 = lowpassFilter2 * int -1; if (lowpassFilter3 > drempelwaarde) { currentdirection == Pos_RB; cirectionchanged = true; } break; }