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
- Committer:
- tvlogman
- Date:
- 2017-09-21
- Revision:
- 17:616ce7bc1f96
- Parent:
- 16:27430afe663e
- Child:
- 18:65e24db15c69
- Child:
- 19:f08b5cd2b7ce
File content as of revision 17:616ce7bc1f96:
#include "mbed.h" #include "MODSERIAL.h" #include "HIDScope.h" #include "QEI.h" #include "FastPWM.h" enum robotStates {KILLED, ACTIVE}; robotStates currentState = KILLED; QEI Encoder(D12,D13,NC,64, QEI::X4_ENCODING); MODSERIAL pc(USBTX, USBRX); HIDScope scope(2); // Defining outputs DigitalOut motor1_direction(D4); PwmOut motor1_pwm(D5); // Defining inputs InterruptIn sw2(SW2); InterruptIn sw3(SW3); InterruptIn button1(D2); AnalogIn pot(A0); // PWM settings float pwmPeriod = 1.0/5000.0; int frequency_pwm = 10000; //10kHz PWM volatile float x; volatile float x_prev =0; volatile float y; // filtered 'output' of ReadAnalogInAndFilter // Initializing encoder Ticker encoderTicker; volatile int counts = 0; volatile float revs = 0.00; void readEncoder(){ counts = Encoder.getPulses(); revs = counts/(131*64.0f); pc.printf("%0.2f revolutions \r\n", revs); // Displaying revs in HIDscope x = revs; // Capture data scope.set(0, x); // store data in first element of scope memory y = (x_prev + x)/2.0; // averaging filter scope.set(1, y); // store data in second element of scope memory x_prev = x; // Prepare for next round scope.send(); // send what's in scope memory to PC } void killSwitch(){ pc.printf("Motors turned off"); currentState = KILLED; } void turnMotorsOn(){ pc.printf("Motors turned on"); currentState = ACTIVE; } void M1switchDirection(){ motor1_direction = !motor1_direction; } int main() { motor1_direction = false; motor1_pwm.period(pwmPeriod);//T=1/f sw2.fall(&killSwitch); sw3.fall(&turnMotorsOn); button1.rise(&M1switchDirection); pc.baud(115200); encoderTicker.attach(readEncoder, 0.1); pc.printf("Encoder ticker attached and baudrate set"); while(true){ switch(currentState){ case KILLED: motor1_pwm.write(0.0); break; case ACTIVE: motor1_pwm.write(pot.read()); // Motor speed proportional to pot meter break; } } }