Diff: main.cpp

Revision:

20:af1a6cd7469b

Parent:

19:53b9729fbab5

Child:

21:d541303a2ea6

--- a/main.cpp	Wed Oct 31 16:19:28 2018 +0000
+++ b/main.cpp	Wed Oct 31 18:22:39 2018 +0000
@@ -1,35 +1,22 @@
 #include "mbed.h"
 
+#include "constants.h"
+
 #include "Button.h"
 #include "Screen.h"
 #include "motor.h"
 #include "motor_calibration.h"
+#include "forward_kinematics.h"
 
 
 enum States {waiting, calib_motor, calib_bicep1, calib_bicep2, homing, operation, failure}; // The possible states of the state machine
 
 // Global variables
-const double PI = 3.14159265359;
-// Main loop wait time per cycle. This does not influence the motor PID or EMG reading frequencies.
-const float main_loop_wait_time = 0.01;
 
-// Time between two button polls. Used to debounce the button presses.
-const float button_poll_interval = 0.05;
-
-const float pid_period = 0.001; // PID sample period in seconds.
-
-const float Kp = 10.0;
-const float Ki = 0.1;
-const float Kd = 0.5;
 
 Motor main_motor(D6, D7, D13, D12);
 Motor sec_motor(D5, D4, D10, D11);
 
-// The motor -> main gear ratio is 25 / 60.
-// The motor -> secondary gear ratio is 25/50
-const float main_gear_ratio = 25.0/60.0;
-const float sec_gear_ratio = 25.0/50.0;
-
 
 AnalogIn potmeter1(A5); // Analoge input van potmeter 1 -> Motor 1
 AnalogIn potmeter2(A4); // Analoge input van potmeter 2 -> Motor 2
@@ -81,6 +68,13 @@
     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,16 +150,19 @@
 
 void do_state_homing()
 {
+    double main_home = PI * 0.5;
+    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");
+        
+        main_motor.set_target_angle(main_home);
+        sec_motor.set_target_angle(sec_home);
     }
     
-    main_motor.set_target_angle(PI*0.5);
-    sec_motor.set_target_angle(0);
-    
     if (ud_button.has_just_been_pressed()) {
         current_state = calib_bicep1;
     }
@@ -186,6 +183,22 @@
         screen.display_left_right_arrow(control_goes_right);
     }
     
+    // 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();
+    
     /*
     double main_target = ((potmeter1.read() * 2) - 1) * PI;
     main_motor.set_target_angle(main_target);