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:
- 5:312186a0604d
- Parent:
- 4:8f67b8327300
- Child:
- 6:056ad27636ff
--- a/main.cpp Tue Oct 23 13:08:22 2018 +0000 +++ b/main.cpp Wed Oct 24 10:30:47 2018 +0000 @@ -17,11 +17,14 @@ AnalogIn emg2( A2 ); AnalogIn emg3( A3 ); +QEI Encoder1(D12,D13,NC,64); +QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING); + +//DigitalOut LED(LED_RED); + Ticker StateTicker; -Ticker EncodingTicker; Ticker printTicker; -Ticker EMG_Read_Ticker; -Ticker sample_timer; + HIDScope scope( 4 ); volatile float Bicep_Right = 0.0; @@ -37,17 +40,17 @@ volatile states Active_State = Calibration; -volatile int counts1; -volatile int counts2; +volatile int counts1 ; +volatile int counts2 ; +volatile float rad_m1; +volatile float rad_m2; void Encoding() { - - QEI Encoder1(D12,D13,NC,64); - QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING); + counts1 = Encoder1.getPulses(); counts2 = Encoder2.getPulses(); - + // Hier gaat iets fout waardoor het 0 wordt!!! float rad_m1 = ((2*pi)/32.0)* (float)counts1; float rad_m2 = ((2*pi)/32.0)* (float)counts2; @@ -82,7 +85,7 @@ referenceVelocity1 = (pot1.read()-0.5f) * 2.0f; } - else if (pot1.read() == 0.5f || !Knop1 == (pot1.read()<0.5f)) + else if (pot1.read() == 0.5f) { referenceVelocity1 = pot1.read() * 0.0f; } @@ -136,7 +139,7 @@ //eventueel nog counts -> rad/s //pc.printf("%f \n %f snelheid Motor1 \n %f snelheid Motor2 \n", Bicep_Right,v1,v2); - pc.printf("%f ", counts1); + pc.printf("%i %i \n",counts1,counts2); } void StateMachine() @@ -144,32 +147,57 @@ switch (Active_State) { case Calibration: - //calibration actions - //pc.printf("Calibration State"); - if (Knop1==false) - { - pc.printf("Entering Homing state \n"); - Active_State = Homing; - } + //calibration actions + //pc.printf("Calibration State"); + if (Knop1==false) + { + pc.printf("Entering Homing state \n"); + Active_State = Homing; + } + + sample(); + EMG_Read(); + Encoding(); + break; case Homing: - // Homing actions - //pc.printf("Homing State"); - if (Knop2==false) - { - pc.printf("Entering Funtioning State \n"); - Active_State = Function; - } + //Homing actions + //pc.printf("Homing State"); + if (Knop2==false) + { + pc.printf("Entering Funtioning State \n"); + Active_State = Function; + } + + sample(); + EMG_Read(); + Encoding(); break; case Function: - //pc.printf("Funtioning State"); - - velocity1(); - velocity2(); - motor1(); - motor2(); + //pc.printf("Funtioning State"); + + if (Knop2==false) + { + pc.printf("Re-entering Homing State \n"); + Active_State = Homing; + } + else if (Knop1==false) + { + pc.printf("Re-entering Calibration State \n"); + Active_State = Calibration; + } + + + sample(); + EMG_Read(); + Encoding(); + velocity1(); + velocity2(); + motor1(); + motor2(); + break; default: @@ -182,16 +210,11 @@ pc.baud(115200); PwmPin1.period_us(30); //60 microseconds pwm period, 16.7 kHz - EncodingTicker.attach(&Encoding, 0.002); - - sample_timer.attach(&sample, 0.002); - EMG_Read_Ticker.attach(&EMG_Read, 0.002); - StateTicker.attach(StateMachine, 0.002); - //printTicker.attach(&Printing, 2.0); + printTicker.attach(&Printing, 2.0); while(true) - { + { } } \ No newline at end of file