File content as of revision 3:22c5eb74817f:

#include "mbed.h"
#include "motorsmlap.h"
motorSmLap motor[]={
    motorSmLap(PA_2,PB_15),
//    motorSmLap(,NULL),
//    motorSmLap(NULL,NULL),
//    motorSmLap(NULL,NULL)
    };
Serial serial(PB_10,PB_11); 
DigitalOut serialControl(PA_8);
DigitalOut addrChecked(PB_13);
DigitalOut headerRecieved(PB_14);
Timeout timeout;
uint8_t pointedMotor;
bool estop =false,doubleHeader = false;
BusIn addr(PB_4,PB_5,PB_6);
DigitalOut beep(PB_7);

int mode = SM;

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

void callback(){
    uint8_t data;
    data = serial.getc();
    if(data == 255) //header
    {
        if(headerRecieved)
            doubleHeader =true;
        else if (doubleHeader)
        {
            estop = true;
            return;      
        }     
        addrChecked = false;
        headerRecieved = true;
    }else
    if(headerRecieved && !addrChecked)
    {
        doubleHeader=false;
        if((data>>4) == addr.read())
        {   
            addrChecked =true;
            pointedMotor = data%4;
            serialControl = 1;
            serial.putc(addr.read());
            serialControl = 0;
        }
    }else if(headerRecieved && addrChecked && !estop)
    {
        doubleHeader = false;
        motor[pointedMotor].setMotorSpeed(data/126.0-1.0);
        addrChecked = false;
        headerRecieved = false;
        //timeout.attach(&forceStop,0.1);
    }
           
}

int main() {
    beep = 0;
    addrChecked=false,headerRecieved=false;
    motor[0].setMotorSpeed(0);
    serial.baud(115200);
    addr.mode(PullUp);
    serialControl = 0;
    serial.attach(&callback);
    
    while(1) {
        if(estop)
            forceStop();
        beep = estop;
        //led = !led
    }
}