De hele robot in 1 keer bam
Dependencies: mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 0:6972d0e91af1
- Child:
- 1:059cca298369
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sun Oct 20 18:06:57 2019 +0000 @@ -0,0 +1,91 @@ +//c++ script for filtering of measured EMG signals +#include "mbed.h" //Base library +#include "HIDScope.h" // to see if program is working and EMG is filtered properly +#include "QEI.h"// is needed for the encoder +#include "MODSERIAL.h"// in order for connection with the pc +#include "BiQuad.h" +#include "FastPWM.h" +#include "Arduino.h" //misschien handig omdat we het EMG arduino board gebruiken (?) +#include "EMGFilters.h" +#include <vector> // For easy array management + +// PC connection +MODSERIAL pc(USBTX, USBRX); + +//EMG inputs definieren +AnalogIn emg1_in (A1); //emg van rechterbicep, voor de x-richting +AnalogIn emg2_in (A2); //emg van linkerbicep, voor de y-richting +AnalogIn emg3_in (A3); //emg van een derde (nog te bepalen) spier, voor het vernaderen van de richting + +//variablen voor EMG +double emg1; +double emg2; +double emg3; +double notch1; +double notch2; +double notch3; +double highpass1; +double highpass2; +double highpass3; +double lowpass1; +double lowpass2; +double lowpass3; +double rectify1; +double rectify2; +double rectify3; + +//values for EMG, dit moet nog aangepast worden aan de hand van de in matlab verkregen coefficienten +const double n1 = 0,9956 //op internet staat overal a0, a1, b0, b1, b2 en dan voor iedere filter de volgende twee letters van het alfabet +const double n2 = -1,8983 +const double n3 = 0,9956 +const double n4 = 1 +const double n5 = -1,8983 +const double n6 = 0.9913 +const double h1 = 0,9229 +const double h2 = -1,8459 +const double h3 = 0,9229 +const double h4 = 1 +const double h5 = -1,8892 +const double n6 = 0,8928 +const double l1 = 1 +const double l2 = 2 + +//BiQuad values, dit moet nog aangepast worden aan de hand van de in matlab verkregen coefficienten +BiQuadChain notch; +BiQuad N1( n1, n2, n3, n4, n5, n6); +BiQuad N2( n1, n2, n3, n4, n5, n6); +BiQuad N3( n1, n2, n3, n4, n5, n6); +BiQuadChain highpass; +BiQuad H1( h1, h2, h3, h4, h5, h6); +BiQuad H2( h1, h2, h3, h4, h5, h6); +BiQuad H3( h1, h2, h3, h4, h5, h6); +BiQuadChain lowpass; +BiQuad L1( l1, l2, l3, l4, l5, l6); +BiQuad L2( l1, l2, l3, l4, l5, l6); +BiQuad L3( l1, l2, l3, l4, l5, l6); + +//filteren van het EMG signaal +//eerst dus EMG signaal uitlezen +emg1 = emg1_in.read(); +emg2 = emg2_in.read(); +emg3 = emg3_in.read(); + +//notch filter toepassen +notch1 = N1.step(emg1); +notch2 = N2.step(emg2); +notch3 = N3.step(emg3); + +//high pass filter +high1 = H1.step(notch1); +high2 = H2.step(notch2); +high3 = H3.step(notch3); + +//rectify toepassen, oftewel absolute waarde van EMG signaal nemen +absolute1 = fabs(high1); +absolute2 = fabs(high2); +absolute3 = fabs(high3); + +//low pass filter +low1 = L1.step(absolute1); +low2 = L2.step(absolute2); +low3 = L3.step(absolute3);