File content as of revision 2:2182df2d7810:

#include "mbed.h"
#include "TS_DISCO_F469NI.h"
#include "LCD_DISCO_F469NI.h"
#include "F469_GUI.hpp"
#include "ident_crac.h"
#include <CAN.h>
#include "fonts.h"

#define BLEU 0xFF0000FF
#define ROUGE 0xFFFF0000
#define VERT 0xFF7FFF00
#define JAUNE 0xFF8F8F25
#define BLANC 0xFF000000
#define SIZE_FIFO 16 

CAN can1(PB_8, PB_9); //laisser le 1er CAN car probleme sinon
CAN can2(PB_5, PB_13);

CANMessage msgRxBuffer[SIZE_FIFO];

Ticker ticker;
TS_DISCO_F469NI ts;
LCD_DISCO_F469NI lcd;
DigitalOut led1(LED1);
DigitalOut led2(LED2);
Timeout timeout;
Timeout AffTime;
Timer timerAck;
Timer timer;

unsigned char test[32] = {32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32};

char counter = 0;
char check;
char Jack = 1;
int check_cartes = 0, demo_fin = 0;
char Ack_strat = 0;
signed char Strat = 0;

void SendRawId (unsigned short id);
void SelectionStrat (unsigned char numeroStrat);
void Setflag(void);
void canProcessRx(void);
void canRx_ISR(void);
signed char Bouton_Strat (void);
void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement);

typedef enum {ATT, CHOIX, DEMO, DEMO_EN_COURS, SELECT_SIDE, TACTIQUE, DETAILS, LAUNCH, WAIT_JACK, FIN} T_etat;
T_etat etat = ATT;
unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
int flagSendCan=1;
unsigned char Cote = 0; //0 -> bleu | 1 -> jaune

