Diff: main.cpp

Revision:

22:9079c6c0d898

Parent:

21:e4569b47945e

Child:

23:8a0a0b959af1

--- a/main.cpp	Fri Oct 25 09:57:50 2019 +0000
+++ b/main.cpp	Fri Oct 25 10:11:29 2019 +0000
@@ -31,6 +31,7 @@
 // EMG Substates
 enum EMG_States { emg_wait, emg_cal_MVC, emg_cal_rest, emg_check_cal, emg_make_scale, emg_operation }; // Define EMG substates
 EMG_States emg_curr_state; // Initialize EMG substate variable
+bool emg_state_changed;
 
 // Global variables for EMG reading
 AnalogIn emg1_in (A1); // Right biceps, x axis
@@ -61,6 +62,7 @@
 // Initialize tickers and timeouts
 Ticker tickSample;
 Ticker tickSampleCalibration;
+Timer timerCalibration;
 Timeout timeoutCalibrationMVC;
 Timeout timeoutCalibrationRest;
 
@@ -216,17 +218,29 @@
 */
 
 // Finish up calibration of MVC
-void calibrationMVCFinished()
+void calibrationFinished()
 {
-    tickSampleCalibration.detach(); // Stop calibration ticker to remove interrupt
+
+    switch( emg_curr_state ) {
+        case emg_cal_MVC:
+            emg1_MVC = getMean(emg1_cal); // Store MVC globally
+            emg1_MVC_stdev = getStdev(emg1_cal, emg1_MVC); // Store MVC stde globally
+            vector<double>().swap(emg1_cal); // Empty vector to prevent memory overflow
 
-    emg1_MVC = getMean(emg1_cal); // Store MVC globally
-    emg1_MVC_stdev = getStdev(emg1_cal, emg1_MVC); // Store MVC stde globally
-    vector<double>().swap(emg1_cal);
+            emg2_MVC = getMean(emg2_cal); // Store MVC globally
+            emg2_MVC_stdev = getStdev(emg2_cal, emg2_MVC); // Store MVC stde globally
+            vector<double>().swap(emg2_cal); // Empty vector to prevent memory overflow
+            break;
+        case emg_cal_rest:
+            emg1_MVC = getMean(emg1_cal); // Store MVC globally
+            emg1_MVC_stdev = getStdev(emg1_cal, emg1_MVC); // Store MVC stde globally
+            vector<double>().swap(emg1_cal); // Empty vector to prevent memory overflow
 
-    emg2_MVC = getMean(emg2_cal); // Store MVC globally
-    emg2_MVC_stdev = getStdev(emg2_cal, emg2_MVC); // Store MVC stde globally
-    vector<double>().swap(emg2_cal);
+            emg2_MVC = getMean(emg2_cal); // Store MVC globally
+            emg2_MVC_stdev = getStdev(emg2_cal, emg2_MVC); // Store MVC stde globally
+            vector<double>().swap(emg2_cal); // Empty vector to prevent memory overflow
+            break;
+    }
 
     led_b = 1; // Turn off calibration led
 }
