Diff: main.cpp

Revision:

25:a1be4cf2ab0b

Parent:

24:540c284e881d

Child:

26:7e81c7db6e7a

--- a/main.cpp	Fri Oct 25 12:02:55 2019 +0000
+++ b/main.cpp	Fri Oct 25 13:55:49 2019 +0000
@@ -29,9 +29,17 @@
 InterruptIn button3(SW3);
 
 // EMG Substates
-enum EMG_States { emg_wait, emg_cal_MVC, emg_cal_rest, emg_check_cal, emg_make_scale, emg_operation }; // Define EMG substates
+enum EMG_States { emg_wait, emg_cal_MVC, emg_cal_rest, emg_make_scale, emg_operation }; // Define EMG substates
 EMG_States emg_curr_state; // Initialize EMG substate variable
-bool emg_state_changed;
+bool emg_state_changed = true;
+
+bool sampleNow = false;
+bool calibrateNow = false;
+bool emg_MVC_cal_done = false;
+bool emg_rest_cal_done = false;
+
+bool button1_pressed = false;
+bool button2_pressed = false;
 
 // Global variables for EMG reading
 AnalogIn emg1_in (A1); // Right biceps, x axis
@@ -44,6 +52,7 @@
 double emg1_rest;
 double emg1_rest_stdev;
 vector<double> emg1_cal;
+int emg1_cal_size;
 
 double emg2;
 double emg2_MVC;
@@ -51,6 +60,7 @@
 double emg2_rest;
 double emg2_rest_stdev;
 vector<double> emg2_cal;
+int emg2_cal_size;
 
 double emg3;
 double emg3_MVC;
@@ -58,6 +68,7 @@
 double emg3_rest;
 double emg3_rest_stdev;
 vector<double> emg3_cal;
+int emg3_cal_size;
 
 // Initialize tickers and timeouts
 Ticker tickSample;
@@ -135,6 +146,18 @@
     return output;
 }
 
+// Handle button press
+void button1Press()
+{
+    button1_pressed = true;
+}
+
+// Handle button press
+void button2Press()
+{
+    button2_pressed = true;
+}
+
 // Check filter stability
 bool checkBQChainStable()
 {
@@ -152,52 +175,75 @@
 /*
 ------ TICKER FUNCTIONS ------
 */
-void tickGlobalFunc()
-{
-    
-}
-
-void sampleCalibration()
+void sampleSignal()
 {
-    // Read EMG inputs
-    emg1 = emg1_in.read();
-    emg2 = emg2_in.read();
-    emg3 = emg3_in.read();
+    if (sampleNow == true) {
+        // Read EMG inputs
+        emg1 = emg1_in.read();
+        emg2 = emg2_in.read();
+        emg3 = emg3_in.read();
+
 
-    // Output raw EMG input
-    //scope.set(0, emg1 );
-    // scope.set(1, emg2 );
+        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)
+
+        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)
 
-    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)
+        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)
 
-    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)
+        scope.set(0, emg1_n);
+        scope.set(1, emg2_n);
 
-    scope.set(0, emg1_n);
-    scope.set(1, emg2_n);
-
-    scope.set(2, emg1_env );
-    scope.set(3, emg2_env );
-    scope.send();
-
-    // IF STATEMENT TOEVOEGEN VOOR CALIBRATIE
-    emg1_cal.push_back(emg1_env); // Add values to calibration vector
-    emg2_cal.push_back(emg2_env); // Add values to calibration vector
+        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();
+            emg2_cal.push_back(emg2_env); // Add values to calibration vector
+            emg2_cal_size = emg1_cal.size();
+            emg3_cal.push_back(emg3_env); // Add values to calibration vector
+            emg3_cal_size = emg1_cal.size();
+        }
+    }
 }
 
 /*
------- EMG CALIBRATION FUNCTIONS ------
+------ EMG CALIBRATION STATES ------
 */
 
