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:
- 22:2e473e9798c0
- Parent:
- 21:d266d1e503ce
- Child:
- 23:917179f72762
diff -r d266d1e503ce -r 2e473e9798c0 main.cpp --- a/main.cpp Fri Oct 06 08:34:14 2017 +0000 +++ b/main.cpp Fri Oct 06 10:43:24 2017 +0000 @@ -11,6 +11,12 @@ 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); @@ -36,10 +42,10 @@ 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(){ @@ -55,20 +61,28 @@ 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 -float getError(){ +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); - float posError = getReferencePosition() - motor1Position; - pc.printf("Error is %0.2f \r \n", posError); - return posError; + 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 motorController(float posError, float integralError){ 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 + 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; } @@ -77,6 +91,9 @@ switch(currentState){ case KILLED: motor1_pwm.write(0.0); + ledR = 0; + ledG = 1; + ledB = 1; break; case ACTIVE: // Set motor direction @@ -92,17 +109,22 @@ } else { motor1_pwm.write(fabs(motorValue)); - } + } + ledR = 1; + ledG = 1; + ledB = 0; break; } } void measureAndControl(){ - float posError = getError(); - float motorValue = motorController(posError); - pc.printf("Motorvalue is %.2f \r \n", motorValue); + getError(posError, integralError); + float motorValue = motorController(posError, integralError); pc.printf("Position error is %.2f \r \n", posError); - pc.printf("Motor direction is %d \r \n", motor1_direction); + 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); } @@ -119,6 +141,7 @@ void M1switchDirection(){ m1_direction = !m1_direction; + pc.printf("Switched direction!"); } @@ -132,7 +155,7 @@ sw3.fall(&turnMotorsOn); button1.rise(&M1switchDirection); pc.baud(115200); - controllerTicker.attach(measureAndControl, 0.1); + controllerTicker.attach(measureAndControl, Ts); pc.printf("Encoder ticker attached and baudrate set"); }