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:
- 21:d266d1e503ce
- Parent:
- 20:4ce3fb543a45
- Child:
- 22:2e473e9798c0
File content as of revision 21:d266d1e503ce:
#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 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 // 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; } // readEncoder reads counts and revs and logs results to serial window float getError(){ counts = Encoder.getPulses(); double motor1Position = 2*3.14*(counts/(131*64.0f)); pc.printf("%0.2f revolutions \r\n", motor1Position); float posError = getReferencePosition() - motor1Position; pc.printf("Error is %0.2f \r \n", posError); return posError; } // 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 k_p = 5*pot2.read(); // use potmeter 2 to adjust k_p within range 0 to 1 double motorValue = (posError*k_p)/maxAngle + 0.35; // 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); 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 posError = getError(); float motorValue = motorController(posError); pc.printf("Motorvalue is %.2f \r \n", motorValue); pc.printf("Position error is %.2f \r \n", posError); 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; } 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, 0.1); pc.printf("Encoder ticker attached and baudrate set"); }