Diff: main.cpp

Revision:

35:e82834e62e44

Parent:

34:13fac02ef324

Child:

36:ec2bb2a02856

--- a/main.cpp	Tue Oct 29 13:50:28 2019 +0000
+++ b/main.cpp	Tue Oct 29 16:00:53 2019 +0000
@@ -15,7 +15,7 @@
 */
 
 // PC serial connection
-HIDScope        scope( 4 ); 
+HIDScope        scope( 4 );
 MODSERIAL pc(USBTX, USBRX);
 
 // LED
@@ -53,9 +53,12 @@
 double emg1_factor;
 double emg1_th;
 double emg1_out;
+double emg1_out_prev;
+double emg1_dt;
 double emg1_norm;
 vector<double> emg1_cal;
 int emg1_cal_size;
+int emg1_dir = 1;
 
 double emg2;
 double emg2_env;
@@ -67,6 +70,7 @@
 double emg2_norm;
 vector<double> emg2_cal;
 int emg2_cal_size;
+int emg2_dir = 1;
 
 double emg3;
 double emg3_env;
@@ -78,6 +82,7 @@
 double emg3_norm;
 vector<double> emg3_cal;
 int emg3_cal_size;
+int emg3_dir = 1;
 
 // Initialize tickers and timeouts
 Ticker tickSample;
@@ -133,11 +138,13 @@
 // Return max value of vector
 double getMax(const vector<double> &vect)
 {
-    double curr_max = 0.0; 
+    double curr_max = 0.0;
     int vect_n = vect.size();
-    
+
     for (int i = 0; i < vect_n; i++) {
-        if (vect[i] > curr_max) { curr_max = vect[i]; };
+        if (vect[i] > curr_max) {
+            curr_max = vect[i];
+        };
     }
     return curr_max;
 }
@@ -174,6 +181,24 @@
     return output;
 }
 
+// Check filter stability
+bool checkBQChainStable()
+{
+    bool n_stable = bqc1_notch.stable(); // Check stability of all BQ Chains
+    bool hp_stable =  bqc1_high.stable();
+    bool l_stable = bqc1_low.stable();
+
+    if (n_stable && hp_stable && l_stable) {
+        return true;
+    } else {
+        return false;
+    }
+}
+
+/*
+------ BUTTON FUNCTIONS ------
+*/
+
 // Handle button press
 void button1Press()
 {
@@ -186,17 +211,29 @@
     button2_pressed = true;
 }
 
-// Check filter stability
-bool checkBQChainStable()
+// Toggle EMG direction
+void toggleEMG1Dir()
 {
-    bool n_stable = bqc1_notch.stable(); // Check stability of all BQ Chains
-    bool hp_stable =  bqc1_high.stable();
-    bool l_stable = bqc1_low.stable();
+    switch( emg1_dir ) {
+        case -1:
+            emg1_dir = 1;
+            break;
+        case 1:
+            emg1_dir = -1;
+            break;
+    }
+}
 
-    if (n_stable && hp_stable && l_stable) {
-        return true;
-    } else {
-        return false;
+// Toggle EMG direction
+void toggleEMG2Dir()
+{
+    switch( emg1_dir ) {
+        case -1:
+            emg1_dir = 1;
+            break;
+        case 1:
+            emg1_dir = -1;
+            break;
     }
 }
 
@@ -291,7 +328,7 @@
     // Entry function
     if ( emg_state_changed == true ) {
         emg_state_changed = false; // Disable entry functions
-        
+
         button1.fall( &button1Press ); // Change to state MVC calibration on button1 press
         button2.fall( &button2Press ); // Change to state rest calibration on button2 press
     }
@@ -376,6 +413,11 @@
         emg2_th = emg2_rest * emg2_factor + margin_percentage/100; // Set normalized rest threshold
         emg3_factor = 1 / emg3_MVC; // Factor to normalize MVC
         emg3_th = emg3_rest * emg3_factor + margin_percentage/100; // Set normalized rest threshold
+        
+        
+        // ------- TO DO: MAKE SURE THESE BUTTONS DO NOT BOUNCE (e.g. with button1.rise() ) ------
+        //button1.fall( &toggleEMG1Dir ); // Change to state MVC calibration on button1 press
+        //button2.fall( &toggleEMG2Dir ); // Change to state rest calibration on button2 press
 
         sampleNow = true; // Enable signal sampling in sampleSignal()
         calibrateNow = false; // Disable calibration vector functionality in sampleSignal()
@@ -386,6 +428,7 @@
     emg2_norm = emg2_env * emg2_factor; // Idem
     emg3_norm = emg3_env * emg3_factor; // Idem
 
+    emg1_out_prev = emg1_out; // Set previous emg_out signal
     // Set normalized EMG output signal (CAN BE MOVED TO EXTERNAL FUNCTION BECAUSE IT IS REPEATED 3 TIMES)
     if ( emg1_norm < emg1_th ) { // If below threshold, emg_out = 0 (ignored)
         emg1_out = 0.0;
@@ -396,6 +439,8 @@
         // Outputs are scaled to range  [0,      1]
         emg1_out = rescale(emg1_norm, 0, 1, emg1_th, 1);
     }
+    emg1_dt = (emg1_out - emg1_out_prev) / Ts; // Calculate derivative of filtered normalized output signal
+    emg1_out = emg1_out * emg1_dir; // Set direction of EMG output
 
     // Idem for emg2
     if ( emg2_norm < emg2_th ) {
@@ -405,6 +450,7 @@
     } else {
         emg2_out = rescale(emg2_norm, 0, 1, emg2_th, 1);
     }
+    emg2_out = emg2_out * emg2_dir; // Set direction of EMG output
 
     // Idem for emg3
     if ( emg3_norm < emg3_th ) {
@@ -418,8 +464,8 @@
     // Set HIDScope outputs
     scope.set(0, emg1 );
     scope.set(1, emg1_env );
-    scope.set(2, emg1_norm );
-    scope.set(3, emg1_out );
+    scope.set(2, emg1_out );
+    scope.set(3, emg1_dt );
     //scope.set(2, emg2_out );
     //scope.set(3, emg3_out );
     scope.send();
@@ -494,7 +540,7 @@
     } else {
         led_r = 0; // LED on
     }
-    
+
     emg_curr_state = emg_wait; // Start off in EMG Wait state
     tickGlobal.attach( &tickGlobalFunc, Ts ); // Start global ticker