Diff: main.cpp

Revision:

57:d0d65376140f

Parent:

56:3f29502fcb55

Child:

58:8d8a98ec2904

--- a/main.cpp	Thu Oct 31 23:24:53 2019 +0000
+++ b/main.cpp	Fri Nov 01 00:05:58 2019 +0000
@@ -60,7 +60,7 @@
 */
 Ticker tickGlobal; // Set global ticker
 Ticker tickLED; // LED ticker
-Timer timerCalibration; // Set EMG Calibration timer
+Timer timerStateChange; // Set timer for various state changes
 
 /*
 ------------------------------ INITIALIZE GLOBAL VARIABLES ------------------------------
@@ -100,6 +100,7 @@
 enum Demo_States { demo_wait, demo_motor_cal, demo_XY }; // Define demo substates
 Demo_States demo_curr_state; // Initialize demo substate variable
 bool demo_state_changed = true; // Enable entry functions
+bool exit_demo = false;
 
 // Button press interrupts (to prevent bounce)
 bool button1_pressed = false;
@@ -470,8 +471,8 @@
         emg_state_changed = false; // Disable entry functions
         set_led_color('b'); // Turn on calibration led (BLUE)
 
-        timerCalibration.reset();
-        timerCalibration.start(); // Sets up timer to stop calibration after Tcal seconds
+        timerStateChange.reset();
+        timerStateChange.start(); // Sets up timer to stop calibration after Tcal seconds
         emg_sampleNow = true; // Enable signal sampling in sampleSignals()
         emg_calibrateNow = true; // Enable calibration vector functionality in sampleSignals()
 
@@ -492,7 +493,7 @@
     scope.send();
 
     // State transition guard
-    if ( timerCalibration.read() >= Tcal ) { // After interval Tcal the calibration step is finished
+    if ( timerStateChange.read() >= Tcal ) { // After interval Tcal the calibration step is finished
         emg_sampleNow = false; // Disable signal sampling in sampleSignals()
         emg_calibrateNow = false; // Disable calibration sampling
         set_led_color('p'); // Change calibration LED (BLUE) to EMG wait mode led (PURPLE)
@@ -838,6 +839,8 @@
     // Entry function
     if ( motor_state_changed == true ) {
         motor_state_changed = false;
+        timerStateChange.reset();
+        timerStateChange.start();
     }
 
     // Do stuff until end condition is true
@@ -846,7 +849,7 @@
     RKI();
 
     // State transition guard
-    if ( button2_pressed ) {
+    if ( timerStateChange.read() >= 3.0f ) { // After 3 seconds move to next state
         button2_pressed = false;
         motor_cal_done = true;
         motor_curr_state = motor_encoder_set;
@@ -974,13 +977,12 @@
         demo_curr_state = demo_XY;
         demo_state_changed = true;
         // More functions
-    } else if (button2_pressed == true) { // Return to global wait
-        button2_pressed = false;
+    } else if ( switch2_pressed == true ) { // Return to global wait
+        switch2_pressed = false;
         demo_curr_state = demo_wait;
         demo_state_changed = true;
         motor_cal_done = false; // Disables motor calibration again (robot is probably not in reference position)
-        global_curr_state = global_wait;
-        global_state_changed = true;
+        exit_demo = true;
     }
 }
 
@@ -989,6 +991,7 @@
     // Entry function
     if ( demo_state_changed == true ) {
         demo_state_changed = false;
+        set_led_color('c'); // Set LED to motor calibration (CYAN)
     }
 
     // Do stuff until end condition is met
@@ -998,6 +1001,7 @@
     if ( motor_cal_done == true ) { // demo_wait
         demo_curr_state = demo_wait;
         demo_state_changed = true;
+        set_led_color('y'); // Set LED to demo mode (YELLOW)
     }
 }
 
@@ -1103,6 +1107,7 @@
     if ( global_state_changed == true ) {
         global_state_changed = false;
         set_led_color('y'); // Set demo mode LED (YELLOW)
+        emg_cal_done = false; // Disables EMG calibration to prevent going into operation mode after exiting demo state
 
         if ( motor_cal_done == true ) {
             demo_curr_state = demo_wait;
@@ -1115,11 +1120,9 @@
     // Do stuff until end condition is met
     demo_state_machine();
 
-    
-
     // State transition guard
-    if ( switch2_pressed == true ) {
-        switch2_pressed = false;
+    if ( exit_demo == true ) {
+        exit_demo = false;
         global_curr_state = global_wait;
         global_state_changed = true;
         set_led_color('o'); // Disable demo mode LED