File content as of revision 114:325e4c158141:

/***RS485 Communication.***/
#define RS485_TX p13
#define RS485_RX p14
Serial RS485(RS485_TX, RS485_RX);
#define EnablePin p7
DigitalOut Enable(EnablePin);
inline void initializeRS485(){
    RS485.format(8,Serial::Even);
}

/*
inline void sendData(int address, int data5) {
    unsigned int data;
    if (data5 <= 31) {
        data = ((address << 5) | data5);
        RS485.putc(data);
    }
}
*/

inline void sendData(int address, unsigned int data8) {
    unsigned int data;
    if (data8 <= 15) {
        data = 128 | (address << 4) | (data8 & 15);
        RS485.putc(data);
    }
    else if (data8 <= 255 && data8 >= 16) {
        data = (address << 4) | (data8 >> 4);
        RS485.putc(data);
        wait(0.000001);
        data = 128 | (address << 4) | (data8 & 15);
        RS485.putc(data);
    }
}


#ifdef IM920
/***IM920 Communication.***/
char rcvBUFF_IM920[40], rcvBUFF_SBDBT[40];
int rcvCOUNT_IM920 = 0, rcvCOUNT_SBDBT = 0;
int rcvFLAG = 0;
int sendFLAG = 0;
int deadCOUNT = 0;
int deadflag = 0;
int oldDATA;
unsigned int cm, a2, b, X, Y;
unsigned int sendDATA = 0;
unsigned long outDATA[4];

Serial IM920_USART(p9, p10);
Serial SBDBT_USART(p28, p27);
DigitalIn BUSY(p8);

Ticker DTimer;

void DEVICE_TX(int DATA, int DEVICE = 0) {
    char sDATA[2];
    if (DATA <= 15)
        sprintf(sDATA, "0%X", DATA);
    else
        sprintf(sDATA, "%X", DATA);
        
    if (DEVICE == 1) {
        IM920_USART.printf("TXDA %s\r\n", sDATA);
    }
    else if (DEVICE == 2) {
        SBDBT_USART.printf("00000000000%s\r\n", sDATA);
    }
    else {
        IM920_USART.printf("TXDA %s\r\n", sDATA);
        SBDBT_USART.printf("00000000000%s\r\n", sDATA);
    }
}

int cvsendDATA(int Buzzer, int LED) {
/*
    if (Buzzer > 1)
        Buzzer = 1;
    else if (Buzzer < 0)
        Buzzer = 0;
    if (LED < 0)
        LED = 0;
    else if (LED >= 16)
        LED = 15;
*/
    Buzzer = 0;
    LED = 0;
    return (Buzzer << 6) | (LED << 2);
}

void cvrecvDATA(char *buffDATA, unsigned long *outputDATA) {
   char s1[2], s2[2], s3[2], s4[2];
   strncpy(s1, buffDATA+11, 2);
   strncpy(s2, buffDATA+14, 2);
   strncpy(s3, buffDATA+17, 2);
   strncpy(s4, buffDATA+20, 2);
   outputDATA[0] = strtoul(s1, (char **) NULL, 16);
   outputDATA[1] = strtoul(s2, (char **) NULL, 16);
   outputDATA[2] = strtoul(s3, (char **) NULL, 16);
   outputDATA[3] = strtoul(s4, (char **) NULL, 16);
}

void cvrecvDATAsbdbt(char *buffDATA, unsigned long *outputDATA) {
   char s1[2], s2[2];
   strncpy(s1, buffDATA+5, 2);
   strncpy(s2, buffDATA+7, 2);
   outputDATA[0] = strtoul(s1, (char **) NULL, 16);
   outputDATA[1] = strtoul(s2, (char **) NULL, 16);
}

void readDATA(unsigned long* outDATA) {
    cm = (outDATA[0] & 128) >> 7;
    a2 = (outDATA[0] & 64) >> 6;
    X = (outDATA[0] & 56) >> 3;
    Y = outDATA[0] & 7;
    b = outDATA[1];
}

void SBDBT_RX() {
    char rcvDATA;
    rcvDATA = SBDBT_USART.getc();
    rcvBUFF_SBDBT[rcvCOUNT_SBDBT] = rcvDATA;
    rcvCOUNT_SBDBT++;
    if (rcvDATA == 0x0A) {
        rcvCOUNT_SBDBT = 0;
        rcvFLAG = 2;
    }
    else if (rcvCOUNT_SBDBT >= 40) {
        rcvCOUNT_SBDBT = 0;
        memset(rcvBUFF_SBDBT, '\0', 40);
    }
}

void IM920_RX() {
    char rcvDATA;
    rcvDATA = IM920_USART.getc();
    rcvBUFF_IM920[rcvCOUNT_IM920] = rcvDATA;
    rcvCOUNT_IM920++;
    if (rcvDATA == 0x0A) {
        rcvCOUNT_IM920 = 0;
        rcvFLAG = 1;
    }
    else if (rcvCOUNT_IM920 >= 40) {
        rcvCOUNT_IM920 = 0;
        memset(rcvBUFF_IM920, '\0', 40);
    }
}

void deadCheck() {
    if (rcvFLAG == 0 && deadCOUNT >= 5) {
        deadflag = 1;
        deadCOUNT = 0;
    }
    else if (rcvFLAG == 0 && deadCOUNT >= 2) {
        deadCOUNT++;
        DEVICE_TX(sendDATA);
    }
    else if (rcvFLAG == 0) {
        deadCOUNT++;
    }
    sendDATA = cvsendDATA(0, 0);
}

