Diff: motor_calibration.h

Revision:

16:9c5ef6fe6780

Child:

18:71a5cc9353af

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motor_calibration.h	Wed Oct 31 13:45:46 2018 +0000
@@ -0,0 +1,35 @@
+#pragma once
+
+#include "mbed.h"
+#include "motor.h"
+
+const double motor_calibration_threshold = 0.5*0.0174533;
+
+
+// Returns whether this motor is considered to be calibrated.
+bool calibrate_motor(Motor &motor, double &last_angle, int &iterations_not_moving) {
+    float current_angle = motor.get_current_angle();
+    if (current_angle - motor_calibration_threshold <= last_angle && last_angle <= current_angle + motor_calibration_threshold)     // If the motor angle is within a margin of the current motor angle
+    {
+        iterations_not_moving++;
+        //calibLED1 = !calibLED1;                              // LED turns blue
+    }
+    else
+    {
+        double motor_angle = current_angle;
+        motor_angle = motor_angle - 30 * motor_calibration_threshold;    // Subtracting five degree angle from the current angle
+        motor.set_target_angle(motor_angle); 
+        iterations_not_moving = 0;            // Set time passed in this state back to zero
+        //calibLED2 = !calibLED2;                              // LED turns red
+    }
+    
+    if (iterations_not_moving >= 10)           // After 5 seconds have passed (10 times Ticker motor_calib)
+    {
+        return true;
+        //alibLED3 = !calibLED3;                              // LED becomes red (purple if both motors are calibrated)
+    }
+
+    last_angle = current_angle;
+    
+    return false;
+}
\ No newline at end of file