Diff: main.cpp

Revision:

39:f9042483b921

Parent:

38:8b597ab8344f

Child:

40:c6dffb676350

diff -r 8b597ab8344f -r f9042483b921 main.cpp
--- a/main.cpp	Wed Oct 30 15:49:09 2019 +0000
+++ b/main.cpp	Wed Oct 30 16:33:01 2019 +0000
@@ -314,14 +314,14 @@
 
         // Extract EMG scale data from calibration
         switch( emg_curr_state ) {
-            case emg_cal_MVC:
+            case emg_cal_MVC: // In case of MVC calibration
                 emg1_MVC = getMax(emg1_cal); // Store max value of MVC globally
                 emg2_MVC = getMax(emg2_cal);
                 emg3_MVC = getMax(emg3_cal);
 
                 emg_MVC_cal_done = true; // Set up transition to EMG operation mode
                 break;
-            case emg_cal_rest:
+            case emg_cal_rest: // In case of rest calibration
                 emg1_rest = getMean(emg1_cal); // Store mean of EMG in rest globally
                 emg2_rest = getMean(emg2_cal);
                 emg3_rest = getMean(emg3_cal);
@@ -343,8 +343,9 @@
     // Entry function
     if ( emg_state_changed == true ) {
         emg_state_changed = false; // Disable entry functions
+        
+        // Compute scale factors for all EMG signals
         double margin_percentage = 5; // Set up % margin for rest
-
         emg1_factor = 1 / emg1_MVC; // Factor to normalize MVC
         emg1_th = emg1_rest * emg1_factor + margin_percentage/100; // Set normalized rest threshold
         emg2_factor = 1 / emg2_MVC; // Factor to normalize MVC
@@ -357,64 +358,13 @@
         //button1.fall( &toggleEMG1Dir ); // Change to state MVC calibration on button1 press
         //button2.fall( &toggleEMG2Dir ); // Change to state rest calibration on button2 press
 
-        emg_sampleNow = true; // Enable signal sampling in sampleSignals()
-        emg_calibrateNow = false; // Disable calibration vector functionality in sampleSignals()
-    }
-
-    // Do stuff until end condition is met
-    emg1_norm = emg1_env * emg1_factor; // Normalize EMG signal with calibrated factor
-    emg2_norm = emg2_env * emg2_factor; // Idem
-    emg3_norm = emg3_env * emg3_factor; // Idem
-
-    emg1_out_prev = emg1_out; // Set previous emg_out signal
-    emg1_dt_prev = emg1_dt; // Set previous emg_out_dt 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;
-    } else if ( emg1_norm > 1.0f ) { // If above MVC (e.g. due to filtering), emg_out = 1 (max value)
-        emg1_out = 1.0;
-    } else { // If in between threshold and MVC, scale EMG signal accordingly
-        // Inputs may be in range       [emg_th, 1]
-        // Outputs are scaled to range  [0,      1]
-        emg1_out = rescale(emg1_norm, 0, 1, emg1_th, 1);
+        emg_cal_done = true; // Let the global substate machine know that EMG calibration is finished
     }
-    emg1_dt = (emg1_out - emg1_out_prev) / Ts; // Calculate derivative of filtered normalized output signal
-    emg1_dtdt = (emg1_dt - emg1_dt_prev) / Ts; // Calculate acceleration of filtered normalized output signal
-    emg1_out = emg1_out * emg1_dir; // Set direction of EMG output
-
-    // Idem for emg2
-    if ( emg2_norm < emg2_th ) {
-        emg2_out = 0.0;
-    } else if ( emg2_norm > 1.0f ) {
-        emg2_out = 1.0;
-    } 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 ) {
-        emg3_out = 0.0;
-    } else if ( emg3_norm > 1.0f ) {
-        emg3_out = 1.0;
-    } else {
-        emg3_out = rescale(emg3_norm, 0, 1, emg3_th, 1);
-    }
-
-    // Set HIDScope outputs
-    scope.set(0, emg1 );
-    scope.set(1, emg1_out );
-    scope.set(2, emg1_dt );
-    scope.set(3, emg1_dtdt );
-    //scope.set(2, emg2_out );
-    //scope.set(3, emg3_out );
-    scope.send();
-
-    led_g = !led_g;
+    
+    // This state only runs its entry functions ONCE and then exits the EMG substate machine
 
     // State transition guard
