Control up to two motors using filtered EMG signals and a PID controller
Dependencies: FastPWM HIDScope MODSERIAL QEI Matrix biquadFilter controller errorFetch mbed motorConfig refGen MatrixMath inverseKinematics
Fork of Minor_test_serial by
main.cpp@15:b76b8cff4d8f, 2017-09-21 (annotated)
- Committer:
- tvlogman
- Date:
- Thu Sep 21 11:00:55 2017 +0000
- Revision:
- 15:b76b8cff4d8f
- Parent:
- 14:664870b5d153
- Child:
- 16:27430afe663e
Changed control scheme to finite state machine
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
vsluiter | 0:c8f15874531b | 1 | #include "mbed.h" |
vsluiter | 0:c8f15874531b | 2 | #include "MODSERIAL.h" |
tvlogman | 8:0067469c3389 | 3 | #include "HIDScope.h" |
tvlogman | 9:5f0e796c9489 | 4 | #include "QEI.h" |
tvlogman | 15:b76b8cff4d8f | 5 | #include "FastPWM.h" |
tvlogman | 15:b76b8cff4d8f | 6 | |
tvlogman | 15:b76b8cff4d8f | 7 | enum robotStates {KILLED, ACTIVE}; |
tvlogman | 15:b76b8cff4d8f | 8 | robotStates currentState = KILLED; |
tvlogman | 8:0067469c3389 | 9 | |
tvlogman | 12:0462757e1ed2 | 10 | QEI Encoder(D12,D13,NC,64, QEI::X4_ENCODING); |
tvlogman | 10:e23cbcdde7e3 | 11 | MODSERIAL pc(USBTX, USBRX); |
tvlogman | 8:0067469c3389 | 12 | |
tvlogman | 14:664870b5d153 | 13 | // Defining outputs |
tvlogman | 13:83e3672b24ee | 14 | DigitalOut motor1_direction(D4); |
tvlogman | 13:83e3672b24ee | 15 | PwmOut motor1_pwm(D5); |
tvlogman | 14:664870b5d153 | 16 | |
tvlogman | 14:664870b5d153 | 17 | // Defining inputs |
tvlogman | 14:664870b5d153 | 18 | InterruptIn sw2(SW2); |
tvlogman | 15:b76b8cff4d8f | 19 | InterruptIn sw3(SW3); |
tvlogman | 15:b76b8cff4d8f | 20 | AnalogIn pot(A0); |
tvlogman | 15:b76b8cff4d8f | 21 | |
tvlogman | 15:b76b8cff4d8f | 22 | // Enabling motors |
tvlogman | 15:b76b8cff4d8f | 23 | bool motorsOn = true; |
tvlogman | 14:664870b5d153 | 24 | |
tvlogman | 14:664870b5d153 | 25 | |
tvlogman | 13:83e3672b24ee | 26 | float pwmPeriod = 1.0/5000.0; |
tvlogman | 13:83e3672b24ee | 27 | |
tvlogman | 10:e23cbcdde7e3 | 28 | Ticker encoderTicker; |
tvlogman | 10:e23cbcdde7e3 | 29 | volatile int counts = 0; |
tvlogman | 12:0462757e1ed2 | 30 | volatile float revs = 0.00; |
tvlogman | 7:1bffab95fc5f | 31 | |
tvlogman | 10:e23cbcdde7e3 | 32 | void readEncoder(){ |
tvlogman | 10:e23cbcdde7e3 | 33 | counts = Encoder.getPulses(); |
tvlogman | 12:0462757e1ed2 | 34 | revs = counts/64.0f; |
tvlogman | 12:0462757e1ed2 | 35 | pc.printf("%i pulses \r\n", counts); |
tvlogman | 12:0462757e1ed2 | 36 | pc.printf("%f revolutions \r\n", revs); |
tvlogman | 10:e23cbcdde7e3 | 37 | } |
tvlogman | 14:664870b5d153 | 38 | |
tvlogman | 14:664870b5d153 | 39 | void killSwitch(){ |
tvlogman | 15:b76b8cff4d8f | 40 | pc.printf("Motors turned off"); |
tvlogman | 15:b76b8cff4d8f | 41 | currentState = KILLED; |
tvlogman | 14:664870b5d153 | 42 | } |
tvlogman | 14:664870b5d153 | 43 | |
tvlogman | 15:b76b8cff4d8f | 44 | void turnMotorsOn(){ |
tvlogman | 15:b76b8cff4d8f | 45 | pc.printf("Motors turned on"); |
tvlogman | 15:b76b8cff4d8f | 46 | currentState = ACTIVE; |
tvlogman | 15:b76b8cff4d8f | 47 | } |
tvlogman | 15:b76b8cff4d8f | 48 | |
tvlogman | 15:b76b8cff4d8f | 49 | |
tvlogman | 14:664870b5d153 | 50 | void M1switchDirection(){ |
tvlogman | 14:664870b5d153 | 51 | motor1_direction = !motor1_direction; |
tvlogman | 14:664870b5d153 | 52 | } |
vsluiter | 0:c8f15874531b | 53 | |
tvlogman | 13:83e3672b24ee | 54 | int frequency_pwm = 10000; //10kHz PWM |
tvlogman | 13:83e3672b24ee | 55 | |
vsluiter | 0:c8f15874531b | 56 | int main() |
tvlogman | 10:e23cbcdde7e3 | 57 | { |
tvlogman | 14:664870b5d153 | 58 | motor1_direction = false; |
tvlogman | 13:83e3672b24ee | 59 | motor1_pwm.period(pwmPeriod);//T=1/f |
tvlogman | 14:664870b5d153 | 60 | sw2.fall(&killSwitch); |
tvlogman | 15:b76b8cff4d8f | 61 | sw3.fall(&turnMotorsOn); |
vsluiter | 0:c8f15874531b | 62 | pc.baud(115200); |
tvlogman | 15:b76b8cff4d8f | 63 | encoderTicker.attach(readEncoder, 1.0); |
tvlogman | 15:b76b8cff4d8f | 64 | |
tvlogman | 10:e23cbcdde7e3 | 65 | pc.printf("Encoder ticker attached and baudrate set"); |
tvlogman | 13:83e3672b24ee | 66 | |
tvlogman | 15:b76b8cff4d8f | 67 | while(true){ |
tvlogman | 15:b76b8cff4d8f | 68 | switch(currentState){ |
tvlogman | 15:b76b8cff4d8f | 69 | case KILLED: |
tvlogman | 15:b76b8cff4d8f | 70 | motor1_pwm.write(0.0); |
tvlogman | 15:b76b8cff4d8f | 71 | break; |
tvlogman | 15:b76b8cff4d8f | 72 | case ACTIVE: |
tvlogman | 15:b76b8cff4d8f | 73 | motor1_pwm.write(pot.read()); // Motor speed proportional to pot meter |
tvlogman | 15:b76b8cff4d8f | 74 | break; |
tvlogman | 15:b76b8cff4d8f | 75 | } |
tvlogman | 15:b76b8cff4d8f | 76 | } |
tvlogman | 15:b76b8cff4d8f | 77 | |
vsluiter | 0:c8f15874531b | 78 | } |
tvlogman | 7:1bffab95fc5f | 79 |