Prog_cate-Mbed - Reception de consigne à revoir -,,,

Dependencies:   Asservissement Encoder_Nucleo_16_bits MYCRAC mbed

Fork of Asserv_mot by Brandon Modon

Files at this revision

API Documentation at this revision

Comitter:
Brand101
Date:
Wed Apr 19 17:50:19 2017 +0000
Commit message:
Dernier version

Changed in this revision

Asservissement.lib Show annotated file Show diff for this revision Revisions of this file
Encoder-Nucleo-16bits.lib Show annotated file Show diff for this revision Revisions of this file
MYCRAC.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r 444ac4067b19 Asservissement.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Asservissement.lib	Wed Apr 19 17:50:19 2017 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/teams/CRAC-Team/code/Asservissement/#162aa6608ffe
diff -r 000000000000 -r 444ac4067b19 Encoder-Nucleo-16bits.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Encoder-Nucleo-16bits.lib	Wed Apr 19 17:50:19 2017 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/teams/CRAC-Team/code/Encoder_Nucleo_16_bits/#c57c8fe63263
diff -r 000000000000 -r 444ac4067b19 MYCRAC.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MYCRAC.lib	Wed Apr 19 17:50:19 2017 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/teams/CRAC-Team/code/MYCRAC/#cbe368f18aeb
diff -r 000000000000 -r 444ac4067b19 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Apr 19 17:50:19 2017 +0000
@@ -0,0 +1,2327 @@
+#include "mbed.h"
+#include "Nucleo_Encoder_16_bits.h"
+#include <math.h>
+#include "MYCRAC_utility.h"
+#include "Asservissement.h"
+#include "global.h"
+#include "MY_Asserv.h"
+#include "My_ident.h"
+
+//#include "CRAC_utility.h"
+
+//------------------------ prototypes de fcts ------------------------------------
+
+void Asser_Pos_Mot1(double pcons_pos);
+void Asser_Pos_Mot2(double pcons_pos);
+
+void Avancer_ligne_droite(long pcons, short vmax, short amax, short dmax);
+void Avancer_ligne_droite_XYTheta(long pcons, short vmax, short amax, short dmax);
+void Rotation(long pcons, short vmax, short amax, short dmax);
+void Rotation_XYTheta(long pcons, short vmax, short amax, short dmax);
+void X_Y_Theta(long px, long py, long ptheta, long sens, short vmax, short amax);
+
+void Recalage(int pcons, short vmax, short amax, short dir, short nv_val);
+void Rayon_De_Courbure(short rayon, short theta, short vmax, short amax, short sens, short dmax);
+
+void Odometrie(void); // void Odometrie(void);
+void Ralentir(short nouvelle_vitesse);
+void Arret(void);
+
+void canProcessRx(void);
+
+void clacul_odo();
+
+void canRx_ISR();
+void MY_ISR();
+
+
+Serial pc(SERIAL_TX, SERIAL_RX);
+
+
+CAN can1(PB_8, PB_9);
+
+CANMessage canmsg = CANMessage();
+
+char message[8]= {0x00,0x01,0x10,0x11,0x00,0x01,0x10,0x11};
+
+CANMessage msgRxBuffer[SIZE_FIFO]; // buffer en r�ception pour le CAN
+unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
+unsigned char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
+
+Ticker timer;
+Ticker cal_odo;
+
+//------------------------ def de var globales ------------------------------------
+
+PwmOut mot1(PA_8);
+PwmOut mot2(PA_5);
+
+DigitalOut INA_M2(PC_6); //p37
+DigitalOut INB_M2(PC_7); //p38
+DigitalOut INA_M1(PB_0); //P26
+DigitalOut INB_M1(PB_1); //P27
+
+Nucleo_Encoder_16_bits encoder1(TIM3, 0xffff, TIM_ENCODERMODE_TI12);
+Nucleo_Encoder_16_bits encoder2(TIM4, 0xffff, TIM_ENCODERMODE_TI12);
+
+double   consigne_pos, consigne_vit,                 // Consignes de position et de vitesse dans les mouvements
+         commande1, commande2,                       // Commande, aka duty cycle des PWM de chaque moteur
+         last_pos1, last_pos2, pos1, pos2,           // Valeurs des compteurs incr�mentaux aux instants t et t-1
+         ErreurPos1, ErreurPos2, last_ErreurPos1, last_ErreurPos2,       // Valeurs des erreurs de position de la boucle d'asservissement
+         Somme_ErreurPos1, Somme_ErreurPos2,                             // Valeurs des int�grales des erreurs
+         Delta_ErreurPos1, Delta_ErreurPos2,                             // Valeurs des d�riv�es des erreurs
+         Kpp1, Kip1, Kdp1, Kpp2, Kip2, Kdp2,                             // Valeurs des correcteurs d'asservissement pour les 2 moteurs
+         Kpp1a, Kip1a, Kdp1a, Kpp2a, Kip2a, Kdp2a,                       // Valeurs des correcteurs d'asservissement pour les 2 moteurs
+         VMAX, AMAX, DMAX,                                               // Valeurs maximales d'acc�leration, d�c�l�ration et vitesse
+         Odo_x, Odo_y, Odo_theta, Odo_val_pos_1, Odo_val_pos_2, Odo_last_val_pos_1, Odo_last_val_pos_2;  // Variables de positions utilis�es pour l'odom�trie
+uint32_t        roue_drt_init, roue_gch_init;                                   // Valeur des compteurs (!= 0) quand on commence un nouveau mouvement
+short    etat_automate, etat_automate_depl, new_message,
+         xytheta_sens, next_move_xyt, next_move, i, cpt, stop, stop_receive, param_xytheta[3],
+         etat_automate_xytheta, ralentare, recalage_debut, couleur_debut, recalage_debut_receive;
+char     nb_ordres, vitesse_danger, Stop_Danger, cpt_ordre, asser_actif;
+struct Ordre_deplacement liste[200];
+
+uint16_t        ms_count = 0,       // Compteur utilis� pour envoyer �chantillonner les positions et faire l'asservissement
+                ms_count1 = 0,      // Compteur utilis� pour envoyer la trame CAN d'odom�trie
+                flag_odo,
+                test3 = 0,
+                test4 = 0,
+                test5 = 0,
+                test6 =0,
+                test7 =0,
+                test8 =0;
+
+
+
+double test1=0,
+       test2=0;
+
+
+//------------------------ main ------------------------------------
+
+int main()
+{
+    pc.baud(230400);
+    pc.printf("Debut du prog");
+
+    INA_M2 = 0;
+    INB_M2 = 0;
+    INA_M1 = 0;
+    INB_M1 = 0;
+
+    mot1.period(1./20000.);
+    mot2.period(1./20000.);
+    can1.frequency(1000000);
+
+    consigne_pos = 0.0;
+    next_move = 0;
+    asser_actif=1;
+    cpt_ordre = 0;
+    nb_ordres = 1;
+    stop = 1;
+    stop_receive = 1;
+    ralentare = 0;
+    recalage_debut_receive = 0;
+
+    VMAX = 50;
+    AMAX = 125;
+    DMAX = 125;
+
+    roue_drt_init = (encoder1.GetCounter() + 20000);
+    roue_gch_init = COEF_ROUE_GAUCHE * ((double)encoder2.GetCounter() + 20000);
+
+    liste[0] = (struct Ordre_deplacement) {
+        TYPE_DEPLACEMENT_STOP,0,0,0,0,0,0,0,0,0,0,0,0,0,0
+    };
+
+
+    timer.attach(&MY_ISR, 0.001);
+    can1.attach(&canRx_ISR); // cr�ation de l'interrupt attach�e � la r�ception sur le CAN
+
+    cal_odo.attach(&clacul_odo, 0.01);
+    
+    Kpp1 = 0.5;
+    Kip1 = 0.0001;
+    Kdp1 = 0.8;
+
+    Kpp2 = 0.5;
+    Kip2 = 0.0001;
+    Kdp2 = 0.8;
+
+    while(1) {
+        //TEST
+        //Asser_Pos_Mot1(0.0);
+        //Asser_Pos_Mot2(0.0);
+        //test3++;
+
+        canProcessRx();
+
+        if(flag_odo) {
+            flag_odo = 0;
+            Odometrie();
+        }
+        pc.printf("init_d:%d inti_g:%d dit:%hd t8:%d\n", roue_drt_init, roue_gch_init, liste[cpt_ordre].distance, test8);
+
+        //pc.printf("com1=%f com2=%f ePos1=%f ePos2=%f  enc2=%d  enc1=%d\n",commande1, commande2, ErreurPos1, ErreurPos2, encoder2.GetCounter(), encoder1.GetCounter());
+        //wait(0.001);
+        //pc.printf("t1=%d t2=%d t3=%d t4=%d t5=%d t6=%d t7=%d      drt_init=%f  g_init=%f \n", test1, test2, test3, test4, test5, test6, test7, roue_drt_init, roue_gch_init);
+        /*
+        write_PWM2(commande2);
+        if(ms_count==19) {
+            test3++;
+        }
+        */
+    }
+}
+
+void clacul_odo()
+{
+    flag_odo = 1;
+}
+
+void canRx_ISR()
+{
+    if (can1.read(msgRxBuffer[FIFO_ecriture])) {
+        FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
+        /*
+        if(msgRxBuffer[FIFO_ecriture].id==RESET_STRAT) mbed_reset();
+        else FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
+        */
+    }
+}
+
+void canProcessRx(void)
+{
+    static signed char FIFO_occupation=0,FIFO_max_occupation=0;
+    CANMessage msgTx=CANMessage();
+    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 ASSERVISSEMENT_INFO_CONSIGNE:
+                SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_INFO_CONSIGNE);
+                Send_assev_info_comfig();
+                break;
+            case ASSERVISSEMENT_CONFIG_KPP_DROITE:
+                SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_CONFIG_KPP_DROITE);
+                break;
+            case ASSERVISSEMENT_CONFIG_KPI_DROITE:
+                SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_CONFIG_KPI_DROITE);
+                break;
+            case ASSERVISSEMENT_CONFIG_KPD_DROITE:
+                SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_CONFIG_KPD_DROITE);
+                break;
+            case ASSERVISSEMENT_CONFIG_KPP_GAUCHE:
+                SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_CONFIG_KPP_GAUCHE);
+                break;
+            case ASSERVISSEMENT_CONFIG_KPI_GAUCHE:
+                SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_CONFIG_KPI_GAUCHE);
+                break;
+            case ASSERVISSEMENT_CONFIG_KPD_GAUCHE:
+                SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_CONFIG_KPD_GAUCHE);
+                break;
+            case ASSERVISSEMENT_ENABLE:
+                SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_ENABLE);
+                asser_actif = msgRxBuffer[FIFO_lecture].data[0];
+                break;
+                /*
+                case ODOMETRIE_SMALL_POSITION:
+                SendAck(ACKNOWLEDGE_MOTEUR, ODOMETRIE_SMALL_POSITION);
+                Odometrie();  // il envoie automatiquement d'odo apr�s calcule
+                break;
+                */
+            case ODOMETRIE_SMALL_VITESSE:
+                SendAck(ACKNOWLEDGE_MOTEUR, ODOMETRIE_SMALL_VITESSE);
+                SendSpeed(consigne_pos, 0);
+                break;
+
+            case ASSERVISSEMENT_STOP:
+                SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_STOP);
+                stop_receive = 1;
+                liste[cpt_ordre].type = TYPE_DEPLACEMENT_STOP;
+                break;
+
+            case ASSERVISSEMENT_SPEED_DANGER:  //
+                SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_SPEED_DANGER);
+                break;
+
+            case ASSERVISSEMENT_XYT:
+                SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_XYT);
+                etat_automate_xytheta = 1;
+                liste[cpt_ordre].type = TYPE_DEPLACEMENT_X_Y_THETA;
+                liste[cpt_ordre].x = (msgRxBuffer[FIFO_lecture].data[0]) + (msgRxBuffer[FIFO_lecture].data[1] << 8);
+                liste[cpt_ordre].y = (msgRxBuffer[FIFO_lecture].data[2]) + (msgRxBuffer[FIFO_lecture].data[3] << 8);
+                liste[cpt_ordre].theta = (msgRxBuffer[FIFO_lecture].data[4]) + (msgRxBuffer[FIFO_lecture].data[5] << 8);
+                liste[cpt_ordre].sens = msgRxBuffer[FIFO_lecture].data[6];
+                break;
+
+            case ASSERVISSEMENT_COURBURE:
+                SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_COURBURE);
+                liste[cpt_ordre].type = TYPE_DEPLACEMENT_RAYON_COURBURE;
+                liste[cpt_ordre].rayon = (msgRxBuffer[FIFO_lecture].data[0]) + (msgRxBuffer[FIFO_lecture].data[1] << 8);
+                liste[cpt_ordre].theta_ray = (msgRxBuffer[FIFO_lecture].data[2]) + (msgRxBuffer[FIFO_lecture].data[3] << 8);
+                liste[cpt_ordre].sens = msgRxBuffer[FIFO_lecture].data[4];
+                liste[cpt_ordre].enchainement = msgRxBuffer[FIFO_lecture].data[5];
+                break;
+
+            case ASSERVISSEMENT_CONFIG:
+                SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_CONFIG);
+                VMAX = ((msgRxBuffer[FIFO_lecture].data[0]) + (msgRxBuffer[FIFO_lecture].data[1] << 8));
+                AMAX = ((msgRxBuffer[FIFO_lecture].data[2]) + (msgRxBuffer[FIFO_lecture].data[3] << 8));
+                ralentare = 1;
+                break;
+
+            case ASSERVISSEMENT_ROTATION:
+                SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_ROTATION);
+                liste[cpt_ordre].type = TYPE_DEPLACEMENT_ROTATION;
+                liste[cpt_ordre].angle = ((msgRxBuffer[FIFO_lecture].data[0]) + (msgRxBuffer[FIFO_lecture].data[1] << 8));
+                Rotate (Odo_theta);
+                break;
+
+            case ASSERVISSEMENT_RECALAGE:
+                SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_RECALAGE);
+                liste[cpt_ordre].type = TYPE_DEPLACEMENT_RECALAGE;
+                liste[cpt_ordre].distance = ((msgRxBuffer[FIFO_lecture].data[0]) + (msgRxBuffer[FIFO_lecture].data[1] << 8));
+                liste[cpt_ordre].recalage = ((msgRxBuffer[FIFO_lecture].data[3]) + (msgRxBuffer[FIFO_lecture].data[4] << 8));
+                liste[cpt_ordre].val_recalage = msgRxBuffer[FIFO_lecture].data[2];
+                recalage_debut = 1;
+                recalage_debut_receive = 1;
+                break;
+
+            case ASSERVISSEMENT_LIGNE_DROITE:   // n'existe pas encore
+                //SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_LIGNE_DROITE);
+                test8 = 10;
+                if(msgRxBuffer[FIFO_lecture].data[2] == 0) {
+                    liste[cpt_ordre].type = TYPE_DEPLACEMENT_LIGNE_DROITE;
+                } else {
+                    liste[cpt_ordre].type = TYPE_DEPLACEMENT_RECALAGE;
+                    liste[cpt_ordre].recalage = ((msgRxBuffer[FIFO_lecture].data[3]) + (msgRxBuffer[FIFO_lecture].data[4] << 8));
+                }
+                recalage_debut_receive = msgRxBuffer[FIFO_lecture].data[2];
+                liste[cpt_ordre].distance = ((msgRxBuffer[FIFO_lecture].data[1] << 8) + (msgRxBuffer[FIFO_lecture].data[0]));
+                liste[cpt_ordre].enchainement = msgRxBuffer[FIFO_lecture].data[5];
+                //GoStraight(); /////////////////////////////////
+                break;
+
+            case MOTOR_IMOBILE:
+                SendAck(ACKNOWLEDGE_MOTEUR, MOTOR_IMOBILE);
+                liste[cpt_ordre].type = TYPE_DEPLACEMENT_IMMOBILE;
+                break;
+
+        }
+
+        FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
+    }
+}
+
+
+void Asser_Pos_Mot1(double pcons_pos)
+{
+    pos1 = (double)encoder1.GetCounter();
+
+    ErreurPos1 = pcons_pos - pos1;
+    Delta_ErreurPos1 = ErreurPos1 - last_ErreurPos1;
+    Somme_ErreurPos1 += ErreurPos1;
+
+    if(Somme_ErreurPos1>1.0) {
+        Somme_ErreurPos1=1.0;
+    } else if(Somme_ErreurPos1<-1.0) {
+        Somme_ErreurPos1=-1.0;
+    }
+
+    commande1 = (Kpp1 * ErreurPos1 + Kip1 * Somme_ErreurPos1 + Kdp1 * Delta_ErreurPos1);
+
+    last_ErreurPos1 = ErreurPos1;
+    last_pos1 = pos1;
+    
+    commande1=commande1/2500.;
+    
+    if(commande1>1.0) {
+        commande1=1.0;
+    } else if(commande1<-1.0) {
+        commande1=-1.0;
+    }
+
+    //test5++;
+}
+
+void Asser_Pos_Mot2(double pcons_pos)
+{
+    pos2 = (double)encoder2.GetCounter();
+
+    ErreurPos2 = pcons_pos - pos2;
+
+    Delta_ErreurPos2 = ErreurPos2 - last_ErreurPos2;
+    Somme_ErreurPos2 += ErreurPos2;
+
+    if(Somme_ErreurPos2>1.0)
+        Somme_ErreurPos2=1.0;
+    else if(Somme_ErreurPos2<-1.0)
+        Somme_ErreurPos2=-1.0;
+
+    commande2 = (Kpp2 * ErreurPos2 + Kip2 * Somme_ErreurPos2 + Kdp2 * Delta_ErreurPos2);
+
+    last_ErreurPos2 = ErreurPos2;
+    last_pos2 = pos2;
+    
+    commande2=commande2/2500.;
+    
+    if(commande2>1.0) {
+        commande2=1.0;
+    } else if(commande2<-1.0) {
+        commande2=-1.0;
+    }
+
+    test6++;
+}
+
+
+void Avancer_ligne_droite(long pcons, short vmax, short amax, short dmax)
+{
+    //Declaration des variables
+    static double tc, ta, td;   //tc temps � vitesse constante, ta en acceleration, td en deceleration
+    static double vmax_tri;     //Vitesse maximale atteinte dans le cas d'un profil en triangle
+
+    if(pcons >= 0) {    //Mode avancer
+        switch(etat_automate_depl) { //Automate de gestion du deplacement
+            case INITIALISATION :      //Etat d'initialisation et de calcul des variables
+                //Remise a zero des variables car on effectue un nouveau deplacement
+                consigne_pos = 0;
+                consigne_vit = 0;
+                cpt = 0;
+
+                //Elaboration du profil de vitesse
+                //Calcul du temps � vitesse constante en fonction de l'acceleration
+                tc = ((double)pcons / (double)vmax) - (((double)vmax * 500.0) / (double)amax) - (((double)vmax * 500.0) / (double)dmax);
+
+                if (tc > 0) {   //Resultat positif, il y a une phase de vitesse constante
+                    etat_automate_depl = ACCELERATION_TRAPEZE;    //Passage a l'etat ACCELERATION_TRAPEZE
+
+                    ta = (((double)vmax * 1000.0) / (double)amax);      //Calcul du temps d'acceleration
+                    td = (((double)vmax * 1000.0) / (double)dmax);      //Calcul du temps de deceleration
+                }
+
+                else if(tc < 0) {   //Resultat negatif, il n'y a pas de phase de vitesse constante
+                    // Le prochain �tat de l'automate sera l'�tat 5
+                    etat_automate_depl = ACCELERATION_TRIANGLE;     //Passage a l'etat ACCELERATION_TRIANGLE
+
+                    //Calcul de la vitesse maximale en profil triangle
+                    vmax_tri = sqrt((2 * pcons) / ((1000.0 / (double)amax) + (1000.0 / (double)dmax)));
+                }
+                break;
+
+            case ACCELERATION_TRAPEZE :    //Etat d'acceleration en profil trapeze
+                cpt ++;
+
+                //Incrementation de la consigne de vitesse par la valeur de l'acceleration
+                consigne_vit += amax;
+
+                //Incrementation de la consigne de position
+                consigne_pos += ((double)consigne_vit / 1000.0);
+
+                if(cpt >= ta) { //Condition pour quitter la phase d'acceleration
+                    etat_automate_depl = VITESSE_CONSTANTE_TRAPEZE;      //Passage a l'etat VITESSE_CONSTANTE
+                    cpt = 0;
+                }
+                break;
+
+            case VITESSE_CONSTANTE_TRAPEZE :   //Etat de vitesse constante en profil trapeze
+                cpt ++;
+
+                //Incrementation de la consigne de position
+                consigne_pos += vmax;
+
+                //Si il n'y a pas d'enchainements
+                if(cpt >= tc && liste[cpt_ordre].enchainement != 1) {   //Condition pour quitter la phase de vitesse constante
+                    etat_automate_depl = DECELERATION_TRAPEZE;      //Passage a l'etat DECELERATION
+                    cpt = 0;
+                }
+
+                //Si on est dans un enchainement, on passe � l'instruction suivante
+                else if(consigne_pos >= pcons && liste[cpt_ordre].enchainement == 1) {
+                    consigne_pos = pcons;
+                    cpt_ordre = 0;
+                }
+                break;
+
+            case DECELERATION_TRAPEZE :    //Etat de deceleration en profil trapeze
+                cpt ++;
+
+                //Incrementation de la consigne de position
+                consigne_pos += vmax - (double)(((double)cpt * (double)dmax) / 1000.0);
+
+                if(cpt >= td) {     //Condition pour quitter la phase de deceleration en profil trapeze
+                    etat_automate_depl = ARRET;     //Passage a l'etat ARRET
+                    consigne_pos = pcons;
+                    cpt = 0;
+                }
+                break;
+
+            case ARRET :       //Etat d'arret
+                if(cpt >= 20) {     //Condition pour sortir de l'etat arret
+                    etat_automate_depl = INITIALISATION;        //Passage a l'etat INITIALISATION
+                    next_move = 1;
+                }
+                cpt ++;
+                break;
+
+            case ACCELERATION_TRIANGLE :        //Etat d'acceleration en profil triangle
+                cpt ++;
+
+                //Incrementation de la consigne de position
+                consigne_pos += (((double)cpt * (double)amax) / 1000.0);
+
+                if((cpt * amax / 1000.0) >= vmax_tri) { //Condition pour sortir de l'etat acceleration en profil triangle
+                    etat_automate_depl = DECELERATION_TRIANGLE;     //Passage a l'etat DECELERATION_TRIANGLE
+                    cpt = 0;
+                }
+                break;
+
+            case DECELERATION_TRIANGLE :        //Etat de deceleration en profil triangle
+                cpt ++;
+
+                //Incrementation de la consigne de position
+                consigne_pos += vmax_tri - (((double)cpt * (double)dmax) / 1000.0);
+
+                if(((double)vmax_tri - (((double)cpt * (double)dmax)) / 1000.0) <= 0) {     //Condition pour sortir de l'etat deceleration en profil triangle
+                    etat_automate_depl = ARRET;     //Passage a l'etat ARRET
+                    cpt = 0;
+                }
+                break;
+        }
+    }
+
+    else if(pcons < 0) {    //Mode reculer
+        switch(etat_automate_depl) {    //Automate de gestion du deplacement
+            case INITIALISATION :      //Etat d'initialisation et de calcul des variables
+                //Remise a zero des variables car on effectue un nouveau deplacement
+                consigne_pos = 0;
+                cpt = 0;
+
+                //Elaboration du profil de vitesse
+                //Calcul du temps � vitesse constante en fonction de l'acceleration
+                tc = ((double)-pcons / (double)vmax) - (((double)vmax * 500.0) / (double)amax) - (((double)vmax * 500.0) / (double)dmax);
+
+                if (tc > 0) {   //Resultat positif, il y a une phase de vitesse constante
+                    etat_automate_depl = ACCELERATION_TRAPEZE;      //Passage a l'etat ACCELERATION_TRAPEZE
+
+                    ta = (((double)vmax * 1000.0) / (double)amax);  //Calcul du temps d'acceleration
+                    td = (((double)vmax * 1000.0) / (double)dmax);  //Calcul du temps de deceleration
+                }
+
+                else if(tc < 0) {   //Resultat negatif, il n'y a pas de phase de vitesse constante
+                    etat_automate_depl = ACCELERATION_TRIANGLE;     //Passage a l'etat ACCELERATION_TRIANGLE
+
+                    //Calcul de la vitesse maximale en profil triangle
+                    vmax_tri = sqrt((2 * -pcons) / ((1000.0 / (double)amax) + (1000.0 / (double)dmax)));
+                }
+                break;
+
+            case ACCELERATION_TRAPEZE :        //Etat d'acceleration en profil trapeze
+                cpt ++;
+
+                //Decrementation de la consigne de position
+                consigne_pos -= (double)(((double)cpt * (double)amax)/1000.0);
+
+                if(cpt >= ta) {     //Condition pour quitter la phase d'acceleration
+                    etat_automate_depl = VITESSE_CONSTANTE_TRAPEZE;     //Passage a l'etat VITESSE_CONSTANTE_TRAPEZE
+                    cpt = 0;
+                }
+                break;
+
+            case VITESSE_CONSTANTE_TRAPEZE :       //Etat de vitesse continue en profil trapeze
+                cpt ++;
+
+                //Decrementation de la consigne de position
+                consigne_pos -= vmax;
+
+                //Si il n'y a pas d'enchainements
+                if(cpt >= tc && liste[cpt_ordre].enchainement != 1) {   //Condition pour sortir de l'etat de vitesse constante en profil trapeze
+                    etat_automate_depl = DECELERATION_TRAPEZE;      //Passage a l'etat de DECELERATION_TRAPEZE
+                    cpt = 0;
+                }
+
+                //Si on est dans un enchainement, on passe � l'instruction suivante
+                else if(consigne_pos >= pcons && liste[cpt_ordre].enchainement == 1) {
+                    consigne_pos = pcons;
+                    cpt_ordre = 0;
+                }
+                break;
+
+            case DECELERATION_TRAPEZE :        //Etat de deceleration en profil trapeze
+                cpt ++;
+
+                //Decrementation de la consigne de position
+                consigne_pos -= vmax - (double)(((double)cpt * (double)dmax) / 1000.0);
+
+                if(cpt >= td) {     //Condition pour sortir de l'etat deceleration en profil trapeze
+                    etat_automate_depl = ARRET;     //Passage a l'etat ARRET
+                    consigne_pos = pcons;
+                    cpt = 0;
+                }
+                break;
+
+            case ARRET :       //Etat d'arret
+                if(cpt >= 20) {     //Condition pour sortir de l'etat arret
+                    etat_automate_depl = INITIALISATION;        //Passage a l'etat d'INITIALISATION
+                    next_move = 1;
+                }
+                cpt ++;
+                break;
+
+            case ACCELERATION_TRIANGLE :    //Etat d'acceleration en profil triangle
+                cpt ++;
+
+                //Decrementation de la consigne de position
+                consigne_pos -= (((double)cpt * (double)amax) / 1000.0);
+
+                if((cpt * amax / 1000.0) >= vmax_tri) {     //Condition pour sortir de l'etat d'acceleration en profil triangle
+                    etat_automate_depl = DECELERATION_TRIANGLE;     //Passage a l'etat DECELERATION_TRIANGLE
+                    cpt = 0;
+                }
+                break;
+
+            case DECELERATION_TRIANGLE :        //Etat de deceleration en profil triangle
+                cpt ++;
+
+                //Decrementation de la consigne de position
+                consigne_pos -= vmax_tri - (((double)cpt * (double)dmax) / 1000.0);
+
+                if(((double)vmax_tri - (((double)cpt * (double)dmax)) / 1000.0) <= 0) {     //Condition pour sortir de l'etat de deceleration en profil triangle
+                    etat_automate_depl = ARRET;     //Passage a l'etat ARRET
+                    cpt = 0;
+                }
+                break;
+        }
+    }
+
+
+    //Calcul des commande 1 et 2
+    Asser_Pos_Mot1(roue_drt_init + consigne_pos);
+    Asser_Pos_Mot2(roue_gch_init + consigne_pos);
+
+    //Ecriture du PWM sur chaque modeur //*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*
+    write_PWM2(commande2);
+    write_PWM1(commande1);
+
+}
+
+
+void Avancer_ligne_droite_XYTheta(long pcons, short vmax, short amax, short dmax)
+{
+    //Declaration des variables
+    //char val1[8];
+    static double tc, ta, td;   //tc temps � vitesse constante, ta en acceleration, td en deceleration
+    static double vmax_tri;     //Vitesse maximale atteinte dans le cas d'un profil en triangle
+
+    if(pcons >= 0) {    //Mode avancer
+        switch(etat_automate_depl) {    //Automate de gestion du deplacement
+            case INITIALISATION :      //Etat d'initialisation et de calcul des variables
+                //remise a zero des variables car on effectue un nouveau deplacement
+                consigne_pos = 0;
+                cpt = 0;
+
+                //Elaboration du profil de vitesse
+                //Calcul du temps a vitesse constante en fonction de l'acceleration
+                tc = ((double)pcons / (double)vmax) - (((double)vmax * 500.0) / (double)amax) - (((double)vmax * 500.0) / (double)dmax);
+
+                if (tc > 0) {   //Resultat positif, il y a une phase de vitesse constante
+                    etat_automate_depl = ACCELERATION_TRAPEZE;  //Passage a l'etat ACCELERATION_TRAPEZE
+
+                    ta = (((double)vmax * 1000.0) / (double)amax);      //Calcul du temps d'acceleration
+                    td = (((double)vmax * 1000.0) / (double)dmax);      //Calcul du temps de deceleration
+                }
+
+                else if(tc < 0) {   //Resultat negatif, il n'y a pas de phase de vitesse constante
+                    etat_automate_depl = ACCELERATION_TRIANGLE;     //Passage a l'etat ACCELERATION_TRIANGLE
+
+                    //Calcul de la vitesse maximale en profil triangle
+                    vmax_tri = sqrt((2 * pcons) / ((1000.0 / (double)amax) + (1000.0 / (double)dmax)));
+                }
+                break;
+
+            case ACCELERATION_TRAPEZE :    //Etat d'acceleration en profil trapeze
+                cpt ++;
+
+                //Incrementation de la consigne de position
+                consigne_pos += (((double)cpt * (double)amax) / 1000.0);
+
+                if(cpt >= ta) {     //Condition pour quitter la phase d'acceleration
+                    etat_automate_depl = VITESSE_CONSTANTE_TRAPEZE;     //Passage a l'etat VITESSE_CONSTANTE_TRAPEZE
+                    cpt = 0;
+                }
+                break;
+
+            case VITESSE_CONSTANTE_TRAPEZE :       //Etat de vitesse constante en profil trapeze
+                cpt ++;
+
+                //Incrementation de la consigne de vitesse par la valeur de l'acceleration
+                consigne_pos += vmax;
+
+                //Si il n'y a pas d'enchainements
+                if(cpt >= tc && liste[cpt_ordre].enchainement != 1) {   //Condition pour quitter la phase de vitesse constante
+                    etat_automate_depl = DECELERATION_TRAPEZE;      //Passage a l'etat DECELERATION_TRAPEZE
+                    cpt = 0;
+                }
+                //Si on est dans un enchainement, on passe � l'instruction suivante
+                else if(consigne_pos >= pcons && liste[cpt_ordre].enchainement == 1) {
+                    consigne_pos = pcons;
+                    cpt_ordre = 0;
+                }
+                break;
+
+            case DECELERATION_TRAPEZE :    //Etat de vitesse constante en profil trapeze
+                cpt ++;
+
+                //Incrementation de la consigne de position
+                consigne_pos += vmax - (double)(((double)cpt * (double)dmax) / 1000.0);
+
+                if(cpt >= td) {     //Condition pour quitter la phase de deceleration en profil trapeze
+                    etat_automate_depl = ARRET;     //Passage a l'etat ARRET
+                    consigne_pos = pcons;
+                    cpt = 0;
+                }
+                break;
+
+            case ARRET :       //Etat d'arret
+                if(cpt >= 20) {
+                    etat_automate_depl = INITIALISATION;    //Passage a l'etat d'INITIALISATION
+                    next_move_xyt = 1;
+
+                }
+                cpt ++;
+                break;
+
+            case ACCELERATION_TRIANGLE :        //Etat d'acceleration en profil triangle
+                cpt ++;
+
+                //Incrementation de la consigne de position
+                consigne_pos += (((double)cpt * (double)amax) / 1000.0);
+
+                if((cpt * amax / 1000.0) >= vmax_tri) {     //Condition pour sortir de la phase d'acceleration en profil triangle
+                    etat_automate_depl = DECELERATION_TRIANGLE;     //Passage a l'etat DECELERATION_TRIANGLE
+                    cpt = 0;
+                }
+                break;
+
+            case DECELERATION_TRIANGLE :        //Etat de deceleration en profil triangle
+                cpt ++;
+
+                //Incrementation de la consigne de position
+                consigne_pos += vmax_tri - (((double)cpt * (double)dmax) / 1000.0);
+
+                if(((double)vmax_tri - (((double)cpt * (double)dmax)) / 1000.0) <= 0) {     //Condition pour sortir de la phase de deceleration en profil triangle
+                    etat_automate_depl = ARRET;     //Passage a l'etat ARRET
+                    cpt = 0;
+                }
+                break;
+        }
+    } else if(pcons < 0) {  //Mode reculer
+        switch(etat_automate_depl) {    //Automate de gestion du deplacement
+            case INITIALISATION :      //Etat d'initialisation et de calcul des variables
+                //Remise a zero des variables car on effectue un nouveau deplacement
+                consigne_pos = 0;
+                cpt = 0;
+
+                //Elaboration du profil de vitesse
+                //Calcul du temps � vitesse constante en fonction de l'acceleration
+                tc = ((double)-pcons / (double)vmax) - (((double)vmax * 500.0) / (double)amax) - (((double)vmax * 500.0) / (double)dmax);
+
+                if (tc > 0) {   //Resultat positif, il y a une phase de vitesse constante
+                    etat_automate_depl = ACCELERATION_TRAPEZE;      //Passage a l'etat ACCELERATION_TRAPEZE
+
+                    ta = (((double)vmax * 1000.0) / (double)amax);      //Calcul du temps d'acceleration
+                    td = (((double)vmax * 1000.0) / (double)dmax);      //Calcul du temps de deceleration
+                } else if(tc < 0) { //Resultat negatif, il n'y a pas de phase de vitesse constante
+                    etat_automate_depl = ACCELERATION_TRIANGLE;     //Passage a l'etat ACCELERATION_TRIANGLE
+
+                    //Calcul de la vitesse maximale en profil triangle
+                    vmax_tri = sqrt((2 * -pcons) / ((1000.0 / (double)amax) + (1000.0 / (double)dmax)));
+                }
+                break;
+
+            case ACCELERATION_TRAPEZE :    //Etat d'acceleration en profil trapeze
+                cpt ++;
+
+                //Decrementation de la consigne de position
+                consigne_pos -= (double)(((double)cpt * (double)amax)/1000.0);
+
+                if(cpt >= ta) { //Condition pour quitter la phase d'acceleration
+                    etat_automate_depl = VITESSE_CONSTANTE_TRAPEZE;     //Passage a l'etat VITESSE_CONSTANTE_TRAPEZE
+                    cpt = 0;
+                }
+                break;
+
+            case VITESSE_CONSTANTE_TRAPEZE :       //Etat de vitesse continue en profil trapeze
+                cpt ++;
+
+                //Decrementation de la consigne de position
+                consigne_pos -= vmax;
+
+                //Si il n'y a pas d'enchainements
+                if(cpt >= tc && liste[cpt_ordre].enchainement != 1) {   //Condition pour sortir de l'etat de vitesse constante en profil trapeze
+                    etat_automate_depl = DECELERATION_TRAPEZE;      //Passage a l'etat de DECELERATION_TRAPEZE
+                    cpt = 0;
+                }
+
+                //Si on est dans un enchainement, on passe � l'instruction suivante
+                else if(consigne_pos >= pcons && liste[cpt_ordre].enchainement == 1) {
+                    consigne_pos = pcons;
+                    cpt_ordre = 0;
+                }
+                break;
+
+            case DECELERATION_TRAPEZE :        //Etat de deceleration en profil trapeze
+                cpt ++;
+
+                //Decrementation de la consigne de position
+                consigne_pos -= vmax - (double)(((double)cpt * (double)dmax) / 1000.0);
+
+                if(cpt >= td) {     //Condition pour sortir de l'etat vitesse continue en profil trapeze
+                    etat_automate_depl = ARRET;     //Passage a l'etat ARRET
+                    consigne_pos = pcons;
+                    cpt = 0;
+                }
+                break;
+
+            case ARRET :       //Etat d'arret
+                if(cpt >= 20) {
+                    etat_automate_depl = INITIALISATION;        //Passage a l'etat INITIALISATION
+                    next_move_xyt = 1;
+                }
+                cpt ++;
+                break;
+
+            case ACCELERATION_TRIANGLE :        //Etat d'acceleration en profil triangle
+                cpt ++;
+
+                //Decrementation de la consigne de position
+                consigne_pos -= (((double)cpt * (double)amax) / 1000.0);
+
+                if((cpt * amax / 1000.0) >= vmax_tri) {     //Condition pour sortir de la phase d'acceleration en profil triangle
+                    etat_automate_depl = DECELERATION_TRIANGLE;     //Passage a l'etat DECELERATION_TRIANGLE
+                    cpt = 0;
+                }
+                break;
+
+            case DECELERATION_TRIANGLE :        //Etat de deceleration en profil triangle
+                cpt ++;
+
+                //Decrementation de la consigne de position
+                consigne_pos -= vmax_tri - (((double)cpt * (double)dmax) / 1000.0);
+
+                if(((double)vmax_tri - (((double)cpt * (double)dmax)) / 1000.0) <= 0) {     //Condition pour sortir de l'etat de deceleration en profil triangle
+                    etat_automate_depl = ARRET;     //Passage a l'etat ARRET
+                    cpt = 0;
+                }
+                break;
+        }
+    }
+
+    //Calcul des commande 1 et 2
+    Asser_Pos_Mot1(roue_drt_init + consigne_pos);
+    Asser_Pos_Mot2(roue_gch_init + consigne_pos);
+
+    //Ecriture du PWM sur chaque modeur //*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*
+    write_PWM2(commande2);
+    write_PWM1(commande1);
+
+}
+
+
+void Rotation(long pcons, short vmax, short amax, short dmax)
+{
+    //Declaration des variables
+    //char val1[8];
+    static double tc, ta, td;   //tc temps � vitesse constante, ta en acceleration, td en deceleration
+    static double vmax_tri;     //Vitesse maximale atteinte dans le cas d'un profil en triangle
+
+    if(pcons >= 0) {    //Mode avancer
+        switch(etat_automate_depl) {    //Automate de gestion de deplacement
+            case INITIALISATION :      //Etat d'initialisation et de calcul des variables
+                //remise a zero des variables car on effectue un nouveau deplacement
+                consigne_pos = 0;
+                cpt = 0;
+
+                //Elaboration du profil de vitesse
+                //Calcul du temps a vitesse constante en fonction de l'acceleration
+                tc = ((double)pcons / (double)vmax) - (((double)vmax * 500.0) / (double)amax) - (((double)vmax * 500.0) / (double)dmax);
+
+                if (tc > 0) {   //Resultat positif, il y a une phase de vitesse constante
+                    etat_automate_depl = ACCELERATION_TRAPEZE;      //Passage a l'etat AACELERATION_TRAPEZE
+
+                    ta = (((double)vmax * 1000.0) / (double)amax);      //Calcul du temps d'acceleration
+                    td = (((double)vmax * 1000.0) / (double)dmax);      //Calcul du temps de deceleration
+                }
+
+                else if(tc < 0) {   //Resultat negatif, il n'y a pas de phase de vitesse constante
+                    etat_automate_depl = ACCELERATION_TRIANGLE;     //Passage a l'etat ACCELERATION_TRIANGLE
+
+                    //Calcul de la vitesse maximale en profil triangle
+                    vmax_tri = sqrt((2 * pcons) / ((1000.0 / (double)amax) + (1000.0 / (double)dmax)));
+                }
+                break;
+
+            case ACCELERATION_TRAPEZE :    //Etat d'acceleration en profil trapeze
+                cpt ++;
+
+                //Incrementation de la consigne de position
+                consigne_pos += (((double)cpt * (double)amax) / 1000.0);
+
+                if(cpt >= ta) {     //Condition pour quitter la phase d'acceleration
+                    etat_automate_depl = VITESSE_CONSTANTE_TRAPEZE;     //Passage a l'etat VITESSE_CONSTANTE_TRAPEZE
+                    cpt = 0;
+                }
+                break;
+
+            case VITESSE_CONSTANTE_TRAPEZE :       //Etat de vitesse constante en profil trapeze
+                cpt ++;
+
+                //Incrementation de la consigne de position
+                consigne_pos += vmax;
+
+                //Si il n'y a pas d'enchainements
+                if(cpt >= tc && liste[cpt_ordre].enchainement != 1) {
+                    etat_automate_depl = DECELERATION_TRAPEZE;      //Passage a l'etat DECELERATION_TRAPEZE
+                    cpt = 0;
+                }
+
+                //Si on est dans un enchainement, on passe � l'instruction suivante
+                else if(consigne_pos >= pcons && liste[cpt_ordre].enchainement == 1) {
+                    consigne_pos = pcons;
+                    cpt_ordre = 0;
+                }
+                break;
+
+            case DECELERATION_TRAPEZE :        //Etat de deceleration en profil trapeze
+                cpt ++;
+
+                //Incrementation de la consigne de position
+                consigne_pos += vmax - (double)(((double)cpt * (double)dmax) / 1000.0);
+
+                if(cpt >= td) {     //Condition pour quitter la phase de deceleration en profil trapeze
+                    etat_automate_depl = ARRET;     //Passage a l'etat ARRET
+                    consigne_pos = pcons;
+                    cpt = 0;
+                }
+                break;
+
+            case ARRET :       //Etat d'arret
+                if(cpt >= 20) {
+                    etat_automate_depl = INITIALISATION;        //Passage a l'etat INITIALISATION
+                    next_move = 1;
+                }
+                cpt ++;
+                break;
+
+            case ACCELERATION_TRIANGLE :        //Etat d'acceleration en profil triangle
+                cpt ++;
+
+                //Incrementation de la consigne de position
+                consigne_pos += (((double)cpt * (double)amax) / 1000.0);    //Condition pour sortir de la phase d'acceleration en profil triangle
+
+                if((cpt * amax / 1000.0) >= vmax_tri) {
+                    etat_automate_depl = DECELERATION_TRIANGLE;     //Passage a l'etat DECELERATION_TRIANGLE
+                    cpt = 0;
+                }
+                break;
+
+            case DECELERATION_TRIANGLE :        //Etat de deceleration en profil triangle
+                cpt ++;
+
+                //Incrementation de la consigne de position
+                consigne_pos += vmax_tri - (((double)cpt * (double)dmax) / 1000.0);
+
+                if(((double)vmax_tri - (((double)cpt * (double)dmax)) / 1000.0) <= 0) {     //Condition pour sortir de la phase de deceleration en profil triangle
+                    etat_automate_depl = ARRET;     //Passage a l'etat ARRET
+                    cpt = 0;
+                }
+                break;
+        }
+    } else if(pcons < 0) {  //Mode reculer
+        switch(etat_automate_depl) {    //Automate de gestion du deplacement
+            case INITIALISATION :      //Etat d'initialisation et de calcul des variables
+                //Remise a zero des variables car on effectue un nouveau deplacement
+                consigne_pos = 0;
+                cpt = 0;
+
+                //Elaboration du profil de vitesse
+                //Calcul du temps � vitesse constante en fonction de l'acceleration
+                tc = ((double)-pcons / (double)vmax) - (((double)vmax * 500.0) / (double)amax) - (((double)vmax * 500.0) / (double)dmax);
+
+                if (tc > 0) {   //Resultat positif, il y a une phase de vitesse constante
+                    etat_automate_depl = ACCELERATION_TRAPEZE;      //Passage a l'etat ACCELERATION_TRAPEZE
+
+                    ta = (((double)vmax * 1000.0) / (double)amax);      //Calcul du temps d'acceleration
+                    td = (((double)vmax * 1000.0) / (double)dmax);      //Calcul du temps de deceleration
+                }
+
+                else if(tc < 0) {   //Resultat negatif, il n'y a pas de phase de vitesse constante
+                    etat_automate_depl = ACCELERATION_TRIANGLE;     //Passage a l'etat ACCELERATION_TRIANGLE
+
+                    //Calcul de la vitesse maximale en profil triangle
+                    vmax_tri = sqrt((2 * -pcons) / ((1000.0 / (double)amax) + (1000.0 / (double)dmax)));
+                }
+                break;
+
+            case ACCELERATION_TRAPEZE :    //Etat d'acceleration en profil trapeze
+                cpt ++;
+
+                //Decrementation de la consigne de position
+                consigne_pos -= (double)(((double)cpt * (double)amax)/1000.0);
+
+                if(cpt >= ta) { //Condition pour quitter la phase d'acceleration
+                    etat_automate_depl = VITESSE_CONSTANTE_TRAPEZE;     //Passage a l'etat VITESSE_CONSTANTE_TRAPEZE
+                    cpt = 0;
+                }
+                break;
+
+            case VITESSE_CONSTANTE_TRAPEZE :       //Etat de vitesse continue en profil trapeze
+                cpt ++;
+
+                //Decrementation de la consigne de position
+                consigne_pos -= vmax;
+
+                //Si il n'y a pas d'enchainements
+                if(cpt >= tc && liste[cpt_ordre].enchainement != 1) {
+                    etat_automate_depl = DECELERATION_TRAPEZE;      //Passage a l'etat de DECELERATION_TRAPEZE
+                    cpt = 0;
+                }
+
+                //Si on est dans un enchainement, on passe � l'instruction suivante
+                else if(consigne_pos >= pcons && liste[cpt_ordre].enchainement == 1) {
+                    consigne_pos = pcons;
+                    cpt_ordre = 0;
+                }
+                break;
+
+            case DECELERATION_TRAPEZE :    //Etat de deceleration en profil trapeze
+                cpt ++;
+
+                //Decrementation de la consigne de position
+                consigne_pos -= vmax - (double)(((double)cpt * (double)dmax) / 1000.0);
+
+                if(cpt >= td) {     //Condition pour sortir de l'etat vitesse continue en profil trapeze
+                    etat_automate_depl = ARRET;     //Passage a l'etat ARRET
+                    consigne_pos = pcons;
+                    cpt = 0;
+                }
+                break;
+
+            case ARRET :   //Etat d'arret
+                if(cpt >= 20) {
+                    etat_automate_depl = INITIALISATION;        //Passage a l'etat INITIALISATION
+                    next_move = 1;
+                }
+                cpt ++;
+                break;
+
+            case ACCELERATION_TRIANGLE :        //Etat d'acceleration en profil triangle
+                cpt ++;
+
+                //Decrementation de la consigne de position
+                consigne_pos -= (((double)cpt * (double)amax) / 1000.0);
+
+                if((cpt * amax / 1000.0) >= vmax_tri) {     //Condition pour sortir de la phase d'acceleration en profil triangle
+                    etat_automate_depl = DECELERATION_TRIANGLE;     //Passage a l'etat DECELERATION_TRIANGLE
+                    cpt = 0;
+                }
+                break;
+
+            case DECELERATION_TRIANGLE :        //Etat de deceleration en profil triangle
+                cpt ++;
+
+                //Decrementation de la consigne de position
+                consigne_pos -= vmax_tri - (((double)cpt * (double)dmax) / 1000.0);
+
+                if(((double)vmax_tri - (((double)cpt * (double)dmax)) / 1000.0) <= 0) { //Condition pour sortir de l'etat de deceleration en profil triangle
+                    etat_automate_depl = ARRET;     //Passage a l'etat ARRET
+                    cpt = 0;
+                }
+                break;
+        }
+    }
+
+    //Calcul des commande 1 et 2
+    Asser_Pos_Mot1(roue_drt_init + consigne_pos);
+    Asser_Pos_Mot2(roue_gch_init - consigne_pos);
+
+    //Ecriture du PWM sur chaque modeur    /*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*
+    write_PWM2(commande2);
+    write_PWM1(commande1);
+}
+
+void Rotation_XYTheta(long pcons, short vmax, short amax, short dmax)
+{
+    //Declaration des variables
+    //char val1[8];
+    static double tc, ta, td;   //tc temps � vitesse constante, ta en acceleration, td en deceleration
+    static double vmax_tri;     //Vitesse maximale atteinte dans le cas d'un profil en triangle
+
+    if(pcons >= 0) {    //Mode avancer
+        switch(etat_automate_depl) {    //Automate de gestion de deplacement
+            case INITIALISATION :      //Etat d'initialisation et de calcul des variables
+                //remise a zero des variables car on effectue un nouveau deplacement
+                consigne_pos = 0;
+                cpt = 0;
+
+                //Elaboration du profil de vitesse
+                //Calcul du temps a vitesse constante en fonction de l'acceleration
+                tc = ((double)pcons / (double)vmax) - (((double)vmax * 500.0) / (double)amax) - (((double)vmax * 500.0) / (double)dmax);
+
+                if (tc > 0) {   //Resultat positif, il y a une phase de vitesse constante
+                    etat_automate_depl = ACCELERATION_TRAPEZE;      //Passage a l'etat AACELERATION_TRAPEZE
+
+                    ta = (((double)vmax * 1000.0) / (double)amax);      //Calcul du temps d'acceleration
+                    td = (((double)vmax * 1000.0) / (double)dmax);      //Calcul du temps de deceleration
+                }
+
+                else if(tc < 0) {   //Resultat negatif, il n'y a pas de phase de vitesse constante
+                    etat_automate_depl = ACCELERATION_TRIANGLE;     //Passage a l'etat ACCELERATION_TRIANGLE
+
+                    //Calcul de la vitesse maximale en profil triangle
+                    vmax_tri = sqrt((2 * pcons) / ((1000.0 / (double)amax) + (1000.0 / (double)dmax)));
+                }
+                break;
+
+            case ACCELERATION_TRAPEZE :    //Etat d'acceleration en profil trapeze
+                cpt ++;
+
+                //Incrementation de la consigne de position
+                consigne_pos += (((double)cpt * (double)amax) / 1000.0);
+
+                if(cpt >= ta) {     //Condition pour quitter la phase d'acceleration
+                    etat_automate_depl = VITESSE_CONSTANTE_TRAPEZE;     //Passage a l'etat VITESSE_CONSTANTE_TRAPEZE
+                    cpt = 0;
+                }
+                break;
+
+            case VITESSE_CONSTANTE_TRAPEZE :       //Etat de vitesse constante en profil trapeze
+                cpt ++;
+
+                //Incrementation de la consigne de position
+                consigne_pos += vmax;
+
+                //Si il n'y a pas d'enchainements
+                if(cpt >= tc && liste[cpt_ordre].enchainement != 1) {
+                    etat_automate_depl = DECELERATION_TRAPEZE;      //Passage a l'etat DECELERATION_TRAPEZE
+                    cpt = 0;
+                }
+
+                //Si on est dans un enchainement, on passe � l'instruction suivante
+                else if(consigne_pos >= pcons && liste[cpt_ordre].enchainement == 1) {
+                    consigne_pos = pcons;
+                    cpt_ordre = 0;
+                }
+                break;
+
+            case DECELERATION_TRAPEZE :        //Etat de deceleration en profil trapeze
+                cpt ++;
+
+                //Incrementation de la consigne de position
+                consigne_pos += vmax - (double)(((double)cpt * (double)dmax) / 1000.0);
+
+                if(cpt >= td) {     //Condition pour quitter la phase de deceleration en profil trapeze
+                    etat_automate_depl = ARRET;     //Passage a l'etat ARRET
+                    consigne_pos = pcons;
+                    cpt = 0;
+                }
+                break;
+
+            case ARRET :       //Etat d'arret
+                if(cpt >= 20) {
+                    etat_automate_depl = INITIALISATION;        //Passage a l'etat INITIALISATION
+                    next_move_xyt = 1;
+                }
+                cpt ++;
+                break;
+
+            case ACCELERATION_TRIANGLE :        //Etat d'acceleration en profil triangle
+                cpt ++;
+
+                //Incrementation de la consigne de position
+                consigne_pos += (((double)cpt * (double)amax) / 1000.0);    //Condition pour sortir de la phase d'acceleration en profil triangle
+
+                if((cpt * amax / 1000.0) >= vmax_tri) {
+                    etat_automate_depl = DECELERATION_TRIANGLE;     //Passage a l'etat DECELERATION_TRIANGLE
+                    cpt = 0;
+                }
+                break;
+
+            case DECELERATION_TRIANGLE :        //Etat de deceleration en profil triangle
+                cpt ++;
+
+                //Incrementation de la consigne de position
+                consigne_pos += vmax_tri - (((double)cpt * (double)dmax) / 1000.0);
+
+                if(((double)vmax_tri - (((double)cpt * (double)dmax)) / 1000.0) <= 0) {     //Condition pour sortir de la phase de deceleration en profil triangle
+                    etat_automate_depl = ARRET;     //Passage a l'etat ARRET
+                    cpt = 0;
+                }
+                break;
+        }
+    } else if(pcons < 0) {  //Mode reculer
+        switch(etat_automate_depl) {    //Automate de gestion du deplacement
+            case INITIALISATION :      //Etat d'initialisation et de calcul des variables
+                //Remise a zero des variables car on effectue un nouveau deplacement
+                consigne_pos = 0;
+                cpt = 0;
+
+                //Elaboration du profil de vitesse
+                //Calcul du temps � vitesse constante en fonction de l'acceleration
+                tc = ((double)-pcons / (double)vmax) - (((double)vmax * 500.0) / (double)amax) - (((double)vmax * 500.0) / (double)dmax);
+
+                if (tc > 0) {   //Resultat positif, il y a une phase de vitesse constante
+                    etat_automate_depl = ACCELERATION_TRAPEZE;      //Passage a l'etat ACCELERATION_TRAPEZE
+
+                    ta = (((double)vmax * 1000.0) / (double)amax);      //Calcul du temps d'acceleration
+                    td = (((double)vmax * 1000.0) / (double)dmax);      //Calcul du temps de deceleration
+                }
+
+                else if(tc < 0) {   //Resultat negatif, il n'y a pas de phase de vitesse constante
+                    etat_automate_depl = ACCELERATION_TRIANGLE;     //Passage a l'etat ACCELERATION_TRIANGLE
+
+                    //Calcul de la vitesse maximale en profil triangle
+                    vmax_tri = sqrt((2 * -pcons) / ((1000.0 / (double)amax) + (1000.0 / (double)dmax)));
+                }
+                break;
+
+            case ACCELERATION_TRAPEZE :    //Etat d'acceleration en profil trapeze
+                cpt ++;
+
+                //Decrementation de la consigne de position
+                consigne_pos -= (double)(((double)cpt * (double)amax)/1000.0);
+
+                if(cpt >= ta) { //Condition pour quitter la phase d'acceleration
+                    etat_automate_depl = VITESSE_CONSTANTE_TRAPEZE;     //Passage a l'etat VITESSE_CONSTANTE_TRAPEZE
+                    cpt = 0;
+                }
+                break;
+
+            case VITESSE_CONSTANTE_TRAPEZE :       //Etat de vitesse continue en profil trapeze
+                cpt ++;
+
+                //Decrementation de la consigne de position
+                consigne_pos -= vmax;
+
+                //Si il n'y a pas d'enchainements
+                if(cpt >= tc && liste[cpt_ordre].enchainement != 1) {
+                    etat_automate_depl = DECELERATION_TRAPEZE;      //Passage a l'etat de DECELERATION_TRAPEZE
+                    cpt = 0;
+                }
+
+                //Si on est dans un enchainement, on passe � l'instruction suivante
+                else if(consigne_pos >= pcons && liste[cpt_ordre].enchainement == 1) {
+                    consigne_pos = pcons;
+                    cpt_ordre = 0;
+                }
+                break;
+
+            case DECELERATION_TRAPEZE :    //Etat de deceleration en profil trapeze
+                cpt ++;
+
+                //Decrementation de la consigne de position
+                consigne_pos -= vmax - (double)(((double)cpt * (double)dmax) / 1000.0);
+
+                if(cpt >= td) {     //Condition pour sortir de l'etat vitesse continue en profil trapeze
+                    etat_automate_depl = ARRET;     //Passage a l'etat ARRET
+                    consigne_pos = pcons;
+                    cpt = 0;
+                }
+                break;
+
+            case ARRET :   //Etat d'arret
+                if(cpt >= 20) {
+                    etat_automate_depl = INITIALISATION;        //Passage a l'etat INITIALISATION
+                    next_move_xyt = 1;
+                }
+                cpt ++;
+                break;
+
+            case ACCELERATION_TRIANGLE :        //Etat d'acceleration en profil triangle
+                cpt ++;
+
+                //Decrementation de la consigne de position
+                consigne_pos -= (((double)cpt * (double)amax) / 1000.0);
+
+                if((cpt * amax / 1000.0) >= vmax_tri) {     //Condition pour sortir de la phase d'acceleration en profil triangle
+                    etat_automate_depl = DECELERATION_TRIANGLE;     //Passage a l'etat DECELERATION_TRIANGLE
+                    cpt = 0;
+                }
+                break;
+
+            case DECELERATION_TRIANGLE :        //Etat de deceleration en profil triangle
+                cpt ++;
+
+                //Decrementation de la consigne de position
+                consigne_pos -= vmax_tri - (((double)cpt * (double)dmax) / 1000.0);
+
+                if(((double)vmax_tri - (((double)cpt * (double)dmax)) / 1000.0) <= 0) { //Condition pour sortir de l'etat de deceleration en profil triangle
+                    etat_automate_depl = ARRET;     //Passage a l'etat ARRET
+                    cpt = 0;
+                }
+                break;
+        }
+    }
+
+    //Calcul des commande 1 et 2
+    Asser_Pos_Mot1(roue_drt_init + consigne_pos);
+    Asser_Pos_Mot2(roue_gch_init - consigne_pos);
+
+    //Ecriture du PWM sur chaque modeur //*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*
+    write_PWM2(commande2);
+    write_PWM1(commande1);
+
+}
+
+void X_Y_Theta(long px, long py, long ptheta, long sens, short vmax, short amax)
+{
+    //Variables pr�sentes Odo_x, Odo_y : position de d�part
+    //Declaration des variables
+    static short dist = 0, ang1 = 0, ang2 = 0;
+    //shar val[4];
+
+    switch(etat_automate_xytheta) {
+        case INIT_X_Y_THETA :   //etat INIT_X_Y_THETA
+            //Mode Avancer
+            if(sens >= 0) {
+                /*pour avancer, on tourne dans le sens horaire//MOI*\*/
+
+                // Son hypoth�nuse correspond � la distance � parcourir
+                dist = (short)sqrt((px - Odo_x)*(px - Odo_x)+(py - Odo_y)*(py - Odo_y));
+
+                // la 1ere rotation correspond � l'angle du triangle, moins l'angle de la position de d�part
+                // C'est-�-dire la tangente du c�t� oppos� sur l'angle adjacent
+                // La fonction atan2 fait le calcul de la tangente en radians, entre Pi et -Pi
+                // On rajoute des coefficients pour passer en degr�s
+                // On ajoute 7200 dixi�mes de degr�s pour �tre s�rs que le r�sultat soit positif
+                ang1 = (short)((atan2((double)(py - Odo_y), (double)(px - Odo_x)) * 1800 / PI) - Odo_theta + 7200) % 3600;
+
+                // On passe le r�sultat entre -1800 et 1800
+                if(ang1 > 1800) ang1 = (ang1 - 3600);
+
+                // La 2� rotation correspond � l'angle de destination, moins l'angle � la fin de la ligne droite,
+                // donc le m�me qu'� la fin de la 1�re rotation, donc l'angle de d�part plus la premi�re rotation
+                // On ajoute 3600 pour �tre s�r d'avoir un r�sultat positif
+                ang2 = (short)(ptheta - ang1 - Odo_theta + 3600) % 3600;
+
+                // On passe le r�sultat entre -1800 et 1800
+                if(ang2 > 1800) ang2 = (ang2 - 3600);
+
+                // On transforme les r�sultats en distance et angles utilisables avec les fonctions d�j� d�finies
+                dist = dist * RESOLUTION_ROUE_CODEUSE / PERIMETRE_ROUE_CODEUSE;
+                ang1 = LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE * ang1 / (3600 * PERIMETRE_ROUE_CODEUSE);
+                ang2 = LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE * ang2 / (3600 * PERIMETRE_ROUE_CODEUSE);
+            }
+
+            //Mode Reculer
+            else if(sens < 0) {
+
+                /*pour avancer, on tourne dans le sens anti-horaire//MOI*\*/
+
+                // Idem qu'au-dessus, mais laligne droite doit �tre faite en marche arri�re
+                // La distance est l'oppos� de celle calcul�e au dessus
+                // Les angles sont les m�mes � 1800 pr�s
+                dist = -(short)sqrt((px-Odo_x)*(px-Odo_x)+(py-Odo_y)*(py-Odo_y));
+
+                //Premiere rotation
+                ang1 = (short)(((atan2((double)(py - Odo_y), (double)(px - Odo_x)) * 1800 / PI) - Odo_theta) + 5400) % 3600;
+                if(ang1 > 1800) ang1 = (ang1 - 3600);
+
+                //Deuxieme rotation
+                ang2 = (short)(ptheta - ang1 - Odo_theta + 3600) % 3600;
+                if(ang2 > 1800) ang2 = (ang2 - 3600);
+
+                // On transforme les r�sultats en distance et angles utilisables avec les fonctions d�j� d�finies
+                dist = dist * RESOLUTION_ROUE_CODEUSE / PERIMETRE_ROUE_CODEUSE;
+                ang1 = LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE * ang1 / (3600 * PERIMETRE_ROUE_CODEUSE);
+                ang2 = LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE * ang2 / (3600 * PERIMETRE_ROUE_CODEUSE);
+            }
+
+            param_xytheta[0] = ang1;
+            param_xytheta[1] = dist;
+            param_xytheta[2] = ang2;
+
+            // La fonction xyth�ta utilise un automate propre, similaire � l'automate g�n�ral
+            etat_automate_xytheta = 1;
+            etat_automate_depl = ROTATION_X_Y_THETA_1;  //Passage a l'etat ROTATION_X_Y_THETA_1
+            break;
+
+        case ROTATION_X_Y_THETA_1 : //etat ROTATION_X_Y_THETA_1
+            //Execution de la fonction de rotation pour la fonction de deplacement X_Y_THETA
+            Rotation_XYTheta(ang1, vmax, amax, DMAX);
+
+            if(next_move_xyt) {
+                roue_drt_init = encoder1.GetCounter();
+
+                //#elif GROS_ROB == 0
+                roue_gch_init = COEF_ROUE_GAUCHE * (double)encoder2.GetCounter();
+                //#endif
+
+                next_move_xyt = 0;
+                etat_automate_xytheta = LIGNE_DROITE_X_Y_THETA; //Passage a l'etat LIGNE_DROITE_X_Y_THETA
+            }
+            break;
+
+        case LIGNE_DROITE_X_Y_THETA :   //etat LIGNE_DROITE_X_Y_THETA_1
+            //Execution de la fonction de ligne droite pour la fonction de deplacement X_Y_THETA
+            Avancer_ligne_droite_XYTheta(dist, vmax, amax, DMAX);
+
+            if(next_move_xyt) {
+                roue_drt_init = encoder1.GetCounter();
+
+                //#elif GROS_ROB == 0
+                roue_gch_init = COEF_ROUE_GAUCHE * (double)encoder2.GetCounter();
+                //#endif
+                next_move_xyt = 0;
+                etat_automate_xytheta = ROTATION_X_Y_THETA_2;   //Passage a l'etat ROTATION_X_Y_THETA_2
+            }
+            break;
+
+        case ROTATION_X_Y_THETA_2 : //etat ROTATION_X_Y_THETA_2
+            //Execution de la fonction de rotation pour la fonction de deplacement X_Y_THETA
+            Rotation_XYTheta(ang2, vmax, amax, DMAX);
+            if(next_move_xyt) {
+                roue_drt_init = encoder1.GetCounter();
+
+                //#elif GROS_ROB == 0
+                roue_gch_init = COEF_ROUE_GAUCHE * (double)encoder2.GetCounter();
+                //#endif
+
+                next_move_xyt = 0;
+                etat_automate_xytheta = INIT_X_Y_THETA; //Passage a l'etat INIT_X_Y_THETA
+                next_move = 1;
+            }
+            break;
+
+        default :   //Etat par defaut, on fait la meme chose que dans l'etat d'initialisation
+            //Mode Avancer
+            if(sens >= 0) {
+                /*pour avancer, on tourne dans le sens horaire//MOI*\*/
+
+                dist = (short)sqrt((px - Odo_x)*(px - Odo_x)+(py - Odo_y)*(py - Odo_y));
+                ang1 = (short)((atan2((double)(py - Odo_y), (double)(px - Odo_x)) * 1800 / PI) - Odo_theta + 7200) % 3600;
+                if(ang1 > 1800)
+                    ang1 = (ang1 - 3600);
+                ang2 = (short)(ptheta - ang1 - Odo_theta + 3600) % 3600;
+                if(ang2 > 1800)
+                    ang2 = (ang2 - 3600);
+
+                dist = dist * RESOLUTION_ROUE_CODEUSE / PERIMETRE_ROUE_CODEUSE;
+                ang1 = LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE * ang1 / (3600 * PERIMETRE_ROUE_CODEUSE);
+                ang2 = LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE * ang2 / (3600 * PERIMETRE_ROUE_CODEUSE);
+            }
+
+            //Mode Reculer
+            else if(sens < 0) {
+                /*pour avancer, on tourne dans le sens anti-horaire//MOI*\*/
+                dist = -(short)sqrt((px-Odo_x)*(px-Odo_x)+(py-Odo_y)*(py-Odo_y));
+                ang1 = (short)(((atan2((double)(py - Odo_y), (double)(px - Odo_x)) * 1800 / PI) - Odo_theta) + 5400) % 3600;
+                if(ang1 > 1800)
+
+                    ang1 = (ang1 - 3600);
+                ang2 = (short)(ptheta - ang1 - Odo_theta + 3600) % 3600;
+                if(ang2 > 1800)
+                    ang2 = (ang2 - 3600);
+
+                dist = dist * RESOLUTION_ROUE_CODEUSE / PERIMETRE_ROUE_CODEUSE;
+                ang1 = LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE * ang1 / (3600 * PERIMETRE_ROUE_CODEUSE);
+                ang2 = LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE * ang2 / (3600 * PERIMETRE_ROUE_CODEUSE);
+            }
+
+            param_xytheta[0] = ang1;
+            param_xytheta[1] = dist;
+            param_xytheta[2] = ang2;
+
+            etat_automate_xytheta = 1;
+            etat_automate_depl = INIT_X_Y_THETA;
+            break;
+    }
+}
+
+void Odometrie(void)  //void Odometrie(void)
+{
+    //Declaration des variables
+    //char val1[8];
+    double dist, ang;   //distance, angle
+
+    //Recuperation des valeurs des compteurs des encodeurs
+    Odo_val_pos_1 = (double)encoder1.GetCounter();
+
+    /* MOI*\
+    //#elif GROS_ROB == 0
+    Odo_val_pos_2 = COEF_ROUE_GAUCHE * (double)encoder2.GetCounter();
+    //#endif
+    */
+    Odo_val_pos_2 = COEF_ROUE_GAUCHE * (double)encoder2.GetCounter();
+
+
+    //Calcul de la distance parcourue
+    dist = 0.5*((Odo_val_pos_1 - Odo_last_val_pos_1) + (Odo_val_pos_2 - Odo_last_val_pos_2));
+
+    //Calcul de la valeur de l'angle parcouru
+    ang = ((Odo_val_pos_1 - Odo_last_val_pos_1) -(Odo_val_pos_2 - Odo_last_val_pos_2))*1800.0*PERIMETRE_ROUE_CODEUSE/(LARGEUR_ROBOT*PI*RESOLUTION_ROUE_CODEUSE);
+
+    //Determination de la position sur le terrain en X, Y, Theta
+    Odo_theta += ang;
+    Odo_x += dist*cos((double)(Odo_theta*PI/1800.0))*PERIMETRE_ROUE_CODEUSE/RESOLUTION_ROUE_CODEUSE;
+    Odo_y += dist*sin((double)(Odo_theta*PI/1800.0))*PERIMETRE_ROUE_CODEUSE/RESOLUTION_ROUE_CODEUSE;
+
+    //Stockage de la derniere valeur de l'odometrie
+    Odo_last_val_pos_1 = Odo_val_pos_1;
+    Odo_last_val_pos_2 = Odo_val_pos_2;
+
+
+    //Condition d'envoi des informations de l'odometrie par CAN
+    Send_odometrie();
+
+    /****************************************/
+    /****************************************/
+    /*               <DEBUG>                */
+    /****************************************/
+    /****************************************/
+
+    Debug_Asserv();
+
+    /****************************************/
+    /****************************************/
+    /*               <DEBUG>                */
+    /****************************************/
+    /****************************************/
+
+
+}
+
+
+void Ralentir(short nouvelle_vitesse)
+{
+    //Si on allait en avant (cpt >= 0), on decremente cpt jusqu'a la nouvelle vitesse
+    if(cpt >= 0) {
+        cpt --;
+        if(cpt <= ((double)nouvelle_vitesse / ((double)AMAX/1000.0))) {     //La vitesse souhaitait est atteinte
+            cpt = (short)((double)nouvelle_vitesse / ((double)AMAX/1000.0));
+            ralentare = 0;
+        }
+    }
+
+    //Si on allait en arri�re (cpt < 0), on incr�mente cpt jusqu'a la nouvelle vitesse
+    else if(cpt < 0) {
+        cpt ++;
+        if(cpt >= -((double)nouvelle_vitesse / ((double)AMAX/1000.0))) {    //La vitesse souhaitait est atteinte
+            cpt = (short)-((double)nouvelle_vitesse / ((double)AMAX/1000.0));
+            ralentare = 0;
+        }
+    }
+
+    //Ralentir fonctionne comme une acc�l�ration standard, donc on modifie la consigne de position
+    consigne_pos += (double)(((double)cpt * (double)AMAX)/1000.0);
+
+    //Calcul de la valeur de commande 1 et 2
+    Asser_Pos_Mot1(roue_drt_init + consigne_pos);
+    Asser_Pos_Mot2(roue_gch_init + consigne_pos);
+
+    //Ecriture de la valeur de la commande souhait�e
+    write_PWM2(commande2);
+    write_PWM1(commande1);
+
+}
+
+void Arret(void)
+{
+    //Recuperation de la valeur de l'odometrie de la roue de droite et de gauche
+    roue_drt_init = (double)encoder1.GetCounter();
+
+    /* MOI*\
+    //#elif GROS_ROB == 0
+    roue_gch_init = COEF_ROUE_GAUCHE * (double)encoder2.GetCounter();
+    //#endif
+    */
+    roue_gch_init = COEF_ROUE_GAUCHE * (double)encoder2.GetCounter();
+
+
+    //Calcul de la valeur des commandes 1 et 2
+    Asser_Pos_Mot1(roue_drt_init);
+    Asser_Pos_Mot2(roue_gch_init);
+
+    //Ecrire un PWM egal a 0
+    write_PWM2(0.0);
+    write_PWM1(0.0);
+
+    next_move = 1;
+
+}
+
+void Recalage(int pcons, short vmax, short amax, short dir, short nv_val)
+{
+    //char val1[8];
+
+    //Mode AVANCER
+    if(pcons >= 0) {
+        switch(etat_automate_depl) {
+            case INIT_RECALAGE :   //etat INIT_RECELAGE
+                etat_automate_depl = ACCELERATION_RECALAGE;     //Passage a l'etat ACCELERATION_RECALAGE
+                consigne_pos = 0;   //Initialisation des differentes variables
+                cpt = 0;
+                break;
+
+            case ACCELERATION_RECALAGE :    //etat ACCELERATION_RECALAGE
+                // Phase d'acc�leration comme si on faisait une ligne droite
+
+                if(consigne_pos >= pcons/2) etat_automate_depl = FIN_RECALAGE;  //Passage a l'etat FIN_RECALAGE
+                cpt ++;
+
+                //Calcul de la consigne de position
+                consigne_pos += (((double)cpt * (double)amax)/1000.0);
+
+                if(cpt >= ((double)vmax / ((double)amax/1000.0))) {
+                    etat_automate_depl = VITESSE_CONSTANTE_RECALAGE;  //Passage a l'etat VITESSE_CONSTANTE_RECALAGE
+                }
+
+                // Si les 2 erreurs sont sup�rieures � un seuil d�fini, on a fini le recalage
+                if(ErreurPos1 >= RECALAGE_TH && ErreurPos2 >= RECALAGE_TH) {
+                    etat_automate_depl = FIN_RECALAGE;  //Passage a l'etat FIN_RECALAGE
+                    commande1 = 0.0;  //Remise a zero des valeurs des commandes
+                    commande2 = 0.0;
+                }
+                // Si on d�tecte une augmentation d'erreur sur une roue, on l'arr�te pour �viter de patiner
+                else if(ErreurPos1 >= RECALAGE_TH)  commande1 = 0.0;
+                else if(ErreurPos2 >= RECALAGE_TH)  commande2 = 0.0;
+                break;
+
+            case VITESSE_CONSTANTE_RECALAGE :   //etat VITESSE_CONSTANTE_RECALAGE
+                // Phase de vitesse constante
+                consigne_pos += vmax;
+
+                // Idem que plus haut, on surveille les erreurs des 2 roues
+                if(ErreurPos1 >= RECALAGE_TH && ErreurPos2 >= RECALAGE_TH) {
+                    etat_automate_depl = FIN_RECALAGE;  //Passage a l'etat FIN_RECALAGE
+                    commande1 = 0.0;  //Remise a zero des valeurs des commandes
+                    commande2 = 0.0;
+                } else if(ErreurPos1 >= RECALAGE_TH)  commande1 = 0.0; //Mise a zero de la commande 1
+                else if(ErreurPos2 >= RECALAGE_TH)  commande2 = 0.0;  //Mise a zero de la commande 2
+                break;
+
+            case FIN_RECALAGE :     //etat FIN_RECALAGE
+                // Fin du recalage, on met � jour les donn�es de position
+                if(cpt >=20) {
+                    //Recalage de x
+                    if(dir == 1) {
+                        Odo_x = nv_val;
+                        // On met l'angle � jour en fonction de la position sur le terrain
+                        // Si on est dans la partie haute ( > la moiti�), on est dans un sens,
+                        // Si on est dans la partie basse, on met � jour dans l'autre sens
+                        // On prend aussi en compte le sens dans lequel on a fait la ligne droite
+
+                        if(nv_val > 1000) {
+                            if(pcons >=0)
+                                Odo_theta = 0;
+                            else
+                                Odo_theta = 1800;
+                        }
+
+                        else {
+                            if(pcons >= 0)
+                                Odo_theta = 1800;
+                            else
+                                Odo_theta = 0;
+                        }
+                    }
+
+                    //Recalage de y
+                    else {
+                        Odo_y = nv_val;
+
+                        if(nv_val > 1500) {
+                            if(pcons >=0)
+                                Odo_theta = 900;
+                            else
+                                Odo_theta = -900;
+                        }
+
+                        else {
+                            if(pcons >= 0)
+                                Odo_theta = -900;
+                            else
+                                Odo_theta = 900;
+                        }
+                    }
+
+                    etat_automate_depl = INIT_RECALAGE;
+
+                    next_move = 1;
+
+                    //Mise a zero des commandes et des moteurs;
+                    commande1 = 0.0;
+                    commande2 = 0.0;
+                    write_PWM1(0.0);
+                    write_PWM2(0.0);
+                }
+                cpt ++;
+                break;
+        }
+    }
+
+    //Mode RECULER
+    else if(pcons < 0) {
+        switch(etat_automate_depl) {
+            case INIT_RECALAGE :    //etat INIT_RECELAGE
+                etat_automate_depl = ACCELERATION_RECALAGE;     //Passage a l'etat ACCELERATION_RECALAGE
+                consigne_pos = 0;   //Initialisation des diff�rentes variables
+                cpt = 0;
+                break;
+
+            case ACCELERATION_RECALAGE :    //etat ACCELERATION_RECALAGE
+                // Phase d'acc�leration comme si on faisait une ligne droite
+                if(consigne_pos <= pcons/2) etat_automate_depl = FIN_RECALAGE;  //Passage a l'etat FIN_RECALAGE
+
+                cpt --;
+                consigne_pos += (double)(((double)cpt * (double)amax)/1000.0);
+
+                if(cpt <= -((double)vmax / ((double)amax/1000.0))) {
+                    etat_automate_depl = VITESSE_CONSTANTE_RECALAGE;    //Passage a l'etat VITESSE_CONSTANTE_RECALAGE
+                }
+
+                // Si les 2 erreurs sont inf�rieures � un seuil d�fini, on a fini le recalage
+                if(ErreurPos1 <= -RECALAGE_TH && ErreurPos2 <= -RECALAGE_TH) {
+                    etat_automate_depl = FIN_RECALAGE;  //Passage a l'etat FIN_RECALAGE
+                    commande1 = 0.0;
+                    commande2 = 0.0;
+                }
+                // Si on d�tecte une augmentation d'erreur sur une roue, on l'arr�te pour �viter de patiner
+                else if(ErreurPos1 <= -RECALAGE_TH) commande1 = 0.0;
+                else if(ErreurPos2 <= -RECALAGE_TH) commande2 = 0.0;
+                break;
+
+            case VITESSE_CONSTANTE_RECALAGE :   //etat VITESSE_CONSTANTE_RECALAGE
+                // Phase de vitesse constante
+                consigne_pos -= vmax;
+
+                // Idem que plus haut, on surveille les erreurs des 2 roues
+                if(ErreurPos1 <= -RECALAGE_TH && ErreurPos2 <= -RECALAGE_TH) {
+                    etat_automate_depl = FIN_RECALAGE;  //Passage a l'etat FIN_RECALAGE
+                    commande1 = 0.0;
+                    commande2 = 0.0;
+                } else if(ErreurPos1 <= -RECALAGE_TH) commande1 = 0.0;
+                else if(ErreurPos2 <= -RECALAGE_TH)commande2 = 0.0;
+                break;
+
+            case FIN_RECALAGE :     //etat FIN_RECALAGE
+                if(cpt >=20) {
+                    //Recalage de x
+                    if(dir == 1) {
+                        Odo_x = nv_val;
+
+                        if(nv_val > 1000) {
+                            if(pcons >=0)
+                                Odo_theta = 0;
+                            else
+                                Odo_theta = 1800;
+                        }
+
+                        else {
+                            if(pcons >= 0)
+                                Odo_theta = 1800;
+                            else
+                                Odo_theta = 0;
+                        }
+                    }
+
+                    //Recalage de y
+                    else {
+                        Odo_y = nv_val;
+
+                        if(nv_val > 1500) {
+                            if(pcons >=0)
+                                Odo_theta = 900;
+                            else
+                                Odo_theta = -900;
+                        }
+
+                        else {
+                            if(pcons >= 0)
+                                Odo_theta = -900;
+                            else
+                                Odo_theta = 900;
+                        }
+                    }
+
+                    etat_automate_depl = 0;
+
+                    next_move = 1;
+
+                    //Mise a zero des commandes et des moteurs
+                    commande1 = 0.0;
+                    commande2 = 0.0;
+                    write_PWM1(0.0);
+                    write_PWM2(0.0);
+                }
+                cpt ++;
+                break;
+        }
+    }
+
+    //Calcul des correcteurs a apporter aux moteurs
+    Asser_Pos_Mot1(roue_drt_init + consigne_pos);
+    Asser_Pos_Mot2(roue_gch_init + consigne_pos);
+
+    //Envoi de la nouvelle valeur de commande aux PWM
+    write_PWM2(commande2);
+    write_PWM1(commande1);
+
+}
+
+
+void Rayon_De_Courbure(short rayon, short theta, short vmax, short amax, short sens, short dmax)
+{
+    //Declaration des variables
+    //char val1[8];
+    static double tc, ta, td, vmax_tri, pcons;
+
+    //Si l'angle a parcourir est positif
+    if(theta >= 0) {
+        switch(etat_automate_depl) {
+            case INIT_RAYON_COURBURE : //etat d'initialisation
+                etat_automate_depl = ACCELERATION_TRAPEZE_RAYON_COURBURE;   //Passage a l'etat ACCELERATION_TRAPEZE_RAYON_COURBURE
+
+                //Mise a zero des variables car on effectue un nouveau deplacement
+                consigne_pos = 0;
+                cpt = 0;
+
+                //Calcul de la position voulue
+                pcons = ((rayon + LARGEUR_ROBOT/2) * 2 * PI * theta / 3600) * RESOLUTION_ROUE_CODEUSE / PERIMETRE_ROUE_CODEUSE;
+
+                //Elaboration du profil de vitesse
+                //Calcul du temps a vitesse constante
+                tc = ((double)pcons / (double)vmax) - (((double)vmax * 500.0) / (double)amax) - (((double)vmax * 500.0) / (double)dmax);
+
+                if (tc > 0) { //Resultat positif, il y a une phase de vitesse constante
+                    etat_automate_depl = ACCELERATION_TRAPEZE_RAYON_COURBURE;   //Passage a l'etat ACCELERATION_TRAPEZE_RAYON_COURBURE
+                    ta = (((double)vmax * 1000.0) / (double)amax);  //Calcul du temps d'acceleration
+                    td = (((double)vmax * 1000.0) / (double)dmax);  //Calcul du temps de deceleration
+
+                    canmsg.id = 0x7F1;
+                    canmsg.len = 0;
+                    can1.write(canmsg);
+
+                }
+
+                //Temps a vitesse constant inexistant
+                else if(tc < 0) { //Resultat negatif, il n'y a pas de phase de vitesse constante
+                    etat_automate_depl = ACCELERATION_TRIANGLE_RAYON_COURBURE;  //Passage a l'etat ACCELERATION_TRIANGLE_RAYON_COURBURE
+
+                    //Calcul de la vitesse maximale en profil triangle
+                    vmax_tri = sqrt((2 * pcons) / ((1000.0 / (double)amax) + (1000.0 / (double)dmax)));
+
+                    canmsg.id = 0x7F5;
+                    canmsg.len = 0;
+                    can1.write(canmsg);
+
+                }
+                break;
+
+            case ACCELERATION_TRAPEZE_RAYON_COURBURE :  //Etat d'acceleration en profil trapeze
+                cpt ++;
+
+                //Incrementation de la consigne de position
+                consigne_pos += (((double)cpt * (double)amax) / 1000.0);
+
+                if(cpt >= ta) { //Condition pour quitter la phase d'acceleration
+                    etat_automate_depl = VITESSE_CONSTANTE_TRAPEZE_RAYON_COURBURE;  //Passage a l'etat VITESSE_CONSTANTE_TRAPEZE_RAYON_COURBURE
+                    cpt = 0;
+
+                    canmsg.id = 0x7F2;
+                    canmsg.len = 0;
+                    can1.write(canmsg);
+
+                }
+                break;
+
+            case VITESSE_CONSTANTE_TRAPEZE_RAYON_COURBURE : //Etat de vitesse constante en profil trapeze
+                cpt ++;
+
+                //Incrementation de la consigne de vitesse par la valeur de la vitesse
+                consigne_pos += vmax;
+
+                //Si il n'y a pas d'enchainements
+                if(cpt >= tc && liste[cpt_ordre].enchainement != 1) { //Condition pour quitter la phase de vitesse constante
+                    etat_automate_depl = DECELERATION_TRAPEZE_RAYON_COURBURE;   //Passage a l'etat DECELERATION_TRAPEZE_RAYON_COURBURE
+                    cpt = 0;
+
+                    canmsg.id = 0x7F3;
+                    canmsg.len = 0;
+                    can1.write(canmsg);
+
+                }
+
+                //Si on est dans un enchainement, on passe � l'instruction suivante
+                else if(consigne_pos >= pcons && liste[cpt_ordre].enchainement == 1) {
+                    consigne_pos = pcons;
+                    cpt_ordre = 0;
+                }
+                break;
+
+            case DECELERATION_TRAPEZE_RAYON_COURBURE :  //Etat de deceleration en profil trapeze
+                cpt ++;
+
+                //Incrementation de la consigne de position
+                consigne_pos += vmax - (double)(((double)cpt * (double)dmax) / 1000.0);
+
+                if(cpt >= td) { //Condition pour quitter la phase de deceleration en profil trapeze
+                    etat_automate_depl = ARRET_RAYON_COURBURE;  //Passage a l'etat ARRET_RAYON_COURBURE
+                    consigne_pos = pcons;
+                    cpt = 0;
+
+                    canmsg.id = 0x7F4;
+                    canmsg.len = 0;
+                    can1.write(canmsg);
+
+                }
+                break;
+
+            case ARRET_RAYON_COURBURE : //Etat d'arret
+                if(cpt >= 20) {
+                    etat_automate_depl = INIT_RAYON_COURBURE; //Passage a l'etat d'INIT_RAYON_COURBURE
+                    next_move = 1;
+
+                }
+                cpt ++;
+                break;
+
+            case ACCELERATION_TRIANGLE_RAYON_COURBURE : //Etat d'acceleration en profil triangle
+                cpt ++;
+
+                //Incrementation de la consigne de position
+                consigne_pos += (((double)cpt * (double)amax) / 1000.0);
+
+                if((cpt * amax / 1000.0) >= vmax_tri) { //Condition pour sortir de la phase d'acceleration en profil triangle
+                    etat_automate_depl = DECELERATION_TRIANGLE_RAYON_COURBURE;  //Passage a l'etat DECELERATION_TRIANGLE_RAYON_COURBURE
+                    cpt = 0;
+
+                    canmsg.id = 0x7F6;
+                    canmsg.len = 0;
+                    can1.write(canmsg);
+
+                }
+                break;
+
+            case DECELERATION_TRIANGLE_RAYON_COURBURE : //Etat de deceleration en profil triangle
+                cpt ++;
+
+                //Incrementation de la consigne de position
+                consigne_pos += vmax_tri - (((double)cpt * (double)dmax) / 1000.0);
+
+                if(((double)vmax_tri - (((double)cpt * (double)dmax)) / 1000.0) <= 0) { //Condition pour sortir de la phase de deceleration en profil triangle
+                    etat_automate_depl = ARRET_RAYON_COURBURE;
+                    cpt = 0;
+
+                    canmsg.id = 0x7F4;
+                    canmsg.len = 0;
+                    can1.write(canmsg);
+
+                }
+                break;
+        }
+    }
+
+    //Si l'angle a parcourir est negatif
+    else if(theta < 0) {
+        switch(etat_automate_depl) {
+            case 0 :
+                etat_automate_depl++;
+                consigne_pos = 0;
+                cpt = 0;
+                pcons = ((rayon + LARGEUR_ROBOT/2) * 2 * PI * theta / 3600) * RESOLUTION_ROUE_CODEUSE / PERIMETRE_ROUE_CODEUSE;
+                tc = ((double)pcons / (double)vmax) - (((double)vmax * 500.0) / (double)amax) - (((double)vmax * 500.0) / (double)dmax);
+                if (tc > 0) {
+                    etat_automate_depl++;
+                    ta = (((double)vmax * 1000.0) / (double)amax);
+                    td = (((double)vmax * 1000.0) / (double)dmax);
+
+                    canmsg.id = 0x7F1;
+                    canmsg.len = 0;
+                    can1.write(canmsg);
+
+                } else if(tc < 0) {
+                    etat_automate_depl = 5;
+                    vmax_tri = sqrt((2 * -pcons) / ((1000.0 / (double)amax) + (1000.0 / (double)dmax)));
+
+                    canmsg.id = 0x7F5;
+                    canmsg.len = 0;
+                    can1.write(canmsg);
+
+                }
+                break;
+            case 1 :
+                cpt ++;
+                consigne_pos -= (((double)cpt * (double)amax) / 1000.0);
+                if(cpt >= ta) {
+                    etat_automate_depl ++;
+                    cpt = 0;
+
+                    canmsg.id = 0x7F2;
+                    canmsg.len = 0;
+                    can1.write(canmsg);
+
+                }
+                break;
+            case 2 :
+                cpt ++;
+                consigne_pos -= vmax;
+                if(cpt >= tc && liste[cpt_ordre].enchainement != 1) {
+                    etat_automate_depl ++;
+                    cpt = 0;
+
+                    canmsg.id = 0x7F3;
+                    canmsg.len = 0;
+                    can1.write(canmsg);
+
+                }
+                //Si on est dans un enchainement, on passe � l'instruction suivante
+                else if(consigne_pos >= pcons && liste[cpt_ordre].enchainement == 1) {
+                    consigne_pos = pcons;
+                    cpt_ordre = 0;
+                }
+                break;
+            case 3 :
+                cpt ++;
+                consigne_pos -= vmax - (double)(((double)cpt * (double)dmax) / 1000.0);
+                if(cpt >= td) {
+                    etat_automate_depl ++;
+                    consigne_pos = pcons;
+                    cpt = 0;
+
+                    canmsg.id = 0x7F4;
+                    canmsg.len = 0;
+                    can1.write(canmsg);
+
+                }
+                break;
+            case 4 :
+                if(cpt >= 20) {
+                    etat_automate_depl = 0;
+                    next_move = 1;
+
+                }
+                cpt ++;
+                break;
+            case 5 :
+                cpt ++;
+                consigne_pos -= (((double)cpt * (double)amax) / 1000.0);
+                if((cpt * amax / 1000.0) >= vmax_tri) {
+                    etat_automate_depl ++;
+                    cpt = 0;
+
+                    canmsg.id = 0x7F6;
+                    canmsg.len = 0;
+                    can1.write(canmsg);
+
+                }
+                break;
+            case 6 :
+                cpt ++;
+                consigne_pos -= vmax_tri - (((double)cpt * (double)dmax) / 1000.0);
+                if(((double)vmax_tri - (((double)cpt * (double)dmax)) / 1000.0) <= 0) {
+                    etat_automate_depl = 4;
+                    cpt = 0;
+
+                    canmsg.id = 0x7F4;
+                    canmsg.len = 0;
+                    can1.write(canmsg);
+
+                }
+                break;
+        }
+    }
+
+    //Determination des commandes en fonction de la direction de deplacement du robot
+    if(sens >= 0) {
+
+        Asser_Pos_Mot1(roue_drt_init + consigne_pos);
+        Asser_Pos_Mot2(roue_gch_init + (consigne_pos * ((2 * rayon) - LARGEUR_ROBOT)/((2 * rayon) + LARGEUR_ROBOT)));
+    } else if(sens < 0) {
+
+        Asser_Pos_Mot1(roue_drt_init + (consigne_pos * ((2 * rayon) - LARGEUR_ROBOT)/((2 * rayon) + LARGEUR_ROBOT)));
+        Asser_Pos_Mot2(roue_gch_init + consigne_pos);
+    }
+
+    //Envoi de la vitesse aux moteurs
+    write_PWM2(commande2);
+    write_PWM1(commande1);
+}
+
+
+void MY_ISR()
+{
+    char val1[8];
+    if(ms_count == 20) {
+        //pc.printf("vit1=%f vit2=%f\n", test1, test2);
+        if(asser_actif) {
+            //Si on a re�u l'instruction de stop...
+            if(stop_receive) {
+                stop_receive = 0;
+                ErreurPos1 = 0;
+                ErreurPos2 = 0;
+                Somme_ErreurPos1 = 0;
+                Somme_ErreurPos2 = 0;
+                Delta_ErreurPos1 = 0;
+                Delta_ErreurPos2 = 0;
+                //On poursuit l'action si on est en rotation
+                if(liste[cpt_ordre].type == TYPE_DEPLACEMENT_ROTATION);
+                //On s'arr�te tout de suite si on est en ligne droite
+                else if(liste[cpt_ordre].type == TYPE_DEPLACEMENT_IMMOBILE || liste[cpt_ordre].type == TYPE_DEPLACEMENT_LIGNE_DROITE || liste[cpt_ordre].type == TYPE_DEPLACEMENT_RAYON_COURBURE) {
+                    //On vide le buffer de mouvements et on pr�voit de s'asservir sur la position atteinte
+                    for(i = 0; i<nb_ordres; i++)  liste[i] = (struct Ordre_deplacement) {
+                        0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
+                    };
+                    liste[0] = (struct Ordre_deplacement) {
+                        TYPE_DEPLACEMENT_STOP,0,0,0,0,0,0,0,0,0,0,0,0,0,0
+                    };
+                    nb_ordres = 0;
+                    etat_automate_depl = 0;
+                    cpt_ordre = 0;
+                }
+                //Si on est en X_Y_Theta, on poursuit la rotation, ou on arr�te la ligne droite
+                else if(liste[cpt_ordre].type == TYPE_DEPLACEMENT_X_Y_THETA) {
+                    if(etat_automate_xytheta == 2) {
+                        //On vide le buffer de mouvements et on pr�voit de s'asservir sur la position atteinte
+                        for(i = 0; i<nb_ordres; i++)  liste[i] = (struct Ordre_deplacement) {
+                            0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
+                        };
+                        liste[0] = (struct Ordre_deplacement) {
+                            TYPE_DEPLACEMENT_STOP,0,0,0,0,0,0,0,0,0,0,0,0,0,0
+                        };
+                        nb_ordres = 0;
+                        cpt_ordre = 0;
+                    }
+                }
+            }
+            //Si on a re�u un changement de vitesse...
+            else if(ralentare) {
+                Ralentir(VMAX);
+                for(i = 0; i<nb_ordres; i++) {
+                    liste[i].vmax = VMAX;
+                    liste[i].amax = AMAX;
+                }
+            }
+
+            else if(recalage_debut_receive) {
+                if(couleur_debut == 1) {
+
+                    //#if GROS_ROB == 0
+                    Odo_x = 1961;
+                    Odo_y = 2800;
+                    Odo_theta = 1800;
+                    liste[0] = (struct Ordre_deplacement) {
+                        TYPE_DEPLACEMENT_LIGNE_DROITE,0,30,500,(971 * RESOLUTION_ROUE_CODEUSE) / PERIMETRE_ROUE_CODEUSE,0,0,0,0,0,0,0,0,0,0
+                    };
+                    liste[1] = (struct Ordre_deplacement) {
+                        TYPE_DEPLACEMENT_ROTATION,0,30,500,0,0,0,900 * LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE / (3600 * PERIMETRE_ROUE_CODEUSE),0,0,0,0,0,0,0
+                    };
+                    liste[2] = (struct Ordre_deplacement) {
+                        TYPE_DEPLACEMENT_RECALAGE,0,30,500,(-1000 * RESOLUTION_ROUE_CODEUSE) / PERIMETRE_ROUE_CODEUSE,2,2961,0,0,0,0,0,0,0,0
+                    };
+                    liste[3] = (struct Ordre_deplacement) {
+                        TYPE_DEPLACEMENT_LIGNE_DROITE,0,30,500,(111 * RESOLUTION_ROUE_CODEUSE) / PERIMETRE_ROUE_CODEUSE,0,0,0,0,0,0,0,0,0,0
+                    };
+                    liste[4] = (struct Ordre_deplacement) {
+                        TYPE_DEPLACEMENT_ROTATION,0,30,500,0,0,0,900 * LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE / (3600 * PERIMETRE_ROUE_CODEUSE),0,0,0,0,0,0,0
+                    };
+
+                    recalage_debut_receive = 0;
+                } else if(couleur_debut == 0) {
+
+
+                    //#if GROS_ROB == 0
+                    Odo_x = 1961;
+                    Odo_y = 200;
+                    Odo_theta = 1800;
+                    liste[0] = (struct Ordre_deplacement) {
+                        TYPE_DEPLACEMENT_LIGNE_DROITE,0,30,500,(971 * RESOLUTION_ROUE_CODEUSE) / PERIMETRE_ROUE_CODEUSE,0,0,0,0,0,0,0,0,0,0
+                    };
+                    liste[1] = (struct Ordre_deplacement) {
+                        TYPE_DEPLACEMENT_ROTATION,0,30,500,0,0,0,-900 * LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE / (3600 * PERIMETRE_ROUE_CODEUSE),0,0,0,0,0,0,0
+                    };
+                    liste[2] = (struct Ordre_deplacement) {
+                        TYPE_DEPLACEMENT_RECALAGE,0,30,500,(-1000 * RESOLUTION_ROUE_CODEUSE) / PERIMETRE_ROUE_CODEUSE,2,39,0,0,0,0,0,0,0,0
+                    };
+                    liste[3] = (struct Ordre_deplacement) {
+                        TYPE_DEPLACEMENT_LIGNE_DROITE,0,30,500,(111 * RESOLUTION_ROUE_CODEUSE) / PERIMETRE_ROUE_CODEUSE,0,0,0,0,0,0,0,0,0,0
+                    };
+                    liste[4] = (struct Ordre_deplacement) {
+                        TYPE_DEPLACEMENT_ROTATION,0,30,500,0,0,0,-900 * LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE / (3600 * PERIMETRE_ROUE_CODEUSE),0,0,0,0,0,0,0
+                    };
+
+                    recalage_debut_receive = 0;
+                }
+            }
+
+            //Si on vient de finir un mouvement...
+            else if(next_move) {
+
+                ErreurPos1 = 0;
+                ErreurPos2 = 0;
+                Somme_ErreurPos1 = 0;
+                Somme_ErreurPos2 = 0;
+                Delta_ErreurPos1 = 0;
+                Delta_ErreurPos2 = 0;
+                //Si on devait s'arr�ter...
+                if(stop) {
+                    //On vide le buffer de mouvements et on pr�voit de s'asservir sur la position atteinte
+                    for(i = 0; i<nb_ordres; i++)  liste[i] = (struct Ordre_deplacement) {
+                        0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
+                    };
+                    liste[0] = (struct Ordre_deplacement) {
+                        TYPE_DEPLACEMENT_IMMOBILE,0,0,0,0,0,0,0,0,0,0,0,0,0,0
+                    };
+                    nb_ordres = 0;
+                    cpt_ordre = 0;
+                    stop = 0;
+                    next_move = 0;
+                    cpt = 0;
+
+                    //On pr�vient qu'on s'est arr�t�
+                    motor_of();
+
+                }
+                //Sinon...
+                else if(!stop) {
+                    next_move = 0;
+                    //On conserve la position du robot pour la prochaine action
+                    roue_drt_init = encoder1.GetCounter();
+
+                    //MOI*\*/ // bug Edouard
+                    //#elif GROS_ROB == 0
+                    roue_gch_init = COEF_ROUE_GAUCHE * (double)encoder2.GetCounter();
+                    //#endif
+
+                    cpt_ordre++;
+
+                    if(recalage_debut == 1) {
+
+                        if(cpt_ordre >= 5) {
+                            recalage_debut = 2;
+                            //On vide le buffer de mouvements et on pr�voit de s'asservir sur la position atteinte
+                            for(i = 0; i<nb_ordres; i++)  liste[i] = (struct Ordre_deplacement) {
+                                0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
+                            };
+                            liste[0] = (struct Ordre_deplacement) {
+                                TYPE_DEPLACEMENT_IMMOBILE,0,0,0,0,0,0,0,0,0,0,0,0,0,0
+                            };
+                            nb_ordres = 0;
+                            cpt_ordre = 0;
+                        }
+                    }
+                    //Si on n'est pas dans un enchainement...
+                    else if(liste[cpt_ordre].enchainement != 1) {
+                        //On envoie le flag de fin d'instruction correspondant sur CAN
+                        switch(liste[cpt_ordre].type) {
+                            case TYPE_DEPLACEMENT_LIGNE_DROITE :
+                                val1[0] = ASSERVISSEMENT_RECALAGE;
+                                break;
+                            case TYPE_DEPLACEMENT_ROTATION :
+                                val1[0] = ASSERVISSEMENT_ROTATION;
+                                break;
+                            case TYPE_DEPLACEMENT_X_Y_THETA :
+                                val1[0] = ASSERVISSEMENT_XYT;
+                                break;
+                            case TYPE_DEPLACEMENT_RAYON_COURBURE :
+                                val1[0] = ASSERVISSEMENT_COURBURE;
+                                break;
+                            case TYPE_DEPLACEMENT_RECALAGE :
+                                val1[0] = ASSERVISSEMENT_RECALAGE;
+                                break;
+                            default :
+                                val1[0] = 0;
+                                break;
+                        }
+
+                        //On vide le buffer de mouvements et on pr�voit de s'asservir sur la position atteinte
+                        for(i = 0; i<nb_ordres; i++)  liste[i] = (struct Ordre_deplacement) {
+                            0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
+                        };
+                        liste[0] = (struct Ordre_deplacement) {
+                            TYPE_DEPLACEMENT_IMMOBILE,0,0,0,0,0,0,0,0,0,0,0,0,0,0
+                        };
+                        nb_ordres = 0;
+                        cpt_ordre = 0;
+                        cpt = 0;
+
+                        //On pr�vient qu'on s'est arr�t�
+                        motor_of();
+                    }
+                }
+            } else {    //On v�rifie le type d'action � effectuer, en fonction du buffer d'instructions
+                test3++;
+                switch(liste[cpt_ordre].type) {
+                    case TYPE_DEPLACEMENT_STOP :
+                        Arret();
+                        break;
+
+                    case TYPE_DEPLACEMENT_IMMOBILE:
+                        test4++;
+                        Asser_Pos_Mot1(roue_drt_init);
+                        Asser_Pos_Mot2(roue_gch_init);
+                        write_PWM2(commande2);
+                        write_PWM1(commande1);
+                        break;
+
+                    case TYPE_DEPLACEMENT_LIGNE_DROITE:
+                        Avancer_ligne_droite(liste[cpt_ordre].distance, VMAX, AMAX, DMAX);
+                        break;
+
+                    case TYPE_DEPLACEMENT_ROTATION:
+                        Rotation(liste[cpt_ordre].angle, VMAX, AMAX, DMAX);
+                        break;
+
+                    case TYPE_DEPLACEMENT_X_Y_THETA:
+                        X_Y_Theta(liste[cpt_ordre].x, liste[cpt_ordre].y, liste[cpt_ordre].theta,
+                                  liste[cpt_ordre].sens, VMAX, AMAX);
+                        break;
+
+                    case TYPE_DEPLACEMENT_RAYON_COURBURE :
+                        Rayon_De_Courbure(liste[cpt_ordre].rayon, liste[cpt_ordre].theta_ray, VMAX,
+                                          AMAX, liste[cpt_ordre].sens, DMAX);
+                        break;
+
+                    case TYPE_DEPLACEMENT_RECALAGE :
+                        Recalage(liste[cpt_ordre].distance, 10, 500, liste[cpt_ordre].recalage, liste[cpt_ordre].val_recalage);
+                        break;
+
+                    default :
+                        Asser_Pos_Mot1(roue_drt_init);
+                        Asser_Pos_Mot2(roue_gch_init);
+                        write_PWM2(commande2);
+                        write_PWM1(commande1);
+                        test7++;
+                        break;
+
+                }
+            }
+            //On calcule l'odom�trie � chaque tour de boucle
+
+            //MOI*\
+            ms_count = 0; // reset ms counter
+            ms_count1 ++;
+            //Odometrie();
+            if(ms_count1 == 5)
+                ms_count1 = 0;
+        } else {
+            ms_count = 0; // reset ms counter
+            ms_count1 ++;
+            //Odometrie();
+            if(ms_count1 == 5)
+                ms_count1 = 0;
+        }
+
+    } else {
+        ms_count ++;
+    }  /*else if(ms_count % 10 == 0) {
+        Odometrie();
+
+    }
+    */
+}
\ No newline at end of file
diff -r 000000000000 -r 444ac4067b19 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Apr 19 17:50:19 2017 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/mbed_official/code/mbed/builds/856d2700e60b
\ No newline at end of file