-    if ( false ) {
+    if ( false ) { // EMG substate machine is terminated after running this state once, so there is no transition to next EMG substate
         emg_curr_state = emg_wait; // Set next state
         emg_state_changed = true; // Enable entry function
     }
@@ -529,6 +479,9 @@
         button2_pressed = false;
         global_curr_state = global_motor_cal;
         global_state_changed = true;
+    } else if ( emg_cal_done && motor_cal_done ) { // OPERATION MODE
+        global_curr_state = global_operation;
+        global_state_changed = true;
     }
 }
 
@@ -540,17 +493,11 @@
         global_state_changed = false;
     }
 
-    // Do stuff until end condition is met
-    doStuff();
+    // Run EMG state machine until emg_cal_done flag is true
+    emg_state_machine();
 
     // State transition guard
-    if ( motor_cal_done == true ) { // OPERATION MODE
-        emg_cal_done = true;
-        global_curr_state = global_operation;
-        global_state_changed = true;
-    } else if ( button1_pressed == true ) { // WAIT MODE
-        button1_pressed = false;
-        emg_cal_done = true;
+    if ( emg_cal_done == true ) { // WAIT MODE
         global_curr_state = global_wait;
         global_state_changed = true;
     }
@@ -586,10 +533,62 @@
     // Entry function
     if ( global_state_changed == true ) {
         global_state_changed = false;
+        
+        emg_sampleNow = true; // Enable signal sampling in sampleSignals()
+        emg_calibrateNow = false; // Disable calibration functionality in sampleSignals()
     }
 
     // Do stuff until end condition is met
-    doStuff();
+    emg1_norm = emg1_env * emg1_factor; // Normalize current EMG signal with calibrated factor
+    emg2_norm = emg2_env * emg2_factor; // Idem
+    emg3_norm = emg3_env * emg3_factor; // Idem
+
+    emg1_out_prev = emg1_out; // Set previous emg_out signal
+    emg1_dt_prev = emg1_dt; // Set previous emg_out_dt 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;
+    } else if ( emg1_norm > 1.0f ) { // If above MVC (e.g. due to filtering), emg_out = 1 (max value)
+        emg1_out = 1.0;
+    } else { // If in between threshold and MVC, scale EMG signal accordingly
+        // Inputs may be in range       [emg_th, 1]
+        // 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_dtdt = (emg1_dt - emg1_dt_prev) / Ts; // Calculate acceleration of filtered normalized output signal
+    emg1_out = emg1_out * emg1_dir; // Set direction of EMG output
+
+    // Idem for emg2
+    if ( emg2_norm < emg2_th ) {
+        emg2_out = 0.0;
+    } else if ( emg2_norm > 1.0f ) {
+        emg2_out = 1.0;
+    } 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 ) {
+        emg3_out = 0.0;
+    } else if ( emg3_norm > 1.0f ) {
+        emg3_out = 1.0;
+    } else {
+        emg3_out = rescale(emg3_norm, 0, 1, emg3_th, 1);
+    }
+
+    // Set HIDScope outputs
+    scope.set(0, emg1 );
+    scope.set(1, emg1_out );
+    scope.set(2, emg1_dt );
+    scope.set(3, emg1_dtdt );
+    //scope.set(2, emg2_out );
+    //scope.set(3, emg3_out );
+    scope.send();
+
+    led_g = !led_g;
 
     // State transition guard
     if ( false ) { // Always stay in operation mode (can be changed)
@@ -611,7 +610,6 @@
             break;
         case global_emg_cal:
             do_global_emg_cal();
-            emg_state_machine();
             break;
         case global_motor_cal:
             do_global_motor_cal();
@@ -636,7 +634,6 @@
         emg2 = emg2_in.read();
         emg3 = emg3_in.read();
 
-
         double emg1_n = bqc1_notch.step( emg1 );         // Filter notch
         double emg1_hp = bqc1_high.step( emg1_n );       // Filter highpass
         double emg1_rectify = fabs( emg1_hp );           // Rectify
@@ -677,7 +674,7 @@
 /*
 ------------------------------ MAIN FUNCTION ------------------------------
 */
-void main()
+int main()
 {
     pc.baud(115200); // MODSERIAL rate
     pc.printf("Starting\r\n");