@@ -249,10 +263,13 @@
 // Run calibration of EMG
 void do_emg_cal()
 {
-    if (stateChanged == true) {
-        stateChanged == false;
+    if ( emg_state_changed == true ) {
+        emg_state_changed == false;
         led_b = 0; // Turn on calibration led
-        switch(emg_curr_state) {
+        timerCalibration.reset();
+        timerCalibration.start();
+
+        switch( emg_curr_state ) {
             case emg_cal_MVC:
                 timeoutCalibrationMVC.attach( &calibrationMVCFinished, Tcal); // Stop MVC calibration after interval
                 tickSampleCalibration.attach( &sampleCalibration, Ts ); // Start sample ticker
@@ -265,100 +282,102 @@
 
         // Allemaal dingen doen tot de end conditions true zijn
 
-        if (timer klaar) {
+        if ( timerCalibration.read() >= Tcal ) {
+            tickSampleCalibration.detach(); // Stop calibration ticker to remove interrupt
+
+            calibrationFinished();
+
             emg_curr_state == emg_wait;
             stateChanged == true;
+
             pc.printf("Calibration step finished");
+        }
     }
-}
 
-/*
-// Run calibration in rest
-void calibrationRest()
-{
-    timeoutCalibrationRest.attach( &calibrationRestFinished, Tcal); // Stop rest calibration after interval
-    tickSampleCalibration.attach( &sampleCalibration, Ts ); // Start sample ticker
-    led_b = 0; // Turn on calibration led
-}
-*/
+    /*
+    // Run calibration in rest
+    void calibrationRest()
+    {
+        timeoutCalibrationRest.attach( &calibrationRestFinished, Tcal); // Stop rest calibration after interval
+        tickSampleCalibration.attach( &sampleCalibration, Ts ); // Start sample ticker
+        led_b = 0; // Turn on calibration led
+    }
+    */
 
 // 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
+    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);
-}
+        pc.printf("Factor: %f   TH: %f\r\n", factor1, emg1_th);
+    }
 
-/*
------- EMG SUBSTATE MACHINE ------
-*/
-void emg_state_machine()
-{
-    switch(emg_curr_state) {
-        case emg_wait:
-            //do_emg_wait();
-            break;
-        case emg_cal_MVC:
-            do_emg_cal();
-            break;
-        case emg_cal_rest:
-            do_emg_cal();
-            break;
-        case emg_check_cal:
-            //do_emg_check_cal();
-            break;
-        case emg_make_scale:
-            //do_make_scale();
-            break;
-        case emg_operation:
-            //do_emg_operation();
-            break;
-    }
-}
-
-void main()
-{
-    pc.baud(115200); // MODSERIAL rate
-    pc.printf("Starting\r\n");
-
-    // tickSample.attach(&sample, Ts); // Initialize sample ticker
-
-    // Create BQ chains to reduce computations
-    bqc1_notch.add( &bq1_notch );
-    bqc1_high.add( &bq1_H1 ).add( &bq1_H2 );
-    bqc1_low.add( &bq1_L1 ).add( &bq1_L2 );
-
-    bqc2_notch.add( &bq2_notch );
-    bqc2_high.add( &bq2_H1 ).add( &bq2_H2 );
-    bqc2_low.add( &bq2_L1 ).add( &bq2_L2 );
-
-    bqc3_notch.add( &bq3_notch );
-    bqc3_high.add( &bq3_H1 ).add( &bq3_H2 );
-    bqc3_low.add( &bq3_L1 ).add( &bq3_L2 );
-
-    led_b = 1; // Turn blue led off at startup
-    led_g = 1; // Turn green led off at startup
-    led_r = 1; // Turn red led off at startup
-
-    // If any filter chain is unstable, red led will light up
-    if (checkBQChainStable) {
-        led_r = 1; // LED off
-    } else {
-        led_r = 0; // LED on
+    /*
+    ------ EMG SUBSTATE MACHINE ------
+    */
+    void emg_state_machine() {
+        switch(emg_curr_state) {
+            case emg_wait:
+                //do_emg_wait();
+                break;
+            case emg_cal_MVC:
+                do_emg_cal();
+                break;
+            case emg_cal_rest:
+                do_emg_cal();
+                break;
+            case emg_check_cal:
+                //do_emg_check_cal();
+                break;
+            case emg_make_scale:
+                //do_make_scale();
+                break;
+            case emg_operation:
+                //do_emg_operation();
+                break;
+        }
     }
 
-    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
+    void main() {
+        pc.baud(115200); // MODSERIAL rate
+        pc.printf("Starting\r\n");
+
+        // tickSample.attach(&sample, Ts); // Initialize sample ticker
 
-    while(true) {
+        // Create BQ chains to reduce computations
+        bqc1_notch.add( &bq1_notch );
+        bqc1_high.add( &bq1_H1 ).add( &bq1_H2 );
+        bqc1_low.add( &bq1_L1 ).add( &bq1_L2 );
+
+        bqc2_notch.add( &bq2_notch );
+        bqc2_high.add( &bq2_H1 ).add( &bq2_H2 );
+        bqc2_low.add( &bq2_L1 ).add( &bq2_L2 );
+
+        bqc3_notch.add( &bq3_notch );
+        bqc3_high.add( &bq3_H1 ).add( &bq3_H2 );
+        bqc3_low.add( &bq3_L1 ).add( &bq3_L2 );
 
-        // 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());
-        wait(1.0f);
-    }
-}
\ No newline at end of file
+        led_b = 1; // Turn blue led off at startup
+        led_g = 1; // Turn green led off at startup
+        led_r = 1; // Turn red led off at startup
+
+        // If any filter chain is unstable, red led will light up
+        if (checkBQChainStable) {
+            led_r = 1; // LED off
+        } else {
+            led_r = 0; // LED on
+        }
+
+        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
+
+        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());
+            wait(1.0f);
+        }
+    }
\ No newline at end of file