File content as of revision 34:a00d3e06d81f:

#include "mbed.h"
#include "motorsmlap.h"
#include "MDC3_0pinConfig.h"
#include "SoftPWM.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);
CAN can(CAN_RX, CAN_TX);

DigitalOut RSControl(RS485_CS);
bool addrChecked;
bool headerRecieved;
DigitalOut debugLED0(LED_0);
DigitalOut debugLED1(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 = 3;
SoftPWM beep(BUZER);
bool brakeing[4] = {false};
bool canRecieved = false;

int mode[4] = {0};

std::vector<unsigned char> buf;
uint8_t canbuf[8];
CANMessage msg;

void canRxIt()
{
    if(can.read(msg))
    {
        canRecieved = true;
        estop = false;
    }
}

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

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

void processData(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.period(1.0 / 2000.0);
    beep = 0.6;
    wait(0.1);
    beep.period(1.0 / 4000.0);
    beep = 0.6;
    wait(0.1);
    beep.period(1.0 / 6000.0);
    beep = 0.6;
    wait(0.1);
    beep = 0.0;
    wait(0.1);
    beep.period(1.0 / 2000.0);
    for(int i = 0; i<addr; i++) {
        beep = 0.6;
        wait(0.075);
        beep = 0.0;
        wait(0.075);
    } // beep addr times
    unsigned char tmp[3] = {0};
    addrChecked=false,headerRecieved=false;
    rs485.baud(115200);
    serial.baud(921600);
    //addr.mode(PullUp);
    RSControl = 0;
    rs485.putc((1<<addr));
    rs485.attach(&callback);
    beep = 0.0;
    can.frequency(125000);
    can.filter(addr,0x7ff,CANStandard);
    serial.printf("addr:%d\n",addr);
    wait(0.01); 
    can.attach(&canRxIt, CAN::RxIrq);
    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);
                debugLED0 = !debugLED0;
            } else {
                buf.erase(buf.begin());
                debugLED0 = false;
            }
        }
        if(canRecieved)
        {
            for(int i=0;i<4;i++)
            {   
                serial.printf("num:%d,data:%d",i,msg.data[i]);
                motor[i].setMotorSpeed((msg.data[i]-126.0)/126.0); 
            }
            serial.printf("\n");
            //processData(tmp);
            debugLED0 = !debugLED0;
            canRecieved = false;
        }
        if(estop)
            forceStop();
        beep = estop*0.5;
        //wait(0.001);
    }
}