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:
- 34:1a70aa045c8f
- Parent:
- 33:6f4858b98fe5
- Child:
- 35:99bf23b34ee3
--- a/main.cpp Fri Oct 20 16:30:26 2017 +0000 +++ b/main.cpp Sun Oct 22 08:01:59 2017 +0000 @@ -8,9 +8,22 @@ #include "controller.h" #include "motorConfig.h" #include "errorFetch.h" +#include "biquadFilter.h" +#include "biquadChain.h" // ADJUSTABLE PARAMETERS +// EMG filter parameters + +// high pass +biquadFilter HPbq1(0.9837,-1.9674, 0.9837,1.0000,-1.9769,0.9770); +biquadFilter HPbq2(1.0000, -2.0000, 1.0000, 1.0000, -1.9903, 0.9904); +biquadChain HPbqc(HPbq1, HPbq2); +// low pass +biquadFilter LPbq1(1.0e-6*0.1202, 1.0e-6*0.2405, 1.0e-6*0.1202, 1.0000, -1.9313, 0.9327); +biquadFilter LPbq2(1.0000, 2.0000, 1.0000, 1.0000, -1.9702, 0.9716); +biquadChain LPbqc(LPbq1, LPbq2); + // Controller parameters const float k_p = 1; const float k_i = 0; // Still needs a reasonable value @@ -44,7 +57,7 @@ InterruptIn button1(D2); InterruptIn button2(D3); AnalogIn pot2(A3); -AnalogIn emg0( A0 ); +//AnalogIn emg0( A0 ); //AnalogIn emg1( A1 ); // Setting up HIDscope @@ -77,11 +90,11 @@ // Function getReferencePosition returns reference angle based on potmeter 1 refGen ref1(A1, maxAngle); -refGen ref2(A1, maxAngle); +//refGen ref2(A1, maxAngle); // readEncoder reads counts and revs and logs results to serial window -errorFetch e1(ref1, gearRatio, Ts); -errorFetch e2(ref2, gearRatio, Ts); +errorFetch e1(gearRatio, Ts); +//errorFetch e2(gearRatio, Ts); // Generate a PID controller with the specified values of k_p, k_d and k_i controller motorController1(k_p, k_d, k_i); @@ -92,10 +105,11 @@ void measureAndControl(){ m1counts = Encoder1.getPulses(); m2counts = Encoder2.getPulses(); - e1.fetchError(m1counts); + float r1 = ref1.getFilteredReference; + e1.fetchError(m1counts, r1); float motorValue = motorController1.control(e1.e_pos, e1.e_int, e1.e_der); - float r = ref1.getReferencePosition(); - sendDataToPc(r, e1.e_pos, emg0.read(), e1.e_der, motorValue); + float r1_unfiltered = ref1.getReference(); + sendDataToPc(r1_unfiltered, r1, e1.e_pos, e1.e_der, motorValue); motor1.setMotor(motorValue); }