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:
- 22:2e473e9798c0
- Parent:
- 21:d266d1e503ce
- Child:
- 23:917179f72762
File content as of revision 22:2e473e9798c0:
#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 // Leds can be used to indicate status DigitalOut ledG(LED_GREEN); DigitalOut ledR(LED_RED); DigitalOut ledB(LED_BLUE); DigitalOut motor1_direction(D4); PwmOut motor1_pwm(D5); // Defining inputs InterruptIn sw2(SW2); InterruptIn sw3(SW3); InterruptIn button1(D2); AnalogIn pot1(A0); AnalogIn pot2(A1); // 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; int posRevRange = 5; // describes the ends of the position range in complete motor output shaft revolutions in both directions const float maxAngle = 2*3.14*posRevRange; // max angle in radians const float Ts = 0.1; // Function getReferencePosition returns reference angle within range float getReferencePosition(){ double r; if(m1_direction == false){ // Clockwise rotation yields positive reference r = maxAngle*pot1.read(); } if(m1_direction == true){ // Counterclockwise rotation yields negative reference r = -1*maxAngle*pot1.read(); } return r; } // Initializing position and integral errors float posError = 0; float integralError = 0; float totalError = posError + integralError; // readEncoder reads counts and revs and logs results to serial window void getError(float &posError, float &integralError){ counts = Encoder.getPulses(); double motor1Position = 2*3.14*(counts/(131*64.0f)); pc.printf("%0.2f revolutions \r\n", motor1Position); posError = getReferencePosition() - motor1Position; integralError = integralError + Ts*totalError; totalError = posError + integralError; pc.printf("Error is %0.2f \r \n", totalError); } // motorController sets a motorValue based on the position error. Sign indicates direction and magnitude indicates speed. Right now this is a simple feedforward float motorController(float posError, float integralError){ float k_p = 5*pot2.read(); // use potmeter 2 to adjust k_p within range 0 to 1 float k_i = 0.0000000001; double motorValue = (posError*k_p)/maxAngle + 0.35 + k_i*integralError; // motorValue is the normalized voltage applied to motor magnitude pin between -1 and 1 added 0.35 as motorvalue of 0.35 does nothing 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); ledR = 0; ledG = 1; ledB = 1; 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)); } ledR = 1; ledG = 1; ledB = 0; break; } } void measureAndControl(){ getError(posError, integralError); float motorValue = motorController(posError, integralError); pc.printf("Position error is %.2f \r \n", posError); pc.printf("Total error is %.2f \r \n", totalError); pc.printf("Integral error is %2f", integralError); //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; pc.printf("Switched direction!"); } 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); controllerTicker.attach(measureAndControl, Ts); pc.printf("Encoder ticker attached and baudrate set"); }