Diff: main.cpp

Revision:

16:9c5ef6fe6780

Parent:

15:f65b4566193e

Child:

19:53b9729fbab5

diff -r f65b4566193e -r 9c5ef6fe6780 main.cpp
--- a/main.cpp	Wed Oct 31 11:05:47 2018 +0000
+++ b/main.cpp	Wed Oct 31 13:45:46 2018 +0000
@@ -3,6 +3,7 @@
 #include "Button.h"
 #include "Screen.h"
 #include "motor.h"
+#include "motor_calibration.h"
 
 
 enum States {waiting, calib_motor, calib_bicep1, calib_bicep2, homing, operation, failure}; // The possible states of the state machine
@@ -84,6 +85,13 @@
 
 void do_state_calib_motor()
 {
+    static double main_last_angle;
+    static double sec_last_angle;
+    static int main_iterations_not_moving;
+    static int sec_iterations_not_moving;
+    static bool main_is_calibrated;
+    static bool sec_is_calibrated;
+      
     if(last_state != current_state) {
         last_state = current_state;
         // State just changed to this one.
@@ -91,9 +99,29 @@
         led_green = 0;
         screen.clear_display();
         screen.display_state_name("Motor calibration");
+        
+        main_last_angle = -10;
+        sec_last_angle = -10;
+        main_iterations_not_moving = 0;
+        sec_iterations_not_moving = 0;
+        main_is_calibrated = false;
+        sec_is_calibrated = false;
     }
     
-    if (ud_button.has_just_been_pressed()) {
+    if (!main_is_calibrated) {
+        main_is_calibrated = calibrate_motor(main_motor, main_last_angle, main_iterations_not_moving);
+        if (main_is_calibrated) {
+            main_motor.define_current_angle_as_x_radians(0.785398); // 45 degrees.
+        }
+    }
+    if (!sec_is_calibrated) {
+        sec_is_calibrated = calibrate_motor(sec_motor, sec_last_angle, sec_iterations_not_moving);
+        if (sec_is_calibrated) {
+            sec_motor.define_current_angle_as_x_radians(-0.733038); // -42 degrees.
+        }
+    }
+    
+    if (main_is_calibrated && sec_is_calibrated) {
         current_state = calib_bicep1;
     }
 }