Diff: main.cpp

Revision:

43:1bd5417ded64

Parent:

42:2937ad8f1032

Child:

44:342af9b3c1a0

--- a/main.cpp	Thu Oct 31 11:04:45 2019 +0000
+++ b/main.cpp	Thu Oct 31 11:45:03 2019 +0000
@@ -88,6 +88,11 @@
 bool operation_state_changed = true; // Enable entry functions
 bool operation_showcard = false; // Internal flag to toggle servo position
 
+// Demo Substate variables
+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
+
 // Button press interrupts (to prevent bounce)
 bool button1_pressed = false;
 bool button2_pressed = false;
@@ -762,17 +767,6 @@
 /*
 ------------------------------ OPERATION GLOBAL FUNCTIONS ------------------------------
 */
-void operationFuncBundle() {
-    EMGOperationFunc();
-    
-    Vx = emg1_out * 15.0f * emg1_dir;
-    Vy = emg2_out * 15.0f * emg2_dir;
-    
-    PID_controller();
-    motorControl();
-    RKI();
-}
-
 void toggleServo()
 {
     if ( operation_showcard == true ) {
@@ -800,7 +794,15 @@
     }
 
     // Do stuff until end condition is met
-    operationFuncBundle(); // Reads all signals and calculates references
+    EMGOperationFunc();
+    
+    Vx = emg1_out * 15.0f * emg1_dir;
+    Vy = emg2_out * 15.0f * emg2_dir;
+    
+    PID_controller();
+    motorControl();
+    RKI();
+    
     motorKillPower(); // Disables motor outputs
     
     if ( switch2_pressed == true) {
@@ -826,7 +828,14 @@
     }
 
     // Do stuff until end condition is met
-    operationFuncBundle();
+    EMGOperationFunc();
+    
+    Vx = emg1_out * 15.0f * emg1_dir;
+    Vy = emg2_out * 15.0f * emg2_dir;
+    
+    PID_controller();
+    motorControl();
+    RKI();
     
     if ( switch2_pressed == true) {
         switch2_pressed = false;
@@ -851,7 +860,14 @@
     }
 
     // Do stuff until end condition is met
-    operationFuncBundle();
+    EMGOperationFunc();
+    
+    Vx = emg1_out * 15.0f * emg1_dir;
+    Vy = emg2_out * 15.0f * emg2_dir;
+    
+    PID_controller();
+    motorControl();
+    RKI();
     
     // Function to move to starting position
 
@@ -927,6 +943,91 @@
 }
 
 /*
+------------------------------ DEMO SUBSTATE FUNCTIONS ------------------------------
+*/
+void do_demo_wait() {
+    // Entry function
+    if ( demo_state_changed == true ) {
+        demo_state_changed = false;
+    }
+
+    // Do nothing until end condition is met
+    
+    // State transition guard
+    if ( button1_pressed == true ) {
+        button1_pressed = false;
+        demo_curr_state = demo_XY;
+        demo_state_changed = true;
+        // More functions
+    } else if (button2_pressed == true) {
+        button2_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;
+    }
+}
+
+void do_demo_motor_cal() {
+    // Entry function
+    if ( demo_state_changed == true ) {
+        demo_state_changed = false;
+    }
+
+    // Do stuff until end condition is met
+    motor_state_machine();
+
+    // State transition guard
+    if ( motor_cal_done == true ) { // WAIT MODE
+        demo_curr_state = demo_wait;
+        demo_state_changed = true;
+    }
+}
+
+void do_demo_XY() {
+    // Entry function
+    if ( demo_state_changed == true ) {
+        demo_state_changed = false;
+    }
+
+    // Do stuff until end condition is met
+    static float t = 0;
+    Vy = 10.0f * sin(1.0f*t);
+    Vx = 0.0f;
+    t += Ts;
+    
+    PID_controller();
+    motorControl();
+    RKI();
+
+    // State transition guard
+    if ( motor_cal_done == true ) { // WAIT MODE
+        demo_curr_state = demo_wait;
+        demo_state_changed = true;
+    }
+}
+
+/*
+------------------------------ DEMO SUBSTATE MACHINE ------------------------------
+*/
+
+void demo_state_machine()
+{
+    switch(demo_curr_state) {
+        case demo_wait:
+            do_demo_wait();
+            break;
+        case demo_motor_cal:
+            do_demo_motor_cal();
+            break;
+        case demo_XY:
+            do_demo_XY();
+            break;
+    }
+}
+
+/*
 ------------------------------ GLOBAL STATE FUNCTIONS ------------------------------
 */
 /* ALL STATES HAVE THE FOLLOWING FORM:
@@ -1069,22 +1170,7 @@
     }
 
     // Do stuff until end condition is met
-
-
-    /*
-    // For testing Vx and Vy
-    static float t=0;
-    Vy=10.0f*sin(1.0f*t);
-    Vx=0.0f;
-    t+=Ts;
-    */
-
-    // Move motors
-    PID_controller();
-    motorControl();
-    RKI();
-
-
+    operation_state_machine();
 
     // Set HIDScope outputs
     scope.set(0, emg1 );