int main()
{
    ts.Init(lcd.GetXSize(), lcd.GetYSize());
    TS_StateTypeDef TS_State;
    CANMessage trame_Tx = CANMessage();
    can2.frequency(1000000); // fréquence de travail 1Mbit/s
    can2.attach(&canRx_ISR); //interrutpion can en reception
    int cpt_dizaine,cpt_unite;
    
    //Initialisation CAN
    trame_Tx.len = 1;
    trame_Tx.format = CANStandard;
    trame_Tx.type = CANData;
    
    /////////////////DEFINITION DES BOUTONS////////////////////
    Button COTE_BLEU(20, 25, 755, 200, "Bleu");
    Button COTE_JAUNE(20, 255, 500, 200, "Jaune");
    Button RETOUR  (530, 255, 245, 200, "--Precedent--");
    Button LANCER  (20, 255, 230, 200, "--LANCER--");
    Button CHECK (275, 255, 230, 200, "Valider");
    Button MATCH (10, 25, 780, 200, "Match");
    Button DEMONSTRATION (10, 255, 780, 200, "Demo");
    Button TEST_MOTEUR(20, 25, 200, 200, "Moteur");
    Button TEST_BRAS(240, 25, 200, 200, "Bras");
    Button FORCE_LAUNCH(530, 255, 245, 200, "Force Launch");
    ////////////////////////////////////////////////////////////
    
    signed char Strategie = 0; //N° de la strategie (1-10)
    
    lcd.SetBackColor(LCD_COLOR_WHITE);
    lcd.SetTextColor(LCD_COLOR_BLACK);
    lcd.Clear (LCD_COLOR_WHITE);
    lcd.SetFont(&Font24);
    lcd.DisplayStringAt(0, 200, (uint8_t *)"Verification des cartes ...", CENTER_MODE);
    FORCE_LAUNCH.Draw(0xFFFF0000, 0);
        
    while(1)
    {
        canProcessRx();
        ts.GetState(&TS_State);
        wait(0.1);        
        switch (etat)
        {
            case ATT :
                if(check_cartes == 1)
                {
                    etat = CHOIX;
                    check_cartes = 0;
                    
                    lcd.Clear (LCD_COLOR_WHITE);
                    lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Match ou demonstration ?", CENTER_MODE);
                    DEMONSTRATION.Draw(LCD_COLOR_LIGHTGREEN, 0);
                    MATCH.Draw(0xFFF01010, 0);
                    //canProcessRx();
                }
                if (FORCE_LAUNCH.Touched())
                {
                    //etat = CHOIX;
                    SendRawId(ECRAN_ALL_CHECK);
                    etat = CHOIX;
                    while(FORCE_LAUNCH.Touched())
                        canProcessRx();
                        
                    lcd.Clear (LCD_COLOR_WHITE);
                    lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Match ou demonstration ?", CENTER_MODE);
                    DEMONSTRATION.Draw(LCD_COLOR_LIGHTGREEN, 0);
                    MATCH.Draw(0xFFF01010, 0);
                }
                break;
                
            case CHOIX :
                //canProcessRx();
                if(DEMONSTRATION.Touched())
                {
                    etat = DEMO;
                    while(DEMONSTRATION.Touched())
                        canProcessRx();
                        
                    lcd.Clear(LCD_COLOR_WHITE);
                    TEST_MOTEUR.Draw(0xFFF0F0F0, 0);
                    TEST_BRAS.Draw(0xFFF0F0F0, 0);
                    RETOUR.Draw(0xFFFF0000, 0);
                    trame_Tx.id = 0x602;
                    trame_Tx.data[0] = 0;
                    can2.write(trame_Tx);
                }
                    
                if(MATCH.Touched())
                {
                    etat = SELECT_SIDE;
                    while(MATCH.Touched())
                        canProcessRx();
                        
                    lcd.Clear(LCD_COLOR_WHITE);
                    lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Choisis un cote gros", CENTER_MODE);
                    COTE_BLEU.Draw(0xFF0070FF, 0);
                    COTE_JAUNE.Draw(0xFFFFF000, 0);
                    RETOUR.Draw(0xFFFF0000, 0);
                    trame_Tx.id = 0x602;
                }
                break;
            
            case DEMO :
                
                trame_Tx.id=0x601;
                //canProcessRx();
                if(TEST_MOTEUR.Touched())
                {
                    Strat = 0x10;
                    trame_Tx.data[0] = Strat;
                    while(TEST_MOTEUR.Touched())
                        canProcessRx();
                    can2.write(trame_Tx);
                    TEST_MOTEUR.Draw(0xFFF0F0F0, 0);
                    //etat = DEMO_EN_COURS;
                }
                    
                if(TEST_BRAS.Touched())
                {
                    Strat = 0x11;
                    trame_Tx.data[0] = Strat;
                    while(TEST_BRAS.Touched())
                        canProcessRx();
                    can2.write(trame_Tx);
                    TEST_MOTEUR.Draw(0xFFF0F0F0, 0);
                    //etat = DEMO_EN_COURS;
                }
                    
                if(RETOUR.Touched())
                {
                    etat = CHOIX;
                    while(RETOUR.Touched())
                        canProcessRx();
                        
                    lcd.Clear (LCD_COLOR_WHITE);
                    lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Match ou demonstration ?", CENTER_MODE);
                    DEMONSTRATION.Draw(LCD_COLOR_LIGHTGREEN, 0);
                    MATCH.Draw(0xFFF01010, 0);
                }
                //SelectionStrat(trame_Tx.data[0]);
                if (Ack_strat == 1)
                {
                    etat = DEMO_EN_COURS;
                    lcd.Clear(LCD_COLOR_WHITE);
                    lcd.SetFont(&Font24);
                    lcd.DisplayStringAt(0, 190, (uint8_t *)"DEMONSTRATION EN COURS", CENTER_MODE);
                    Ack_strat = 0;
                }
                
                break;
                
            case DEMO_EN_COURS:

                if (demo_fin == 1)
                {
                    etat = DEMO;
                    lcd.Clear(LCD_COLOR_WHITE);
                    TEST_MOTEUR.Draw(0xFFF0F0F0, 0);
                    TEST_BRAS.Draw(0xFFF0F0F0, 0);
                    RETOUR.Draw(0xFFFF0000, 0);
                    trame_Tx.id = 0x602;
                    trame_Tx.data[0] = 0;
                    can2.write(trame_Tx);
                }
                Ack_strat = 0;
                
            case SELECT_SIDE :
                    
                //canProcessRx();
                if(COTE_BLEU.Touched()) 
                {
                    Cote = 0x0;
                    trame_Tx.data[0] = Cote;
                    can2.write(trame_Tx);
                    while(COTE_BLEU.Touched())
                        canProcessRx();
                }
                    
                if(COTE_JAUNE.Touched())
                {
                    Cote = 0x1;
                    trame_Tx.data[0] = Cote;
                    can2.write(trame_Tx);
                    while(COTE_JAUNE.Touched())
                        canProcessRx();
                }
                    
                if(RETOUR.Touched())
                {
                    etat = CHOIX;
                    while(RETOUR.Touched())
                        canProcessRx();
                        
                    lcd.Clear (LCD_COLOR_WHITE);
                    lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Match ou demonstration ?", CENTER_MODE);
                    DEMONSTRATION.Draw(LCD_COLOR_LIGHTGREEN, 0);
                    MATCH.Draw(0xFFF01010, 0);
                }
                break;
                    
            case TACTIQUE :
                if (Cote == 0){
                    lcd.Clear(0xFF0070FF);
                    lcd.SetBackColor(0xFF0070FF);
                    }
                else if (Cote == 1){
                    lcd.Clear(0xFFFFF000);
                    lcd.SetBackColor(0xFFFFF000);
                    }
                else {
                    lcd.Clear(0xFF0070FF);
                    lcd.SetBackColor(0xFF0070FF);
                    }
                
                lcd.SetTextColor(LCD_COLOR_BLACK);    
                lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Choisis une strat gros", CENTER_MODE);
                
                Strategie = Bouton_Strat(); // retourne valeur de Strategie si bouton strat renvoi -1 on reviens en arriere
                if (Strategie == -1) 
                {
                    etat = SELECT_SIDE;
                    lcd.Clear(LCD_COLOR_WHITE);
                    lcd.SetBackColor(LCD_COLOR_WHITE);
                    lcd.SetTextColor(LCD_COLOR_BLACK);
                    lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Choisis un cote gros", CENTER_MODE);
                    COTE_BLEU.Draw(0xFF0070FF, 0);
                    COTE_JAUNE.Draw(0xFFFFF000, 0);
                    RETOUR.Draw(0xFFFF0000, 0);
                    trame_Tx.id = 0x602;
                }
                else 
                {
                    etat = DETAILS;
                    lcd.Clear(LCD_COLOR_WHITE);
                    CHECK.Draw(0xFF00FF00);
                    RETOUR.Draw(0xFFFF0000);
                    SelectionStrat(Strategie);
                }  
                
                break;
            
            case DETAILS :
                    //canProcessRx();
                if (CHECK.Touched())
                {
                    SendRawId(ECRAN_START_MATCH);
                    //etat = WAIT_JACK;
                    while(CHECK.Touched())
                        canProcessRx();
                }
                    
                if(RETOUR.Touched())
                {
                    etat = TACTIQUE;
                    while(RETOUR.Touched());
                }
                break;
            
                
            case WAIT_JACK : 
                lcd.SetBackColor(LCD_COLOR_WHITE);
                lcd.SetTextColor(LCD_COLOR_BLACK);
                lcd.Clear(LCD_COLOR_WHITE);
                lcd.SetFont(&Font24);
                if (Cote == 0){
                    lcd.Clear(0xFF0070FF);
                    lcd.SetBackColor(0xFF0070FF);
                    }
                else if (Cote == 1){
                    lcd.Clear(0xFFFFF000);
                    lcd.SetBackColor(0xFFFFF000);
                    }
                else {
                    lcd.Clear(0xFF0070FF);
                    lcd.SetBackColor(0xFF0070FF);
                    }
                SendRawId(DEBUG_FAKE_JAKE); // PERMET DE SIMULER LE RETRAIT DU JACK POUR QUE LE ROBOT DEMARRE APRES LA VALIDATION DE LA STRAT
               /* while (Jack == 1){
                lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"En attente du Jack", CENTER_MODE); 
                }*/  
                cpt_dizaine = 57, cpt_unite = 48;
                while (cpt_dizaine !=47){
                    lcd.DisplayChar(390,190,cpt_dizaine);
                    while (cpt_unite != 47)
                    {
                        lcd.DisplayChar(410,190,cpt_unite);//la fonction affiche un caractere en ASCII d'ou la valeur de cpt
                        cpt_unite--;
                        wait(1);
                    }
                    cpt_dizaine--;
                    cpt_unite = 57;
                }
                lcd.DisplayStringAt(0, 190, (uint8_t *)"FIN DU MATCH", CENTER_MODE);
                wait(5);
                etat = FIN;
                break;
                
            case FIN :
                lcd.Clear (LCD_COLOR_WHITE);
                lcd.SetBackColor(LCD_COLOR_WHITE); 
                lcd.SetFont(&Font24);
                lcd.DisplayStringAt(0, 170, (uint8_t *)"REDEMARAGE", CENTER_MODE);
                lcd.DisplayStringAt(0, 190, (uint8_t *)"NECESSAIRE", CENTER_MODE);
                while(1); // force le redemarage du robot  
                break;
                
        }    
        
    }
}
    

