File content as of revision 19:53b9729fbab5:

#pragma once

#include "mbed.h"
#include "motor.h"

const double motor_rad_per_angle = 0.0174533;
const double motor_calibration_threshold = 0.1*motor_rad_per_angle;


// 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 - 3 * motor_rad_per_angle;    // 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 >= 40)           // 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;
}