Prog_cate-Mbed - Reception de consigne à revoir -,,,
Dependencies: Asservissement Encoder_Nucleo_16_bits MYCRAC mbed
Fork of Asserv_mot by
main.cpp
- Committer:
- Brand101
- Date:
- 2017-04-19
- Revision:
- 0:444ac4067b19
File content as of revision 0:444ac4067b19:
#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(); } */ }