Diff: main.cpp

Revision:

21:d541303a2ea6

Parent:

20:af1a6cd7469b

Child:

23:fb681b074a92

Child:

35:38a5af0afee8

--- a/main.cpp	Wed Oct 31 18:22:39 2018 +0000
+++ b/main.cpp	Wed Oct 31 18:54:07 2018 +0000
@@ -7,6 +7,7 @@
 #include "motor.h"
 #include "motor_calibration.h"
 #include "forward_kinematics.h"
+#include "inverse_kinematics.h"
 
 
 enum States {waiting, calib_motor, calib_bicep1, calib_bicep2, homing, operation, failure}; // The possible states of the state machine
@@ -170,6 +171,8 @@
 
 void do_state_operation()
 {
+    static bool debug_forward_kinematics;
+        
     if(last_state != current_state) {
         last_state = current_state;
         // State just changed to this one.
@@ -181,23 +184,48 @@
         
         screen.display_up_down_arrow(control_goes_up);
         screen.display_left_right_arrow(control_goes_right);
+        
+        debug_forward_kinematics = true;
     }
     
-    // Using potmeters for debugging purposes;
-    double main_angle = ((potmeter1.read() * 2) - 1) * PI;
-    double sec_angle = ((potmeter2.read() * 2) - 1) * PI;
-    
-    double e_x = 0.0;
-    double e_y = 0.0;
+    if (debug_forward_kinematics) {
+        // Using potmeters for debugging purposes;
+        double main_angle = ((potmeter1.read() * 2) - 1) * PI;
+        double sec_angle = ((potmeter2.read() * 2) - 1) * PI;
+        
+        double e_x = 0.0;
+        double e_y = 0.0;
+        
+        forward_kinematics(main_angle, sec_angle, e_x, e_y);
+        
+        screen.get_screen_handle()->setTextCursor(0, 0);
+        screen.get_screen_handle()->printf("M_a: %.6f    \n", main_angle);
+        screen.get_screen_handle()->printf("S_a: %.6f    \n", sec_angle);
+        screen.get_screen_handle()->printf("X:   %.6f    \n", e_x);
+        screen.get_screen_handle()->printf("Y:   %.6f    ", e_y);
+        screen.display();
     
-    forward_kinematics(main_angle, sec_angle, e_x, e_y);
+    } else {
+        // Using potmeters for debugging purposes;
+        double e_x = potmeter1.read();
+        double e_y = potmeter2.read();
+        
+        double main_angle = 0.0;
+        double sec_angle = 0.0;
+        
+        inverse_kinematics(e_x, e_y, main_angle, sec_angle);
+        
+        screen.get_screen_handle()->setTextCursor(0, 0);
+        screen.get_screen_handle()->printf("E_x: %.6f    \n", e_x);
+        screen.get_screen_handle()->printf("E_y: %.6f    \n", e_y);
+        screen.get_screen_handle()->printf("M_a: %.6f      \n", main_angle);
+        screen.get_screen_handle()->printf("S_a: %.6f      ", sec_angle);
+        screen.display();
+    }
     
-    screen.get_screen_handle()->setTextCursor(0, 0);
-    screen.get_screen_handle()->printf("M_a: %.6f\n", main_angle);
-    screen.get_screen_handle()->printf("S_a: %.6f\n", sec_angle);
-    screen.get_screen_handle()->printf("X:   %.6f\n", e_x);
-    screen.get_screen_handle()->printf("Y:   %.6f", e_y);
-    screen.display();
+    if (lr_button.has_just_been_pressed()) {
+        debug_forward_kinematics = !debug_forward_kinematics;
+    }
     
     /*
     double main_target = ((potmeter1.read() * 2) - 1) * PI;