Diff: main.cpp

Revision:

34:ae62ebf4d494

Parent:

33:543debddb3a9

Child:

39:f119ca6fc821

--- a/main.cpp	Thu Nov 01 14:39:55 2018 +0000
+++ b/main.cpp	Thu Nov 01 14:55:47 2018 +0000
@@ -13,7 +13,10 @@
 
 enum States {waiting, calib_motor, calib_bicep1, calib_bicep2, homing, operation, demo, failure}; // The possible states of the state machine
 
-// Global variables
+
+
+// Controll directions for the demo controller.
+enum DebugControlDirection {debug_up, debug_down, debug_left, debug_right};
 
 
 Motor main_motor(D6, D7, D13, D12);
@@ -267,6 +270,11 @@
 }
 
 void do_state_demo() {
+    static DebugControlDirection control_direction;
+    static const double max_speed = 0.01;
+    static double speed_x;
+    static double speed_y;
+    
     if(last_state != current_state) {
         last_state = current_state;
         // State just changed.
@@ -277,31 +285,44 @@
         screen.clear_display();
         screen.display_state_name("Demo mode!");
         
+        control_direction = debug_up;
+        
+        speed_x = 0;
+        speed_y = 0;
+        
         screen.display_up_down_arrow(control_goes_up);
         screen.display_left_right_arrow(control_goes_right);
     }
     
     if (lr_button.has_just_been_pressed()) {
-        control_goes_up = !control_goes_up;
-        control_goes_right = !control_goes_right;
-        screen.display_up_down_arrow(control_goes_up);
-        screen.display_left_right_arrow(control_goes_right);
+        switch (control_direction) {
+            case debug_up:
+                control_direction = debug_right;
+                speed_x = max_speed;
+                speed_y = 0;
+                break;
+            case debug_right:
+                control_direction = debug_down;
+                speed_x = 0;
+                speed_y = -max_speed;
+                break;
+            case debug_down:
+                control_direction = debug_left;
+                speed_x = -max_speed;
+                speed_y = 0;
+                break;
+            case debug_left:
+                control_direction = debug_up;
+                speed_x = 0;
+                speed_y = max_speed;
+                break;
+        }
     }
     
-    if (ud_button.has_just_been_pressed()) {
+    if (ud_button.is_pressed()) {
         
         led_blue = 0;
-        
-        double speed_x = 0.01;
-        double speed_y = 0.01;
-        
-        if (!control_goes_right) {
-            speed_x = -speed_x;    
-        }
-        if (!control_goes_up) {
-            speed_y = -speed_y;
-        }
-        
+               
         
         double main_cur_angle = main_motor.get_current_angle();
         double sec_cur_angle = sec_motor.get_current_angle();