File content as of revision 0:52f6905317cb:

/*#include "mbed.h"
#include "DC_Motor_Controller.h"

//DigitalIn home_button(D13);     // Button for setting the home button
DC_Motor_PID dc_motor_1(D7, D6, D10, D11, 792); //M1, M2, INA, INB, PPR
DC_Motor_PID dc_motor_2(D4, D5, D9, D8, 792); //M1, M2, INA, INB, PPR
Serial myPC(USBTX, USBRX);  // import serial for the UART attached toUSB
// DigitalOut control_1_1(D7); // control pin on L298N - 1
// DigitalOut control_1_2(D6); // control pin on L298N - 2
// DigitalOut control_2_1(D4); // control pin on L298N - 1
// DigitalOut control_2_2(D5); // control pin on L298N - 2
// PwmOut speed(D6);           // pwm signal for
DigitalOut myLED(LED1);
DigitalIn button(USER_BUTTON);
// DigitalIn valve(D9, PullDown);
DigitalIn firing1(D2);
DigitalIn firing2(D3);
// DigitalOut valve_1 = D12; // valve 1
// DigitalOut valve_2 = D13; // valve 2
void reset(DigitalIn firing_pin, DC_Motor_PID& dc_motor, int forward)
{
    if (forward)
    {
        dc_motor.set_out(1, 0);
    }
    else
    {
        dc_motor.set_out(0, 1);
    }

    myPC.printf("Enter reset, \r\n");
    while (1)
    {
        if (firing_pin == 0)
        {
            myPC.printf("Firing switch detected. Stop. Record reference point %d.\r\n", dc_motor.get_pulse());
            dc_motor.reset();
            dc_motor.set_out(0, 0);
            while (firing_pin == 0)
                ;
            return;
        }
    }
};
float pulse_to_angle(int pulses)
{
    return pulses * 360 / 2 / 792;
}
float angle_to_pulse(int angle)
{
    return angle / 360 * 2 * 792;
}
int main()
{
    // DC_Motor_PID dc_motor_1(D7, D6, D10, D11, 792); //M1, M2, INA, INB, PPR
    // DC_Motor_PID dc_motor_2(D4, D5, D9, D8, 792);   //M1, M2, INA, INB, PPR
    dc_motor_1.reset();
    dc_motor_2.reset();
    // initialize pwm
    // speed.period(0.01);
    // speed = 0.9f;
    // initialize control pins on L298N
    dc_motor_1.reset();
    dc_motor_2.reset();
    myPC.printf("Initialization Finished \n\r");
    myPC.printf("Firing1 is %d, Firing2 is %d, button is %d. \n\r", firing1.read(), firing2.read(), button.read()); // valve omitted
    // open catcher
    // valve_1 = 1;
    // valve_2 = 0; // tentative
    // myPC.printf("Catcher opened. valves are %d%d. \n\r", valve_1, valve_2);

    // Variables
    int ref_pick_1 = 0;
    int ref_pick_2 = 0;
    int angle_long_1 = 0;
    int angle_short_1 = 0; // angles used in phase 1
    int angle_long_2 = 0;
    int angle_short_2 = 0; // angles used in phase 2
    int stage = 0;         // stages for picking arrow
    // Press the user button to start initialization
    while (1)
    {
        if (button == 1)
        {
            while (button == 1)
                ;
            break;
        }
    }

    myPC.printf("=========== Start configuring the long motor ==========\r\n");
    // myPC.printf("motor1 starts to rotate until firing pin is reached.\r\n");
    reset(firing1, dc_motor_1, 1);
    myPC.printf("Motor 1 reset finished. Press button to reset Motor 2.\r\n");
    // if (firing1 == 1)
    // {
    //     myPC.printf("Firing1 switch detected. Stop. Record reference point_1  %d.\r\n", dc_motor_1.get_pulse());
    //     control_1_1 = 0;
    //     control_1_2 = 0;
    //     ref_pick_1 = dc_motor_1.get_pulse(); // stop -> record or reverse????
    //     while (firing1 == 1)
    //         ;
    // }
    while (button == 0)
        ; // idle
    while (button == 1)
        ; // button pressed
    myPC.printf("=========== Start configuring the short motor ==========\r\n");
    // myPC.printf("motor1 starts to rotate until firing pin is reached.\r\n");
    reset(firing2, dc_motor_2, 1);

    // if (firing2 == 1)
    // {
    //     myPC.printf("firing2 switch detected. Stop. Record reference point_2  %d.\r\n", dc_motor_2.get_pulse());
    //     control_2_1 = 0;
    //     control_2_2 = 0;
    //     ref_pick_2 = dc_motor_2.get_pulse(); // stop -> record or reverse????
    //     while (firing2 == 1)
    //         ;
    // }

    myPC.printf("=========== Initialization finished. Press user button to pick up arrows ==========\r\n");

    while (1)
    {
        if (button == 1)
        {
            myPC.printf("Start picking up arrows\r\n");
            while (button == 1)
                ;
            if (stage == 0)
            {
                myPC.printf("Begin stage 1\r\n");
                myPC.printf("Rotating long one for % degree. \r\n", angle_long_1);
                myPC.printf("Current angle is %.4f, target angle is %.4f. \r\n", pulse_to_angle(dc_motor_1.get_pulse()), pulse_to_angle(dc_motor_1.get_pulse()) + angle_long_1);
                dc_motor_1.move_angle(angle_long_1);
                myPC.printf("Rotating short one for % degree. \r\n", angle_short_1);
                myPC.printf("Current angle is %.4f, target angle is %.4f. \r\n", pulse_to_angle(dc_motor_2.get_pulse()), pulse_to_angle(dc_motor_2.get_pulse()) + angle_short_1);
                dc_motor_2.move_angle(angle_short_1);
                stage++;
                myPC.printf("Finished stage 1\r\n");
            }
            else if (stage == 1)
            {
                myPC.printf("Begin stage 2\r\n");
                myPC.printf("Rotating long one for % degree. \r\n", angle_long_2);
                myPC.printf("Current angle is %.4f, target angle is %.4f. \r\n", pulse_to_angle(dc_motor_1.get_pulse()), pulse_to_angle(dc_motor_1.get_pulse()) + angle_long_2);
                dc_motor_1.move_angle(angle_long_2);
                myPC.printf("Rotating short one for % degree. \r\n", angle_short_1);
                myPC.printf("Current angle is %.4f, target angle is %.4f. \r\n", pulse_to_angle(dc_motor_2.get_pulse()), pulse_to_angle(dc_motor_2.get_pulse()) + angle_short_2);
                dc_motor_2.move_angle(angle_short_2);
                stage++;
                myPC.printf("Finished stage 2\r\n");
            }
        }

        // another button for controlling the valve
        // if(valve == 0)
        // {

        //     valve_1 = 1 - valve_1;
        //     valve_2 = 1 - valve_2;
        //     myPC.printf("Change catcher state. %d%d.\r\n", valve_1, valve_2);
        //     while (valve)
        //         ;
        // }
        // if finished one process, reset
        if (stage > 1)
        {
            myPC.printf("=========== Start resetting the long motor ==========\r\n");
            reset(firing1, dc_motor_1, 0); // forward = 0 -> rotate backward
            myPC.printf("Motor 1 reset finished. Press button to reset Motor 2.\r\n");
            myPC.printf("=========== Start resetting the short motor ==========\r\n");
            reset(firing2, dc_motor_2, 0);
            myPC.printf("=========== Resetting finished. Press user button to pick up arrows ==========\r\n");
            stage = 0;
        }
    }
}*/