![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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@16:27430afe663e, 2017-09-21 (annotated)
- Committer:
- tvlogman
- Date:
- Thu Sep 21 11:20:03 2017 +0000
- Revision:
- 16:27430afe663e
- Parent:
- 15:b76b8cff4d8f
- Child:
- 17:616ce7bc1f96
Added a reverse-direction button on button1 of the Biorobotics shield
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 | 16:27430afe663e | 12 | HIDScope scope(2); |
tvlogman | 8:0067469c3389 | 13 | |
tvlogman | 14:664870b5d153 | 14 | // Defining outputs |
tvlogman | 13:83e3672b24ee | 15 | DigitalOut motor1_direction(D4); |
tvlogman | 13:83e3672b24ee | 16 | PwmOut motor1_pwm(D5); |
tvlogman | 14:664870b5d153 | 17 | |
tvlogman | 14:664870b5d153 | 18 | // Defining inputs |
tvlogman | 14:664870b5d153 | 19 | InterruptIn sw2(SW2); |
tvlogman | 15:b76b8cff4d8f | 20 | InterruptIn sw3(SW3); |
tvlogman | 16:27430afe663e | 21 | InterruptIn button1(D2); |
tvlogman | 15:b76b8cff4d8f | 22 | AnalogIn pot(A0); |
tvlogman | 15:b76b8cff4d8f | 23 | |
tvlogman | 16:27430afe663e | 24 | // PWM settings |
tvlogman | 16:27430afe663e | 25 | float pwmPeriod = 1.0/5000.0; |
tvlogman | 16:27430afe663e | 26 | int frequency_pwm = 10000; //10kHz PWM |
tvlogman | 14:664870b5d153 | 27 | |
tvlogman | 16:27430afe663e | 28 | volatile float x; |
tvlogman | 16:27430afe663e | 29 | volatile float x_prev =0; |
tvlogman | 16:27430afe663e | 30 | volatile float y; // filtered 'output' of ReadAnalogInAndFilter |
tvlogman | 16:27430afe663e | 31 | volatile float ledBrightness = 0.00; |
tvlogman | 14:664870b5d153 | 32 | |
tvlogman | 13:83e3672b24ee | 33 | |
tvlogman | 10:e23cbcdde7e3 | 34 | Ticker encoderTicker; |
tvlogman | 10:e23cbcdde7e3 | 35 | volatile int counts = 0; |
tvlogman | 12:0462757e1ed2 | 36 | volatile float revs = 0.00; |
tvlogman | 7:1bffab95fc5f | 37 | |
tvlogman | 16:27430afe663e | 38 | |
tvlogman | 16:27430afe663e | 39 | |
tvlogman | 10:e23cbcdde7e3 | 40 | void readEncoder(){ |
tvlogman | 10:e23cbcdde7e3 | 41 | counts = Encoder.getPulses(); |
tvlogman | 12:0462757e1ed2 | 42 | revs = counts/64.0f; |
tvlogman | 16:27430afe663e | 43 | |
tvlogman | 16:27430afe663e | 44 | // Displaying revs in HIDscope |
tvlogman | 16:27430afe663e | 45 | x = revs; // Capture data |
tvlogman | 16:27430afe663e | 46 | scope.set(0, x); // store data in first element of scope memory |
tvlogman | 16:27430afe663e | 47 | y = (x_prev + x)/2.0; // averaging filter |
tvlogman | 16:27430afe663e | 48 | scope.set(1, y); // store data in second element of scope memory |
tvlogman | 16:27430afe663e | 49 | x_prev = x; // Prepare for next round |
tvlogman | 16:27430afe663e | 50 | |
tvlogman | 16:27430afe663e | 51 | scope.send(); // send what's in scope memory to PC |
tvlogman | 10:e23cbcdde7e3 | 52 | } |
tvlogman | 14:664870b5d153 | 53 | |
tvlogman | 14:664870b5d153 | 54 | void killSwitch(){ |
tvlogman | 15:b76b8cff4d8f | 55 | pc.printf("Motors turned off"); |
tvlogman | 15:b76b8cff4d8f | 56 | currentState = KILLED; |
tvlogman | 14:664870b5d153 | 57 | } |
tvlogman | 14:664870b5d153 | 58 | |
tvlogman | 15:b76b8cff4d8f | 59 | void turnMotorsOn(){ |
tvlogman | 15:b76b8cff4d8f | 60 | pc.printf("Motors turned on"); |
tvlogman | 15:b76b8cff4d8f | 61 | currentState = ACTIVE; |
tvlogman | 15:b76b8cff4d8f | 62 | } |
tvlogman | 15:b76b8cff4d8f | 63 | |
tvlogman | 15:b76b8cff4d8f | 64 | |
tvlogman | 14:664870b5d153 | 65 | void M1switchDirection(){ |
tvlogman | 14:664870b5d153 | 66 | motor1_direction = !motor1_direction; |
tvlogman | 14:664870b5d153 | 67 | } |
vsluiter | 0:c8f15874531b | 68 | |
vsluiter | 0:c8f15874531b | 69 | int main() |
tvlogman | 10:e23cbcdde7e3 | 70 | { |
tvlogman | 14:664870b5d153 | 71 | motor1_direction = false; |
tvlogman | 13:83e3672b24ee | 72 | motor1_pwm.period(pwmPeriod);//T=1/f |
tvlogman | 14:664870b5d153 | 73 | sw2.fall(&killSwitch); |
tvlogman | 15:b76b8cff4d8f | 74 | sw3.fall(&turnMotorsOn); |
tvlogman | 16:27430afe663e | 75 | button1.rise(&M1switchDirection); |
vsluiter | 0:c8f15874531b | 76 | pc.baud(115200); |
tvlogman | 16:27430afe663e | 77 | encoderTicker.attach(readEncoder, 0.1); |
tvlogman | 15:b76b8cff4d8f | 78 | |
tvlogman | 10:e23cbcdde7e3 | 79 | pc.printf("Encoder ticker attached and baudrate set"); |
tvlogman | 13:83e3672b24ee | 80 | |
tvlogman | 15:b76b8cff4d8f | 81 | while(true){ |
tvlogman | 15:b76b8cff4d8f | 82 | switch(currentState){ |
tvlogman | 15:b76b8cff4d8f | 83 | case KILLED: |
tvlogman | 15:b76b8cff4d8f | 84 | motor1_pwm.write(0.0); |
tvlogman | 15:b76b8cff4d8f | 85 | break; |
tvlogman | 15:b76b8cff4d8f | 86 | case ACTIVE: |
tvlogman | 15:b76b8cff4d8f | 87 | motor1_pwm.write(pot.read()); // Motor speed proportional to pot meter |
tvlogman | 15:b76b8cff4d8f | 88 | break; |
tvlogman | 15:b76b8cff4d8f | 89 | } |
tvlogman | 15:b76b8cff4d8f | 90 | } |
tvlogman | 15:b76b8cff4d8f | 91 | |
vsluiter | 0:c8f15874531b | 92 | } |
tvlogman | 7:1bffab95fc5f | 93 |