Project of Biorobotics
Dependencies: HIDScope MODSERIAL QEI mbed biquadFilter
Fork of TutorialPES by
Diff: main.cpp
- Revision:
- 4:8f67b8327300
- Parent:
- 3:c8f0fc045505
- Child:
- 5:312186a0604d
diff -r c8f0fc045505 -r 8f67b8327300 main.cpp --- a/main.cpp Mon Oct 22 14:51:46 2018 +0000 +++ b/main.cpp Tue Oct 23 13:08:22 2018 +0000 @@ -9,6 +9,7 @@ PwmOut PwmPin1(D5); PwmOut PwmPin2(D6); DigitalIn Knop1(D2); +DigitalIn Knop2(D3); AnalogIn pot1 (A5); AnalogIn pot2 (A4); AnalogIn emg0( A0 ); @@ -16,18 +17,9 @@ AnalogIn emg2( A2 ); AnalogIn emg3( A3 ); -DigitalIn a1( D10); -DigitalIn b1( D11); -DigitalIn a2( D12); -DigitalIn b2( D13); - - +Ticker StateTicker; Ticker EncodingTicker; Ticker printTicker; -Ticker mycontrollerTicker1; -Ticker mycontrollerTicker2; -Ticker Velo1; -Ticker Velo2; Ticker EMG_Read_Ticker; Ticker sample_timer; HIDScope scope( 4 ); @@ -37,20 +29,29 @@ volatile float Tricep_Right = 0.0; volatile float Tricep_Left = 0.0; volatile const float maxVelocity = 8.4; // in rad/s +volatile const double pi = 3.14159265358979; volatile float referenceVelocity1 = 0.5; //dit is de gecentreerde waarde en dus de nulstand volatile float referenceVelocity2 = 0.5; +enum states{Calibration, Homing, Function}; + +volatile states Active_State = Calibration; + volatile int counts1; volatile int counts2; - + void Encoding() { - - QEI Encoder1(D12,D13,NC,32); + + QEI Encoder1(D12,D13,NC,64); QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING); counts1 = Encoder1.getPulses(); counts2 = Encoder2.getPulses(); + float rad_m1 = ((2*pi)/32.0)* (float)counts1; + float rad_m2 = ((2*pi)/32.0)* (float)counts2; + + // pc.printf("%f & %f ....\n",rad_m1, rad_m2); } void EMG_Read() @@ -136,26 +137,59 @@ //pc.printf("%f \n %f snelheid Motor1 \n %f snelheid Motor2 \n", Bicep_Right,v1,v2); pc.printf("%f ", counts1); - - } +void StateMachine() +{ + switch (Active_State) + { + case Calibration: + //calibration actions + //pc.printf("Calibration State"); + if (Knop1==false) + { + pc.printf("Entering Homing state \n"); + Active_State = Homing; + } + break; + + case Homing: + // Homing actions + //pc.printf("Homing State"); + if (Knop2==false) + { + pc.printf("Entering Funtioning State \n"); + Active_State = Function; + } + break; + + case Function: + //pc.printf("Funtioning State"); + + velocity1(); + velocity2(); + motor1(); + motor2(); + break; + + default: + pc.printf("UNKNOWN COMMAND"); + } +} + int main() { pc.baud(115200); - PwmPin1.period_us(40); //60 microseconds pwm period, 16.7 kHz + 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); - mycontrollerTicker1.attach(motor1, 0.002);//500Hz - Velo1.attach(velocity1, 0.002); - mycontrollerTicker2.attach(motor2, 0.002); - Velo2.attach(velocity2, 0.002); - - printTicker.attach(&Printing, 2.0); + StateTicker.attach(StateMachine, 0.002); + + //printTicker.attach(&Printing, 2.0); while(true) {