void SendRawId (unsigned short id)
{
    CANMessage msgTx=CANMessage();
    msgTx.id=id;
    msgTx.len=0;
    can2.write(msgTx);
}

void SelectionStrat (unsigned char numeroStrat)
{
    lcd.SetBackColor(LCD_COLOR_WHITE);
    lcd.SetTextColor(LCD_COLOR_BLACK);
    lcd.SetFont(&Font24);
    switch (numeroStrat)
    {
        case 0x1 :
            //description de Strategie n°1
            lcd.DisplayStringAt(20, 100, (uint8_t *)"Strategie 1 :", LEFT_MODE);
            break;
            
        case 0x2 :
            //description de Strategie n°2
            lcd.DisplayStringAt(20, 100, (uint8_t *)"Strategie 2 :", LEFT_MODE);
            break;
            
        case 0x3 :
            //description de Strategie n°3
            lcd.DisplayStringAt(20, 100, (uint8_t *)"Strategie 3 :", LEFT_MODE);
            break;
            
        case 0x4 :
            //description de Strategie n°4
            lcd.DisplayStringAt(20, 100, (uint8_t *)"Strategie 4 :", LEFT_MODE);
            break;
            
        case 0x5 :
            //description de Strategie n°5
            lcd.DisplayStringAt(20, 100, (uint8_t *)"Strategie 5 :", LEFT_MODE);
            break;
            
        case 0x6 :
            //description de Strategie n°5
            lcd.DisplayStringAt(20, 100, (uint8_t *)"Strategie 6 :", LEFT_MODE);
            break;
            
        case 0x7 :
            //description de Strategie n°5
            lcd.DisplayStringAt(20, 100, (uint8_t *)"Strategie 7 :", LEFT_MODE);
            break;
            
        case 0x8 :
            //description de Strategie n°5
            lcd.DisplayStringAt(20, 100, (uint8_t *)"Strategie 8 :", LEFT_MODE);
            break;
            
        case 0x9 :
            //description de Strategie n°5
            lcd.DisplayStringAt(20, 100, (uint8_t *)"Strategie 9 :", LEFT_MODE);
            break;
            
        case 0xA :
            //description de Strategie n°5
            lcd.DisplayStringAt(20, 100, (uint8_t *)"Strategie 10 :", LEFT_MODE);
            break;
            
        case 0x10 :
            lcd.DisplayStringAt(20, 100, (uint8_t *)"Strategie 10 :", LEFT_MODE);
            break;
        }
}

