![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Facking kut filter werkt EINDELIJK !311!!111!!1!!!!!!!!!!!!!!!!!!!
Dependencies: HIDScope MODSERIAL Matrix QEI biquadFilter mbed
Fork of Turning_Motor_V6 by
Diff: main.cpp
- Revision:
- 7:c5c648898881
- Parent:
- 6:056ad27636ff
- Child:
- 8:b6b09226a421
--- a/main.cpp Fri Oct 26 06:56:18 2018 +0000 +++ b/main.cpp Tue Oct 30 15:41:58 2018 +0000 @@ -2,6 +2,7 @@ #include "MODSERIAL.h" #include "HIDScope.h" #include "QEI.h" +#include "BiQuad.h" MODSERIAL pc(USBTX, USBRX); DigitalOut DirectionPin1(D4); @@ -17,7 +18,7 @@ AnalogIn emg2( A2 ); AnalogIn emg3( A3 ); -QEI Encoder1(D12,D13,NC,64); +QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING); QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING); //DigitalOut LED(LED_RED); @@ -25,7 +26,20 @@ Ticker StateTicker; Ticker printTicker; -HIDScope scope( 4 ); +HIDScope scope(4); + +BiQuadChain bqc1; +BiQuadChain bqc2; +BiQuadChain bqc3; +BiQuadChain bqc4; +BiQuadChain bqc5; +BiQuadChain bqc6; +BiQuadChain bqc7; +BiQuadChain bqc8; +BiQuad BqNotch1( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01 ); +BiQuad BqNotch2( 1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01); +BiQuad BqHP( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 ); +BiQuad BqLP( 8.76555e-05, 1.75311e-04, 8.76555e-05, -1.97334e+00, 9.73695e-01 ); volatile float Bicep_Right = 0.0; volatile float Bicep_Left = 0.0; @@ -44,15 +58,52 @@ volatile int counts2 ; volatile float rad_m1; volatile float rad_m2; +volatile float q_1; +volatile float q_2; +volatile float r_1; +volatile float r_2; +volatile const float r_3 = 2.0; // + +volatile float FilterHP_Bi_R; +volatile float Filterabs_Bi_R; +volatile float Filtered_Bi_R; +volatile float FilterHP_Bi_L; +volatile float Filterabs_Bi_L; +volatile float Filtered_Bi_L; +volatile float FilterHP_Tri_R; +volatile float Filterabs_Tri_R; +volatile float Filtered_Tri_R; +volatile float FilterHP_Tri_L; +volatile float Filterabs_Tri_L; +volatile float Filtered_Tri_L; +void filter() +{ + FilterHP_Bi_R = bqc1.step( emg0.read() ); + Filterabs_Bi_R = fabs(FilterHP_Bi_R); + Filtered_Bi_R = bqc2.step( Filterabs_Bi_R ); + + FilterHP_Bi_L = bqc3.step( emg1.read() ); + Filterabs_Bi_L = fabs(FilterHP_Bi_L); + Filtered_Bi_L = bqc4.step( Filterabs_Bi_L ); + + FilterHP_Tri_R = bqc5.step( emg2.read() ); + Filterabs_Tri_R = fabs(FilterHP_Tri_R); + Filtered_Tri_R = bqc6.step( Filterabs_Tri_R ); + + FilterHP_Tri_L = bqc7.step( emg3.read() ); + Filterabs_Tri_L = fabs(FilterHP_Tri_L); + Filtered_Tri_L = bqc8.step( Filterabs_Tri_L ); +} + void Encoding() { counts1 = Encoder1.getPulses(); counts2 = Encoder2.getPulses(); // Hier gaat iets fout waardoor het 0 wordt!!! - float rad_m1 = ((2.0*pi)/32.0)* (float)counts1; - float rad_m2 = ((2.0*pi)/32.0)* (float)counts2; + rad_m1 = ((2.0*pi)/32.0)* (float)counts1; + rad_m2 = ((2.0*pi)/32.0)* (float)counts2; // pc.printf("%f & %f ....\n",rad_m1, rad_m2); } @@ -68,10 +119,10 @@ void sample() { - scope.set(0, emg0.read() ); - scope.set(1, emg1.read() ); - scope.set(2, emg2.read() ); - scope.set(3, emg3.read() ); + scope.set(0, Filtered_Bi_R ); + scope.set(1, Filtered_Bi_L ); + scope.set(2, Filtered_Tri_R ); + scope.set(3, Filtered_Tri_L ); scope.send(); } @@ -142,6 +193,18 @@ pc.printf("%i %i \n",counts1,counts2); } +void inverse_kinematics() +{ + float JacPs [2][2]; + JacPs[0][0] = 2.0; + JacPs[0][1] = 3.0; + JacPs[1][0] = 4.0; + JacPs[1][1] = 5.0; + pc.printf("%f ", JacPs[0][0]); +} + + + void StateMachine() { switch (Active_State) @@ -154,10 +217,10 @@ pc.printf("Entering Homing state \n"); Active_State = Homing; } - + filter(); sample(); - EMG_Read(); - Encoding(); + //EMG_Read(); + Encoding(); break; @@ -169,9 +232,9 @@ pc.printf("Entering Funtioning State \n"); Active_State = Function; } - + filter(); sample(); - EMG_Read(); + //EMG_Read(); Encoding(); break; @@ -189,9 +252,9 @@ Active_State = Calibration; } - + filter(); sample(); - EMG_Read(); + //EMG_Read(); Encoding(); velocity1(); velocity2(); @@ -209,7 +272,14 @@ { pc.baud(115200); PwmPin1.period_us(30); //60 microseconds pwm period, 16.7 kHz - + bqc1.add( &BqNotch1 ).add( &BqNotch2 ).add( &BqHP ); + bqc2.add(&BqLP); + bqc3.add( &BqNotch1 ).add( &BqNotch2 ).add( &BqHP ); + bqc4.add(&BqLP); + bqc5.add( &BqNotch1 ).add( &BqNotch2 ).add( &BqHP ); + bqc6.add(&BqLP); + bqc7.add( &BqNotch1 ).add( &BqNotch2 ).add( &BqHP ); + bqc8.add(&BqLP); StateTicker.attach(StateMachine, 0.002); printTicker.attach(&Printing, 2.0);