inline void initializeIM920(){
    IM920_USART.baud(9600);
    IM920_USART.format(8, Serial::None, 1);
    IM920_USART.attach(IM920_RX, Serial::RxIrq);
    SBDBT_USART.baud(9600);
    SBDBT_USART.format(8, Serial::None, 1);
    SBDBT_USART.attach(SBDBT_RX, Serial::RxIrq);
    DTimer.attach(deadCheck, 0.3);
    sendDATA = cvsendDATA(0, 0);
}

inline void readIM920(){
    if (rcvFLAG == 1) {
        if (strlen(rcvBUFF_IM920) > 8) {
            cvrecvDATA(rcvBUFF_IM920, outDATA);
            readDATA(outDATA);
            deadCOUNT = 0;
            deadflag = 0;
            DEVICE_TX(sendDATA, 1);
        }
        memset(rcvBUFF_IM920, '\0', 40);
        rcvFLAG = 0;
    }
    else if (rcvFLAG == 2) {
        if (strlen(rcvBUFF_SBDBT) > 4) {
            cvrecvDATAsbdbt(rcvBUFF_SBDBT, outDATA);
            readDATA(outDATA);
            deadCOUNT = 0;
            deadflag = 0;
            DEVICE_TX(sendDATA, 2);
        }
        memset(rcvBUFF_SBDBT, '\0', 40);
        rcvFLAG = 0;
    }
}
#else
/***Get state ps3con.***/
#define SBDBT_TX p28
#define SBDBT_RX p27
Serial SBDBT(SBDBT_TX, SBDBT_RX);
#define ps3_start 0x80
short toggle            = 0;
short edge_circle       = 0;
short edge_triangle     = 0;
short edge_square       = 0;
short edge_r1           = 0;
short edge_l1           = 0;
short edge_r2           = 0;
short edge_l2           = 0;
short edge_l_up         = 0;
short edge_l_down       = 0;
short edge_r_up         = 0;
short edge_r_down       = 0;
short edge_right        = 0;
short edge_left         = 0;
short edge_up           = 0;
short edge_down         = 0;
short edge_cross        = 0;
short square            = 0;
short triangle          = 0;
short circle            = 0;
short cross             = 0;
short up                = 0;
short down              = 0;
short right             = 0;
short left              = 0;
short l1                = 0;
short l2                = 0;
short r1                = 0;
short r2                = 0;
signed int analog_lx    = 0;
signed int analog_ly    = 0;
signed int analog_rx    = 0;
signed int analog_ry    = 0;
int count               = 0;
int sample              = 0;
int ps3_data[7];
inline void data_clear();
inline void data_check();
inline void SBDBT_interrupt();
inline void initializeSBDBT();

inline void data_clear()
{
    for(int i=0; i<7; i++) ps3_data[i]=0;
}
inline void data_check()
{
    square=0,triangle=0,circle=0,cross=0,up=0,down=0,right=0,left=0,l1=0,l2=0,r1=0,r2=0;
    analog_lx=0,analog_ly=0,analog_rx=0,analog_ry=0;
    if((ps3_data[0]==0x00)&&(ps3_data[1]==0x01)) up=1;
    else if((ps3_data[0]==0x00)&&(ps3_data[1]==0x02)) down=1;
    else if((ps3_data[0]==0x00)&&(ps3_data[1]==0x04)) right=1;
    else if((ps3_data[0]==0x00)&&(ps3_data[1]==0x08)) left=1;
    else if((ps3_data[0]==0x00)&&(ps3_data[1]==0x10)) triangle=1;
    else if((ps3_data[0]==0x00)&&(ps3_data[1]==0x20)) cross=1;
    else if((ps3_data[0]==0x00)&&(ps3_data[1]==0x40)) circle=1;
    else if((ps3_data[0]==0x01)&&(ps3_data[1]==0x00)) square=1;
    else if((ps3_data[0]==0x02)&&(ps3_data[1]==0x00)) l1=1;
    else if((ps3_data[0]==0x04)&&(ps3_data[1]==0x00)) l2=1;
    else if((ps3_data[0]==0x08)&&(ps3_data[1]==0x00)) r1=1;
    else if((ps3_data[0]==0x10)&&(ps3_data[1]==0x00)) r2=1;
    analog_lx=64-(signed int)ps3_data[2];
    analog_ly=64-(signed int)ps3_data[3];
    analog_rx=64-(signed int)ps3_data[4];
    analog_ry=64-(signed int)ps3_data[5];
    if(!circle) edge_circle=1;
    if(!triangle) edge_triangle=1;
    if(!r1) edge_r1=1;
    if(!l1) edge_l1=1;
    if(!r2) edge_r2=1;
    if(!l2) edge_l2=1;
    if(!right) edge_right=1;
    if(!left) edge_left=1; 
    if(!up) edge_up=1;
    if(!square) edge_square=1;
    if(!down) edge_down=1;
    if(!cross) edge_cross=1;
}
///interrupt SBDBT RX.
inline void SBDBT_interrupt()
{
    sample=SBDBT.getc();
    if(count==7) data_clear();
    if(sample==ps3_start) count=0;
    else {
        ps3_data[count]=sample;
        count++;
    }
    if(count==6) data_check();
}
inline void initializeSBDBT()
{
    wait(0.1);
    while(analog_ly==-64)
    for(int i=0; i<7; i++) ps3_data[i]=0;
    SBDBT.baud(2400);
    SBDBT.format(8, Serial::None, 1);
    SBDBT.attach(SBDBT_interrupt, Serial::RxIrq);
}
#endif