![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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:
- 25:df780572cfc8
- Parent:
- 24:672abc3f02b7
- Child:
- 26:4f84448b4d46
--- a/main.cpp Fri Oct 06 12:34:54 2017 +0000 +++ b/main.cpp Fri Oct 06 12:37:54 2017 +0000 @@ -62,29 +62,29 @@ } // Initializing position and integral errors -float posError = 0; -float integralError = 0; -float derivativeError = 0; +float e_pos = 0; +float e_int = 0; +float e_der = 0; float e_prev = 0; -float totalError = posError + integralError + derivativeError; +float e = e_pos + e_int + e_der; // readEncoder reads counts and revs and logs results to serial window -void getError(float &posError, float &integralError, float &derivativeError){ +void getError(float &e_pos, float &e_int, float &e_der){ 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*posError; - derivativeError = derivativeError - e_prev; - totalError = posError + integralError; - pc.printf("Error is %0.2f \r \n", totalError); + e_pos = getReferencePosition() - motor1Position; + e_int = e_int + Ts*e_pos; + e_der = e_der - e_prev; + e = e_pos + e_int; + pc.printf("Error is %0.2f \r \n", e); } // 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 motorController(float e_pos, float e_int){ float k_p = 5*pot2.read(); // use potmeter 2 to adjust k_p within range 0 to 1 float k_i = 0.001; // How do I pick a reasonable value for k_i? - 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 + double motorValue = (e_pos*k_p)/maxAngle + 0.35 + k_i*e_int; // 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; } @@ -120,11 +120,11 @@ } 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); + getError(e_pos, e_int); + float motorValue = motorController(e_pos, e_int); + pc.printf("Position error is %.2f \r \n", e_pos); + pc.printf("Total error is %.2f \r \n", e); + pc.printf("Integral error is %2f", e_int); //pc.printf("Motorvalue is %.2f \r \n", motorValue); //pc.printf("Motor direction is %d \r \n", motor1_direction); setMotor1(motorValue);