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:
- 32:1bb406d2b3c3
- Parent:
- 31:cc08254ab7b5
- Child:
- 33:6f4858b98fe5
diff -r cc08254ab7b5 -r 1bb406d2b3c3 main.cpp --- a/main.cpp Tue Oct 17 09:52:12 2017 +0000 +++ b/main.cpp Fri Oct 20 12:24:37 2017 +0000 @@ -6,6 +6,7 @@ #include "refGen.h" #include "controller.h" #include "motorConfig.h" +#include "errorFetch.h" // Defining relevant constant parameters const float gearRatio = 131; @@ -15,7 +16,8 @@ const float k_i = 0; // Still needs a reasonable value const float k_d = 0; // Again, still need to pick a reasonable value -QEI Encoder(D12,D13,NC,64, QEI::X4_ENCODING); +QEI Encoder1(D12,D13,NC,64, QEI::X4_ENCODING); +QEI Encoder2(D11,D10,NC,64, QEI::X4_ENCODING); MODSERIAL pc(USBTX, USBRX); HIDScope scope(5); @@ -24,7 +26,9 @@ InterruptIn sw3(SW3); InterruptIn button1(D2); InterruptIn button2(D3); -AnalogIn pot2(A1); +AnalogIn pot2(A3); +AnalogIn emg0( A0 ); +//AnalogIn emg1( A1 ); // PWM settings float pwmPeriod = 1.0/5000.0; @@ -66,34 +70,12 @@ const float Ts = 0.1; // Function getReferencePosition returns reference angle based on potmeter 1 -refGen ref(A0); +refGen ref1(A1); //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 - } +errorFetch err1(Encoder1, gearRatio, Ts, ref1); // Generate a PID controller with the specified values of k_p, k_d and k_i controller motorController1(k_p, k_d, k_i); @@ -102,10 +84,10 @@ motorConfig motor1(LED_GREEN,LED_RED,LED_BLUE,D5,D4); 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); + err1.fetchError(); + float motorValue = motorController1.control(err1.e_pos, err1.e_int, err1.e_der); + float r = ref1.getReferencePosition(ref.maxAngle, ref1.r_direction); + sendDataToPc(r, err1.e_pos, emg0.read(), err1.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);