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-16
- Revision:
- 30:65f0c9ecf810
- Parent:
- 29:9aa4d63a9bd1
- Child:
- 31:cc08254ab7b5
File content as of revision 30:65f0c9ecf810:
#include "mbed.h" #include "MODSERIAL.h" #include "HIDScope.h" #include "QEI.h" #include "FastPWM.h" #include "refGen.h" #include "controller.h" // Defining relevant constant parameters const float gearRatio = 131; // Controller parameters const float k_p = 1; const float k_i = 0; // Still needs a reasonable value const float k_d = 0; // Again, still need to pick a reasonable value enum robotStates {KILLED, ACTIVE}; robotStates currentState = KILLED; QEI Encoder(D12,D13,NC,64, QEI::X4_ENCODING); MODSERIAL pc(USBTX, USBRX); HIDScope scope(5); // 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); InterruptIn button2(D3); AnalogIn pot2(A1); // PWM settings float pwmPeriod = 1.0/5000.0; int frequency_pwm = 10000; //10kHz PWM // Setting up HIDscope volatile float x; volatile float y; volatile float z; volatile float q; volatile float k; void sendDataToPc(float data1, float data2, float data3, float data4, float data5){ // Capture data x = data1; y = data2; z = data3; q = data4; k = data5; scope.set(0, x); scope.set(1, y); scope.set(2, z); scope.set(3, q); scope.set(4, z); scope.send(); // send what's in scope memory to PC } // Initializing encoder Ticker encoderTicker; Ticker controllerTicker; volatile int counts = 0; volatile float revs = 0.00; // MOTOR CONTROL PART bool r_direction = false; int posRevRange = 2; // 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 based on potmeter 1 refGen ref(A0); //ref.getReferencePosition(maxAngle); // Initializing position and integral errors to zero float e_pos = 0; float e_int = 0; float e_der = 0; float e_prev = 0; // readEncoder reads counts and revs and logs results to serial window void getError(float &e_pos, float &e_int, float &e_der){ // Getting encoder counts and calculating motor position counts = Encoder.getPulses(); double motor1Position = 2*3.14*(counts/(gearRatio*64.0f)); // Computing position error e_pos = ref.getReferencePosition(maxAngle, r_direction) - motor1Position; // Limiting the integral error to prevent integrator saturation if(fabs(e_int) <= 5){ e_int = e_int + Ts*e_pos; } // Derivative error e_der = (e_pos - e_prev)/Ts; e_prev = e_pos; // Store current position error as we'll need it to compute the next derivative error } // motorController sets a motorValue based on the position error. Sign indicates direction and magnitude indicates speed. Right now this is a simple feedforward controller motorController1(k_p, k_d, k_i); // 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); // Set motor direction if (motorValue >=0){ // corresponds to CW rotation of motor axle motor1_direction = 0; } else if(motorValue < 0){ // corresponds to CCW rotation of motor axle motor1_direction = 1; pc.printf("Bah!"); } ledR = 0; ledG = 1; ledB = 1; break; case ACTIVE: pc.printf("Got into ACTIVE \r\n"); // Set motor direction if (motorValue >=0){ // corresponds to CW rotation of motor axle motor1_direction.write(0); } else if(motorValue < 0){ // corresponds to CCW rotation of motor axle motor1_direction.write(1); } // Set motor speed if (fabs(motorValue)>1){ motor1_pwm = 1; } else { motor1_pwm.write(fabs(motorValue) + 0.4); } ledR = 1; ledG = 1; ledB = 0; break; } } void measureAndControl(){ getError(e_pos, e_int, e_der); float motorValue = motorController1.control(e_pos, e_int, e_der); float r = ref.getReferencePosition(maxAngle, r_direction); sendDataToPc(r, e_pos, e_int, e_der, motorValue); pc.printf("Motorvalue is %.2f \r\n", motorValue); pc.printf("Error is %.2f \r\n", e_pos); pc.printf("Reference is %.2f \r\n", r); pc.printf("motor1 direction is %d \r\n", motor1_direction.read()); setMotor1(motorValue); } void killSwitch(){ pc.printf("Motors turned off"); currentState = KILLED; } void turnMotorsOn(){ pc.printf("Motors turned on"); currentState = ACTIVE; } void rSwitchDirection(){ r_direction = !r_direction; pc.printf("Switched reference direction! \r\n"); } 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); button2.rise(&rSwitchDirection); pc.baud(115200); controllerTicker.attach(measureAndControl, Ts); pc.printf("Encoder ticker attached and baudrate set"); }