Diff: main.cpp

Revision:

54:a14acf0d1683

Parent:

53:35282a3e0cad

Child:

55:a4e63ef1587c

--- a/main.cpp	Thu Oct 31 21:11:16 2019 +0000
+++ b/main.cpp	Thu Oct 31 21:17:07 2019 +0000
@@ -557,7 +557,27 @@
     }
 }
 
+/*
+------------------------------ 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_operation:
+            do_emg_operation();
+            break;
+    }
+}
 
 /*
 ------------------------------ MOTOR GLOBAL VARIABLES & CONSTANTS ------------------------------
@@ -781,16 +801,6 @@
     motor2Direction = motordir2;
 }
 
-void changeservo()
-{
-    servo_button1= extButton1.read();
-    if (servo_button1 == 1) {
-        myservo.SetPosition(2000);
-    } else {
-        myservo.SetPosition(1000);
-    }
-}
-
 void motorKillPower()
 {
     motor1Power.write(0.0f);
@@ -845,26 +855,20 @@
 }
 
 /*
------------------------------- OPERATION GLOBAL VARIABLES & CONSTANTS ------------------------------
+------------------------------ MOTOR SUBSTATE MACHINE ------------------------------
 */
-
-/*
------------------------------- OPERATION GLOBAL FUNCTIONS ------------------------------
-*/
-void toggleServo()
+void motor_state_machine()
 {
-    if ( operation_showcard == true ) {
-        myservo.SetPosition(2000);
-        operation_showcard = !operation_showcard;
-    }
-
-    else {
-        myservo.SetPosition(1000);
-        operation_showcard = !operation_showcard;
+    switch(motor_curr_state) {
+        case motor_encoder_set:
+            do_motor_encoder_set();
+            break;
+        case motor_finish:
+            do_motor_finish();
+            break;
     }
 }
 
-
 /*
 ------------------------------ OPERATION SUBSTATE FUNCTIONS ------------------------------
 */
@@ -889,11 +893,6 @@
 
     motorKillPower(); // Disables motor outputs
 
-    if ( switch2_pressed == true) {
-        switch2_pressed = false;
-        toggleServo();
-    }
-
     // State transition guard
     if ( button1_pressed == true ) {
         button1_pressed = false;
@@ -928,11 +927,6 @@
     motorControl();
     RKI();
 
-    if ( switch2_pressed == true) {
-        switch2_pressed = false;
-        toggleServo();
-    }
-
     // State transition guard
     if ( button1_pressed == true ) {
         button1_pressed = false;
@@ -947,44 +941,6 @@
 }
 
 /*
------------------------------- 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_operation:
-            do_emg_operation();
-            break;
-    }
-}
-
-/*
------------------------------- MOTOR SUBSTATE MACHINE ------------------------------
-*/
-
-void motor_state_machine()
-{
-    switch(motor_curr_state) {
-        case motor_encoder_set:
-            do_motor_encoder_set();
-            break;
-        case motor_finish:
-            do_motor_finish();
-            break;
-    }
-}
-
-/*
 ------------------------------ OPERATION SUBSTATE MACHINE ------------------------------
 */
 
@@ -1302,7 +1258,7 @@
 }
 
 /*
------------------------------- READ SAMPLES ------------------------------
+------------------------------ OTHER MAIN LOOP FUNCTIONS ------------------------------
 */
 void sampleSignals()
 {
@@ -1350,6 +1306,16 @@
     countsPrev2 = counts2af;
 }
 
+void changeservo()
+{
+    servo_button1 = extButton1.read();
+    if (servo_button1 == 1) {
+        myservo.SetPosition(2000);
+    } else {
+        myservo.SetPosition(1000);
+    }
+}
+
 /*
 ------------------------------ GLOBAL PROGRAM LOOP ------------------------------
 */
@@ -1413,7 +1379,6 @@
         //pc.printf("q1: %f q2: %f \r\n",q1*rad2deg,q2*rad2deg);
         //pc.printf("Motor1ref: %f Motor1angle: %f\r\n",motor1_ref*rad2deg/5.5f,motor1_angle*rad2deg/5.5f);
         //pc.printf("Motor2ref: %f Motor2angle: %f\r\n",motor2_ref*rad2deg/2.75f,motor2_angle*rad2deg/2.75f);
-
         //pc.printf("Xe: %f Ye: %f\r\n",xe,ye);
         wait(0.5f);
     }