File content as of revision 3:5c32522fbe3f:

#include "mbed.h"
//#include "define.h"

UARTSerial serie(D10,D2,115200);
Serial pc(SERIAL_TX, SERIAL_RX);

DigitalOut led1(LED1);

void command(uint8_t command[3], uint8_t value);
//void packet_build(uint8_t buffer[],uint8_t command[3] ,uint8_t value);
uint16_t crc16(uint8_t *packet, int nBytes);

uint8_t avancer[] = {8,5,1};
uint8_t reculer[] = {9,5,1};

uint8_t tourner_droite[] = {10,5,1};
uint8_t tourner_gauche[] = {11,5,1};

int main(){   
    int i;
    led1=0;   
        
    while(1){
        for(i=0; i<15; i++) {
            command( avancer ,i);
            wait_ms(100);
        }
        for(i=15; i>0; i--) {
            command( avancer ,i);
            wait_ms(100);
        }
        
        for(i=0; i<15; i++) {
            command( reculer ,i);
            wait_ms(100);
        }
        for(i=15; i>0; i--) {
            command( reculer ,i);
            wait_ms(100);
        }
    }
}


void command(uint8_t command[3], uint8_t value){
    uint8_t envoi[command[1]], reponse[command[2]];

    envoi[0] = 128;
    envoi[1] = command[0];
    envoi[2] = value;
    envoi[3] = crc16(envoi,3)>>8;
    envoi[4] = crc16(envoi,3);
    
    serie.write(envoi,command[1]);
    pc.printf("%i-%i-%i-%i-%i\n",envoi[0],envoi[1],envoi[2],envoi[3],envoi[4]);
    serie.read(reponse,command[2]);
    pc.printf("%i\n",reponse[0]);
}

/*void packet_build(uint8_t buffer[],uint8_t command[3] ,uint8_t value){
    buffer[0] = 128;
    buffer[1] = command[0];
    buffer[2] = value;
    buffer[3] = crc16(buffer,3)>>8;
    buffer[4] = crc16(buffer,3);
}*/

//Calculates CRC16 of nBytes of data in byte array message
uint16_t crc16(uint8_t *packet, int nBytes){
    uint8_t byte, bit;
    uint16_t crc=0;
    
    for(byte=0 ; byte < nBytes ; byte++) {
        crc = crc ^ (packet[byte] << 8);
        for (bit = 0; bit < 8; bit++) {
            if (crc & 0x8000) {
                crc = (crc << 1) ^ 0x1021;
            } else {
                crc = crc << 1;
            }
        }
    }
    return crc;
}