Diff: main.cpp

Revision:

26:7e81c7db6e7a

Parent:

25:a1be4cf2ab0b

Child:

27:f18da01093c9

--- a/main.cpp	Fri Oct 25 13:55:49 2019 +0000
+++ b/main.cpp	Fri Oct 25 14:32:35 2019 +0000
@@ -29,7 +29,7 @@
 InterruptIn button3(SW3);
 
 // EMG Substates
-enum EMG_States { emg_wait, emg_cal_MVC, emg_cal_rest, emg_make_scale, emg_operation }; // Define EMG substates
+enum EMG_States { emg_wait, emg_cal_MVC, emg_cal_rest, emg_operation }; // Define EMG substates
 EMG_States emg_curr_state; // Initialize EMG substate variable
 bool emg_state_changed = true;
 
@@ -47,26 +47,38 @@
 AnalogIn emg3_in (A3); // Third muscle, TBD
 
 double emg1;
+double emg1_env;
 double emg1_MVC;
 double emg1_MVC_stdev;
 double emg1_rest;
 double emg1_rest_stdev;
+double emg1_factor;
+double emg1_th;
+double emg1_out;
 vector<double> emg1_cal;
 int emg1_cal_size;
 
 double emg2;
+double emg2_env;
 double emg2_MVC;
 double emg2_MVC_stdev;
 double emg2_rest;
 double emg2_rest_stdev;
+double emg2_factor;
+double emg2_th;
+double emg2_out;
 vector<double> emg2_cal;
 int emg2_cal_size;
 
 double emg3;
+double emg3_env;
 double emg3_MVC;
 double emg3_MVC_stdev;
 double emg3_rest;
 double emg3_rest_stdev;
+double emg3_factor;
+double emg3_th;
+double emg3_out;
 vector<double> emg3_cal;
 int emg3_cal_size;
 
@@ -146,6 +158,13 @@
     return output;
 }
 
+// Rescale values to certain range
+double rescale(double input, double out_min, double out_max, double in_min, double in_max)
+{
+    double output = out_min + ((input-in_min)/(in_max-in_min))*(out_max-out_min); // Based on MATLAB rescale function
+    return output;
+}
+
 // Handle button press
 void button1Press()
 {
@@ -187,17 +206,17 @@
         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
-        double emg1_env = bqc1_low.step( emg1_rectify ); // Filter lowpass (completes envelope)
+        emg1_env = bqc1_low.step( emg1_rectify ); // Filter lowpass (completes envelope)
 
         double emg2_n = bqc2_notch.step( emg2 );         // Filter notch
         double emg2_hp = bqc2_high.step( emg2_n );       // Filter highpass
         double emg2_rectify = fabs( emg2_hp );           // Rectify
-        double emg2_env = bqc2_low.step( emg2_rectify ); // Filter lowpass (completes envelope)
+        emg2_env = bqc2_low.step( emg2_rectify ); // Filter lowpass (completes envelope)
 
         double emg3_n = bqc3_notch.step( emg3 );         // Filter notch
         double emg3_hp = bqc3_high.step( emg3_n );       // Filter highpass
         double emg3_rectify = fabs( emg3_hp );           // Rectify
-        double emg3_env = bqc3_low.step( emg3_rectify ); // Filter lowpass (completes envelope)
+        emg3_env = bqc3_low.step( emg3_rectify ); // Filter lowpass (completes envelope)
 
         scope.set(0, emg1_n);
         scope.set(1, emg2_n);
@@ -205,7 +224,7 @@
         scope.set(2, emg1_env );
         scope.set(3, emg2_env );
         scope.send();
-        
+
         if (calibrateNow) {
             emg1_cal.push_back(emg1_env); // Add values to calibration vector
             emg1_cal_size = emg1_cal.size();
@@ -298,7 +317,7 @@
         emg_curr_state = emg_cal_rest;
         emg_state_changed = true;
     } else if ( emg_MVC_cal_done && emg_rest_cal_done ) {
-        emg_curr_state = emg_make_scale;
+        emg_curr_state = emg_operation;
         emg_state_changed = true;
     }
 }
@@ -314,7 +333,7 @@
         timerCalibration.start();
         sampleNow = true; // Enable signal sampling in sampleSignal()
         calibrateNow = true;
-        
+
         emg1_cal.reserve(Fs * Tcal);
         emg2_cal.reserve(Fs * Tcal);
         emg3_cal.reserve(Fs * Tcal);
@@ -345,34 +364,59 @@
     }
 }
 
-void do_emg_make_scale() {
+void do_emg_operation()
+{
     // Entry function
     if ( emg_state_changed == true ) {
-        emg_state_changed == false;
-        // More functions
+        emg_state_changed = false;
+        double margin_percentage = 10; // 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
+        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
     }
 
     // Do stuff until end condition is met
-    doStuff();
+    
+    double emg1_norm = emg1_env * emg1_factor;
+    double emg2_norm = emg2_env * emg2_factor;
+    double emg3_norm = emg3_env * emg3_factor;
+    
+    if ( emg1_norm < emg1_th ) {
+        emg1_out = 0.0;
+    } else if ( emg1_norm > 1.0f ) {
+        emg1_out = 1.0;
+    } else {
+        emg1_out = rescale(emg1_norm, emg1_th, 1, 0, 1);
+    }
+    
+    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, emg2_th, 1, 0, 1);
+    }
+    
+    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, emg3_th, 1, 0, 1);
+    }
 
     // State transition guard
-    if ( endCondition == true ) {
-        emg_curr_state == next_state;
-        emg_state_changed == true;
+    if ( false ) {
+        emg_curr_state = emg_wait;
+        emg_state_changed = true;
         // More functions
     }
 }
 
-// Determine scale factors for operation mode
-void makeScale()
-{
-    double margin_percentage = 10; // Set up % margin for rest
-    double factor1 = 1 / emg1_MVC; // Factor to normalize MVC
-    double emg1_th = emg1_rest * factor1 + margin_percentage/100; // Set normalized rest threshold
-
-    // pc.printf("Factor: %f   TH: %f\r\n", factor1, emg1_th);
-}
-
 /*
 ------ EMG SUBSTATE MACHINE ------
 */
@@ -388,11 +432,8 @@
         case emg_cal_rest:
             do_emg_cal();
             break;
-        case emg_make_scale:
-            do_emg_make_scale();
-            break;
         case emg_operation:
-            //do_emg_operation();
+            do_emg_operation();
             break;
     }
 }