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-10-06
- Revision:
- 20:4ce3fb543a45
- Parent:
- 19:f08b5cd2b7ce
- Child:
- 21:d266d1e503ce
File content as of revision 20:4ce3fb543a45:
#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); // 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; Ticker controllerTicker; volatile int counts = 0; volatile float revs = 0.00; // MOTOR CONTROL PART bool m1_direction = false; // Function getReferenceVelocity returns reference r between -8.4 and 8.4 rad/s float getReferenceVelocity(){ const float maxVelocity = 8.4; // max velocity in rad/s double r; if(m1_direction == false){ // Clockwise rotation yields positive reference r = maxVelocity*pot.read(); } if(m1_direction == true){ // Counterclockwise rotation yields negative reference r = -1*maxVelocity*pot.read(); } return r; } // motorController sets a motorValue based on the reference. Sign indicates direction and magnitude indicates speed. Right now this is a simple feedforward float motorController(float reference){ const float motorGain = 8.4; // max power of the motor should normalize to a motorValue magnitude of 1 double motorValue = reference/motorGain; // motorValue is the normalized voltage applied to motor magnitude pin between -1 and 1 return motorValue; } // setMotor1 sets the direction and magnitude pins of motor1 depending on the given motorValue. Negative motorValue means clockwise rotation void setMotor1(float motorValue){ switch(currentState){ case KILLED: motor1_pwm.write(0.0); break; case ACTIVE: // Set motor direction if (motorValue >=0){ motor1_direction = 0; } else { motor1_direction = 1; } // Set motor speed if (fabs(motorValue)>1){ motor1_pwm = 1; } else { motor1_pwm.write(fabs(motorValue)); } break; } } void measureAndControl(){ float referenceVelocity = getReferenceVelocity(); float motorValue = motorController(referenceVelocity); pc.printf("Motorvalue is %.2f \r \n", motorValue); pc.printf("Motor direction is %d \r \n", motor1_direction); setMotor1(motorValue); } void killSwitch(){ pc.printf("Motors turned off"); currentState = KILLED; } void turnMotorsOn(){ pc.printf("Motors turned on"); currentState = ACTIVE; } void M1switchDirection(){ m1_direction = !m1_direction; } // readEncoder reads counts and revs and logs results to serial window void readEncoder(){ counts = Encoder.getPulses(); revs = counts/(131*64.0f); pc.printf("%0.2f revolutions \r\n", revs); float reference = getReferenceVelocity(); pc.printf("Reference velocity %0.2f \r\n", reference); } int main() { pc.printf("Main function"); motor1_direction = 0; // False = clockwise rotation motor1_pwm.period(pwmPeriod);//T=1/f sw2.fall(&killSwitch); sw3.fall(&turnMotorsOn); button1.rise(&M1switchDirection); pc.baud(115200); encoderTicker.attach(readEncoder, 2); controllerTicker.attach(measureAndControl, 0.1); pc.printf("Encoder ticker attached and baudrate set"); }