-// Finish up calibration of MVC
+/* ALL STATES HAVE THE FOLLOWING FORM:
+void do_state_function() {
+    // Entry function
+    if ( emg_state_changed == true ) {
+        emg_state_changed == false;
+        // More functions
+    }
+
+    // Do stuff until end condition is met
+    doStuff();
+
+    // State transition guard
+    if ( endCondition == true ) {
+        emg_curr_state == next_state;
+        emg_state_changed == true;
+        // More functions
+    }
+}
+*/
+
+// Finish up calibration
 void calibrationFinished()
 {
-
     switch( emg_curr_state ) {
         case emg_cal_MVC:
             emg1_MVC = getMean(emg1_cal); // Store MVC globally
@@ -205,6 +251,12 @@
 
             emg2_MVC = getMean(emg2_cal); // Store MVC globally
             emg2_MVC_stdev = getStdev(emg2_cal, emg2_MVC); // Store MVC stdev globally
+
+            emg3_MVC = getMean(emg3_cal); // Store MVC globally
+            emg3_MVC_stdev = getStdev(emg3_cal, emg3_MVC); // Store MVC stdev globally
+
+
+            emg_MVC_cal_done = true;
             break;
         case emg_cal_rest:
             emg1_rest = getMean(emg1_cal); // Store rest EMG globally
@@ -212,30 +264,67 @@
 
             emg2_rest = getMean(emg2_cal); // Store rest EMG globally
             emg2_rest_stdev = getStdev(emg2_cal, emg2_rest); // Store MVC stdev globally
+
+            emg3_rest = getMean(emg3_cal); // Store rest EMG globally
+            emg3_rest_stdev = getStdev(emg3_cal, emg3_rest); // Store MVC stdev globally
+
+
+            emg_rest_cal_done = true;
             break;
     }
     vector<double>().swap(emg1_cal); // Empty vector to prevent memory overflow
     vector<double>().swap(emg2_cal); // Empty vector to prevent memory overflow
+    vector<double>().swap(emg3_cal); // Empty vector to prevent memory overflow
+}
+
+// EMG Waiting state
+void do_emg_wait()
+{
+    // Entry function
+    if ( emg_state_changed == true ) {
+        emg_state_changed = false;
+        // pc.printf("State: emg_wait\r\n");
+    }
+
+    // Do stuff until end condition is met
+
+    // State transition guard
+    if ( button1_pressed ) {
+        button1_pressed = false;
+        emg_curr_state = emg_cal_MVC;
+        emg_state_changed = true;
+    } else if ( button2_pressed ) {
+        button2_pressed = false;
+        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_state_changed = true;
+    }
 }
 
 // Run calibration of EMG
 void do_emg_cal()
 {
     if ( emg_state_changed == true ) {
-        emg_state_changed == false;
-        pc.printf("Starting calibration");
+        emg_state_changed = false;
+        // pc.printf("Starting calibration :");
         led_b = 0; // Turn on calibration led
         timerCalibration.reset();
         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);
 
         switch( emg_curr_state ) {
             case emg_cal_MVC:
-                pc.printf("MVC Calibration");
-                tickSampleCalibration.attach( &sampleCalibration, Ts ); // Start sample ticker
+                // pc.printf("MVC\r\n");
                 break;
             case emg_cal_rest:
-                pc.printf("Rest calibration");
-                tickSampleCalibration.attach( &sampleCalibration, Ts ); // Start sample ticker
+                // pc.printf("Rest\r\n");
                 break;
         }
     }
@@ -243,15 +332,34 @@
     // Allemaal dingen doen tot de end conditions true zijn
 
     if ( timerCalibration.read() >= Tcal ) { // After interval Tcal the calibration step is finished
-        tickSampleCalibration.detach(); // Stop calibration ticker to remove interrupt
+        sampleNow = false; // Disable signal sampling in sampleSignal()
+        calibrateNow = false; // Disable calibration sampling
 
         calibrationFinished(); // Process calibration data
         led_b = 1; // Turn off calibration led
 
-        emg_curr_state == emg_wait; // Set next state
-        stateChanged == true; // State has changed (to run 
+        emg_curr_state = emg_wait; // Set next state
+        emg_state_changed = true; // State has changed (to run
+
+        // pc.printf("Calibration step finished");
+    }
+}
 
-        pc.printf("Calibration step finished");
+void do_emg_make_scale() {
+    // Entry function
+    if ( emg_state_changed == true ) {
+        emg_state_changed == false;
+        // More functions
+    }
+
+    // Do stuff until end condition is met
+    doStuff();
+
+    // State transition guard
+    if ( endCondition == true ) {
+        emg_curr_state == next_state;
+        emg_state_changed == true;
+        // More functions
     }
 }
 
@@ -262,7 +370,7 @@
     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);
+    // pc.printf("Factor: %f   TH: %f\r\n", factor1, emg1_th);
 }
 
 /*
@@ -272,7 +380,7 @@
 {
     switch(emg_curr_state) {
         case emg_wait:
-            //do_emg_wait();
+            do_emg_wait();
             break;
         case emg_cal_MVC:
             do_emg_cal();
@@ -280,11 +388,8 @@
         case emg_cal_rest:
             do_emg_cal();
             break;
-        case emg_check_cal:
-            //do_emg_check_cal();
-            break;
         case emg_make_scale:
-            //do_make_scale();
+            do_emg_make_scale();
             break;
         case emg_operation:
             //do_emg_operation();
@@ -292,6 +397,15 @@
     }
 }
 
+// Global loop of program
+void tickGlobalFunc()
+{
+    sampleSignal();
+    emg_state_machine();
+    // controller();
+    // outputToMotors();
+}
+
 void main()
 {
     pc.baud(115200); // MODSERIAL rate
@@ -317,23 +431,23 @@
     led_r = 1; // Turn red led off at startup
 
     // If any filter chain is unstable, red led will light up
-    if (checkBQChainStable) {
+    if (checkBQChainStable()) {
         led_r = 1; // LED off
     } else {
         led_r = 0; // LED on
     }
-    
-    tickGlobal.attach(
+    emg_curr_state = emg_wait;
 
-    button1.fall( &calibrationMVC ); // Run MVC calibration on button press
-    button2.fall( &calibrationRest ); // Run rest calibration on button press
-    button3.fall( &makeScale ); // Create scale factors and close calibration at button press
+    tickGlobal.attach( &tickGlobalFunc, Ts );
+
+    button1.fall( &button1Press ); // Run MVC calibration on button press
+    button2.fall( &button2Press ); // Run rest calibration on button press
 
     while(true) {
 
         // Show that system is running
         // led_g = !led_g;
-        pc.printf("Vector emg1_cal: %i      vector emg2_cal: %i\r\n", emg1_cal.size(), emg2_cal.size());
+        pc.printf("currentState: %i     vectors size: %i    %i    %i\r\n", emg_curr_state, emg1_cal_size, emg2_cal_size, emg3_cal_size);
         wait(1.0f);
     }
 }
\ No newline at end of file