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
Diff: main.cpp
- Revision:
- 31:cc08254ab7b5
- Parent:
- 30:65f0c9ecf810
- Child:
- 32:1bb406d2b3c3
diff -r 65f0c9ecf810 -r cc08254ab7b5 main.cpp --- a/main.cpp Mon Oct 16 09:42:04 2017 +0000 +++ b/main.cpp Tue Oct 17 09:52:12 2017 +0000 @@ -5,6 +5,7 @@ #include "FastPWM.h" #include "refGen.h" #include "controller.h" +#include "motorConfig.h" // Defining relevant constant parameters const float gearRatio = 131; @@ -14,23 +15,10 @@ 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); @@ -107,51 +95,11 @@ 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 +// Generate a PID controller with the specified values of k_p, k_d and k_i 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; - } - } +motorConfig motor1(LED_GREEN,LED_RED,LED_BLUE,D5,D4); void measureAndControl(){ getError(e_pos, e_int, e_der); @@ -161,18 +109,8 @@ 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; + //pc.printf("motor1 direction is %d \r\n", motor1_direction.read()); + motor1.setMotor(motorValue); } void rSwitchDirection(){ @@ -184,10 +122,8 @@ 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); + sw2.fall(&motor1,&motorConfig::killSwitch); + sw3.fall(&motor1, &motorConfig::turnMotorOn); button2.rise(&rSwitchDirection); pc.baud(115200); controllerTicker.attach(measureAndControl, Ts);