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:
- 40:7418f46a1ac0
- Parent:
- 39:d065ad7a978d
- Child:
- 41:9678fd827d25
diff -r d065ad7a978d -r 7418f46a1ac0 main.cpp --- a/main.cpp Thu Oct 26 13:29:56 2017 +0000 +++ b/main.cpp Thu Oct 26 14:02:49 2017 +0000 @@ -35,7 +35,7 @@ BiQuad LPbq2(1.0000, 2.0000, 1.0000, 1.0000, -1.8934, 0.9085); // Controller parameters -const float k_p = 1; +const float k_p = 10; const float k_i = 0; // Still needs a reasonable value const float k_d = 0; // Again, still need to pick a reasonable value @@ -131,18 +131,29 @@ // Read encoders and EMG signal (unnfiltered reference) m1counts = Encoder1.getPulses(); m2counts = Encoder2.getPulses(); - float r1_uf = ref1.getReference(); - float r2_uf = ref2.getReference(); - pc.printf("In controller ticker \r\n"); + + double m1position = e1.fetchMotorPosition(m1counts); + double m2position = e2.fetchMotorPosition(m2counts); + + // measuring EMG signals to use as basis for reference + + float emg1 = ref1.getReference(); + float emg2 = ref2.getReference(); + + // Filtering the EMG signals - // Filter reference - float r1 = HPbqc.step(r1_uf); - r1 = fabs(r1); - r1 = LPbqc.step(r1); + double thet1dot = HPbqc.step(emg1); + thet1dot = fabs(thet1dot); + thet1dot = LPbqc.step(thet1dot); + - float r2 = HPbqc.step(r2_uf); - r2 = fabs(r2); - r2 = LPbqc.step(r2); + double thet2dot = HPbqc.step(emg2); + thet2dot = fabs(thet2dot); + thet2dot = LPbqc.step(thet2dot); + + // Pseudo-integrating prescribed angular velocities to obtain prescribed angles + double thet1 = m1position + thet1dot*Ts; + double thet2 = m1position + thet2dot*Ts; // Finite state machine switch(currentState){ @@ -164,10 +175,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(m1counts, r1); - e2.fetchError(m2counts, r2); + e1.fetchError(m1position, thet1); + e2.fetchError(m2position, thet1); - sendDataToPc(r1, r2, e1.e_pos, e2.e_pos, 0.0,0.0); // just send the EMG signal value to HIDscope + sendDataToPc(thet1, thet2, e1.e_pos, e2.e_pos, 0.0,0.0); // just send the EMG signal value to HIDscope break; } @@ -178,8 +189,8 @@ } // Compute error - e1.fetchError(m1counts, r1); - e2.fetchError(m2counts, r2); + e1.fetchError(m1position, thet1); + e2.fetchError(m2position, thet2); // Compute motor value using controller and set motor float motorValue1 = motorController1.control(e1.e_pos, e1.e_int, e1.e_der); @@ -188,7 +199,7 @@ motor2.setMotor(motorValue2); // Send data to HIDscope - sendDataToPc(r1, r2, e1.e_pos, e2.e_pos, motorValue1, motorValue2); + sendDataToPc(thet1, thet2, e1.e_pos, e2.e_pos, motorValue1, motorValue2); // Set LED to blue ledR = 1; @@ -222,9 +233,7 @@ // fill the array with sample data if it is not yet filled if(Ncal < calSamples){ - pc.printf("%.2f \r\n", r1_uf); - EMGsamples.push_back(r1_uf); - pc.printf("%.2f \r\n", EMGsamples.end()); + EMGsamples.push_back(emg1); Ncal++; } // if array is filled compute the mean value and switch to active state @@ -243,7 +252,7 @@ ledR = 1; ledG = 0; ledB = 1; - sendDataToPc(r1_uf, r2_uf, 0.0, 0.0, 0.0, 0.0); + sendDataToPc(thet1, thet2, 0.0, 0.0, 0.0, 0.0); break; } }