Diff: main.cpp

Revision:

25:cc81f2120eda

Parent:

24:e1092f95c82b

Child:

26:a8f4a117cc0d

--- a/main.cpp	Thu Nov 01 08:40:10 2018 +0000
+++ b/main.cpp	Thu Nov 01 09:10:21 2018 +0000
@@ -121,6 +121,31 @@
     }
 }
 
+void do_state_homing()
+{
+    const double home_x = 0.6524; // Meters.
+    const double home_y = 0.3409;
+    
+    double main_home = PI *2;
+    double sec_home = 0;
+    
+    if(last_state != current_state) {
+        last_state = current_state;
+        // State just changed to this one.
+        screen.clear_display();
+        screen.display_state_name("Homing");
+        
+        //inverse_kinematics(home_x, home_y, main_home, sec_home);
+        
+        main_motor.set_target_angle(main_home);
+        sec_motor.set_target_angle(sec_home);
+    }
+    
+    if (ud_button.has_just_been_pressed()) {
+        current_state = calib_bicep1;
+    }
+}
+
 void do_state_calib_bicep1()
 {
     if(last_state != current_state) {
@@ -149,31 +174,6 @@
     }
 }
 
-void do_state_homing()
-{
-    const double home_x = 0.6524; // Meters.
-    const double home_y = 0.3409;
-    
-    double main_home = PI *2;
-    double sec_home = 0;
-    
-    if(last_state != current_state) {
-        last_state = current_state;
-        // State just changed to this one.
-        screen.clear_display();
-        screen.display_state_name("Homing");
-        
-        //inverse_kinematics(home_x, home_y, main_home, sec_home);
-        
-        main_motor.set_target_angle(main_home);
-        sec_motor.set_target_angle(sec_home);
-    }
-    
-    if (ud_button.has_just_been_pressed()) {
-        current_state = calib_bicep1;
-    }
-}
-
 void do_state_operation()
 {
     static bool debug_forward_kinematics;
@@ -238,15 +238,18 @@
     double sec_target = ((potmeter2.read() * 2) - 1) * PI;
     sec_motor.set_target_angle(sec_target);
     
+    if (lr_button.has_just_been_pressed()) {
+    control_goes_right = !control_goes_right;
+    screen.display_left_right_arrow(control_goes_right);
+    }
+    */
+    
     if (ud_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);
-    }
-    if (lr_button.has_just_been_pressed()) {
-        control_goes_right = !control_goes_right;
         screen.display_left_right_arrow(control_goes_right);
     }
-    */
 }
 
 void do_state_failure()