De hele robot in 1 keer bam
Dependencies: mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 2:d3e9788ab1b3
- Parent:
- 1:059cca298369
- Child:
- 3:c0ece64850db
--- a/main.cpp Sun Oct 20 18:28:59 2019 +0000 +++ b/main.cpp Sun Oct 20 18:45:13 2019 +0000 @@ -1,16 +1,16 @@ //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 "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); +// PC serial connection +// MODSERIAL pc(USBTX, USBRX); //EMG inputs definieren AnalogIn emg1_in (A1); //emg van rechterbicep, voor de x-richting @@ -34,8 +34,7 @@ double rectify2; double rectify3; -//values for EMG, dit moet nog aangepast worden aan de hand van de in matlab verkregen coefficienten -// Notch (iirnotch Q factor 35 @50Hz) from MATLAB: 0.995636295063941 -1.89829218816065 0.995636295063941 1 -1.89829218816065 0.991272590127882 +// 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; @@ -43,7 +42,7 @@ const double n_a1 = -1.89829218816065; const double n_a2 = 0.991272590127882; -// Highpass (butter 4th order @10Hz cutoff) from MATLAB: 0.922946103200875 -1.84589220640175 0.922946103200875 1 -1.88920703055163 0.892769008131025 +// 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; @@ -51,7 +50,7 @@ const double h_a1 = -1.88920703055163; const double h_02 = 0.892769008131025; -// Lowpass (butter 4th order @5Hz cutoff) from MATLAB: 5.32116245737504e-08 1.06423249147501e-07 5.32116245737504e-08 1 -1.94396715039462 0.944882378004138 +// 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; @@ -60,32 +59,29 @@ const double l_a2 = 0.944882378004138; -//BiQuad values, dit moet nog aangepast worden aan de hand van de in matlab verkregen coefficienten -BiQuadChain notch; -BiQuad bq_n1( n1, n2, n3, n4, n5, n6); -BiQuad bq_n2( n1, n2, n3, n4, n5, n6); -BiQuad bq_n3( n1, n2, n3, n4, n5, n6); -BiQuadChain highpass; -BiQuad bq_h1( h1, h2, h3, h4, h5, h6); -BiQuad bq_h2( h1, h2, h3, h4, h5, h6); -BiQuad bq_h3( h1, h2, h3, h4, h5, h6); -BiQuadChain lowpass; -BiQuad bq_l1( l1, l2, l3, l4, l5, l6); -BiQuad bq_l2( l1, l2, l3, l4, l5, l6); -BiQuad bq_l3( l1, l2, l3, l4, l5, l6); +//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); -//filteren van het EMG signaal -//eerst dus EMG signaal uitlezen -emg1 = emg1_in.read(); -emg2 = emg2_in.read(); -emg3 = emg3_in.read(); +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 +//high pass filter high1 = H1.step(notch1); high2 = H2.step(notch2); high3 = H3.step(notch3);