Diff: main.cpp

Revision:

2:141cfcafe72b

Parent:

1:cfa5abca6d43

Child:

3:4b19b6cf6cc7

--- a/main.cpp	Tue Oct 30 11:01:00 2018 +0000
+++ b/main.cpp	Tue Oct 30 11:21:09 2018 +0000
@@ -12,58 +12,122 @@
 
 
 States current_state;   // Defining the state we are currently in
+States last_state;      // To detect state changes.
 Ticker loop_ticker;     // Ticker for the loop function
 
 // Order of buttons: up_down, left_right, panic
 // D2, D3, D8
 Button ud_button(PTA4);
-Button lr_button(PTC6);
-Button p_button(D8);
+Button lr_button(D3);
+Button p_button(PTC6);
 
 DigitalOut led_red(LED_RED);
 DigitalOut led_green(LED_GREEN);
 
+
+void do_state_waiting() {
+    if (ud_button.is_pressed()) {
+        current_state = calib_motor;
+    }
+    
+    led_green = 1;
+}
+
+void do_state_calib_motor() {
+    if(last_state != current_state) {
+        last_state = current_state;
+        // State just changed to this one.
+        
+        led_green = 0;
+    }
+}
+
+void do_state_calib_bicep1() {
+    if(last_state != current_state) {
+        last_state = current_state;
+        // State just changed to this one.
+    }
+}
+
+void do_state_calib_bicep2() {
+    if(last_state != current_state) {
+        last_state = current_state;
+        // State just changed to this one.
+    }
+}
+
+void do_state_homing() {
+    if(last_state != current_state) {
+        last_state = current_state;
+        // State just changed to this one.
+    }
+}
+
+void do_state_operation() {
+    if(last_state != current_state) {
+        last_state = current_state;
+        // State just changed to this one.
+    }
+}
+
+void do_state_failure() {
+    if(last_state != current_state) {
+        last_state = current_state;
+        // State just changed.
+        // Update the display.
+        led_red = 0;
+        led_green = 1;
+    }
+    
+    // Stop the motors!
+}
+
+
 void main_loop()
 {
     ud_button.update();
     lr_button.update();
     p_button.update();
     
-
-    led_red = !ud_button.has_just_been_pressed();
-    
-    led_green = lr_button.is_pressed();
-    
     switch (current_state) {
         case waiting:
-            //do_state_waiting():
+            do_state_waiting();
             break;
         case calib_motor:
-            //do_state_calib_motor():
+            do_state_calib_motor();
             break;
         case calib_bicep1:
-            //do_state_calib_bicep1():
+            do_state_calib_bicep1();
             break;
         case calib_bicep2:
-            //do_state_calib_bicep2():
+            do_state_calib_bicep2();
             break;
         case homing:
-            //do_state_homing():
+            do_state_homing();
             break;
         case operation:
-            //do_state_operation():
+            do_state_operation();
             break;
         case failure:
-            //do_state_failure():
+            do_state_failure();
             break;
     }
     
+    // Check if the panic button was pressed.
+    // Doesn't matter in which state we are, we need to go to failure.
+    if (p_button.is_pressed()) {
+        current_state = failure;
+    }    
 }
 
 int main()
 {
     led_red = 1;
+    
+    // Start in the waiting state.
     current_state = waiting;
+    last_state = waiting;
+    
     while (true) {
         main_loop();
         wait(main_loop_wait_time);