File content as of revision 0:52f6905317cb:

#include "mbed.h"
#include "DC_Motor_Controller.h"

DC_Motor_PID dc_motor2(D10, D12, PC_8, PC_9, 792); //M1, M2, INA, INB, PPR; picker // D11, D10 see works?  original: D4, D7 for encoder, D12, D13 for out

Serial myPC(USBTX, USBRX);  // import serial for the UART attached toUSB

DigitalOut myLED(LED1);
DigitalIn button(USER_BUTTON);
DigitalIn firing3 = D7; // Pick, short
DigitalIn firing4 = D4; // Pick, long
DigitalIn firing5 = D8;
DigitalOut faul2_1 = A1;
DigitalOut faul2_2 = A2;
 DigitalOut valve_1 = D0; // valve 1
 DigitalOut valve_2 = D1; // valve 2

int main()
{
    firing3.mode(PullUp); // default 0
    firing4.mode(PullUp); // default 0
    firing5.mode(PullUp); // default 0
    int stage_pick = 0;
    dc_motor2.reset();
    dc_motor2.set_pid(0.008, 0, 0);

    myPC.printf("Initialization Finished \n\r");
    myPC.printf("Firing3 is %d, Firing4 is %d, button is %d. \n\r", firing3.read(), firing4.read(), button.read()); // valve omitted
    while(1)
    {
        if(button == 1)
        {
            if(stage_pick == 0)
            {
                myPC.printf("dc_motor reset\r\n");
                dc_motor2.set_out(0.5, 0);
                while(firing3 == 0);
                if(firing3 == 1)
                {
                    dc_motor2.set_out(0,0);
                    myPC.printf("short: firing3 pin detected\n\r");
                }
                stage_pick++;
                valve_1 = 1; valve_2 = 0;
            }
            else if(stage_pick == 1)
            {
                myPC.printf("faulhaber 2 RefPos\n\r");
                faul2_1 = 1;
                while(firing4 == 0);
                faul2_1 = 0;  
                myPC.printf("long: firing4 detected \n\r"); //內
                stage_pick++;              
            }
            else if(stage_pick == 2)
            {
                myPC.printf("dc_motor rotate to pick up\n\r");
                dc_motor2.set_out(0,0.5);
                wait(0.3);
                dc_motor2.set_out(0,0);
                stage_pick++;
                
            }
            else if(stage_pick == 3)
            {
                myPC.printf("faul2 to pick the arrows \n\r"); // 外
                faul2_2 = 1;
                while(firing5 == 0);
                wait(0.3);
                faul2_2 = 0;
                myPC.printf("Firing5 touched \n\r"); 
                
                stage_pick++; // use firing 5
            }
            else if(stage_pick == 4)
            {                
                myPC.printf("faul2 rotate to vertical \n\r"); // 內一啲
                faul2_1 = 1;
                wait(2.6);
                faul2_1 = 0;
                stage_pick++;
            }
            else if(stage_pick == 5)
            {
                myPC.printf("dc_motor rotate to final pos\n\r");
                dc_motor2.set_out(0,1);
                wait(0.4);
                dc_motor2.set_out(0,0);
                stage_pick++;
            }
            else if(stage_pick == 6)
            {
                myPC.printf("faul2 rotate to final pos \n\r"); // 內一啲
                faul2_1 = 1;
                wait(1.9);
                faul2_1 = 0;
                stage_pick = 0;
            }
            while(button == 1);
            wait(0.3);
        }
        
    }
    
        
}