Tristan Vlogman / Mbed 2 deprecated locomotion_pid_action_refactor_EMG

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 First Last

Diff: main.cpp

Revision:
41:9678fd827d25
Parent:
40:7418f46a1ac0
Child:
42:ae78ff03d9d6
--- 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