void Setflag(void)
{
    flagSendCan = 1;
}

void canProcessRx(void)
{
    static signed char FIFO_occupation=0,FIFO_max_occupation=0;
    char useless1 = 0;

    FIFO_occupation=FIFO_ecriture-FIFO_lecture;
    if(FIFO_occupation<0)
        FIFO_occupation=FIFO_occupation+SIZE_FIFO;
    if(FIFO_max_occupation<FIFO_occupation)
        FIFO_max_occupation=FIFO_occupation;
    if(FIFO_occupation!=0) {

        switch(msgRxBuffer[FIFO_lecture].id) {
            case ECRAN_PRINTF_1:
                for(useless1 = 0; useless1 <8; useless1++) {
                    test[useless1] = msgRxBuffer[FIFO_lecture].data[useless1];
                }
                //updateDisplay();
                break;
            case ECRAN_PRINTF_2:
                for(useless1 = 0; useless1 <8; useless1++) {
                    test[useless1+8] = msgRxBuffer[FIFO_lecture].data[useless1];
                }
                //updateDisplay();
                //Ecrire_CANtxt(test);
                break;
            case ECRAN_PRINTF_3:
                for(useless1 = 0; useless1 <8; useless1++) {
                    test[useless1+2*8] = msgRxBuffer[FIFO_lecture].data[useless1];
                }
                //updateDisplay();
                //Ecrire_CANtxt(test);
                break;
            case ECRAN_PRINTF_4:
                for(useless1 = 0; useless1 <8; useless1++) {
                    test[useless1+3*8] = msgRxBuffer[FIFO_lecture].data[useless1];
                }
                //updateDisplay();
                //Ecrire_CANtxt(test);
                break;
            case ECRAN_PRINTF_CLEAR:
                etat = CHOIX;
                break;
            case CHECK_IHM:
                SendRawId(ALIVE_IHM);
                break;
            case RESET_IHM:
                etat = CHOIX;
                break;
            case CHECK_BALISE:
                //Check[1].color = Orange;
                //DrawChecks();
                break;
            case ALIVE_BALISE:
                //Check[1].color = Green;
                //DrawChecks();
                break;
            case CHECK_MOTEUR:
                //Check[2].color = Orange;
                //DrawChecks();
                break;
            case ALIVE_MOTEUR:
                //Check[2].color = Green;
                //DrawChecks();
                break;
            case CHECK_ACTIONNEURS:
                //Check[3].color = Orange;
                //DrawChecks();
                break;
            case ALIVE_ACTIONNEURS:
                //Check[3].color = Green;
                //DrawChecks();
                break;
            case ECRAN_ALL_CHECK:
                check_cartes=1;
                //DrawEcran(1);
                //EcranActuel = 1;
                //menusOnEcranActuel = Liste_ecrans[EcranActuel-1].menus;
                break; 
            case ECRAN_ACK_STRAT:
                if (msgRxBuffer[FIFO_lecture].data[0] == Strat)
                    Ack_strat = 1;
                break;
            case ECRAN_ACK_COLOR:
                if (etat != DEMO)
                    etat = TACTIQUE;
                break;
            case ECRAN_ACK_START_MATCH:
                if (etat == DEMO_EN_COURS) 
                {
                    SendRawId(DEBUG_FAKE_JAKE);
                }
                else
                {
                etat = WAIT_JACK;
                }
                break;
            case GLOBAL_START:
                Jack=0;
                break;
                
            case DEBUG_STRATEGIE_AUTOMATE : 
                    if (msgRxBuffer[FIFO_lecture].data[0] == 19) {
                        demo_fin = 1;
                        }
                    break;
                
            default:
                break;
        }
        FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
    }
}

