![](/media/cache/group/9e3cc099b3b04bca937a1cca1da81b19.jpg.50x50_q85.jpg)
De hele robot in 1 keer bam
Dependencies: mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM
main.cpp
- Committer:
- Jellehierck
- Date:
- 2019-10-20
- Revision:
- 2:d3e9788ab1b3
- Parent:
- 1:059cca298369
- Child:
- 3:c0ece64850db
File content as of revision 2:d3e9788ab1b3:
//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 serial 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; // Notch filter coefficients (iirnotch Q factor 35 @50Hz) from MATLAB: 0.995636295063941 -1.89829218816065 0.995636295063941 1 -1.89829218816065 0.991272590127882 const double n_b0 = 0.995636295063941; const double n_b1 = -1.89829218816065; const double n_b2 = 0.995636295063941; const double n_a0 = 1; const double n_a1 = -1.89829218816065; const double n_a2 = 0.991272590127882; // Highpass filter coefficients (butter 4th order @10Hz cutoff) from MATLAB: 0.922946103200875 -1.84589220640175 0.922946103200875 1 -1.88920703055163 0.892769008131025 const double h_b0 = 0.922946103200875; const double h_b1 = -1.84589220640175; const double h_b2 = 0.922946103200875; const double h_a0 = 1; const double h_a1 = -1.88920703055163; const double h_02 = 0.892769008131025; // Lowpass filter coefficients (butter 4th order @5Hz cutoff) from MATLAB: 5.32116245737504e-08 1.06423249147501e-07 5.32116245737504e-08 1 -1.94396715039462 0.944882378004138 const double l_b0 = 5.32116245737504e-08; const double l_b1 = 1.06423249147501e-07; const double l_b2 = 5.32116245737504e-08; const double l_a0 = 1; const double l_a1 = -1.94396715039462; const double l_a2 = 0.944882378004138; //BiQuad filters intialization BiQuadChain notch; BiQuad bq_notch( n_b0, n_b1, n_b2, n_a0, n_a1, n_a2); BiQuadChain highpass; BiQuad bq_high( h_b0, h_b1, h_b2, h_a0, h_a1, h_a2); BiQuadChain lowpass; BiQuad bq_notch( l_b0, l_b1, l_b2, l_a0, l_a1, l_a2); void sample() { 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);