File content as of revision 0:a60467f3655b:

/***************************************************************************
SERIAL INTERRUPT PROGRAM
This program attaches to the serial communication link so that when a
character is typed on the keyboard the program generates an interrupt and
jumps to the interrupt service routine. In this program the User LED
normally blinks ON and OFF continuously. Every time the letter "a" is
pressed on the keyboard the LED is ON and stops blinking for 4 seconds.
Save the file and click Compile button on the menu bar. The program will now be compiled
In reverse, when the letter "b" is pressed the LED is OFF and the blinking
stops for 4 seconds. After the interrupt finishes the LED blinking
continuous.
**************************************************************************/
#include "mbed.h"
#include "QEI.h"

//DigitalIn home_button(D13);     // Button for setting the home button
QEI dc_motor(D6, D7, NC, 792); // Create QEI object for increment encoder

Serial myPC(USBTX, USBRX); // import serial for the UART attached toUSB
DigitalOut control_1(D4);  // control pin on L298N - 1
DigitalOut control_2(D5);  // control pin on L298N - 2
//PwmOut speed(D6);          // pwm signal for
//DigitalOut myLED(LED1);
DigitalIn button(USER_BUTTON);
DigitalIn firing(D8);
void go_angle(int angle){      //  Move motor to specific angle from home point
    int tar_pulse = ((double) angle / 360.0)* (2.0 * 792.0);    //  Calculating target pulse number
    int cur_pulse = dc_motor.getPulses();
    
    while ( tar_pulse < cur_pulse - 100 || tar_pulse > cur_pulse + 100 ){
        
        if (tar_pulse > cur_pulse){     //  Rotate motor clockwise
            control_1 = 0;
            control_2 = 1;
        }
        else {                          //  Rotate motor counter clockwrise 
            control_1 = 1;
            control_2 = 0;
        }
        cur_pulse = dc_motor.getPulses();   //  Get increment encoder current pulse number
    }
    control_1 = 0;       //  Stop motor
    control_2 = 0;
};
void go_pulse(int diff){      //  Move motor to specific angle from home point
//    int tar_pulse = ((double) angle / 360.0)* (2.0 * 792.0);    //  Calculating target pulse number
    int cur_pulse = dc_motor.getPulses();
    int tar_pulse = cur_pulse + diff;
//    control_1 = 0;       //  Stop motor
//    control_2 = 0;
//    wait(0.3);
    while ( abs(tar_pulse - cur_pulse) > 100){
    
        if (tar_pulse > cur_pulse){     //  Rotate motor clockwise
            control_1 = 0;
            control_2 = 1;
        }
        else {                          //  Rotate motor counter clockwrise 
            control_1 = 1;
            control_2 = 0;
        }
        cur_pulse = dc_motor.getPulses();   //  Get increment encoder current pulse number
        myPC.printf("tar phase is %d, cur phase is %d\r\n",tar_pulse,cur_pulse);
    }
    control_1 = 0;       //  Stop motor
    control_2 = 0;
};
void reset(DigitalIn firing_pin, QEI &dc_motor, DigitalOut out1, DigitalOut out2)
{
    out1 = 1;
    out2 = 0;
    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.getPulses());
            dc_motor.reset();
            out1 = 0; out2 = 0;
            while(firing_pin == 0);
            return;
        }   
    }
};
int main()
{
    dc_motor.reset();
    // initialize pwm
//    speed.period(0.01);
//    speed = 0.9f;
    // initialize control pins on L298N
    control_1 = 0;
    control_2 = 0;
    myPC.printf("Initialization Finished \n\r");
    myPC.printf("Firing is %d, button is %d. \n\r", firing.read(), button.read());
    //    myPC.printf("control 1 is %d, control 2 is %d.\n\r", control_1, control_2);
    int cur_pulse = dc_motor.getPulses();

    // Variables
    int ref_pulse = 0;
    int load_per_pulse = 3000; // no. of pulses for loading one arrow
    // int load_per_angle = 0;
    int big_big_pulse = 10000; // after loading 5 arrows, rotate this to reset.
    int num_arrows = 2;    // number of arrows on-robot
    int cnt = 0;           // for counting how many
    // Press the user button to start initialization
    while (1)
    {
        if (button == 1)
        {
            myPC.printf("Step 1: motor starts to rotate until firing pin is reached.\r\n");
            control_1 = 1;
            control_2 = 0;
            while (button == 1)
                ;
            break;
        }
        
    }
    reset(firing, dc_motor, control_1,control_2);

    while (1)
    {   
        if (button == 1)
        {            
            if(cnt < num_arrows)
            {
                myPC.printf("Loading No. %d arrow.\r\n", ++cnt);
                myPC.printf("Current phase is %d, target phase is %d. \r\n", dc_motor.getPulses(), dc_motor.getPulses() - load_per_pulse);
                go_pulse(-load_per_pulse);
                myPC.printf("After rotating, current phase is %d. \r\n", dc_motor.getPulses());
                while (button == 1)
                    ;
            }
            else
            {
                myPC.printf("All arrows have been loaded. Reset \r\n");
                reset(firing, dc_motor, control_1,control_2);
                cnt = 0;
            }
        }
    }
}