void canRx_ISR (void)
{
    if (can2.read(msgRxBuffer[FIFO_ecriture])) {
        led2 = !led2;
        if(msgRxBuffer[FIFO_ecriture].id==RESET_IHM)
            lcd.DisplayStringAt(0, LINE(6), (uint8_t *)"Erreur", CENTER_MODE);//mbed_reset();
        else FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
    }
}

signed char Bouton_Strat (void)
{   
    CANMessage msgTx=CANMessage();
    msgTx.id=0x601;
    msgTx.len=1;
    
    Button STRAT_1 (20, 10, 150, 150, "Strategie 1");
    Button STRAT_2 (180, 10, 150, 150, "Strategie 2");
    Button STRAT_3 (340, 10, 150, 150, "Strategie 3");
    Button STRAT_4 (20, 165, 150, 150, "Strategie 4");
    Button STRAT_5 (180, 165, 150, 150, "Strategie 5");
    Button STRAT_6 (340, 165, 150, 150, "Strategie 6");
    Button STRAT_7 (20, 320, 150, 150, "Strategie 7");
    Button STRAT_8 (180, 320, 150, 150, "Strategie 8");
    Button STRAT_9 (340, 320, 150, 150, "Strategie 9");
    Button STRAT_10 (530, 25, 245, 200, "Strategie 10");
    Button RETOUR  (530, 255, 245, 200, "--Precedent--");
    //Definition des boutons
    
    Ack_strat = 0;
    Strat = 0;
    STRAT_1.Draw(0xFFF0F0F0, 0);
    STRAT_2.Draw(0xFFF0F0F0, 0);
    STRAT_3.Draw(0xFFF0F0F0, 0);
    STRAT_4.Draw(0xFFF0F0F0, 0);
    STRAT_5.Draw(0xFFF0F0F0, 0);
    STRAT_6.Draw(0xFFF0F0F0, 0);
    STRAT_7.Draw(0xFFF0F0F0, 0);
    STRAT_8.Draw(0xFFF0F0F0, 0);
    STRAT_9.Draw(0xFFF0F0F0, 0);  
    STRAT_10.Draw(0xFFF0F0F0, 0);              
    RETOUR.Draw(0xFFFF0000, 0);
                
    while(Ack_strat == 0) 
    {
        canProcessRx();
        if (RETOUR.Touched())
        return -1;
        while(RETOUR.Touched());
                   //////////////////////////////STRATEGIE N°1
                    if (STRAT_1.Touched()){
                        Strat = 1;
                        msgTx.data[0] = 0x1;
                        can2.write(msgTx);
                        while(STRAT_1.Touched());
                        }
                    /////////////////////////////STRATEGIE N°2
                     if (STRAT_2.Touched()){
                        Strat = 2;
                        msgTx.data[0] = 0x2;
                        can2.write(msgTx);
                        while(STRAT_2.Touched());
                        }
                    //////////////////////////////STRATEGIE N°3
                     if (STRAT_3.Touched()){
                        Strat = 3;
                        msgTx.data[0] = 0x3;
                        can2.write(msgTx);
                        while(STRAT_3.Touched());
                        }
                    /////////////////////////////STRATEGIE N°4
                     if (STRAT_4.Touched()){
                        Strat = 4;
                        msgTx.data[0] = 0x4;
                        can2.write(msgTx);
                        while(STRAT_4.Touched());
                        }
                    ///////////////////////////////STRATEGIE N°5
                     if (STRAT_5.Touched()){
                        Strat = 5;
                        msgTx.data[0] = 0x5;  
                        can2.write(msgTx);                     
                        while(STRAT_5.Touched());
                        }
                    ////////////////////////////////STRATEGIE N°6
                     if (STRAT_6.Touched()){
                        Strat = 6;
                        msgTx.data[0] = 0x6;
                        can2.write(msgTx);
                        while(STRAT_6.Touched());
                        }
                    /////////////////////////////////STRATEGIE N°7
                     if (STRAT_7.Touched()){
                        Strat = 7;
                        msgTx.data[0] = 0x7;
                        can2.write(msgTx);
                        while(STRAT_7.Touched());
                        }
                    /////////////////////////////////STRATEGIE N°8
                     if (STRAT_8.Touched()){
                        Strat = 8;
                        msgTx.data[0] = 0x8;
                        can2.write(msgTx);
                        while(STRAT_8.Touched());
                        }
                    /////////////////////////////////STRATEGIE N°9
                     if (STRAT_9.Touched()){
                        Strat = 9;
                        msgTx.data[0] = 0x9;
                        can2.write(msgTx);
                        while(STRAT_9.Touched());
                        }
                    ///////////////////////////////////STRATEGIE N°10
                     if (STRAT_10.Touched()){
                        Strat = 10;
                        msgTx.data[0] = 0xA;
                        can2.write(msgTx);
                        while(STRAT_10.Touched());
                        }
    }
    return Strat;
}

//FONCTION DE L'ASSERVISSEMENT POUR LA DEMONSTRATION
void GoStraight (signed short distance,unsigned char recalage, unsigned short newValue, unsigned char isEnchainement)
{
    CANMessage msgTx=CANMessage();
    msgTx.id=ASSERVISSEMENT_RECALAGE;
    msgTx.len=6;
    msgTx.format=CANStandard;
    msgTx.type=CANData;
    // x sur 2 octets
    msgTx.data[0]=(unsigned char)distance;
    msgTx.data[1]=(unsigned char)(distance>>8);
    //Recalage sur 1 octet
    msgTx.data[2]=recalage;
    //Valeur du recalage sur 2 octets
    msgTx.data[3]=(unsigned char)newValue;
    msgTx.data[4]=(unsigned char)(newValue>>8);
    //Enchainement sur 1 octet
    msgTx.data[5]=isEnchainement;

    can1.write(msgTx);
    //wait_ms(500);
}