De hele robot in 1 keer bam
Dependencies: mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM
main.cpp
- Committer:
- IsaRobin
- Date:
- 2019-10-20
- Revision:
- 0:6972d0e91af1
- Child:
- 1:059cca298369
File content as of revision 0:6972d0e91af1:
//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);