File content as of revision 7:5ebe1058ca5b:

#include "mbed.h"
#include "motorsmlap.h"
#include "MDC3_0pinConfig.h"
#include <vector>
#define TIMEOUT 0.5
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);
bool addrChecked;
bool headerRecieved;
DigitalOut LED0(LED_0);
DigitalOut LED1(LED_1);
BusOut debugLED(LED_2,LED_3);
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);
bool brakeing[4] = {false};

int mode[4] = {0};

std::vector<unsigned char> buf;

unsigned char buf[100];

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

void callback(){
    LED0 = != LED0;
    buf.push_back(rs485.getc());
}

void processData(const unsigned char[] data)
{
    if(data[0]^data[1] == data[2])
    if((data[0]>>5) == addr)
    {
        addrChecked =true;
        pointedMotor = data[0]%4;
        mode[pointedMotor] = ((data[0]>>4)%2);
        motor[pointedMotor].braking = (data[0]>>3)%2;

        motor[pointedMotor].setMode(mode[pointedMotor]);
        if(data[1] == 126){
            motor[pointedMotor].setMotorSpeed(0);
            //serial.printf("STOP");
        }else{
            motor[pointedMotor].setMotorSpeed((data[1]-126.0)/126.0);
        }
        addrChecked = false;
        headerRecieved = false;
        serial.putc(pointedMotor);
        timeout.attach(&forceStop,TIMEOUT);
    }
}

int main() {
    for(int i= 0; i< 4; i++)
        motor[i].setMotorSpeed(0);
    beep = true;
    unsigned char tmp[3] = {0};
    addrChecked=false,headerRecieved=false;
    //motor[0].setMotorSpeed(0);
    rs485.baud(38400);
    serial.baud(115200);
    addr.mode(PullUp);
    RSControl = 0;
    wait(addr+0.5);
    rs485.putc((1<<addr));
    rs485.attach(&callback);
    beep = false;
    while(1) {
        debugLED = addr;
        if(buf.size() > 4)
        {
          if (buf[0] == 255)
          {
            tmp[0] = buf[1];
            tmp[1] = buf[2];
            tmp[2] = buf[3];
            processData(tmp);
            buf.erase(buf.begin(),buf.begin()+3)
          }else{
            buf.erase(buf.begin())
          }
        }
        if(estop)
            forceStop();
        beep = estop;
        wait(0.1);
    }
}