File content as of revision 6:656fb0834a1a:

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

Motor motor1(D6, D7, D13, D12);             // Defining motor pins (PWM, direction, encoder)
Motor motor2(D5, D4, D10, D11);             // Defining motor pins (PWM, direction, encoder)
DigitalOut calibLED1(LED_BLUE);
DigitalOut calibLED2(LED_RED); 
DigitalOut calibLED3(LED_GREEN);

const float pid_period = 0.001;             // PID sample period in seconds.
const double Kp = 10.0;                     // PID proportional
const double Ki = 0.1;                      // PID integral
const double Kd = 0.5;                      // PID differential
const double motor_threshold = 0.5*0.0174533;   // One degree

Ticker motor_calib1;                        // Ticker for motor1 calibration
Ticker motor_calib2;                        // Ticker for motor2 calibration

float motor_angle1 = 2;                     // Set motor angle to arbitrary value for first loop
float motor_angle2 = 2;
float time_passed_in_this_state1 = 0;       // Time passed in the final state of motor 1
float time_passed_in_this_state2 = 0;       // Time passed in the final state of motor 2
float last_angle1 = -2;                     // Last angle
float last_angle2 = -2;

void Motor1_Calibration()
{   
    float current_angle1 = motor1.get_current_angle();
    if (current_angle1 - motor_threshold <= last_angle1 && last_angle1 <= current_angle1 + motor_threshold)     // If the motor angle is within a margin of the current motor angle
    {
        time_passed_in_this_state1++;
        calibLED1 = !calibLED1;                              // LED turns blue
    }
    else
    {
        motor_angle1 = current_angle1;
        motor_angle1 = motor_angle1 - 30*motor_threshold;    // Subtracting five degree angle from the current angle
        motor1.set_target_angle(motor_angle1); 
        time_passed_in_this_state1 = 0;            // Set time passed in this state back to zero
        calibLED2 = !calibLED2;                              // LED turns red
    }
    
    if (time_passed_in_this_state1 >= 10)           // After 5 seconds have passed (10 times Ticker motor_calib)
    {
        motor1.define_current_angle_as_x_radians(0.785398); // Defines beginstate motor 2 as -42 degrees
        motor_calib1.detach();                      // Stop looping the function
        calibLED3 = !calibLED3;                              // LED becomes red (purple if both motors are calibrated)
    }

    last_angle1 = current_angle1;
}

void Motor2_Calibration()
{   
    float current_angle2 = motor2.get_current_angle();
    if (current_angle2 - motor_threshold <= last_angle2 && last_angle2 <= current_angle2 + motor_threshold)     // If the motor angle is within a margin of the current motor angle
    {
        time_passed_in_this_state2++;
        calibLED1 = !calibLED1;                              // LED becomes blue
    }
    else
    {
        motor_angle2 = motor2.get_current_angle();
        motor_angle2 = motor_angle2 - 5*motor_threshold;    // Subtracting five degree angle from the current angle
        motor2.set_target_angle(motor_angle2); 
        time_passed_in_this_state2 = 0;            // Set time passed in this state back to zero
        calibLED2 = !calibLED2;
    }
    
    if (time_passed_in_this_state2 >= 10)           // After 5 seconds have passed (10 times Ticker motor_calib)
    {
        motor2.define_current_angle_as_x_radians(-0.733038); // Defines beginstate motor 2 as -42 degrees
        motor_calib2.detach();                      // Stop looping the function
        calibLED3 = 0;                              // LED becomes red (purple if both motors are calibrated)
        wait(0.1f);
        calibLED3 = !calibLED3;
    }
    
    last_angle2 = current_angle2;
}

int main()
{
    calibLED1 = 1;
    calibLED2 = 1;
    calibLED3 = 1;
    motor1.set_pid_k_values(Kp, Ki, Kd);    // Attach PID-controller values
    motor2.set_pid_k_values(Kp, Ki, Kd);
    motor1.start(pid_period);               // Attach PID sample time
    motor2.start(pid_period);
    motor_calib1.attach(Motor1_Calibration,0.2);    // Ticker for motor calibration fucntion
    motor_calib2.attach(Motor2_Calibration,0.2);
    while (true) {}                         // Empty while loop to keep function from stopping
}