File content as of revision 4:2fb782334564:

#include "mbed.h"
#include "motorsmlap.h"
#include "MDC3_0pinConfig.h"

using namespace pinConfig;
motorSmLap motor[]={
    motorSmLap(DIR_H_0,DIR_L_0,PWM0),
    motorSmLap(DIR_H_1,DIR_L_1,PWM1),
    motorSmLap(DIR_H_2,DIR_L_2,PWM2),
    motorSmLap(DIR_H_3,DIR_L_3,PWM3)
};
Serial rs485(RS485_TX,RS485_RX);
Serial serial(UART1_TX,UART1_RX);

DigitalOut RSControl(RS485_CS);
DigitalOut addrChecked(LED_0);
DigitalOut headerRecieved(LED_1);
Timeout timeout;
uint8_t pointedMotor;
bool estop =false,doubleHeader = false;
//BusIn addr(DIP_0,DIP_1,DIP_2);
uint8_t addr = 2;
DigitalOut beep(BUZER);

int mode[4] = {0};

void forceStop()
{
    for(int i = 0;i < 1; i++)
    {
        motor[i].setMotorSpeed(0);
    }    
}

void callback(){
    const uint8_t data = rs485.getc();
    serial.putc(data);
    if(data == 255) //header
    {
        if(headerRecieved){
            doubleHeader =true;
            return;
        }
        else if (doubleHeader)
        {
            estop = true;
            return;      
        }     
        addrChecked = false;
        headerRecieved = true;
        return;
    }else
    if(headerRecieved && !addrChecked)
    {
        //serial.printf("data :%d",data);
        doubleHeader=false;
        if((data>>5) == addr)
        {
            addrChecked =true;
            pointedMotor = data%4;
            mode[pointedMotor] = (data>>4)%2;   
            //motor[pointedMotor].setMode(false);
            RSControl = 1;
            //rs485.putc(addr);
            RSControl = 0;
        }else{
            headerRecieved = false;
            addrChecked = false;
        }
        return;
    }else if(headerRecieved && addrChecked && !estop)
    {
        motor[pointedMotor].setMode(mode[pointedMotor]);
        doubleHeader = false;
        motor[pointedMotor].setMotorSpeed(data/126.0-1.0);
        addrChecked = false;
        headerRecieved = false;
        serial.putc(pointedMotor);     
        //timeout.attach(&forceStop,0.1);
    }else{
        doubleHeader = false;
        headerRecieved =false;
        addrChecked = false;
    }
       
}

int main() {
    beep = 0;
    addrChecked=false,headerRecieved=false;
    //motor[0].setMotorSpeed(0);
    rs485.baud(115200);
    serial.baud(115200);
   // addr.mode(PullUp);
    RSControl = 0;
    wait(0.5);
    rs485.putc((1<<addr));
    rs485.attach(&callback);

    while(1) {
        if(estop)
            forceStop();
        beep = estop;
        wait(0.1);
    }
}