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:
- 41:9678fd827d25
- Parent:
- 40:7418f46a1ac0
- Child:
- 42:ae78ff03d9d6
diff -r 7418f46a1ac0 -r 9678fd827d25 main.cpp --- a/main.cpp Thu Oct 26 14:02:49 2017 +0000 +++ b/main.cpp Thu Oct 26 19:39:46 2017 +0000 @@ -20,8 +20,9 @@ // calibration time const int calSamples = 100; +// Initialize average and max EMG value for calibration to 0 and 1 respectively volatile float avgEMGvalue = 0; - +volatile double maxEMGvalue = 1; // high pass BiQuadChain HPbqc; @@ -135,25 +136,29 @@ double m1position = e1.fetchMotorPosition(m1counts); double m2position = e2.fetchMotorPosition(m2counts); - // measuring EMG signals to use as basis for reference + // measuring and normalizing EMG signals to use as basis for reference - float emg1 = ref1.getReference(); - float emg2 = ref2.getReference(); + float emg1 = ref1.getReference()/maxEMGvalue; + float emg2 = ref2.getReference()/maxEMGvalue; + // Filtering the EMG signals - double thet1dot = HPbqc.step(emg1); - thet1dot = fabs(thet1dot); - thet1dot = LPbqc.step(thet1dot); + double thet1 = HPbqc.step(emg1); + thet1 = fabs(thet1); + thet1 = LPbqc.step(thet1); - double thet2dot = HPbqc.step(emg2); - thet2dot = fabs(thet2dot); - thet2dot = LPbqc.step(thet2dot); + double thet2 = HPbqc.step(emg2); + thet2 = fabs(thet2); + 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; - // Pseudo-integrating prescribed angular velocities to obtain prescribed angles - double thet1 = m1position + thet1dot*Ts; - double thet2 = m1position + thet2dot*Ts; + // 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 + // Finite state machine switch(currentState){ @@ -240,6 +245,9 @@ 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 max = *max_element(vector.begin(), vector.end()); pc.printf("Avg emg value is %.2f changed state to active", avgEMGvalue); // State transition logic