Diff: main.cpp

Revision:

32:b63b5837bcb1

Parent:

30:a45bbfa6bd22

Child:

33:543debddb3a9

--- a/main.cpp	Thu Nov 01 13:29:49 2018 +0000
+++ b/main.cpp	Thu Nov 01 14:11:30 2018 +0000
@@ -8,9 +8,10 @@
 #include "motor_calibration.h"
 #include "forward_kinematics.h"
 #include "inverse_kinematics.h"
+#include "end_effector_control.h"
 
 
-enum States {waiting, calib_motor, calib_bicep1, calib_bicep2, homing, operation, failure}; // The possible states of the state machine
+enum States {waiting, calib_motor, calib_bicep1, calib_bicep2, homing, operation, demo, failure}; // The possible states of the state machine
 
 // Global variables
 
@@ -69,13 +70,6 @@
     if (ud_button.has_just_been_pressed()) {
         current_state = calib_motor;
     }
-    
-    // TODO:
-    // THIS OPTION IS ONLY HERE FOR DEBUGGING PURPOSES.
-    // REMOVE WHEN THE DEMO STATE IS IMPLEMENTED.
-    if (lr_button.has_just_been_pressed()) {
-        current_state = operation;
-    }
 }
 
 void do_state_calib_motor()
@@ -156,6 +150,9 @@
     if (ud_button.has_just_been_pressed()) {
         current_state = calib_bicep1;
     }
+    if (lr_button.has_just_been_pressed()) {
+        current_state = demo;
+    }
 }
 
 void do_state_calib_bicep1()
@@ -264,6 +261,62 @@
     }
 }
 
+void do_state_demo() {
+    if(last_state != current_state) {
+        last_state = current_state;
+        // State just changed.
+        // Update the display.
+        led_red = 1;
+        led_green = 0;
+        led_blue = 1;
+        screen.clear_display();
+        screen.display_state_name("Demo mode!");
+        
+        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);
+    }
+    
+    if (ud_button.has_just_been_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();
+        
+        double main_target, sec_target;
+        
+        end_effector_control(speed_x, speed_y, main_cur_angle, sec_cur_angle, main_target, sec_target);
+        
+        main_motor.set_target_angle(main_target);
+        sec_motor.set_target_angle(sec_target);
+        
+        screen.get_screen_handle()->setTextCursor(0, 0);
+        screen.get_screen_handle()->printf("M_a: %.6f    \n", main_cur_angle);
+        screen.get_screen_handle()->printf("S_a: %.6f    \n", sec_cur_angle);
+        screen.get_screen_handle()->printf("Vx:  %.6f \n", main_target);
+        screen.get_screen_handle()->printf("Vy:  %.6f ", sec_target);
+        screen.display();
+    }
+}
+
 void do_state_failure()
 {
     if(last_state != current_state) {
@@ -308,6 +361,9 @@
         case operation:
             do_state_operation();
             break;
+        case demo:
+            do_state_demo();
+            break;
         case failure:
             do_state_failure();
             break;