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:
- 42:ae78ff03d9d6
- Parent:
- 41:9678fd827d25
- Child:
- 43:dd0888f86357
--- a/main.cpp Thu Oct 26 19:39:46 2017 +0000 +++ b/main.cpp Fri Oct 27 10:37:43 2017 +0000 @@ -1,5 +1,6 @@ #include <vector> #include <numeric> +#include <algorithm> #include "mbed.h" #include "Matrix.h" #include "MODSERIAL.h" @@ -18,7 +19,7 @@ // EMG filter parameters // calibration time -const int calSamples = 100; +const int calSamples = 1000; // Initialize average and max EMG value for calibration to 0 and 1 respectively volatile float avgEMGvalue = 0; @@ -32,11 +33,11 @@ // low pass BiQuadChain LPbqc; -BiQuad LPbq1(1.0e-5*1.3294, 1.0e-5*2.6587, 1.0e-5*1.3294, 1.0000, -1.7783, 0.7924); -BiQuad LPbq2(1.0000, 2.0000, 1.0000, 1.0000, -1.8934, 0.9085); +BiQuad LPbq1(1.0e-7*1.2023, 1.0e-7*2.4046, 1.0e-7*1.2023, 1.0000, -1.9313, 0.9327); +BiQuad LPbq2(1.0000, 2.0000, 1.0000, 1.0000, -1.9702, 0.9716); // Controller parameters -const float k_p = 10; +const float k_p = 1; const float k_i = 0; // Still needs a reasonable value const float k_d = 0; // Again, still need to pick a reasonable value @@ -108,13 +109,13 @@ // REFERENCE PARAMETERS -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 +int posRevRange = 1; // describes the ends of the position range in complete motor output shaft revolutions in both directions +const float maxAngle = 1*3.14*posRevRange; // max angle in radians // Function getReferencePosition returns reference angle based on potmeter 1 -refGen ref1(A1, maxAngle); -refGen ref2(A1, maxAngle); +refGen ref1(A2, maxAngle); +refGen ref2(A3, maxAngle); // readEncoder reads counts and revs and logs results to serial window errorFetch e1(gearRatio, Ts); @@ -138,24 +139,27 @@ // measuring and normalizing EMG signals to use as basis for reference - float emg1 = ref1.getReference()/maxEMGvalue; - float emg2 = ref2.getReference()/maxEMGvalue; + double emg1 = ref1.getReference(); + double emg2 = ref2.getReference(); // Filtering the EMG signals - - double thet1 = HPbqc.step(emg1); - thet1 = fabs(thet1); - thet1 = LPbqc.step(thet1); + + double emg1HP = HPbqc.step(emg1); + double emg1HP_abs = fabs(emg1HP); + double emg1HPLP_abs = LPbqc.step(emg1HP_abs); +// thet1 = fabs(thet1); +// thet1 = LPbqc.step(thet1); - double thet2 = HPbqc.step(emg2); - thet2 = fabs(thet2); - thet2 = LPbqc.step(thet2); + double emg2HP = HPbqc.step(emg2); + double emg2HP_abs = fabs(emg2); + double emg2HPLP_abs = LPbqc.step(emg2HP_abs); +// thet2 = LPbqc.step(thet2); // Something worth trying: set a position setpoint that constantly changes but will never be reached until EMG value is 0 as it is computed from the robot's current position - double thet1 = m1position + thet1; - double thet2 = m1position + thet2; +// thet1 = m1position + thet1; +// thet2 = m1position + thet2; // Other possibility: use thet1 and thet2 directly as reference angles. That'll require the user to hold muscle tension to stay in a certain position though @@ -180,10 +184,10 @@ ledB = 1; // need something here to check if "killswitch" has been pressed (?) // NOTE: state transition is handled using buttons triggering functions motorConfig::kill() and motorConfig::turnMotorOn - e1.fetchError(m1position, thet1); - e2.fetchError(m2position, thet1); +// e1.fetchError(m1position, thet1); +// e2.fetchError(m2position, thet1); - sendDataToPc(thet1, thet2, e1.e_pos, e2.e_pos, 0.0,0.0); // just send the EMG signal value to HIDscope + sendDataToPc(emg1, emg1HP_abs, emg1HPLP_abs, emg2, emg2HP, emg2HPLP_abs); // just send the EMG signal value to HIDscope break; } @@ -194,8 +198,8 @@ } // Compute error - e1.fetchError(m1position, thet1); - e2.fetchError(m2position, thet2); + e1.fetchError(m1position, emg1); + e2.fetchError(m2position, emg2); // Compute motor value using controller and set motor float motorValue1 = motorController1.control(e1.e_pos, e1.e_int, e1.e_der); @@ -204,7 +208,7 @@ motor2.setMotor(motorValue2); // Send data to HIDscope - sendDataToPc(thet1, thet2, e1.e_pos, e2.e_pos, motorValue1, motorValue2); + sendDataToPc(emg1, emg2, e1.e_pos, e2.e_pos, motorValue1, motorValue2); // Set LED to blue ledR = 1; @@ -245,9 +249,8 @@ else { // Set new avgEMGvalue avgEMGvalue = std::accumulate(EMGsamples.begin(), EMGsamples.end(), 0)/calSamples; // still needs to return this value - double maxEMGvalue = std::*max_element(EMGsamples.begin(), EMGsamples.end()); + double maxEMGvalue = *std::max_element(EMGsamples.begin(), EMGsamples.end()); - double max = *max_element(vector.begin(), vector.end()); pc.printf("Avg emg value is %.2f changed state to active", avgEMGvalue); // State transition logic @@ -260,7 +263,7 @@ ledR = 1; ledG = 0; ledB = 1; - sendDataToPc(thet1, thet2, 0.0, 0.0, 0.0, 0.0); +// sendDataToPc(thet1, thet2, 0.0, 0.0, 0.0, 0.0); break; } } @@ -305,7 +308,7 @@ sw2.fall(&killSwitch); sw3.fall(&activateRobot); button1.rise(&r1SwitchDirection); - button2.rise(&calibrateRobot); + button2.rise(&r2SwitchDirection); controllerTicker.attach(measureAndControl, Ts); pc.printf("Encoder ticker attached and baudrate set");