![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Prog_cate-Mbed - Reception de consigne à revoir -,,,
Dependencies: Asservissement Encoder_Nucleo_16_bits MYCRAC mbed
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