De hele robot in 1 keer bam
Dependencies: mbed QEI Servo HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 1:059cca298369
- Parent:
- 0:6972d0e91af1
- Child:
- 2:d3e9788ab1b3
diff -r 6972d0e91af1 -r 059cca298369 main.cpp --- a/main.cpp Sun Oct 20 18:06:57 2019 +0000 +++ b/main.cpp Sun Oct 20 18:28:59 2019 +0000 @@ -35,34 +35,44 @@ 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 +// Notch (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 (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 (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 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); +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 H1( h1, h2, h3, h4, h5, h6); -BiQuad H2( h1, h2, h3, h4, h5, h6); -BiQuad H3( h1, h2, h3, h4, h5, h6); +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 L1( l1, l2, l3, l4, l5, l6); -BiQuad L2( l1, l2, l3, l4, l5, l6); -BiQuad L3( l1, l2, l3, l4, l5, l6); +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); //filteren van het EMG signaal //eerst dus EMG signaal uitlezen