CRAC Team
Prog_cate-Mbed - Reception de consigne à revoir -,,,
Dependencies: Asservissement Encoder_Nucleo_16_bits MYCRAC mbed
Fork of
Asserv_mot
by 
Brandon Modon
Diff: main.cpp
- Revision:
- 0:444ac4067b19
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