File content as of revision 1:e9e4edd823e5:

#include "mbed.h"
#include "AX12.h"
#include <math.h>
Serial device(D1, D0);
DigitalOut led1(LED2);

AX12 ax12(PA_9,PA_10,0x01,1000000);
DigitalOut TxEn (D4);

void Rx_interrupt();
void SetSerial(int c);

int state_of_ST1 = 0;
int state_of_ST2 = 0;
int state_of_ST3 = 0;

float u_j1=0;
float u_j2=0;
float u_j3=0;

void drive_motor_1();
void drive_motor_2();
void drive_motor_3();
void calculation();

//GREEN-------------------------------------------------------------
//motor set 1
DigitalOut ENA_1(PB_14);
DigitalOut DIR_1(PC_4);
DigitalOut PUL_1(PB_13);

//motor set 2
DigitalOut ENA_2(PB_2);
DigitalOut DIR_2(PB_15);
DigitalOut PUL_2(PB_1);

//motor set 3
DigitalOut ENA_3(PC_8);
DigitalOut DIR_3(PC_5);
DigitalOut PUL_3(PC_6);

//Buzzer
DigitalOut Buzzer(PA_12);

//Vacum
DigitalOut VACUM(PB_12);

//Servo

//Limit Switch
DigitalIn LSwitch_1(PC_0);
DigitalIn LSwitch_2(PC_1);
DigitalIn LSwitch_3(PB_0);
DigitalIn Home_Switch(PA_1);

// SET VELOCITY
float VelocityST_1 = 30; // rpm
float VelocityST_2 = 100; // rpm
float VelocityST_3 = 100; // rpm

void open_vacum(){
    VACUM = 1;
}
void close_vacum(){
    VACUM = 0;
}
void open_buzzer(){
    Buzzer = 1;
}
void close_buzzer(){
    Buzzer = 0;
}
void set_home(){
    if (LSwitch_2.read() != 0){
        state_of_ST2=2;
        u_j2=180;          
        drive_motor_2();
    }
    if (LSwitch_1.read() != 0){
        state_of_ST1 =2;    
        u_j1=180;
        drive_motor_1(); 
        state_of_ST1 =1;    
        u_j1=45;
        drive_motor_1(); 
    }
    if (LSwitch_3.read() != 0){
        state_of_ST3=2;
        u_j3=180;
        drive_motor_3(); 
        state_of_ST3=1;
        u_j3=70;
        drive_motor_3();      
    }
    ax12.SetGoal(90, 1);
}
//GREEN-------------------------------------------------------------

int data_size = 16;
char data[16] = {};
char package = 0;
char num_data = 0;

float q[4] = {};
void ConvertData2q()
{
    char q_mode[4] = {data[3], data[6], data[9], data[12]};
    char q_int[4] = {data[4], data[7], data[10], data[13]};
    char q_dec[4] = {data[5], data[8], data[11], data[14]};
    for(int i=0;i<4;i++)
    {
        if(q_mode[i] == 1)
        {
            q[i] = q_int[i]+q_dec[i]/100;
        }
        else if(q_mode[i] == 2)
        {
            q[i] = (-1)*(q_int[i]+q_dec[i]/100);
        }
    }
}

int main()
{
    device.baud(115200);
    device.attach(&Rx_interrupt);
    close_buzzer();
    close_vacum();
    //set_home();
    while(1)
    {
        if (package == 1)
        {
            package = 0;
            if(data[2]==1){
                open_buzzer();
                open_vacum();
                set_home();
                data[2]=0;
                }
            if(data[2]==2){
                ConvertData2q();
                state_of_ST1=data[3];
                u_j1=abs(q[0]);
                state_of_ST2=data[6];
                u_j2=abs(q[1]);
                state_of_ST3=data[9];
                u_j3=abs(q[2]);
                drive_motor_1();
                drive_motor_2();
                drive_motor_3();
                TxEn = 1;
                ax12.SetCRSpeed(0.1);
                ax12.SetGoal(abs(q[3]), 1);
                if(abs(q[3])==180){
                    open_vacum();
                }
                if(abs(q[3])==90){
                    wait(3);
                    close_vacum();
                }
        }
    }
    }
}

void Rx_interrupt()
{
    char c = device.getc();
    int i = (int)c;
    SetSerial(i);
}
void SetSerial(int c)
{
    if (num_data < 2){
        if (c == 255){
            data[num_data] = c;
            num_data++;
        }else{
            data[num_data] = c;
            num_data = 0;
        }
    }
    else if (num_data < data_size){
        data[num_data] = c;
        num_data++;
        if (num_data >= data_size){
            if (data[data_size-1]==255){
                num_data = 0;
                package = 1;
            }
            else num_data = 0;
        }
    }
}
void drive_motor_1(){
    float round_1 = u_j1 * 8000/360;//1:10 rpm x step pluse u_j1 default 60000
    float pluseforST_1 =(60/(VelocityST_1*800))/2;
    if (state_of_ST1==1){
            ENA_1 = 1;
            DIR_1 = 0;
            for (int i=0; i< round_1; i++){
                PUL_1 = 1;
                wait(pluseforST_1);//default 0.0005
                PUL_1 = 0;
                wait(pluseforST_1);//default 0.0005
            }
            //state_of_ST1 = 0;
        }
        if (state_of_ST1==2){
            ENA_1 = 1;
            DIR_1 = 1;
            for (int i=0; i< round_1; i++){
                PUL_1 = 1;
                wait(pluseforST_1);//default 0.0005
                PUL_1 = 0;
                wait(pluseforST_1);//default 0.0005 
                if (LSwitch_1.read() == 0)
                {
                    break;
                }
            }
        }
    }
void drive_motor_2(){
    float round_2 = u_j2 * 40000/360;// 1:20rpm x step pluse u_j1 default 60000
    float pluseforST_2 =(60/(VelocityST_2*800))/2;
    if (state_of_ST2==1){
            ENA_2 = 1;
            DIR_2 = 1;
            for (int i=0; i< round_2; i++){
                PUL_2 = 1;
                wait(pluseforST_2);//default 0.0005
                PUL_2 = 0;
                wait(pluseforST_2);//default 0.0005
            }
            //state_of_ST1 = 0;

        }
        if (state_of_ST2==2){
            ENA_2 = 1;
            DIR_2 = 0;
            for (int i=0; i< round_2; i++){
                PUL_2 = 1;
                wait(pluseforST_2);//default 0.0005
                PUL_2 = 0;
                wait(pluseforST_2);//default 0.0005 
                if (LSwitch_2.read() == 0)
                {
                    break;
                }
            }
        }
        }

void drive_motor_3(){
    float round_3 = u_j3 * 16000 /360;//1:40 rpm x step pluse u_j1 default 60000
    float pluseforST_3 =(60/(VelocityST_3*800))/2;
    if (state_of_ST3==1){
            ENA_3 = 1;
            DIR_3 = 0;
            for (int i=0; i< round_3; i++){
                PUL_3 = 1;
                wait(pluseforST_3);//default 0.0005
                PUL_3 = 0;
                wait(pluseforST_3);//default 0.0005
            }
            //state_of_ST1 = 0;
        }
        if (state_of_ST3==2){
            ENA_3 = 1;
            DIR_3 = 1;
            for (int i=0; i< round_3; i++){
                PUL_3 = 1;
                wait(pluseforST_3);//default 0.0005
                PUL_3 = 0;
                wait(pluseforST_3);//default 0.0005 
                if (LSwitch_3.read() == 0)
                {
                    break;
                }
            }
        }
        }