CRAC Team / Mbed 2 deprecated Asserv_mot

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

Dependencies:   Asservissement Encoder_Nucleo_16_bits MYCRAC mbed

Fork of Asserv_mot by Brandon Modon

main.cpp@0:444ac4067b19, 2017-04-19 (annotated)

Committer:
Brand101
Date:
Wed Apr 19 17:50:19 2017 +0000
Revision:
0:444ac4067b19
Dernier version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Brand101 0:444ac4067b19 1 #include "mbed.h"
Brand101 0:444ac4067b19 2 #include "Nucleo_Encoder_16_bits.h"
Brand101 0:444ac4067b19 3 #include <math.h>
Brand101 0:444ac4067b19 4 #include "MYCRAC_utility.h"
Brand101 0:444ac4067b19 5 #include "Asservissement.h"
Brand101 0:444ac4067b19 6 #include "global.h"
Brand101 0:444ac4067b19 7 #include "MY_Asserv.h"
Brand101 0:444ac4067b19 8 #include "My_ident.h"
Brand101 0:444ac4067b19 9
Brand101 0:444ac4067b19 10 //#include "CRAC_utility.h"
Brand101 0:444ac4067b19 11
Brand101 0:444ac4067b19 12 //------------------------ prototypes de fcts ------------------------------------
Brand101 0:444ac4067b19 13
Brand101 0:444ac4067b19 14 void Asser_Pos_Mot1(double pcons_pos);
Brand101 0:444ac4067b19 15 void Asser_Pos_Mot2(double pcons_pos);
Brand101 0:444ac4067b19 16
Brand101 0:444ac4067b19 17 void Avancer_ligne_droite(long pcons, short vmax, short amax, short dmax);
Brand101 0:444ac4067b19 18 void Avancer_ligne_droite_XYTheta(long pcons, short vmax, short amax, short dmax);
Brand101 0:444ac4067b19 19 void Rotation(long pcons, short vmax, short amax, short dmax);
Brand101 0:444ac4067b19 20 void Rotation_XYTheta(long pcons, short vmax, short amax, short dmax);
Brand101 0:444ac4067b19 21 void X_Y_Theta(long px, long py, long ptheta, long sens, short vmax, short amax);
Brand101 0:444ac4067b19 22
Brand101 0:444ac4067b19 23 void Recalage(int pcons, short vmax, short amax, short dir, short nv_val);
Brand101 0:444ac4067b19 24 void Rayon_De_Courbure(short rayon, short theta, short vmax, short amax, short sens, short dmax);
Brand101 0:444ac4067b19 25
Brand101 0:444ac4067b19 26 void Odometrie(void); // void Odometrie(void);
Brand101 0:444ac4067b19 27 void Ralentir(short nouvelle_vitesse);
Brand101 0:444ac4067b19 28 void Arret(void);
Brand101 0:444ac4067b19 29
Brand101 0:444ac4067b19 30 void canProcessRx(void);
Brand101 0:444ac4067b19 31
Brand101 0:444ac4067b19 32 void clacul_odo();
Brand101 0:444ac4067b19 33
Brand101 0:444ac4067b19 34 void canRx_ISR();
Brand101 0:444ac4067b19 35 void MY_ISR();
Brand101 0:444ac4067b19 36
Brand101 0:444ac4067b19 37
Brand101 0:444ac4067b19 38 Serial pc(SERIAL_TX, SERIAL_RX);
Brand101 0:444ac4067b19 39
Brand101 0:444ac4067b19 40
Brand101 0:444ac4067b19 41 CAN can1(PB_8, PB_9);
Brand101 0:444ac4067b19 42
Brand101 0:444ac4067b19 43 CANMessage canmsg = CANMessage();
Brand101 0:444ac4067b19 44
Brand101 0:444ac4067b19 45 char message[8]= {0x00,0x01,0x10,0x11,0x00,0x01,0x10,0x11};
Brand101 0:444ac4067b19 46
Brand101 0:444ac4067b19 47 CANMessage msgRxBuffer[SIZE_FIFO]; // buffer en r�ception pour le CAN
Brand101 0:444ac4067b19 48 unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
Brand101 0:444ac4067b19 49 unsigned char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
Brand101 0:444ac4067b19 50
Brand101 0:444ac4067b19 51 Ticker timer;
Brand101 0:444ac4067b19 52 Ticker cal_odo;
Brand101 0:444ac4067b19 53
Brand101 0:444ac4067b19 54 //------------------------ def de var globales ------------------------------------
Brand101 0:444ac4067b19 55
Brand101 0:444ac4067b19 56 PwmOut mot1(PA_8);
Brand101 0:444ac4067b19 57 PwmOut mot2(PA_5);
Brand101 0:444ac4067b19 58
Brand101 0:444ac4067b19 59 DigitalOut INA_M2(PC_6); //p37
Brand101 0:444ac4067b19 60 DigitalOut INB_M2(PC_7); //p38
Brand101 0:444ac4067b19 61 DigitalOut INA_M1(PB_0); //P26
Brand101 0:444ac4067b19 62 DigitalOut INB_M1(PB_1); //P27
Brand101 0:444ac4067b19 63
Brand101 0:444ac4067b19 64 Nucleo_Encoder_16_bits encoder1(TIM3, 0xffff, TIM_ENCODERMODE_TI12);
Brand101 0:444ac4067b19 65 Nucleo_Encoder_16_bits encoder2(TIM4, 0xffff, TIM_ENCODERMODE_TI12);
Brand101 0:444ac4067b19 66
Brand101 0:444ac4067b19 67 double consigne_pos, consigne_vit, // Consignes de position et de vitesse dans les mouvements
Brand101 0:444ac4067b19 68 commande1, commande2, // Commande, aka duty cycle des PWM de chaque moteur
Brand101 0:444ac4067b19 69 last_pos1, last_pos2, pos1, pos2, // Valeurs des compteurs incr�mentaux aux instants t et t-1
Brand101 0:444ac4067b19 70 ErreurPos1, ErreurPos2, last_ErreurPos1, last_ErreurPos2, // Valeurs des erreurs de position de la boucle d'asservissement
Brand101 0:444ac4067b19 71 Somme_ErreurPos1, Somme_ErreurPos2, // Valeurs des int�grales des erreurs
Brand101 0:444ac4067b19 72 Delta_ErreurPos1, Delta_ErreurPos2, // Valeurs des d�riv�es des erreurs
Brand101 0:444ac4067b19 73 Kpp1, Kip1, Kdp1, Kpp2, Kip2, Kdp2, // Valeurs des correcteurs d'asservissement pour les 2 moteurs
Brand101 0:444ac4067b19 74 Kpp1a, Kip1a, Kdp1a, Kpp2a, Kip2a, Kdp2a, // Valeurs des correcteurs d'asservissement pour les 2 moteurs
Brand101 0:444ac4067b19 75 VMAX, AMAX, DMAX, // Valeurs maximales d'acc�leration, d�c�l�ration et vitesse
Brand101 0:444ac4067b19 76 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
Brand101 0:444ac4067b19 77 uint32_t roue_drt_init, roue_gch_init; // Valeur des compteurs (!= 0) quand on commence un nouveau mouvement
Brand101 0:444ac4067b19 78 short etat_automate, etat_automate_depl, new_message,
Brand101 0:444ac4067b19 79 xytheta_sens, next_move_xyt, next_move, i, cpt, stop, stop_receive, param_xytheta[3],
Brand101 0:444ac4067b19 80 etat_automate_xytheta, ralentare, recalage_debut, couleur_debut, recalage_debut_receive;
Brand101 0:444ac4067b19 81 char nb_ordres, vitesse_danger, Stop_Danger, cpt_ordre, asser_actif;
Brand101 0:444ac4067b19 82 struct Ordre_deplacement liste[200];
Brand101 0:444ac4067b19 83
Brand101 0:444ac4067b19 84 uint16_t ms_count = 0, // Compteur utilis� pour envoyer �chantillonner les positions et faire l'asservissement
Brand101 0:444ac4067b19 85 ms_count1 = 0, // Compteur utilis� pour envoyer la trame CAN d'odom�trie
Brand101 0:444ac4067b19 86 flag_odo,
Brand101 0:444ac4067b19 87 test3 = 0,
Brand101 0:444ac4067b19 88 test4 = 0,
Brand101 0:444ac4067b19 89 test5 = 0,
Brand101 0:444ac4067b19 90 test6 =0,
Brand101 0:444ac4067b19 91 test7 =0,
Brand101 0:444ac4067b19 92 test8 =0;
Brand101 0:444ac4067b19 93
Brand101 0:444ac4067b19 94
Brand101 0:444ac4067b19 95
Brand101 0:444ac4067b19 96 double test1=0,
Brand101 0:444ac4067b19 97 test2=0;
Brand101 0:444ac4067b19 98
Brand101 0:444ac4067b19 99
Brand101 0:444ac4067b19 100 //------------------------ main ------------------------------------
Brand101 0:444ac4067b19 101
Brand101 0:444ac4067b19 102 int main()
Brand101 0:444ac4067b19 103 {
Brand101 0:444ac4067b19 104 pc.baud(230400);
Brand101 0:444ac4067b19 105 pc.printf("Debut du prog");
Brand101 0:444ac4067b19 106
Brand101 0:444ac4067b19 107 INA_M2 = 0;
Brand101 0:444ac4067b19 108 INB_M2 = 0;
Brand101 0:444ac4067b19 109 INA_M1 = 0;
Brand101 0:444ac4067b19 110 INB_M1 = 0;
Brand101 0:444ac4067b19 111
Brand101 0:444ac4067b19 112 mot1.period(1./20000.);
Brand101 0:444ac4067b19 113 mot2.period(1./20000.);
Brand101 0:444ac4067b19 114 can1.frequency(1000000);
Brand101 0:444ac4067b19 115
Brand101 0:444ac4067b19 116 consigne_pos = 0.0;
Brand101 0:444ac4067b19 117 next_move = 0;
Brand101 0:444ac4067b19 118 asser_actif=1;
Brand101 0:444ac4067b19 119 cpt_ordre = 0;
Brand101 0:444ac4067b19 120 nb_ordres = 1;
Brand101 0:444ac4067b19 121 stop = 1;
Brand101 0:444ac4067b19 122 stop_receive = 1;
Brand101 0:444ac4067b19 123 ralentare = 0;
Brand101 0:444ac4067b19 124 recalage_debut_receive = 0;
Brand101 0:444ac4067b19 125
Brand101 0:444ac4067b19 126 VMAX = 50;
Brand101 0:444ac4067b19 127 AMAX = 125;
Brand101 0:444ac4067b19 128 DMAX = 125;
Brand101 0:444ac4067b19 129
Brand101 0:444ac4067b19 130 roue_drt_init = (encoder1.GetCounter() + 20000);
Brand101 0:444ac4067b19 131 roue_gch_init = COEF_ROUE_GAUCHE * ((double)encoder2.GetCounter() + 20000);
Brand101 0:444ac4067b19 132
Brand101 0:444ac4067b19 133 liste[0] = (struct Ordre_deplacement) {
Brand101 0:444ac4067b19 134 TYPE_DEPLACEMENT_STOP,0,0,0,0,0,0,0,0,0,0,0,0,0,0
Brand101 0:444ac4067b19 135 };
Brand101 0:444ac4067b19 136
Brand101 0:444ac4067b19 137
Brand101 0:444ac4067b19 138 timer.attach(&MY_ISR, 0.001);
Brand101 0:444ac4067b19 139 can1.attach(&canRx_ISR); // cr�ation de l'interrupt attach�e � la r�ception sur le CAN
Brand101 0:444ac4067b19 140
Brand101 0:444ac4067b19 141 cal_odo.attach(&clacul_odo, 0.01);
Brand101 0:444ac4067b19 142
Brand101 0:444ac4067b19 143 Kpp1 = 0.5;
Brand101 0:444ac4067b19 144 Kip1 = 0.0001;
Brand101 0:444ac4067b19 145 Kdp1 = 0.8;
Brand101 0:444ac4067b19 146
Brand101 0:444ac4067b19 147 Kpp2 = 0.5;
Brand101 0:444ac4067b19 148 Kip2 = 0.0001;
Brand101 0:444ac4067b19 149 Kdp2 = 0.8;
Brand101 0:444ac4067b19 150
Brand101 0:444ac4067b19 151 while(1) {
Brand101 0:444ac4067b19 152 //TEST
Brand101 0:444ac4067b19 153 //Asser_Pos_Mot1(0.0);
Brand101 0:444ac4067b19 154 //Asser_Pos_Mot2(0.0);
Brand101 0:444ac4067b19 155 //test3++;
Brand101 0:444ac4067b19 156
Brand101 0:444ac4067b19 157 canProcessRx();
Brand101 0:444ac4067b19 158
Brand101 0:444ac4067b19 159 if(flag_odo) {
Brand101 0:444ac4067b19 160 flag_odo = 0;
Brand101 0:444ac4067b19 161 Odometrie();
Brand101 0:444ac4067b19 162 }
Brand101 0:444ac4067b19 163 pc.printf("init_d:%d inti_g:%d dit:%hd t8:%d\n", roue_drt_init, roue_gch_init, liste[cpt_ordre].distance, test8);
Brand101 0:444ac4067b19 164
Brand101 0:444ac4067b19 165 //pc.printf("com1=%f com2=%f ePos1=%f ePos2=%f enc2=%d enc1=%d\n",commande1, commande2, ErreurPos1, ErreurPos2, encoder2.GetCounter(), encoder1.GetCounter());
Brand101 0:444ac4067b19 166 //wait(0.001);
Brand101 0:444ac4067b19 167 //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);
Brand101 0:444ac4067b19 168 /*
Brand101 0:444ac4067b19 169 write_PWM2(commande2);
Brand101 0:444ac4067b19 170 if(ms_count==19) {
Brand101 0:444ac4067b19 171 test3++;
Brand101 0:444ac4067b19 172 }
Brand101 0:444ac4067b19 173 */
Brand101 0:444ac4067b19 174 }
Brand101 0:444ac4067b19 175 }
Brand101 0:444ac4067b19 176
Brand101 0:444ac4067b19 177 void clacul_odo()
Brand101 0:444ac4067b19 178 {
Brand101 0:444ac4067b19 179 flag_odo = 1;
Brand101 0:444ac4067b19 180 }
Brand101 0:444ac4067b19 181
Brand101 0:444ac4067b19 182 void canRx_ISR()
Brand101 0:444ac4067b19 183 {
Brand101 0:444ac4067b19 184 if (can1.read(msgRxBuffer[FIFO_ecriture])) {
Brand101 0:444ac4067b19 185 FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
Brand101 0:444ac4067b19 186 /*
Brand101 0:444ac4067b19 187 if(msgRxBuffer[FIFO_ecriture].id==RESET_STRAT) mbed_reset();
Brand101 0:444ac4067b19 188 else FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
Brand101 0:444ac4067b19 189 */
Brand101 0:444ac4067b19 190 }
Brand101 0:444ac4067b19 191 }
Brand101 0:444ac4067b19 192
Brand101 0:444ac4067b19 193 void canProcessRx(void)
Brand101 0:444ac4067b19 194 {
Brand101 0:444ac4067b19 195 static signed char FIFO_occupation=0,FIFO_max_occupation=0;
Brand101 0:444ac4067b19 196 CANMessage msgTx=CANMessage();
Brand101 0:444ac4067b19 197 FIFO_occupation=FIFO_ecriture-FIFO_lecture;
Brand101 0:444ac4067b19 198 if(FIFO_occupation<0)
Brand101 0:444ac4067b19 199 FIFO_occupation=FIFO_occupation+SIZE_FIFO;
Brand101 0:444ac4067b19 200 if(FIFO_max_occupation<FIFO_occupation)
Brand101 0:444ac4067b19 201 FIFO_max_occupation=FIFO_occupation;
Brand101 0:444ac4067b19 202 if(FIFO_occupation!=0) {
Brand101 0:444ac4067b19 203
Brand101 0:444ac4067b19 204 switch(msgRxBuffer[FIFO_lecture].id) {
Brand101 0:444ac4067b19 205 case ASSERVISSEMENT_INFO_CONSIGNE:
Brand101 0:444ac4067b19 206 SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_INFO_CONSIGNE);
Brand101 0:444ac4067b19 207 Send_assev_info_comfig();
Brand101 0:444ac4067b19 208 break;
Brand101 0:444ac4067b19 209 case ASSERVISSEMENT_CONFIG_KPP_DROITE:
Brand101 0:444ac4067b19 210 SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_CONFIG_KPP_DROITE);
Brand101 0:444ac4067b19 211 break;
Brand101 0:444ac4067b19 212 case ASSERVISSEMENT_CONFIG_KPI_DROITE:
Brand101 0:444ac4067b19 213 SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_CONFIG_KPI_DROITE);
Brand101 0:444ac4067b19 214 break;
Brand101 0:444ac4067b19 215 case ASSERVISSEMENT_CONFIG_KPD_DROITE:
Brand101 0:444ac4067b19 216 SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_CONFIG_KPD_DROITE);
Brand101 0:444ac4067b19 217 break;
Brand101 0:444ac4067b19 218 case ASSERVISSEMENT_CONFIG_KPP_GAUCHE:
Brand101 0:444ac4067b19 219 SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_CONFIG_KPP_GAUCHE);
Brand101 0:444ac4067b19 220 break;
Brand101 0:444ac4067b19 221 case ASSERVISSEMENT_CONFIG_KPI_GAUCHE:
Brand101 0:444ac4067b19 222 SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_CONFIG_KPI_GAUCHE);
Brand101 0:444ac4067b19 223 break;
Brand101 0:444ac4067b19 224 case ASSERVISSEMENT_CONFIG_KPD_GAUCHE:
Brand101 0:444ac4067b19 225 SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_CONFIG_KPD_GAUCHE);
Brand101 0:444ac4067b19 226 break;
Brand101 0:444ac4067b19 227 case ASSERVISSEMENT_ENABLE:
Brand101 0:444ac4067b19 228 SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_ENABLE);
Brand101 0:444ac4067b19 229 asser_actif = msgRxBuffer[FIFO_lecture].data[0];
Brand101 0:444ac4067b19 230 break;
Brand101 0:444ac4067b19 231 /*
Brand101 0:444ac4067b19 232 case ODOMETRIE_SMALL_POSITION:
Brand101 0:444ac4067b19 233 SendAck(ACKNOWLEDGE_MOTEUR, ODOMETRIE_SMALL_POSITION);
Brand101 0:444ac4067b19 234 Odometrie(); // il envoie automatiquement d'odo apr�s calcule
Brand101 0:444ac4067b19 235 break;
Brand101 0:444ac4067b19 236 */
Brand101 0:444ac4067b19 237 case ODOMETRIE_SMALL_VITESSE:
Brand101 0:444ac4067b19 238 SendAck(ACKNOWLEDGE_MOTEUR, ODOMETRIE_SMALL_VITESSE);
Brand101 0:444ac4067b19 239 SendSpeed(consigne_pos, 0);
Brand101 0:444ac4067b19 240 break;
Brand101 0:444ac4067b19 241
Brand101 0:444ac4067b19 242 case ASSERVISSEMENT_STOP:
Brand101 0:444ac4067b19 243 SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_STOP);
Brand101 0:444ac4067b19 244 stop_receive = 1;
Brand101 0:444ac4067b19 245 liste[cpt_ordre].type = TYPE_DEPLACEMENT_STOP;
Brand101 0:444ac4067b19 246 break;
Brand101 0:444ac4067b19 247
Brand101 0:444ac4067b19 248 case ASSERVISSEMENT_SPEED_DANGER: //
Brand101 0:444ac4067b19 249 SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_SPEED_DANGER);
Brand101 0:444ac4067b19 250 break;
Brand101 0:444ac4067b19 251
Brand101 0:444ac4067b19 252 case ASSERVISSEMENT_XYT:
Brand101 0:444ac4067b19 253 SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_XYT);
Brand101 0:444ac4067b19 254 etat_automate_xytheta = 1;
Brand101 0:444ac4067b19 255 liste[cpt_ordre].type = TYPE_DEPLACEMENT_X_Y_THETA;
Brand101 0:444ac4067b19 256 liste[cpt_ordre].x = (msgRxBuffer[FIFO_lecture].data[0]) + (msgRxBuffer[FIFO_lecture].data[1] << 8);
Brand101 0:444ac4067b19 257 liste[cpt_ordre].y = (msgRxBuffer[FIFO_lecture].data[2]) + (msgRxBuffer[FIFO_lecture].data[3] << 8);
Brand101 0:444ac4067b19 258 liste[cpt_ordre].theta = (msgRxBuffer[FIFO_lecture].data[4]) + (msgRxBuffer[FIFO_lecture].data[5] << 8);
Brand101 0:444ac4067b19 259 liste[cpt_ordre].sens = msgRxBuffer[FIFO_lecture].data[6];
Brand101 0:444ac4067b19 260 break;
Brand101 0:444ac4067b19 261
Brand101 0:444ac4067b19 262 case ASSERVISSEMENT_COURBURE:
Brand101 0:444ac4067b19 263 SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_COURBURE);
Brand101 0:444ac4067b19 264 liste[cpt_ordre].type = TYPE_DEPLACEMENT_RAYON_COURBURE;
Brand101 0:444ac4067b19 265 liste[cpt_ordre].rayon = (msgRxBuffer[FIFO_lecture].data[0]) + (msgRxBuffer[FIFO_lecture].data[1] << 8);
Brand101 0:444ac4067b19 266 liste[cpt_ordre].theta_ray = (msgRxBuffer[FIFO_lecture].data[2]) + (msgRxBuffer[FIFO_lecture].data[3] << 8);
Brand101 0:444ac4067b19 267 liste[cpt_ordre].sens = msgRxBuffer[FIFO_lecture].data[4];
Brand101 0:444ac4067b19 268 liste[cpt_ordre].enchainement = msgRxBuffer[FIFO_lecture].data[5];
Brand101 0:444ac4067b19 269 break;
Brand101 0:444ac4067b19 270
Brand101 0:444ac4067b19 271 case ASSERVISSEMENT_CONFIG:
Brand101 0:444ac4067b19 272 SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_CONFIG);
Brand101 0:444ac4067b19 273 VMAX = ((msgRxBuffer[FIFO_lecture].data[0]) + (msgRxBuffer[FIFO_lecture].data[1] << 8));
Brand101 0:444ac4067b19 274 AMAX = ((msgRxBuffer[FIFO_lecture].data[2]) + (msgRxBuffer[FIFO_lecture].data[3] << 8));
Brand101 0:444ac4067b19 275 ralentare = 1;
Brand101 0:444ac4067b19 276 break;
Brand101 0:444ac4067b19 277
Brand101 0:444ac4067b19 278 case ASSERVISSEMENT_ROTATION:
Brand101 0:444ac4067b19 279 SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_ROTATION);
Brand101 0:444ac4067b19 280 liste[cpt_ordre].type = TYPE_DEPLACEMENT_ROTATION;
Brand101 0:444ac4067b19 281 liste[cpt_ordre].angle = ((msgRxBuffer[FIFO_lecture].data[0]) + (msgRxBuffer[FIFO_lecture].data[1] << 8));
Brand101 0:444ac4067b19 282 Rotate (Odo_theta);
Brand101 0:444ac4067b19 283 break;
Brand101 0:444ac4067b19 284
Brand101 0:444ac4067b19 285 case ASSERVISSEMENT_RECALAGE:
Brand101 0:444ac4067b19 286 SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_RECALAGE);
Brand101 0:444ac4067b19 287 liste[cpt_ordre].type = TYPE_DEPLACEMENT_RECALAGE;
Brand101 0:444ac4067b19 288 liste[cpt_ordre].distance = ((msgRxBuffer[FIFO_lecture].data[0]) + (msgRxBuffer[FIFO_lecture].data[1] << 8));
Brand101 0:444ac4067b19 289 liste[cpt_ordre].recalage = ((msgRxBuffer[FIFO_lecture].data[3]) + (msgRxBuffer[FIFO_lecture].data[4] << 8));
Brand101 0:444ac4067b19 290 liste[cpt_ordre].val_recalage = msgRxBuffer[FIFO_lecture].data[2];
Brand101 0:444ac4067b19 291 recalage_debut = 1;
Brand101 0:444ac4067b19 292 recalage_debut_receive = 1;
Brand101 0:444ac4067b19 293 break;
Brand101 0:444ac4067b19 294
Brand101 0:444ac4067b19 295 case ASSERVISSEMENT_LIGNE_DROITE: // n'existe pas encore
Brand101 0:444ac4067b19 296 //SendAck(ACKNOWLEDGE_MOTEUR, ASSERVISSEMENT_LIGNE_DROITE);
Brand101 0:444ac4067b19 297 test8 = 10;
Brand101 0:444ac4067b19 298 if(msgRxBuffer[FIFO_lecture].data[2] == 0) {
Brand101 0:444ac4067b19 299 liste[cpt_ordre].type = TYPE_DEPLACEMENT_LIGNE_DROITE;
Brand101 0:444ac4067b19 300 } else {
Brand101 0:444ac4067b19 301 liste[cpt_ordre].type = TYPE_DEPLACEMENT_RECALAGE;
Brand101 0:444ac4067b19 302 liste[cpt_ordre].recalage = ((msgRxBuffer[FIFO_lecture].data[3]) + (msgRxBuffer[FIFO_lecture].data[4] << 8));
Brand101 0:444ac4067b19 303 }
Brand101 0:444ac4067b19 304 recalage_debut_receive = msgRxBuffer[FIFO_lecture].data[2];
Brand101 0:444ac4067b19 305 liste[cpt_ordre].distance = ((msgRxBuffer[FIFO_lecture].data[1] << 8) + (msgRxBuffer[FIFO_lecture].data[0]));
Brand101 0:444ac4067b19 306 liste[cpt_ordre].enchainement = msgRxBuffer[FIFO_lecture].data[5];
Brand101 0:444ac4067b19 307 //GoStraight(); /////////////////////////////////
Brand101 0:444ac4067b19 308 break;
Brand101 0:444ac4067b19 309
Brand101 0:444ac4067b19 310 case MOTOR_IMOBILE:
Brand101 0:444ac4067b19 311 SendAck(ACKNOWLEDGE_MOTEUR, MOTOR_IMOBILE);
Brand101 0:444ac4067b19 312 liste[cpt_ordre].type = TYPE_DEPLACEMENT_IMMOBILE;
Brand101 0:444ac4067b19 313 break;
Brand101 0:444ac4067b19 314
Brand101 0:444ac4067b19 315 }
Brand101 0:444ac4067b19 316
Brand101 0:444ac4067b19 317 FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
Brand101 0:444ac4067b19 318 }
Brand101 0:444ac4067b19 319 }
Brand101 0:444ac4067b19 320
Brand101 0:444ac4067b19 321
Brand101 0:444ac4067b19 322 void Asser_Pos_Mot1(double pcons_pos)
Brand101 0:444ac4067b19 323 {
Brand101 0:444ac4067b19 324 pos1 = (double)encoder1.GetCounter();
Brand101 0:444ac4067b19 325
Brand101 0:444ac4067b19 326 ErreurPos1 = pcons_pos - pos1;
Brand101 0:444ac4067b19 327 Delta_ErreurPos1 = ErreurPos1 - last_ErreurPos1;
Brand101 0:444ac4067b19 328 Somme_ErreurPos1 += ErreurPos1;
Brand101 0:444ac4067b19 329
Brand101 0:444ac4067b19 330 if(Somme_ErreurPos1>1.0) {
Brand101 0:444ac4067b19 331 Somme_ErreurPos1=1.0;
Brand101 0:444ac4067b19 332 } else if(Somme_ErreurPos1<-1.0) {
Brand101 0:444ac4067b19 333 Somme_ErreurPos1=-1.0;
Brand101 0:444ac4067b19 334 }
Brand101 0:444ac4067b19 335
Brand101 0:444ac4067b19 336 commande1 = (Kpp1 * ErreurPos1 + Kip1 * Somme_ErreurPos1 + Kdp1 * Delta_ErreurPos1);
Brand101 0:444ac4067b19 337
Brand101 0:444ac4067b19 338 last_ErreurPos1 = ErreurPos1;
Brand101 0:444ac4067b19 339 last_pos1 = pos1;
Brand101 0:444ac4067b19 340
Brand101 0:444ac4067b19 341 commande1=commande1/2500.;
Brand101 0:444ac4067b19 342
Brand101 0:444ac4067b19 343 if(commande1>1.0) {
Brand101 0:444ac4067b19 344 commande1=1.0;
Brand101 0:444ac4067b19 345 } else if(commande1<-1.0) {
Brand101 0:444ac4067b19 346 commande1=-1.0;
Brand101 0:444ac4067b19 347 }
Brand101 0:444ac4067b19 348
Brand101 0:444ac4067b19 349 //test5++;
Brand101 0:444ac4067b19 350 }
Brand101 0:444ac4067b19 351
Brand101 0:444ac4067b19 352 void Asser_Pos_Mot2(double pcons_pos)
Brand101 0:444ac4067b19 353 {
Brand101 0:444ac4067b19 354 pos2 = (double)encoder2.GetCounter();
Brand101 0:444ac4067b19 355
Brand101 0:444ac4067b19 356 ErreurPos2 = pcons_pos - pos2;
Brand101 0:444ac4067b19 357
Brand101 0:444ac4067b19 358 Delta_ErreurPos2 = ErreurPos2 - last_ErreurPos2;
Brand101 0:444ac4067b19 359 Somme_ErreurPos2 += ErreurPos2;
Brand101 0:444ac4067b19 360
Brand101 0:444ac4067b19 361 if(Somme_ErreurPos2>1.0)
Brand101 0:444ac4067b19 362 Somme_ErreurPos2=1.0;
Brand101 0:444ac4067b19 363 else if(Somme_ErreurPos2<-1.0)
Brand101 0:444ac4067b19 364 Somme_ErreurPos2=-1.0;
Brand101 0:444ac4067b19 365
Brand101 0:444ac4067b19 366 commande2 = (Kpp2 * ErreurPos2 + Kip2 * Somme_ErreurPos2 + Kdp2 * Delta_ErreurPos2);
Brand101 0:444ac4067b19 367
Brand101 0:444ac4067b19 368 last_ErreurPos2 = ErreurPos2;
Brand101 0:444ac4067b19 369 last_pos2 = pos2;
Brand101 0:444ac4067b19 370
Brand101 0:444ac4067b19 371 commande2=commande2/2500.;
Brand101 0:444ac4067b19 372
Brand101 0:444ac4067b19 373 if(commande2>1.0) {
Brand101 0:444ac4067b19 374 commande2=1.0;
Brand101 0:444ac4067b19 375 } else if(commande2<-1.0) {
Brand101 0:444ac4067b19 376 commande2=-1.0;
Brand101 0:444ac4067b19 377 }
Brand101 0:444ac4067b19 378
Brand101 0:444ac4067b19 379 test6++;
Brand101 0:444ac4067b19 380 }
Brand101 0:444ac4067b19 381
Brand101 0:444ac4067b19 382
Brand101 0:444ac4067b19 383 void Avancer_ligne_droite(long pcons, short vmax, short amax, short dmax)
Brand101 0:444ac4067b19 384 {
Brand101 0:444ac4067b19 385 //Declaration des variables
Brand101 0:444ac4067b19 386 static double tc, ta, td; //tc temps � vitesse constante, ta en acceleration, td en deceleration
Brand101 0:444ac4067b19 387 static double vmax_tri; //Vitesse maximale atteinte dans le cas d'un profil en triangle
Brand101 0:444ac4067b19 388
Brand101 0:444ac4067b19 389 if(pcons >= 0) { //Mode avancer
Brand101 0:444ac4067b19 390 switch(etat_automate_depl) { //Automate de gestion du deplacement
Brand101 0:444ac4067b19 391 case INITIALISATION : //Etat d'initialisation et de calcul des variables
Brand101 0:444ac4067b19 392 //Remise a zero des variables car on effectue un nouveau deplacement
Brand101 0:444ac4067b19 393 consigne_pos = 0;
Brand101 0:444ac4067b19 394 consigne_vit = 0;
Brand101 0:444ac4067b19 395 cpt = 0;
Brand101 0:444ac4067b19 396
Brand101 0:444ac4067b19 397 //Elaboration du profil de vitesse
Brand101 0:444ac4067b19 398 //Calcul du temps � vitesse constante en fonction de l'acceleration
Brand101 0:444ac4067b19 399 tc = ((double)pcons / (double)vmax) - (((double)vmax * 500.0) / (double)amax) - (((double)vmax * 500.0) / (double)dmax);
Brand101 0:444ac4067b19 400
Brand101 0:444ac4067b19 401 if (tc > 0) { //Resultat positif, il y a une phase de vitesse constante
Brand101 0:444ac4067b19 402 etat_automate_depl = ACCELERATION_TRAPEZE; //Passage a l'etat ACCELERATION_TRAPEZE
Brand101 0:444ac4067b19 403
Brand101 0:444ac4067b19 404 ta = (((double)vmax * 1000.0) / (double)amax); //Calcul du temps d'acceleration
Brand101 0:444ac4067b19 405 td = (((double)vmax * 1000.0) / (double)dmax); //Calcul du temps de deceleration
Brand101 0:444ac4067b19 406 }
Brand101 0:444ac4067b19 407
Brand101 0:444ac4067b19 408 else if(tc < 0) { //Resultat negatif, il n'y a pas de phase de vitesse constante
Brand101 0:444ac4067b19 409 // Le prochain �tat de l'automate sera l'�tat 5
Brand101 0:444ac4067b19 410 etat_automate_depl = ACCELERATION_TRIANGLE; //Passage a l'etat ACCELERATION_TRIANGLE
Brand101 0:444ac4067b19 411
Brand101 0:444ac4067b19 412 //Calcul de la vitesse maximale en profil triangle
Brand101 0:444ac4067b19 413 vmax_tri = sqrt((2 * pcons) / ((1000.0 / (double)amax) + (1000.0 / (double)dmax)));
Brand101 0:444ac4067b19 414 }
Brand101 0:444ac4067b19 415 break;
Brand101 0:444ac4067b19 416
Brand101 0:444ac4067b19 417 case ACCELERATION_TRAPEZE : //Etat d'acceleration en profil trapeze
Brand101 0:444ac4067b19 418 cpt ++;
Brand101 0:444ac4067b19 419
Brand101 0:444ac4067b19 420 //Incrementation de la consigne de vitesse par la valeur de l'acceleration
Brand101 0:444ac4067b19 421 consigne_vit += amax;
Brand101 0:444ac4067b19 422
Brand101 0:444ac4067b19 423 //Incrementation de la consigne de position
Brand101 0:444ac4067b19 424 consigne_pos += ((double)consigne_vit / 1000.0);
Brand101 0:444ac4067b19 425
Brand101 0:444ac4067b19 426 if(cpt >= ta) { //Condition pour quitter la phase d'acceleration
Brand101 0:444ac4067b19 427 etat_automate_depl = VITESSE_CONSTANTE_TRAPEZE; //Passage a l'etat VITESSE_CONSTANTE
Brand101 0:444ac4067b19 428 cpt = 0;
Brand101 0:444ac4067b19 429 }
Brand101 0:444ac4067b19 430 break;
Brand101 0:444ac4067b19 431
Brand101 0:444ac4067b19 432 case VITESSE_CONSTANTE_TRAPEZE : //Etat de vitesse constante en profil trapeze
Brand101 0:444ac4067b19 433 cpt ++;
Brand101 0:444ac4067b19 434
Brand101 0:444ac4067b19 435 //Incrementation de la consigne de position
Brand101 0:444ac4067b19 436 consigne_pos += vmax;
Brand101 0:444ac4067b19 437
Brand101 0:444ac4067b19 438 //Si il n'y a pas d'enchainements
Brand101 0:444ac4067b19 439 if(cpt >= tc && liste[cpt_ordre].enchainement != 1) { //Condition pour quitter la phase de vitesse constante
Brand101 0:444ac4067b19 440 etat_automate_depl = DECELERATION_TRAPEZE; //Passage a l'etat DECELERATION
Brand101 0:444ac4067b19 441 cpt = 0;
Brand101 0:444ac4067b19 442 }
Brand101 0:444ac4067b19 443
Brand101 0:444ac4067b19 444 //Si on est dans un enchainement, on passe � l'instruction suivante
Brand101 0:444ac4067b19 445 else if(consigne_pos >= pcons && liste[cpt_ordre].enchainement == 1) {
Brand101 0:444ac4067b19 446 consigne_pos = pcons;
Brand101 0:444ac4067b19 447 cpt_ordre = 0;
Brand101 0:444ac4067b19 448 }
Brand101 0:444ac4067b19 449 break;
Brand101 0:444ac4067b19 450
Brand101 0:444ac4067b19 451 case DECELERATION_TRAPEZE : //Etat de deceleration en profil trapeze
Brand101 0:444ac4067b19 452 cpt ++;
Brand101 0:444ac4067b19 453
Brand101 0:444ac4067b19 454 //Incrementation de la consigne de position
Brand101 0:444ac4067b19 455 consigne_pos += vmax - (double)(((double)cpt * (double)dmax) / 1000.0);
Brand101 0:444ac4067b19 456
Brand101 0:444ac4067b19 457 if(cpt >= td) { //Condition pour quitter la phase de deceleration en profil trapeze
Brand101 0:444ac4067b19 458 etat_automate_depl = ARRET; //Passage a l'etat ARRET
Brand101 0:444ac4067b19 459 consigne_pos = pcons;
Brand101 0:444ac4067b19 460 cpt = 0;
Brand101 0:444ac4067b19 461 }
Brand101 0:444ac4067b19 462 break;
Brand101 0:444ac4067b19 463
Brand101 0:444ac4067b19 464 case ARRET : //Etat d'arret
Brand101 0:444ac4067b19 465 if(cpt >= 20) { //Condition pour sortir de l'etat arret
Brand101 0:444ac4067b19 466 etat_automate_depl = INITIALISATION; //Passage a l'etat INITIALISATION
Brand101 0:444ac4067b19 467 next_move = 1;
Brand101 0:444ac4067b19 468 }
Brand101 0:444ac4067b19 469 cpt ++;
Brand101 0:444ac4067b19 470 break;
Brand101 0:444ac4067b19 471
Brand101 0:444ac4067b19 472 case ACCELERATION_TRIANGLE : //Etat d'acceleration en profil triangle
Brand101 0:444ac4067b19 473 cpt ++;
Brand101 0:444ac4067b19 474
Brand101 0:444ac4067b19 475 //Incrementation de la consigne de position
Brand101 0:444ac4067b19 476 consigne_pos += (((double)cpt * (double)amax) / 1000.0);
Brand101 0:444ac4067b19 477
Brand101 0:444ac4067b19 478 if((cpt * amax / 1000.0) >= vmax_tri) { //Condition pour sortir de l'etat acceleration en profil triangle
Brand101 0:444ac4067b19 479 etat_automate_depl = DECELERATION_TRIANGLE; //Passage a l'etat DECELERATION_TRIANGLE
Brand101 0:444ac4067b19 480 cpt = 0;
Brand101 0:444ac4067b19 481 }
Brand101 0:444ac4067b19 482 break;
Brand101 0:444ac4067b19 483
Brand101 0:444ac4067b19 484 case DECELERATION_TRIANGLE : //Etat de deceleration en profil triangle
Brand101 0:444ac4067b19 485 cpt ++;
Brand101 0:444ac4067b19 486
Brand101 0:444ac4067b19 487 //Incrementation de la consigne de position
Brand101 0:444ac4067b19 488 consigne_pos += vmax_tri - (((double)cpt * (double)dmax) / 1000.0);
Brand101 0:444ac4067b19 489
Brand101 0:444ac4067b19 490 if(((double)vmax_tri - (((double)cpt * (double)dmax)) / 1000.0) <= 0) { //Condition pour sortir de l'etat deceleration en profil triangle
Brand101 0:444ac4067b19 491 etat_automate_depl = ARRET; //Passage a l'etat ARRET
Brand101 0:444ac4067b19 492 cpt = 0;
Brand101 0:444ac4067b19 493 }
Brand101 0:444ac4067b19 494 break;
Brand101 0:444ac4067b19 495 }
Brand101 0:444ac4067b19 496 }
Brand101 0:444ac4067b19 497
Brand101 0:444ac4067b19 498 else if(pcons < 0) { //Mode reculer
Brand101 0:444ac4067b19 499 switch(etat_automate_depl) { //Automate de gestion du deplacement
Brand101 0:444ac4067b19 500 case INITIALISATION : //Etat d'initialisation et de calcul des variables
Brand101 0:444ac4067b19 501 //Remise a zero des variables car on effectue un nouveau deplacement
Brand101 0:444ac4067b19 502 consigne_pos = 0;
Brand101 0:444ac4067b19 503 cpt = 0;
Brand101 0:444ac4067b19 504
Brand101 0:444ac4067b19 505 //Elaboration du profil de vitesse
Brand101 0:444ac4067b19 506 //Calcul du temps � vitesse constante en fonction de l'acceleration
Brand101 0:444ac4067b19 507 tc = ((double)-pcons / (double)vmax) - (((double)vmax * 500.0) / (double)amax) - (((double)vmax * 500.0) / (double)dmax);
Brand101 0:444ac4067b19 508
Brand101 0:444ac4067b19 509 if (tc > 0) { //Resultat positif, il y a une phase de vitesse constante
Brand101 0:444ac4067b19 510 etat_automate_depl = ACCELERATION_TRAPEZE; //Passage a l'etat ACCELERATION_TRAPEZE
Brand101 0:444ac4067b19 511
Brand101 0:444ac4067b19 512 ta = (((double)vmax * 1000.0) / (double)amax); //Calcul du temps d'acceleration
Brand101 0:444ac4067b19 513 td = (((double)vmax * 1000.0) / (double)dmax); //Calcul du temps de deceleration
Brand101 0:444ac4067b19 514 }
Brand101 0:444ac4067b19 515
Brand101 0:444ac4067b19 516 else if(tc < 0) { //Resultat negatif, il n'y a pas de phase de vitesse constante
Brand101 0:444ac4067b19 517 etat_automate_depl = ACCELERATION_TRIANGLE; //Passage a l'etat ACCELERATION_TRIANGLE
Brand101 0:444ac4067b19 518
Brand101 0:444ac4067b19 519 //Calcul de la vitesse maximale en profil triangle
Brand101 0:444ac4067b19 520 vmax_tri = sqrt((2 * -pcons) / ((1000.0 / (double)amax) + (1000.0 / (double)dmax)));
Brand101 0:444ac4067b19 521 }
Brand101 0:444ac4067b19 522 break;
Brand101 0:444ac4067b19 523
Brand101 0:444ac4067b19 524 case ACCELERATION_TRAPEZE : //Etat d'acceleration en profil trapeze
Brand101 0:444ac4067b19 525 cpt ++;
Brand101 0:444ac4067b19 526
Brand101 0:444ac4067b19 527 //Decrementation de la consigne de position
Brand101 0:444ac4067b19 528 consigne_pos -= (double)(((double)cpt * (double)amax)/1000.0);
Brand101 0:444ac4067b19 529
Brand101 0:444ac4067b19 530 if(cpt >= ta) { //Condition pour quitter la phase d'acceleration
Brand101 0:444ac4067b19 531 etat_automate_depl = VITESSE_CONSTANTE_TRAPEZE; //Passage a l'etat VITESSE_CONSTANTE_TRAPEZE
Brand101 0:444ac4067b19 532 cpt = 0;
Brand101 0:444ac4067b19 533 }
Brand101 0:444ac4067b19 534 break;
Brand101 0:444ac4067b19 535
Brand101 0:444ac4067b19 536 case VITESSE_CONSTANTE_TRAPEZE : //Etat de vitesse continue en profil trapeze
Brand101 0:444ac4067b19 537 cpt ++;
Brand101 0:444ac4067b19 538
Brand101 0:444ac4067b19 539 //Decrementation de la consigne de position
Brand101 0:444ac4067b19 540 consigne_pos -= vmax;
Brand101 0:444ac4067b19 541
Brand101 0:444ac4067b19 542 //Si il n'y a pas d'enchainements
Brand101 0:444ac4067b19 543 if(cpt >= tc && liste[cpt_ordre].enchainement != 1) { //Condition pour sortir de l'etat de vitesse constante en profil trapeze
Brand101 0:444ac4067b19 544 etat_automate_depl = DECELERATION_TRAPEZE; //Passage a l'etat de DECELERATION_TRAPEZE
Brand101 0:444ac4067b19 545 cpt = 0;
Brand101 0:444ac4067b19 546 }
Brand101 0:444ac4067b19 547
Brand101 0:444ac4067b19 548 //Si on est dans un enchainement, on passe � l'instruction suivante
Brand101 0:444ac4067b19 549 else if(consigne_pos >= pcons && liste[cpt_ordre].enchainement == 1) {
Brand101 0:444ac4067b19 550 consigne_pos = pcons;
Brand101 0:444ac4067b19 551 cpt_ordre = 0;
Brand101 0:444ac4067b19 552 }
Brand101 0:444ac4067b19 553 break;
Brand101 0:444ac4067b19 554
Brand101 0:444ac4067b19 555 case DECELERATION_TRAPEZE : //Etat de deceleration en profil trapeze
Brand101 0:444ac4067b19 556 cpt ++;
Brand101 0:444ac4067b19 557
Brand101 0:444ac4067b19 558 //Decrementation de la consigne de position
Brand101 0:444ac4067b19 559 consigne_pos -= vmax - (double)(((double)cpt * (double)dmax) / 1000.0);
Brand101 0:444ac4067b19 560
Brand101 0:444ac4067b19 561 if(cpt >= td) { //Condition pour sortir de l'etat deceleration en profil trapeze
Brand101 0:444ac4067b19 562 etat_automate_depl = ARRET; //Passage a l'etat ARRET
Brand101 0:444ac4067b19 563 consigne_pos = pcons;
Brand101 0:444ac4067b19 564 cpt = 0;
Brand101 0:444ac4067b19 565 }
Brand101 0:444ac4067b19 566 break;
Brand101 0:444ac4067b19 567
Brand101 0:444ac4067b19 568 case ARRET : //Etat d'arret
Brand101 0:444ac4067b19 569 if(cpt >= 20) { //Condition pour sortir de l'etat arret
Brand101 0:444ac4067b19 570 etat_automate_depl = INITIALISATION; //Passage a l'etat d'INITIALISATION
Brand101 0:444ac4067b19 571 next_move = 1;
Brand101 0:444ac4067b19 572 }
Brand101 0:444ac4067b19 573 cpt ++;
Brand101 0:444ac4067b19 574 break;
Brand101 0:444ac4067b19 575
Brand101 0:444ac4067b19 576 case ACCELERATION_TRIANGLE : //Etat d'acceleration en profil triangle
Brand101 0:444ac4067b19 577 cpt ++;
Brand101 0:444ac4067b19 578
Brand101 0:444ac4067b19 579 //Decrementation de la consigne de position
Brand101 0:444ac4067b19 580 consigne_pos -= (((double)cpt * (double)amax) / 1000.0);
Brand101 0:444ac4067b19 581
Brand101 0:444ac4067b19 582 if((cpt * amax / 1000.0) >= vmax_tri) { //Condition pour sortir de l'etat d'acceleration en profil triangle
Brand101 0:444ac4067b19 583 etat_automate_depl = DECELERATION_TRIANGLE; //Passage a l'etat DECELERATION_TRIANGLE
Brand101 0:444ac4067b19 584 cpt = 0;
Brand101 0:444ac4067b19 585 }
Brand101 0:444ac4067b19 586 break;
Brand101 0:444ac4067b19 587
Brand101 0:444ac4067b19 588 case DECELERATION_TRIANGLE : //Etat de deceleration en profil triangle
Brand101 0:444ac4067b19 589 cpt ++;
Brand101 0:444ac4067b19 590
Brand101 0:444ac4067b19 591 //Decrementation de la consigne de position
Brand101 0:444ac4067b19 592 consigne_pos -= vmax_tri - (((double)cpt * (double)dmax) / 1000.0);
Brand101 0:444ac4067b19 593
Brand101 0:444ac4067b19 594 if(((double)vmax_tri - (((double)cpt * (double)dmax)) / 1000.0) <= 0) { //Condition pour sortir de l'etat de deceleration en profil triangle
Brand101 0:444ac4067b19 595 etat_automate_depl = ARRET; //Passage a l'etat ARRET
Brand101 0:444ac4067b19 596 cpt = 0;
Brand101 0:444ac4067b19 597 }
Brand101 0:444ac4067b19 598 break;
Brand101 0:444ac4067b19 599 }
Brand101 0:444ac4067b19 600 }
Brand101 0:444ac4067b19 601
Brand101 0:444ac4067b19 602
Brand101 0:444ac4067b19 603 //Calcul des commande 1 et 2
Brand101 0:444ac4067b19 604 Asser_Pos_Mot1(roue_drt_init + consigne_pos);
Brand101 0:444ac4067b19 605 Asser_Pos_Mot2(roue_gch_init + consigne_pos);
Brand101 0:444ac4067b19 606
Brand101 0:444ac4067b19 607 //Ecriture du PWM sur chaque modeur //*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*
Brand101 0:444ac4067b19 608 write_PWM2(commande2);
Brand101 0:444ac4067b19 609 write_PWM1(commande1);
Brand101 0:444ac4067b19 610
Brand101 0:444ac4067b19 611 }
Brand101 0:444ac4067b19 612
Brand101 0:444ac4067b19 613
Brand101 0:444ac4067b19 614 void Avancer_ligne_droite_XYTheta(long pcons, short vmax, short amax, short dmax)
Brand101 0:444ac4067b19 615 {
Brand101 0:444ac4067b19 616 //Declaration des variables
Brand101 0:444ac4067b19 617 //char val1[8];
Brand101 0:444ac4067b19 618 static double tc, ta, td; //tc temps � vitesse constante, ta en acceleration, td en deceleration
Brand101 0:444ac4067b19 619 static double vmax_tri; //Vitesse maximale atteinte dans le cas d'un profil en triangle
Brand101 0:444ac4067b19 620
Brand101 0:444ac4067b19 621 if(pcons >= 0) { //Mode avancer
Brand101 0:444ac4067b19 622 switch(etat_automate_depl) { //Automate de gestion du deplacement
Brand101 0:444ac4067b19 623 case INITIALISATION : //Etat d'initialisation et de calcul des variables
Brand101 0:444ac4067b19 624 //remise a zero des variables car on effectue un nouveau deplacement
Brand101 0:444ac4067b19 625 consigne_pos = 0;
Brand101 0:444ac4067b19 626 cpt = 0;
Brand101 0:444ac4067b19 627
Brand101 0:444ac4067b19 628 //Elaboration du profil de vitesse
Brand101 0:444ac4067b19 629 //Calcul du temps a vitesse constante en fonction de l'acceleration
Brand101 0:444ac4067b19 630 tc = ((double)pcons / (double)vmax) - (((double)vmax * 500.0) / (double)amax) - (((double)vmax * 500.0) / (double)dmax);
Brand101 0:444ac4067b19 631
Brand101 0:444ac4067b19 632 if (tc > 0) { //Resultat positif, il y a une phase de vitesse constante
Brand101 0:444ac4067b19 633 etat_automate_depl = ACCELERATION_TRAPEZE; //Passage a l'etat ACCELERATION_TRAPEZE
Brand101 0:444ac4067b19 634
Brand101 0:444ac4067b19 635 ta = (((double)vmax * 1000.0) / (double)amax); //Calcul du temps d'acceleration
Brand101 0:444ac4067b19 636 td = (((double)vmax * 1000.0) / (double)dmax); //Calcul du temps de deceleration
Brand101 0:444ac4067b19 637 }
Brand101 0:444ac4067b19 638
Brand101 0:444ac4067b19 639 else if(tc < 0) { //Resultat negatif, il n'y a pas de phase de vitesse constante
Brand101 0:444ac4067b19 640 etat_automate_depl = ACCELERATION_TRIANGLE; //Passage a l'etat ACCELERATION_TRIANGLE
Brand101 0:444ac4067b19 641
Brand101 0:444ac4067b19 642 //Calcul de la vitesse maximale en profil triangle
Brand101 0:444ac4067b19 643 vmax_tri = sqrt((2 * pcons) / ((1000.0 / (double)amax) + (1000.0 / (double)dmax)));
Brand101 0:444ac4067b19 644 }
Brand101 0:444ac4067b19 645 break;
Brand101 0:444ac4067b19 646
Brand101 0:444ac4067b19 647 case ACCELERATION_TRAPEZE : //Etat d'acceleration en profil trapeze
Brand101 0:444ac4067b19 648 cpt ++;
Brand101 0:444ac4067b19 649
Brand101 0:444ac4067b19 650 //Incrementation de la consigne de position
Brand101 0:444ac4067b19 651 consigne_pos += (((double)cpt * (double)amax) / 1000.0);
Brand101 0:444ac4067b19 652
Brand101 0:444ac4067b19 653 if(cpt >= ta) { //Condition pour quitter la phase d'acceleration
Brand101 0:444ac4067b19 654 etat_automate_depl = VITESSE_CONSTANTE_TRAPEZE; //Passage a l'etat VITESSE_CONSTANTE_TRAPEZE
Brand101 0:444ac4067b19 655 cpt = 0;
Brand101 0:444ac4067b19 656 }
Brand101 0:444ac4067b19 657 break;
Brand101 0:444ac4067b19 658
Brand101 0:444ac4067b19 659 case VITESSE_CONSTANTE_TRAPEZE : //Etat de vitesse constante en profil trapeze
Brand101 0:444ac4067b19 660 cpt ++;
Brand101 0:444ac4067b19 661
Brand101 0:444ac4067b19 662 //Incrementation de la consigne de vitesse par la valeur de l'acceleration
Brand101 0:444ac4067b19 663 consigne_pos += vmax;
Brand101 0:444ac4067b19 664
Brand101 0:444ac4067b19 665 //Si il n'y a pas d'enchainements
Brand101 0:444ac4067b19 666 if(cpt >= tc && liste[cpt_ordre].enchainement != 1) { //Condition pour quitter la phase de vitesse constante
Brand101 0:444ac4067b19 667 etat_automate_depl = DECELERATION_TRAPEZE; //Passage a l'etat DECELERATION_TRAPEZE
Brand101 0:444ac4067b19 668 cpt = 0;
Brand101 0:444ac4067b19 669 }
Brand101 0:444ac4067b19 670 //Si on est dans un enchainement, on passe � l'instruction suivante
Brand101 0:444ac4067b19 671 else if(consigne_pos >= pcons && liste[cpt_ordre].enchainement == 1) {
Brand101 0:444ac4067b19 672 consigne_pos = pcons;
Brand101 0:444ac4067b19 673 cpt_ordre = 0;
Brand101 0:444ac4067b19 674 }
Brand101 0:444ac4067b19 675 break;
Brand101 0:444ac4067b19 676
Brand101 0:444ac4067b19 677 case DECELERATION_TRAPEZE : //Etat de vitesse constante en profil trapeze
Brand101 0:444ac4067b19 678 cpt ++;
Brand101 0:444ac4067b19 679
Brand101 0:444ac4067b19 680 //Incrementation de la consigne de position
Brand101 0:444ac4067b19 681 consigne_pos += vmax - (double)(((double)cpt * (double)dmax) / 1000.0);
Brand101 0:444ac4067b19 682
Brand101 0:444ac4067b19 683 if(cpt >= td) { //Condition pour quitter la phase de deceleration en profil trapeze
Brand101 0:444ac4067b19 684 etat_automate_depl = ARRET; //Passage a l'etat ARRET
Brand101 0:444ac4067b19 685 consigne_pos = pcons;
Brand101 0:444ac4067b19 686 cpt = 0;
Brand101 0:444ac4067b19 687 }
Brand101 0:444ac4067b19 688 break;
Brand101 0:444ac4067b19 689
Brand101 0:444ac4067b19 690 case ARRET : //Etat d'arret
Brand101 0:444ac4067b19 691 if(cpt >= 20) {
Brand101 0:444ac4067b19 692 etat_automate_depl = INITIALISATION; //Passage a l'etat d'INITIALISATION
Brand101 0:444ac4067b19 693 next_move_xyt = 1;
Brand101 0:444ac4067b19 694
Brand101 0:444ac4067b19 695 }
Brand101 0:444ac4067b19 696 cpt ++;
Brand101 0:444ac4067b19 697 break;
Brand101 0:444ac4067b19 698
Brand101 0:444ac4067b19 699 case ACCELERATION_TRIANGLE : //Etat d'acceleration en profil triangle
Brand101 0:444ac4067b19 700 cpt ++;
Brand101 0:444ac4067b19 701
Brand101 0:444ac4067b19 702 //Incrementation de la consigne de position
Brand101 0:444ac4067b19 703 consigne_pos += (((double)cpt * (double)amax) / 1000.0);
Brand101 0:444ac4067b19 704
Brand101 0:444ac4067b19 705 if((cpt * amax / 1000.0) >= vmax_tri) { //Condition pour sortir de la phase d'acceleration en profil triangle
Brand101 0:444ac4067b19 706 etat_automate_depl = DECELERATION_TRIANGLE; //Passage a l'etat DECELERATION_TRIANGLE
Brand101 0:444ac4067b19 707 cpt = 0;
Brand101 0:444ac4067b19 708 }
Brand101 0:444ac4067b19 709 break;
Brand101 0:444ac4067b19 710
Brand101 0:444ac4067b19 711 case DECELERATION_TRIANGLE : //Etat de deceleration en profil triangle
Brand101 0:444ac4067b19 712 cpt ++;
Brand101 0:444ac4067b19 713
Brand101 0:444ac4067b19 714 //Incrementation de la consigne de position
Brand101 0:444ac4067b19 715 consigne_pos += vmax_tri - (((double)cpt * (double)dmax) / 1000.0);
Brand101 0:444ac4067b19 716
Brand101 0:444ac4067b19 717 if(((double)vmax_tri - (((double)cpt * (double)dmax)) / 1000.0) <= 0) { //Condition pour sortir de la phase de deceleration en profil triangle
Brand101 0:444ac4067b19 718 etat_automate_depl = ARRET; //Passage a l'etat ARRET
Brand101 0:444ac4067b19 719 cpt = 0;
Brand101 0:444ac4067b19 720 }
Brand101 0:444ac4067b19 721 break;
Brand101 0:444ac4067b19 722 }
Brand101 0:444ac4067b19 723 } else if(pcons < 0) { //Mode reculer
Brand101 0:444ac4067b19 724 switch(etat_automate_depl) { //Automate de gestion du deplacement
Brand101 0:444ac4067b19 725 case INITIALISATION : //Etat d'initialisation et de calcul des variables
Brand101 0:444ac4067b19 726 //Remise a zero des variables car on effectue un nouveau deplacement
Brand101 0:444ac4067b19 727 consigne_pos = 0;
Brand101 0:444ac4067b19 728 cpt = 0;
Brand101 0:444ac4067b19 729
Brand101 0:444ac4067b19 730 //Elaboration du profil de vitesse
Brand101 0:444ac4067b19 731 //Calcul du temps � vitesse constante en fonction de l'acceleration
Brand101 0:444ac4067b19 732 tc = ((double)-pcons / (double)vmax) - (((double)vmax * 500.0) / (double)amax) - (((double)vmax * 500.0) / (double)dmax);
Brand101 0:444ac4067b19 733
Brand101 0:444ac4067b19 734 if (tc > 0) { //Resultat positif, il y a une phase de vitesse constante
Brand101 0:444ac4067b19 735 etat_automate_depl = ACCELERATION_TRAPEZE; //Passage a l'etat ACCELERATION_TRAPEZE
Brand101 0:444ac4067b19 736
Brand101 0:444ac4067b19 737 ta = (((double)vmax * 1000.0) / (double)amax); //Calcul du temps d'acceleration
Brand101 0:444ac4067b19 738 td = (((double)vmax * 1000.0) / (double)dmax); //Calcul du temps de deceleration
Brand101 0:444ac4067b19 739 } else if(tc < 0) { //Resultat negatif, il n'y a pas de phase de vitesse constante
Brand101 0:444ac4067b19 740 etat_automate_depl = ACCELERATION_TRIANGLE; //Passage a l'etat ACCELERATION_TRIANGLE
Brand101 0:444ac4067b19 741
Brand101 0:444ac4067b19 742 //Calcul de la vitesse maximale en profil triangle
Brand101 0:444ac4067b19 743 vmax_tri = sqrt((2 * -pcons) / ((1000.0 / (double)amax) + (1000.0 / (double)dmax)));
Brand101 0:444ac4067b19 744 }
Brand101 0:444ac4067b19 745 break;
Brand101 0:444ac4067b19 746
Brand101 0:444ac4067b19 747 case ACCELERATION_TRAPEZE : //Etat d'acceleration en profil trapeze
Brand101 0:444ac4067b19 748 cpt ++;
Brand101 0:444ac4067b19 749
Brand101 0:444ac4067b19 750 //Decrementation de la consigne de position
Brand101 0:444ac4067b19 751 consigne_pos -= (double)(((double)cpt * (double)amax)/1000.0);
Brand101 0:444ac4067b19 752
Brand101 0:444ac4067b19 753 if(cpt >= ta) { //Condition pour quitter la phase d'acceleration
Brand101 0:444ac4067b19 754 etat_automate_depl = VITESSE_CONSTANTE_TRAPEZE; //Passage a l'etat VITESSE_CONSTANTE_TRAPEZE
Brand101 0:444ac4067b19 755 cpt = 0;
Brand101 0:444ac4067b19 756 }
Brand101 0:444ac4067b19 757 break;
Brand101 0:444ac4067b19 758
Brand101 0:444ac4067b19 759 case VITESSE_CONSTANTE_TRAPEZE : //Etat de vitesse continue en profil trapeze
Brand101 0:444ac4067b19 760 cpt ++;
Brand101 0:444ac4067b19 761
Brand101 0:444ac4067b19 762 //Decrementation de la consigne de position
Brand101 0:444ac4067b19 763 consigne_pos -= vmax;
Brand101 0:444ac4067b19 764
Brand101 0:444ac4067b19 765 //Si il n'y a pas d'enchainements
Brand101 0:444ac4067b19 766 if(cpt >= tc && liste[cpt_ordre].enchainement != 1) { //Condition pour sortir de l'etat de vitesse constante en profil trapeze
Brand101 0:444ac4067b19 767 etat_automate_depl = DECELERATION_TRAPEZE; //Passage a l'etat de DECELERATION_TRAPEZE
Brand101 0:444ac4067b19 768 cpt = 0;
Brand101 0:444ac4067b19 769 }
Brand101 0:444ac4067b19 770
Brand101 0:444ac4067b19 771 //Si on est dans un enchainement, on passe � l'instruction suivante
Brand101 0:444ac4067b19 772 else if(consigne_pos >= pcons && liste[cpt_ordre].enchainement == 1) {
Brand101 0:444ac4067b19 773 consigne_pos = pcons;
Brand101 0:444ac4067b19 774 cpt_ordre = 0;
Brand101 0:444ac4067b19 775 }
Brand101 0:444ac4067b19 776 break;
Brand101 0:444ac4067b19 777
Brand101 0:444ac4067b19 778 case DECELERATION_TRAPEZE : //Etat de deceleration en profil trapeze
Brand101 0:444ac4067b19 779 cpt ++;
Brand101 0:444ac4067b19 780
Brand101 0:444ac4067b19 781 //Decrementation de la consigne de position
Brand101 0:444ac4067b19 782 consigne_pos -= vmax - (double)(((double)cpt * (double)dmax) / 1000.0);
Brand101 0:444ac4067b19 783
Brand101 0:444ac4067b19 784 if(cpt >= td) { //Condition pour sortir de l'etat vitesse continue en profil trapeze
Brand101 0:444ac4067b19 785 etat_automate_depl = ARRET; //Passage a l'etat ARRET
Brand101 0:444ac4067b19 786 consigne_pos = pcons;
Brand101 0:444ac4067b19 787 cpt = 0;
Brand101 0:444ac4067b19 788 }
Brand101 0:444ac4067b19 789 break;
Brand101 0:444ac4067b19 790
Brand101 0:444ac4067b19 791 case ARRET : //Etat d'arret
Brand101 0:444ac4067b19 792 if(cpt >= 20) {
Brand101 0:444ac4067b19 793 etat_automate_depl = INITIALISATION; //Passage a l'etat INITIALISATION
Brand101 0:444ac4067b19 794 next_move_xyt = 1;
Brand101 0:444ac4067b19 795 }
Brand101 0:444ac4067b19 796 cpt ++;
Brand101 0:444ac4067b19 797 break;
Brand101 0:444ac4067b19 798
Brand101 0:444ac4067b19 799 case ACCELERATION_TRIANGLE : //Etat d'acceleration en profil triangle
Brand101 0:444ac4067b19 800 cpt ++;
Brand101 0:444ac4067b19 801
Brand101 0:444ac4067b19 802 //Decrementation de la consigne de position
Brand101 0:444ac4067b19 803 consigne_pos -= (((double)cpt * (double)amax) / 1000.0);
Brand101 0:444ac4067b19 804
Brand101 0:444ac4067b19 805 if((cpt * amax / 1000.0) >= vmax_tri) { //Condition pour sortir de la phase d'acceleration en profil triangle
Brand101 0:444ac4067b19 806 etat_automate_depl = DECELERATION_TRIANGLE; //Passage a l'etat DECELERATION_TRIANGLE
Brand101 0:444ac4067b19 807 cpt = 0;
Brand101 0:444ac4067b19 808 }
Brand101 0:444ac4067b19 809 break;
Brand101 0:444ac4067b19 810
Brand101 0:444ac4067b19 811 case DECELERATION_TRIANGLE : //Etat de deceleration en profil triangle
Brand101 0:444ac4067b19 812 cpt ++;
Brand101 0:444ac4067b19 813
Brand101 0:444ac4067b19 814 //Decrementation de la consigne de position
Brand101 0:444ac4067b19 815 consigne_pos -= vmax_tri - (((double)cpt * (double)dmax) / 1000.0);
Brand101 0:444ac4067b19 816
Brand101 0:444ac4067b19 817 if(((double)vmax_tri - (((double)cpt * (double)dmax)) / 1000.0) <= 0) { //Condition pour sortir de l'etat de deceleration en profil triangle
Brand101 0:444ac4067b19 818 etat_automate_depl = ARRET; //Passage a l'etat ARRET
Brand101 0:444ac4067b19 819 cpt = 0;
Brand101 0:444ac4067b19 820 }
Brand101 0:444ac4067b19 821 break;
Brand101 0:444ac4067b19 822 }
Brand101 0:444ac4067b19 823 }
Brand101 0:444ac4067b19 824
Brand101 0:444ac4067b19 825 //Calcul des commande 1 et 2
Brand101 0:444ac4067b19 826 Asser_Pos_Mot1(roue_drt_init + consigne_pos);
Brand101 0:444ac4067b19 827 Asser_Pos_Mot2(roue_gch_init + consigne_pos);
Brand101 0:444ac4067b19 828
Brand101 0:444ac4067b19 829 //Ecriture du PWM sur chaque modeur //*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*
Brand101 0:444ac4067b19 830 write_PWM2(commande2);
Brand101 0:444ac4067b19 831 write_PWM1(commande1);
Brand101 0:444ac4067b19 832
Brand101 0:444ac4067b19 833 }
Brand101 0:444ac4067b19 834
Brand101 0:444ac4067b19 835
Brand101 0:444ac4067b19 836 void Rotation(long pcons, short vmax, short amax, short dmax)
Brand101 0:444ac4067b19 837 {
Brand101 0:444ac4067b19 838 //Declaration des variables
Brand101 0:444ac4067b19 839 //char val1[8];
Brand101 0:444ac4067b19 840 static double tc, ta, td; //tc temps � vitesse constante, ta en acceleration, td en deceleration
Brand101 0:444ac4067b19 841 static double vmax_tri; //Vitesse maximale atteinte dans le cas d'un profil en triangle
Brand101 0:444ac4067b19 842
Brand101 0:444ac4067b19 843 if(pcons >= 0) { //Mode avancer
Brand101 0:444ac4067b19 844 switch(etat_automate_depl) { //Automate de gestion de deplacement
Brand101 0:444ac4067b19 845 case INITIALISATION : //Etat d'initialisation et de calcul des variables
Brand101 0:444ac4067b19 846 //remise a zero des variables car on effectue un nouveau deplacement
Brand101 0:444ac4067b19 847 consigne_pos = 0;
Brand101 0:444ac4067b19 848 cpt = 0;
Brand101 0:444ac4067b19 849
Brand101 0:444ac4067b19 850 //Elaboration du profil de vitesse
Brand101 0:444ac4067b19 851 //Calcul du temps a vitesse constante en fonction de l'acceleration
Brand101 0:444ac4067b19 852 tc = ((double)pcons / (double)vmax) - (((double)vmax * 500.0) / (double)amax) - (((double)vmax * 500.0) / (double)dmax);
Brand101 0:444ac4067b19 853
Brand101 0:444ac4067b19 854 if (tc > 0) { //Resultat positif, il y a une phase de vitesse constante
Brand101 0:444ac4067b19 855 etat_automate_depl = ACCELERATION_TRAPEZE; //Passage a l'etat AACELERATION_TRAPEZE
Brand101 0:444ac4067b19 856
Brand101 0:444ac4067b19 857 ta = (((double)vmax * 1000.0) / (double)amax); //Calcul du temps d'acceleration
Brand101 0:444ac4067b19 858 td = (((double)vmax * 1000.0) / (double)dmax); //Calcul du temps de deceleration
Brand101 0:444ac4067b19 859 }
Brand101 0:444ac4067b19 860
Brand101 0:444ac4067b19 861 else if(tc < 0) { //Resultat negatif, il n'y a pas de phase de vitesse constante
Brand101 0:444ac4067b19 862 etat_automate_depl = ACCELERATION_TRIANGLE; //Passage a l'etat ACCELERATION_TRIANGLE
Brand101 0:444ac4067b19 863
Brand101 0:444ac4067b19 864 //Calcul de la vitesse maximale en profil triangle
Brand101 0:444ac4067b19 865 vmax_tri = sqrt((2 * pcons) / ((1000.0 / (double)amax) + (1000.0 / (double)dmax)));
Brand101 0:444ac4067b19 866 }
Brand101 0:444ac4067b19 867 break;
Brand101 0:444ac4067b19 868
Brand101 0:444ac4067b19 869 case ACCELERATION_TRAPEZE : //Etat d'acceleration en profil trapeze
Brand101 0:444ac4067b19 870 cpt ++;
Brand101 0:444ac4067b19 871
Brand101 0:444ac4067b19 872 //Incrementation de la consigne de position
Brand101 0:444ac4067b19 873 consigne_pos += (((double)cpt * (double)amax) / 1000.0);
Brand101 0:444ac4067b19 874
Brand101 0:444ac4067b19 875 if(cpt >= ta) { //Condition pour quitter la phase d'acceleration
Brand101 0:444ac4067b19 876 etat_automate_depl = VITESSE_CONSTANTE_TRAPEZE; //Passage a l'etat VITESSE_CONSTANTE_TRAPEZE
Brand101 0:444ac4067b19 877 cpt = 0;
Brand101 0:444ac4067b19 878 }
Brand101 0:444ac4067b19 879 break;
Brand101 0:444ac4067b19 880
Brand101 0:444ac4067b19 881 case VITESSE_CONSTANTE_TRAPEZE : //Etat de vitesse constante en profil trapeze
Brand101 0:444ac4067b19 882 cpt ++;
Brand101 0:444ac4067b19 883
Brand101 0:444ac4067b19 884 //Incrementation de la consigne de position
Brand101 0:444ac4067b19 885 consigne_pos += vmax;
Brand101 0:444ac4067b19 886
Brand101 0:444ac4067b19 887 //Si il n'y a pas d'enchainements
Brand101 0:444ac4067b19 888 if(cpt >= tc && liste[cpt_ordre].enchainement != 1) {
Brand101 0:444ac4067b19 889 etat_automate_depl = DECELERATION_TRAPEZE; //Passage a l'etat DECELERATION_TRAPEZE
Brand101 0:444ac4067b19 890 cpt = 0;
Brand101 0:444ac4067b19 891 }
Brand101 0:444ac4067b19 892
Brand101 0:444ac4067b19 893 //Si on est dans un enchainement, on passe � l'instruction suivante
Brand101 0:444ac4067b19 894 else if(consigne_pos >= pcons && liste[cpt_ordre].enchainement == 1) {
Brand101 0:444ac4067b19 895 consigne_pos = pcons;
Brand101 0:444ac4067b19 896 cpt_ordre = 0;
Brand101 0:444ac4067b19 897 }
Brand101 0:444ac4067b19 898 break;
Brand101 0:444ac4067b19 899
Brand101 0:444ac4067b19 900 case DECELERATION_TRAPEZE : //Etat de deceleration en profil trapeze
Brand101 0:444ac4067b19 901 cpt ++;
Brand101 0:444ac4067b19 902
Brand101 0:444ac4067b19 903 //Incrementation de la consigne de position
Brand101 0:444ac4067b19 904 consigne_pos += vmax - (double)(((double)cpt * (double)dmax) / 1000.0);
Brand101 0:444ac4067b19 905
Brand101 0:444ac4067b19 906 if(cpt >= td) { //Condition pour quitter la phase de deceleration en profil trapeze
Brand101 0:444ac4067b19 907 etat_automate_depl = ARRET; //Passage a l'etat ARRET
Brand101 0:444ac4067b19 908 consigne_pos = pcons;
Brand101 0:444ac4067b19 909 cpt = 0;
Brand101 0:444ac4067b19 910 }
Brand101 0:444ac4067b19 911 break;
Brand101 0:444ac4067b19 912
Brand101 0:444ac4067b19 913 case ARRET : //Etat d'arret
Brand101 0:444ac4067b19 914 if(cpt >= 20) {
Brand101 0:444ac4067b19 915 etat_automate_depl = INITIALISATION; //Passage a l'etat INITIALISATION
Brand101 0:444ac4067b19 916 next_move = 1;
Brand101 0:444ac4067b19 917 }
Brand101 0:444ac4067b19 918 cpt ++;
Brand101 0:444ac4067b19 919 break;
Brand101 0:444ac4067b19 920
Brand101 0:444ac4067b19 921 case ACCELERATION_TRIANGLE : //Etat d'acceleration en profil triangle
Brand101 0:444ac4067b19 922 cpt ++;
Brand101 0:444ac4067b19 923
Brand101 0:444ac4067b19 924 //Incrementation de la consigne de position
Brand101 0:444ac4067b19 925 consigne_pos += (((double)cpt * (double)amax) / 1000.0); //Condition pour sortir de la phase d'acceleration en profil triangle
Brand101 0:444ac4067b19 926
Brand101 0:444ac4067b19 927 if((cpt * amax / 1000.0) >= vmax_tri) {
Brand101 0:444ac4067b19 928 etat_automate_depl = DECELERATION_TRIANGLE; //Passage a l'etat DECELERATION_TRIANGLE
Brand101 0:444ac4067b19 929 cpt = 0;
Brand101 0:444ac4067b19 930 }
Brand101 0:444ac4067b19 931 break;
Brand101 0:444ac4067b19 932
Brand101 0:444ac4067b19 933 case DECELERATION_TRIANGLE : //Etat de deceleration en profil triangle
Brand101 0:444ac4067b19 934 cpt ++;
Brand101 0:444ac4067b19 935
Brand101 0:444ac4067b19 936 //Incrementation de la consigne de position
Brand101 0:444ac4067b19 937 consigne_pos += vmax_tri - (((double)cpt * (double)dmax) / 1000.0);
Brand101 0:444ac4067b19 938
Brand101 0:444ac4067b19 939 if(((double)vmax_tri - (((double)cpt * (double)dmax)) / 1000.0) <= 0) { //Condition pour sortir de la phase de deceleration en profil triangle
Brand101 0:444ac4067b19 940 etat_automate_depl = ARRET; //Passage a l'etat ARRET
Brand101 0:444ac4067b19 941 cpt = 0;
Brand101 0:444ac4067b19 942 }
Brand101 0:444ac4067b19 943 break;
Brand101 0:444ac4067b19 944 }
Brand101 0:444ac4067b19 945 } else if(pcons < 0) { //Mode reculer
Brand101 0:444ac4067b19 946 switch(etat_automate_depl) { //Automate de gestion du deplacement
Brand101 0:444ac4067b19 947 case INITIALISATION : //Etat d'initialisation et de calcul des variables
Brand101 0:444ac4067b19 948 //Remise a zero des variables car on effectue un nouveau deplacement
Brand101 0:444ac4067b19 949 consigne_pos = 0;
Brand101 0:444ac4067b19 950 cpt = 0;
Brand101 0:444ac4067b19 951
Brand101 0:444ac4067b19 952 //Elaboration du profil de vitesse
Brand101 0:444ac4067b19 953 //Calcul du temps � vitesse constante en fonction de l'acceleration
Brand101 0:444ac4067b19 954 tc = ((double)-pcons / (double)vmax) - (((double)vmax * 500.0) / (double)amax) - (((double)vmax * 500.0) / (double)dmax);
Brand101 0:444ac4067b19 955
Brand101 0:444ac4067b19 956 if (tc > 0) { //Resultat positif, il y a une phase de vitesse constante
Brand101 0:444ac4067b19 957 etat_automate_depl = ACCELERATION_TRAPEZE; //Passage a l'etat ACCELERATION_TRAPEZE
Brand101 0:444ac4067b19 958
Brand101 0:444ac4067b19 959 ta = (((double)vmax * 1000.0) / (double)amax); //Calcul du temps d'acceleration
Brand101 0:444ac4067b19 960 td = (((double)vmax * 1000.0) / (double)dmax); //Calcul du temps de deceleration
Brand101 0:444ac4067b19 961 }
Brand101 0:444ac4067b19 962
Brand101 0:444ac4067b19 963 else if(tc < 0) { //Resultat negatif, il n'y a pas de phase de vitesse constante
Brand101 0:444ac4067b19 964 etat_automate_depl = ACCELERATION_TRIANGLE; //Passage a l'etat ACCELERATION_TRIANGLE
Brand101 0:444ac4067b19 965
Brand101 0:444ac4067b19 966 //Calcul de la vitesse maximale en profil triangle
Brand101 0:444ac4067b19 967 vmax_tri = sqrt((2 * -pcons) / ((1000.0 / (double)amax) + (1000.0 / (double)dmax)));
Brand101 0:444ac4067b19 968 }
Brand101 0:444ac4067b19 969 break;
Brand101 0:444ac4067b19 970
Brand101 0:444ac4067b19 971 case ACCELERATION_TRAPEZE : //Etat d'acceleration en profil trapeze
Brand101 0:444ac4067b19 972 cpt ++;
Brand101 0:444ac4067b19 973
Brand101 0:444ac4067b19 974 //Decrementation de la consigne de position
Brand101 0:444ac4067b19 975 consigne_pos -= (double)(((double)cpt * (double)amax)/1000.0);
Brand101 0:444ac4067b19 976
Brand101 0:444ac4067b19 977 if(cpt >= ta) { //Condition pour quitter la phase d'acceleration
Brand101 0:444ac4067b19 978 etat_automate_depl = VITESSE_CONSTANTE_TRAPEZE; //Passage a l'etat VITESSE_CONSTANTE_TRAPEZE
Brand101 0:444ac4067b19 979 cpt = 0;
Brand101 0:444ac4067b19 980 }
Brand101 0:444ac4067b19 981 break;
Brand101 0:444ac4067b19 982
Brand101 0:444ac4067b19 983 case VITESSE_CONSTANTE_TRAPEZE : //Etat de vitesse continue en profil trapeze
Brand101 0:444ac4067b19 984 cpt ++;
Brand101 0:444ac4067b19 985
Brand101 0:444ac4067b19 986 //Decrementation de la consigne de position
Brand101 0:444ac4067b19 987 consigne_pos -= vmax;
Brand101 0:444ac4067b19 988
Brand101 0:444ac4067b19 989 //Si il n'y a pas d'enchainements
Brand101 0:444ac4067b19 990 if(cpt >= tc && liste[cpt_ordre].enchainement != 1) {
Brand101 0:444ac4067b19 991 etat_automate_depl = DECELERATION_TRAPEZE; //Passage a l'etat de DECELERATION_TRAPEZE
Brand101 0:444ac4067b19 992 cpt = 0;
Brand101 0:444ac4067b19 993 }
Brand101 0:444ac4067b19 994
Brand101 0:444ac4067b19 995 //Si on est dans un enchainement, on passe � l'instruction suivante
Brand101 0:444ac4067b19 996 else if(consigne_pos >= pcons && liste[cpt_ordre].enchainement == 1) {
Brand101 0:444ac4067b19 997 consigne_pos = pcons;
Brand101 0:444ac4067b19 998 cpt_ordre = 0;
Brand101 0:444ac4067b19 999 }
Brand101 0:444ac4067b19 1000 break;
Brand101 0:444ac4067b19 1001
Brand101 0:444ac4067b19 1002 case DECELERATION_TRAPEZE : //Etat de deceleration en profil trapeze
Brand101 0:444ac4067b19 1003 cpt ++;
Brand101 0:444ac4067b19 1004
Brand101 0:444ac4067b19 1005 //Decrementation de la consigne de position
Brand101 0:444ac4067b19 1006 consigne_pos -= vmax - (double)(((double)cpt * (double)dmax) / 1000.0);
Brand101 0:444ac4067b19 1007
Brand101 0:444ac4067b19 1008 if(cpt >= td) { //Condition pour sortir de l'etat vitesse continue en profil trapeze
Brand101 0:444ac4067b19 1009 etat_automate_depl = ARRET; //Passage a l'etat ARRET
Brand101 0:444ac4067b19 1010 consigne_pos = pcons;
Brand101 0:444ac4067b19 1011 cpt = 0;
Brand101 0:444ac4067b19 1012 }
Brand101 0:444ac4067b19 1013 break;
Brand101 0:444ac4067b19 1014
Brand101 0:444ac4067b19 1015 case ARRET : //Etat d'arret
Brand101 0:444ac4067b19 1016 if(cpt >= 20) {
Brand101 0:444ac4067b19 1017 etat_automate_depl = INITIALISATION; //Passage a l'etat INITIALISATION
Brand101 0:444ac4067b19 1018 next_move = 1;
Brand101 0:444ac4067b19 1019 }
Brand101 0:444ac4067b19 1020 cpt ++;
Brand101 0:444ac4067b19 1021 break;
Brand101 0:444ac4067b19 1022
Brand101 0:444ac4067b19 1023 case ACCELERATION_TRIANGLE : //Etat d'acceleration en profil triangle
Brand101 0:444ac4067b19 1024 cpt ++;
Brand101 0:444ac4067b19 1025
Brand101 0:444ac4067b19 1026 //Decrementation de la consigne de position
Brand101 0:444ac4067b19 1027 consigne_pos -= (((double)cpt * (double)amax) / 1000.0);
Brand101 0:444ac4067b19 1028
Brand101 0:444ac4067b19 1029 if((cpt * amax / 1000.0) >= vmax_tri) { //Condition pour sortir de la phase d'acceleration en profil triangle
Brand101 0:444ac4067b19 1030 etat_automate_depl = DECELERATION_TRIANGLE; //Passage a l'etat DECELERATION_TRIANGLE
Brand101 0:444ac4067b19 1031 cpt = 0;
Brand101 0:444ac4067b19 1032 }
Brand101 0:444ac4067b19 1033 break;
Brand101 0:444ac4067b19 1034
Brand101 0:444ac4067b19 1035 case DECELERATION_TRIANGLE : //Etat de deceleration en profil triangle
Brand101 0:444ac4067b19 1036 cpt ++;
Brand101 0:444ac4067b19 1037
Brand101 0:444ac4067b19 1038 //Decrementation de la consigne de position
Brand101 0:444ac4067b19 1039 consigne_pos -= vmax_tri - (((double)cpt * (double)dmax) / 1000.0);
Brand101 0:444ac4067b19 1040
Brand101 0:444ac4067b19 1041 if(((double)vmax_tri - (((double)cpt * (double)dmax)) / 1000.0) <= 0) { //Condition pour sortir de l'etat de deceleration en profil triangle
Brand101 0:444ac4067b19 1042 etat_automate_depl = ARRET; //Passage a l'etat ARRET
Brand101 0:444ac4067b19 1043 cpt = 0;
Brand101 0:444ac4067b19 1044 }
Brand101 0:444ac4067b19 1045 break;
Brand101 0:444ac4067b19 1046 }
Brand101 0:444ac4067b19 1047 }
Brand101 0:444ac4067b19 1048
Brand101 0:444ac4067b19 1049 //Calcul des commande 1 et 2
Brand101 0:444ac4067b19 1050 Asser_Pos_Mot1(roue_drt_init + consigne_pos);
Brand101 0:444ac4067b19 1051 Asser_Pos_Mot2(roue_gch_init - consigne_pos);
Brand101 0:444ac4067b19 1052
Brand101 0:444ac4067b19 1053 //Ecriture du PWM sur chaque modeur /*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*
Brand101 0:444ac4067b19 1054 write_PWM2(commande2);
Brand101 0:444ac4067b19 1055 write_PWM1(commande1);
Brand101 0:444ac4067b19 1056 }
Brand101 0:444ac4067b19 1057
Brand101 0:444ac4067b19 1058 void Rotation_XYTheta(long pcons, short vmax, short amax, short dmax)
Brand101 0:444ac4067b19 1059 {
Brand101 0:444ac4067b19 1060 //Declaration des variables
Brand101 0:444ac4067b19 1061 //char val1[8];
Brand101 0:444ac4067b19 1062 static double tc, ta, td; //tc temps � vitesse constante, ta en acceleration, td en deceleration
Brand101 0:444ac4067b19 1063 static double vmax_tri; //Vitesse maximale atteinte dans le cas d'un profil en triangle
Brand101 0:444ac4067b19 1064
Brand101 0:444ac4067b19 1065 if(pcons >= 0) { //Mode avancer
Brand101 0:444ac4067b19 1066 switch(etat_automate_depl) { //Automate de gestion de deplacement
Brand101 0:444ac4067b19 1067 case INITIALISATION : //Etat d'initialisation et de calcul des variables
Brand101 0:444ac4067b19 1068 //remise a zero des variables car on effectue un nouveau deplacement
Brand101 0:444ac4067b19 1069 consigne_pos = 0;
Brand101 0:444ac4067b19 1070 cpt = 0;
Brand101 0:444ac4067b19 1071
Brand101 0:444ac4067b19 1072 //Elaboration du profil de vitesse
Brand101 0:444ac4067b19 1073 //Calcul du temps a vitesse constante en fonction de l'acceleration
Brand101 0:444ac4067b19 1074 tc = ((double)pcons / (double)vmax) - (((double)vmax * 500.0) / (double)amax) - (((double)vmax * 500.0) / (double)dmax);
Brand101 0:444ac4067b19 1075
Brand101 0:444ac4067b19 1076 if (tc > 0) { //Resultat positif, il y a une phase de vitesse constante
Brand101 0:444ac4067b19 1077 etat_automate_depl = ACCELERATION_TRAPEZE; //Passage a l'etat AACELERATION_TRAPEZE
Brand101 0:444ac4067b19 1078
Brand101 0:444ac4067b19 1079 ta = (((double)vmax * 1000.0) / (double)amax); //Calcul du temps d'acceleration
Brand101 0:444ac4067b19 1080 td = (((double)vmax * 1000.0) / (double)dmax); //Calcul du temps de deceleration
Brand101 0:444ac4067b19 1081 }
Brand101 0:444ac4067b19 1082
Brand101 0:444ac4067b19 1083 else if(tc < 0) { //Resultat negatif, il n'y a pas de phase de vitesse constante
Brand101 0:444ac4067b19 1084 etat_automate_depl = ACCELERATION_TRIANGLE; //Passage a l'etat ACCELERATION_TRIANGLE
Brand101 0:444ac4067b19 1085
Brand101 0:444ac4067b19 1086 //Calcul de la vitesse maximale en profil triangle
Brand101 0:444ac4067b19 1087 vmax_tri = sqrt((2 * pcons) / ((1000.0 / (double)amax) + (1000.0 / (double)dmax)));
Brand101 0:444ac4067b19 1088 }
Brand101 0:444ac4067b19 1089 break;
Brand101 0:444ac4067b19 1090
Brand101 0:444ac4067b19 1091 case ACCELERATION_TRAPEZE : //Etat d'acceleration en profil trapeze
Brand101 0:444ac4067b19 1092 cpt ++;
Brand101 0:444ac4067b19 1093
Brand101 0:444ac4067b19 1094 //Incrementation de la consigne de position
Brand101 0:444ac4067b19 1095 consigne_pos += (((double)cpt * (double)amax) / 1000.0);
Brand101 0:444ac4067b19 1096
Brand101 0:444ac4067b19 1097 if(cpt >= ta) { //Condition pour quitter la phase d'acceleration
Brand101 0:444ac4067b19 1098 etat_automate_depl = VITESSE_CONSTANTE_TRAPEZE; //Passage a l'etat VITESSE_CONSTANTE_TRAPEZE
Brand101 0:444ac4067b19 1099 cpt = 0;
Brand101 0:444ac4067b19 1100 }
Brand101 0:444ac4067b19 1101 break;
Brand101 0:444ac4067b19 1102
Brand101 0:444ac4067b19 1103 case VITESSE_CONSTANTE_TRAPEZE : //Etat de vitesse constante en profil trapeze
Brand101 0:444ac4067b19 1104 cpt ++;
Brand101 0:444ac4067b19 1105
Brand101 0:444ac4067b19 1106 //Incrementation de la consigne de position
Brand101 0:444ac4067b19 1107 consigne_pos += vmax;
Brand101 0:444ac4067b19 1108
Brand101 0:444ac4067b19 1109 //Si il n'y a pas d'enchainements
Brand101 0:444ac4067b19 1110 if(cpt >= tc && liste[cpt_ordre].enchainement != 1) {
Brand101 0:444ac4067b19 1111 etat_automate_depl = DECELERATION_TRAPEZE; //Passage a l'etat DECELERATION_TRAPEZE
Brand101 0:444ac4067b19 1112 cpt = 0;
Brand101 0:444ac4067b19 1113 }
Brand101 0:444ac4067b19 1114
Brand101 0:444ac4067b19 1115 //Si on est dans un enchainement, on passe � l'instruction suivante
Brand101 0:444ac4067b19 1116 else if(consigne_pos >= pcons && liste[cpt_ordre].enchainement == 1) {
Brand101 0:444ac4067b19 1117 consigne_pos = pcons;
Brand101 0:444ac4067b19 1118 cpt_ordre = 0;
Brand101 0:444ac4067b19 1119 }
Brand101 0:444ac4067b19 1120 break;
Brand101 0:444ac4067b19 1121
Brand101 0:444ac4067b19 1122 case DECELERATION_TRAPEZE : //Etat de deceleration en profil trapeze
Brand101 0:444ac4067b19 1123 cpt ++;
Brand101 0:444ac4067b19 1124
Brand101 0:444ac4067b19 1125 //Incrementation de la consigne de position
Brand101 0:444ac4067b19 1126 consigne_pos += vmax - (double)(((double)cpt * (double)dmax) / 1000.0);
Brand101 0:444ac4067b19 1127
Brand101 0:444ac4067b19 1128 if(cpt >= td) { //Condition pour quitter la phase de deceleration en profil trapeze
Brand101 0:444ac4067b19 1129 etat_automate_depl = ARRET; //Passage a l'etat ARRET
Brand101 0:444ac4067b19 1130 consigne_pos = pcons;
Brand101 0:444ac4067b19 1131 cpt = 0;
Brand101 0:444ac4067b19 1132 }
Brand101 0:444ac4067b19 1133 break;
Brand101 0:444ac4067b19 1134
Brand101 0:444ac4067b19 1135 case ARRET : //Etat d'arret
Brand101 0:444ac4067b19 1136 if(cpt >= 20) {
Brand101 0:444ac4067b19 1137 etat_automate_depl = INITIALISATION; //Passage a l'etat INITIALISATION
Brand101 0:444ac4067b19 1138 next_move_xyt = 1;
Brand101 0:444ac4067b19 1139 }
Brand101 0:444ac4067b19 1140 cpt ++;
Brand101 0:444ac4067b19 1141 break;
Brand101 0:444ac4067b19 1142
Brand101 0:444ac4067b19 1143 case ACCELERATION_TRIANGLE : //Etat d'acceleration en profil triangle
Brand101 0:444ac4067b19 1144 cpt ++;
Brand101 0:444ac4067b19 1145
Brand101 0:444ac4067b19 1146 //Incrementation de la consigne de position
Brand101 0:444ac4067b19 1147 consigne_pos += (((double)cpt * (double)amax) / 1000.0); //Condition pour sortir de la phase d'acceleration en profil triangle
Brand101 0:444ac4067b19 1148
Brand101 0:444ac4067b19 1149 if((cpt * amax / 1000.0) >= vmax_tri) {
Brand101 0:444ac4067b19 1150 etat_automate_depl = DECELERATION_TRIANGLE; //Passage a l'etat DECELERATION_TRIANGLE
Brand101 0:444ac4067b19 1151 cpt = 0;
Brand101 0:444ac4067b19 1152 }
Brand101 0:444ac4067b19 1153 break;
Brand101 0:444ac4067b19 1154
Brand101 0:444ac4067b19 1155 case DECELERATION_TRIANGLE : //Etat de deceleration en profil triangle
Brand101 0:444ac4067b19 1156 cpt ++;
Brand101 0:444ac4067b19 1157
Brand101 0:444ac4067b19 1158 //Incrementation de la consigne de position
Brand101 0:444ac4067b19 1159 consigne_pos += vmax_tri - (((double)cpt * (double)dmax) / 1000.0);
Brand101 0:444ac4067b19 1160
Brand101 0:444ac4067b19 1161 if(((double)vmax_tri - (((double)cpt * (double)dmax)) / 1000.0) <= 0) { //Condition pour sortir de la phase de deceleration en profil triangle
Brand101 0:444ac4067b19 1162 etat_automate_depl = ARRET; //Passage a l'etat ARRET
Brand101 0:444ac4067b19 1163 cpt = 0;
Brand101 0:444ac4067b19 1164 }
Brand101 0:444ac4067b19 1165 break;
Brand101 0:444ac4067b19 1166 }
Brand101 0:444ac4067b19 1167 } else if(pcons < 0) { //Mode reculer
Brand101 0:444ac4067b19 1168 switch(etat_automate_depl) { //Automate de gestion du deplacement
Brand101 0:444ac4067b19 1169 case INITIALISATION : //Etat d'initialisation et de calcul des variables
Brand101 0:444ac4067b19 1170 //Remise a zero des variables car on effectue un nouveau deplacement
Brand101 0:444ac4067b19 1171 consigne_pos = 0;
Brand101 0:444ac4067b19 1172 cpt = 0;
Brand101 0:444ac4067b19 1173
Brand101 0:444ac4067b19 1174 //Elaboration du profil de vitesse
Brand101 0:444ac4067b19 1175 //Calcul du temps � vitesse constante en fonction de l'acceleration
Brand101 0:444ac4067b19 1176 tc = ((double)-pcons / (double)vmax) - (((double)vmax * 500.0) / (double)amax) - (((double)vmax * 500.0) / (double)dmax);
Brand101 0:444ac4067b19 1177
Brand101 0:444ac4067b19 1178 if (tc > 0) { //Resultat positif, il y a une phase de vitesse constante
Brand101 0:444ac4067b19 1179 etat_automate_depl = ACCELERATION_TRAPEZE; //Passage a l'etat ACCELERATION_TRAPEZE
Brand101 0:444ac4067b19 1180
Brand101 0:444ac4067b19 1181 ta = (((double)vmax * 1000.0) / (double)amax); //Calcul du temps d'acceleration
Brand101 0:444ac4067b19 1182 td = (((double)vmax * 1000.0) / (double)dmax); //Calcul du temps de deceleration
Brand101 0:444ac4067b19 1183 }
Brand101 0:444ac4067b19 1184
Brand101 0:444ac4067b19 1185 else if(tc < 0) { //Resultat negatif, il n'y a pas de phase de vitesse constante
Brand101 0:444ac4067b19 1186 etat_automate_depl = ACCELERATION_TRIANGLE; //Passage a l'etat ACCELERATION_TRIANGLE
Brand101 0:444ac4067b19 1187
Brand101 0:444ac4067b19 1188 //Calcul de la vitesse maximale en profil triangle
Brand101 0:444ac4067b19 1189 vmax_tri = sqrt((2 * -pcons) / ((1000.0 / (double)amax) + (1000.0 / (double)dmax)));
Brand101 0:444ac4067b19 1190 }
Brand101 0:444ac4067b19 1191 break;
Brand101 0:444ac4067b19 1192
Brand101 0:444ac4067b19 1193 case ACCELERATION_TRAPEZE : //Etat d'acceleration en profil trapeze
Brand101 0:444ac4067b19 1194 cpt ++;
Brand101 0:444ac4067b19 1195
Brand101 0:444ac4067b19 1196 //Decrementation de la consigne de position
Brand101 0:444ac4067b19 1197 consigne_pos -= (double)(((double)cpt * (double)amax)/1000.0);
Brand101 0:444ac4067b19 1198
Brand101 0:444ac4067b19 1199 if(cpt >= ta) { //Condition pour quitter la phase d'acceleration
Brand101 0:444ac4067b19 1200 etat_automate_depl = VITESSE_CONSTANTE_TRAPEZE; //Passage a l'etat VITESSE_CONSTANTE_TRAPEZE
Brand101 0:444ac4067b19 1201 cpt = 0;
Brand101 0:444ac4067b19 1202 }
Brand101 0:444ac4067b19 1203 break;
Brand101 0:444ac4067b19 1204
Brand101 0:444ac4067b19 1205 case VITESSE_CONSTANTE_TRAPEZE : //Etat de vitesse continue en profil trapeze
Brand101 0:444ac4067b19 1206 cpt ++;
Brand101 0:444ac4067b19 1207
Brand101 0:444ac4067b19 1208 //Decrementation de la consigne de position
Brand101 0:444ac4067b19 1209 consigne_pos -= vmax;
Brand101 0:444ac4067b19 1210
Brand101 0:444ac4067b19 1211 //Si il n'y a pas d'enchainements
Brand101 0:444ac4067b19 1212 if(cpt >= tc && liste[cpt_ordre].enchainement != 1) {
Brand101 0:444ac4067b19 1213 etat_automate_depl = DECELERATION_TRAPEZE; //Passage a l'etat de DECELERATION_TRAPEZE
Brand101 0:444ac4067b19 1214 cpt = 0;
Brand101 0:444ac4067b19 1215 }
Brand101 0:444ac4067b19 1216
Brand101 0:444ac4067b19 1217 //Si on est dans un enchainement, on passe � l'instruction suivante
Brand101 0:444ac4067b19 1218 else if(consigne_pos >= pcons && liste[cpt_ordre].enchainement == 1) {
Brand101 0:444ac4067b19 1219 consigne_pos = pcons;
Brand101 0:444ac4067b19 1220 cpt_ordre = 0;
Brand101 0:444ac4067b19 1221 }
Brand101 0:444ac4067b19 1222 break;
Brand101 0:444ac4067b19 1223
Brand101 0:444ac4067b19 1224 case DECELERATION_TRAPEZE : //Etat de deceleration en profil trapeze
Brand101 0:444ac4067b19 1225 cpt ++;
Brand101 0:444ac4067b19 1226
Brand101 0:444ac4067b19 1227 //Decrementation de la consigne de position
Brand101 0:444ac4067b19 1228 consigne_pos -= vmax - (double)(((double)cpt * (double)dmax) / 1000.0);
Brand101 0:444ac4067b19 1229
Brand101 0:444ac4067b19 1230 if(cpt >= td) { //Condition pour sortir de l'etat vitesse continue en profil trapeze
Brand101 0:444ac4067b19 1231 etat_automate_depl = ARRET; //Passage a l'etat ARRET
Brand101 0:444ac4067b19 1232 consigne_pos = pcons;
Brand101 0:444ac4067b19 1233 cpt = 0;
Brand101 0:444ac4067b19 1234 }
Brand101 0:444ac4067b19 1235 break;
Brand101 0:444ac4067b19 1236
Brand101 0:444ac4067b19 1237 case ARRET : //Etat d'arret
Brand101 0:444ac4067b19 1238 if(cpt >= 20) {
Brand101 0:444ac4067b19 1239 etat_automate_depl = INITIALISATION; //Passage a l'etat INITIALISATION
Brand101 0:444ac4067b19 1240 next_move_xyt = 1;
Brand101 0:444ac4067b19 1241 }
Brand101 0:444ac4067b19 1242 cpt ++;
Brand101 0:444ac4067b19 1243 break;
Brand101 0:444ac4067b19 1244
Brand101 0:444ac4067b19 1245 case ACCELERATION_TRIANGLE : //Etat d'acceleration en profil triangle
Brand101 0:444ac4067b19 1246 cpt ++;
Brand101 0:444ac4067b19 1247
Brand101 0:444ac4067b19 1248 //Decrementation de la consigne de position
Brand101 0:444ac4067b19 1249 consigne_pos -= (((double)cpt * (double)amax) / 1000.0);
Brand101 0:444ac4067b19 1250
Brand101 0:444ac4067b19 1251 if((cpt * amax / 1000.0) >= vmax_tri) { //Condition pour sortir de la phase d'acceleration en profil triangle
Brand101 0:444ac4067b19 1252 etat_automate_depl = DECELERATION_TRIANGLE; //Passage a l'etat DECELERATION_TRIANGLE
Brand101 0:444ac4067b19 1253 cpt = 0;
Brand101 0:444ac4067b19 1254 }
Brand101 0:444ac4067b19 1255 break;
Brand101 0:444ac4067b19 1256
Brand101 0:444ac4067b19 1257 case DECELERATION_TRIANGLE : //Etat de deceleration en profil triangle
Brand101 0:444ac4067b19 1258 cpt ++;
Brand101 0:444ac4067b19 1259
Brand101 0:444ac4067b19 1260 //Decrementation de la consigne de position
Brand101 0:444ac4067b19 1261 consigne_pos -= vmax_tri - (((double)cpt * (double)dmax) / 1000.0);
Brand101 0:444ac4067b19 1262
Brand101 0:444ac4067b19 1263 if(((double)vmax_tri - (((double)cpt * (double)dmax)) / 1000.0) <= 0) { //Condition pour sortir de l'etat de deceleration en profil triangle
Brand101 0:444ac4067b19 1264 etat_automate_depl = ARRET; //Passage a l'etat ARRET
Brand101 0:444ac4067b19 1265 cpt = 0;
Brand101 0:444ac4067b19 1266 }
Brand101 0:444ac4067b19 1267 break;
Brand101 0:444ac4067b19 1268 }
Brand101 0:444ac4067b19 1269 }
Brand101 0:444ac4067b19 1270
Brand101 0:444ac4067b19 1271 //Calcul des commande 1 et 2
Brand101 0:444ac4067b19 1272 Asser_Pos_Mot1(roue_drt_init + consigne_pos);
Brand101 0:444ac4067b19 1273 Asser_Pos_Mot2(roue_gch_init - consigne_pos);
Brand101 0:444ac4067b19 1274
Brand101 0:444ac4067b19 1275 //Ecriture du PWM sur chaque modeur //*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*/*
Brand101 0:444ac4067b19 1276 write_PWM2(commande2);
Brand101 0:444ac4067b19 1277 write_PWM1(commande1);
Brand101 0:444ac4067b19 1278
Brand101 0:444ac4067b19 1279 }
Brand101 0:444ac4067b19 1280
Brand101 0:444ac4067b19 1281 void X_Y_Theta(long px, long py, long ptheta, long sens, short vmax, short amax)
Brand101 0:444ac4067b19 1282 {
Brand101 0:444ac4067b19 1283 //Variables pr�sentes Odo_x, Odo_y : position de d�part
Brand101 0:444ac4067b19 1284 //Declaration des variables
Brand101 0:444ac4067b19 1285 static short dist = 0, ang1 = 0, ang2 = 0;
Brand101 0:444ac4067b19 1286 //shar val[4];
Brand101 0:444ac4067b19 1287
Brand101 0:444ac4067b19 1288 switch(etat_automate_xytheta) {
Brand101 0:444ac4067b19 1289 case INIT_X_Y_THETA : //etat INIT_X_Y_THETA
Brand101 0:444ac4067b19 1290 //Mode Avancer
Brand101 0:444ac4067b19 1291 if(sens >= 0) {
Brand101 0:444ac4067b19 1292 /*pour avancer, on tourne dans le sens horaire//MOI*\*/
Brand101 0:444ac4067b19 1293
Brand101 0:444ac4067b19 1294 // Son hypoth�nuse correspond � la distance � parcourir
Brand101 0:444ac4067b19 1295 dist = (short)sqrt((px - Odo_x)*(px - Odo_x)+(py - Odo_y)*(py - Odo_y));
Brand101 0:444ac4067b19 1296
Brand101 0:444ac4067b19 1297 // la 1ere rotation correspond � l'angle du triangle, moins l'angle de la position de d�part
Brand101 0:444ac4067b19 1298 // C'est-�-dire la tangente du c�t� oppos� sur l'angle adjacent
Brand101 0:444ac4067b19 1299 // La fonction atan2 fait le calcul de la tangente en radians, entre Pi et -Pi
Brand101 0:444ac4067b19 1300 // On rajoute des coefficients pour passer en degr�s
Brand101 0:444ac4067b19 1301 // On ajoute 7200 dixi�mes de degr�s pour �tre s�rs que le r�sultat soit positif
Brand101 0:444ac4067b19 1302 ang1 = (short)((atan2((double)(py - Odo_y), (double)(px - Odo_x)) * 1800 / PI) - Odo_theta + 7200) % 3600;
Brand101 0:444ac4067b19 1303
Brand101 0:444ac4067b19 1304 // On passe le r�sultat entre -1800 et 1800
Brand101 0:444ac4067b19 1305 if(ang1 > 1800) ang1 = (ang1 - 3600);
Brand101 0:444ac4067b19 1306
Brand101 0:444ac4067b19 1307 // La 2� rotation correspond � l'angle de destination, moins l'angle � la fin de la ligne droite,
Brand101 0:444ac4067b19 1308 // donc le m�me qu'� la fin de la 1�re rotation, donc l'angle de d�part plus la premi�re rotation
Brand101 0:444ac4067b19 1309 // On ajoute 3600 pour �tre s�r d'avoir un r�sultat positif
Brand101 0:444ac4067b19 1310 ang2 = (short)(ptheta - ang1 - Odo_theta + 3600) % 3600;
Brand101 0:444ac4067b19 1311
Brand101 0:444ac4067b19 1312 // On passe le r�sultat entre -1800 et 1800
Brand101 0:444ac4067b19 1313 if(ang2 > 1800) ang2 = (ang2 - 3600);
Brand101 0:444ac4067b19 1314
Brand101 0:444ac4067b19 1315 // On transforme les r�sultats en distance et angles utilisables avec les fonctions d�j� d�finies
Brand101 0:444ac4067b19 1316 dist = dist * RESOLUTION_ROUE_CODEUSE / PERIMETRE_ROUE_CODEUSE;
Brand101 0:444ac4067b19 1317 ang1 = LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE * ang1 / (3600 * PERIMETRE_ROUE_CODEUSE);
Brand101 0:444ac4067b19 1318 ang2 = LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE * ang2 / (3600 * PERIMETRE_ROUE_CODEUSE);
Brand101 0:444ac4067b19 1319 }
Brand101 0:444ac4067b19 1320
Brand101 0:444ac4067b19 1321 //Mode Reculer
Brand101 0:444ac4067b19 1322 else if(sens < 0) {
Brand101 0:444ac4067b19 1323
Brand101 0:444ac4067b19 1324 /*pour avancer, on tourne dans le sens anti-horaire//MOI*\*/
Brand101 0:444ac4067b19 1325
Brand101 0:444ac4067b19 1326 // Idem qu'au-dessus, mais laligne droite doit �tre faite en marche arri�re
Brand101 0:444ac4067b19 1327 // La distance est l'oppos� de celle calcul�e au dessus
Brand101 0:444ac4067b19 1328 // Les angles sont les m�mes � 1800 pr�s
Brand101 0:444ac4067b19 1329 dist = -(short)sqrt((px-Odo_x)*(px-Odo_x)+(py-Odo_y)*(py-Odo_y));
Brand101 0:444ac4067b19 1330
Brand101 0:444ac4067b19 1331 //Premiere rotation
Brand101 0:444ac4067b19 1332 ang1 = (short)(((atan2((double)(py - Odo_y), (double)(px - Odo_x)) * 1800 / PI) - Odo_theta) + 5400) % 3600;
Brand101 0:444ac4067b19 1333 if(ang1 > 1800) ang1 = (ang1 - 3600);
Brand101 0:444ac4067b19 1334
Brand101 0:444ac4067b19 1335 //Deuxieme rotation
Brand101 0:444ac4067b19 1336 ang2 = (short)(ptheta - ang1 - Odo_theta + 3600) % 3600;
Brand101 0:444ac4067b19 1337 if(ang2 > 1800) ang2 = (ang2 - 3600);
Brand101 0:444ac4067b19 1338
Brand101 0:444ac4067b19 1339 // On transforme les r�sultats en distance et angles utilisables avec les fonctions d�j� d�finies
Brand101 0:444ac4067b19 1340 dist = dist * RESOLUTION_ROUE_CODEUSE / PERIMETRE_ROUE_CODEUSE;
Brand101 0:444ac4067b19 1341 ang1 = LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE * ang1 / (3600 * PERIMETRE_ROUE_CODEUSE);
Brand101 0:444ac4067b19 1342 ang2 = LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE * ang2 / (3600 * PERIMETRE_ROUE_CODEUSE);
Brand101 0:444ac4067b19 1343 }
Brand101 0:444ac4067b19 1344
Brand101 0:444ac4067b19 1345 param_xytheta[0] = ang1;
Brand101 0:444ac4067b19 1346 param_xytheta[1] = dist;
Brand101 0:444ac4067b19 1347 param_xytheta[2] = ang2;
Brand101 0:444ac4067b19 1348
Brand101 0:444ac4067b19 1349 // La fonction xyth�ta utilise un automate propre, similaire � l'automate g�n�ral
Brand101 0:444ac4067b19 1350 etat_automate_xytheta = 1;
Brand101 0:444ac4067b19 1351 etat_automate_depl = ROTATION_X_Y_THETA_1; //Passage a l'etat ROTATION_X_Y_THETA_1
Brand101 0:444ac4067b19 1352 break;
Brand101 0:444ac4067b19 1353
Brand101 0:444ac4067b19 1354 case ROTATION_X_Y_THETA_1 : //etat ROTATION_X_Y_THETA_1
Brand101 0:444ac4067b19 1355 //Execution de la fonction de rotation pour la fonction de deplacement X_Y_THETA
Brand101 0:444ac4067b19 1356 Rotation_XYTheta(ang1, vmax, amax, DMAX);
Brand101 0:444ac4067b19 1357
Brand101 0:444ac4067b19 1358 if(next_move_xyt) {
Brand101 0:444ac4067b19 1359 roue_drt_init = encoder1.GetCounter();
Brand101 0:444ac4067b19 1360
Brand101 0:444ac4067b19 1361 //#elif GROS_ROB == 0
Brand101 0:444ac4067b19 1362 roue_gch_init = COEF_ROUE_GAUCHE * (double)encoder2.GetCounter();
Brand101 0:444ac4067b19 1363 //#endif
Brand101 0:444ac4067b19 1364
Brand101 0:444ac4067b19 1365 next_move_xyt = 0;
Brand101 0:444ac4067b19 1366 etat_automate_xytheta = LIGNE_DROITE_X_Y_THETA; //Passage a l'etat LIGNE_DROITE_X_Y_THETA
Brand101 0:444ac4067b19 1367 }
Brand101 0:444ac4067b19 1368 break;
Brand101 0:444ac4067b19 1369
Brand101 0:444ac4067b19 1370 case LIGNE_DROITE_X_Y_THETA : //etat LIGNE_DROITE_X_Y_THETA_1
Brand101 0:444ac4067b19 1371 //Execution de la fonction de ligne droite pour la fonction de deplacement X_Y_THETA
Brand101 0:444ac4067b19 1372 Avancer_ligne_droite_XYTheta(dist, vmax, amax, DMAX);
Brand101 0:444ac4067b19 1373
Brand101 0:444ac4067b19 1374 if(next_move_xyt) {
Brand101 0:444ac4067b19 1375 roue_drt_init = encoder1.GetCounter();
Brand101 0:444ac4067b19 1376
Brand101 0:444ac4067b19 1377 //#elif GROS_ROB == 0
Brand101 0:444ac4067b19 1378 roue_gch_init = COEF_ROUE_GAUCHE * (double)encoder2.GetCounter();
Brand101 0:444ac4067b19 1379 //#endif
Brand101 0:444ac4067b19 1380 next_move_xyt = 0;
Brand101 0:444ac4067b19 1381 etat_automate_xytheta = ROTATION_X_Y_THETA_2; //Passage a l'etat ROTATION_X_Y_THETA_2
Brand101 0:444ac4067b19 1382 }
Brand101 0:444ac4067b19 1383 break;
Brand101 0:444ac4067b19 1384
Brand101 0:444ac4067b19 1385 case ROTATION_X_Y_THETA_2 : //etat ROTATION_X_Y_THETA_2
Brand101 0:444ac4067b19 1386 //Execution de la fonction de rotation pour la fonction de deplacement X_Y_THETA
Brand101 0:444ac4067b19 1387 Rotation_XYTheta(ang2, vmax, amax, DMAX);
Brand101 0:444ac4067b19 1388 if(next_move_xyt) {
Brand101 0:444ac4067b19 1389 roue_drt_init = encoder1.GetCounter();
Brand101 0:444ac4067b19 1390
Brand101 0:444ac4067b19 1391 //#elif GROS_ROB == 0
Brand101 0:444ac4067b19 1392 roue_gch_init = COEF_ROUE_GAUCHE * (double)encoder2.GetCounter();
Brand101 0:444ac4067b19 1393 //#endif
Brand101 0:444ac4067b19 1394
Brand101 0:444ac4067b19 1395 next_move_xyt = 0;
Brand101 0:444ac4067b19 1396 etat_automate_xytheta = INIT_X_Y_THETA; //Passage a l'etat INIT_X_Y_THETA
Brand101 0:444ac4067b19 1397 next_move = 1;
Brand101 0:444ac4067b19 1398 }
Brand101 0:444ac4067b19 1399 break;
Brand101 0:444ac4067b19 1400
Brand101 0:444ac4067b19 1401 default : //Etat par defaut, on fait la meme chose que dans l'etat d'initialisation
Brand101 0:444ac4067b19 1402 //Mode Avancer
Brand101 0:444ac4067b19 1403 if(sens >= 0) {
Brand101 0:444ac4067b19 1404 /*pour avancer, on tourne dans le sens horaire//MOI*\*/
Brand101 0:444ac4067b19 1405
Brand101 0:444ac4067b19 1406 dist = (short)sqrt((px - Odo_x)*(px - Odo_x)+(py - Odo_y)*(py - Odo_y));
Brand101 0:444ac4067b19 1407 ang1 = (short)((atan2((double)(py - Odo_y), (double)(px - Odo_x)) * 1800 / PI) - Odo_theta + 7200) % 3600;
Brand101 0:444ac4067b19 1408 if(ang1 > 1800)
Brand101 0:444ac4067b19 1409 ang1 = (ang1 - 3600);
Brand101 0:444ac4067b19 1410 ang2 = (short)(ptheta - ang1 - Odo_theta + 3600) % 3600;
Brand101 0:444ac4067b19 1411 if(ang2 > 1800)
Brand101 0:444ac4067b19 1412 ang2 = (ang2 - 3600);
Brand101 0:444ac4067b19 1413
Brand101 0:444ac4067b19 1414 dist = dist * RESOLUTION_ROUE_CODEUSE / PERIMETRE_ROUE_CODEUSE;
Brand101 0:444ac4067b19 1415 ang1 = LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE * ang1 / (3600 * PERIMETRE_ROUE_CODEUSE);
Brand101 0:444ac4067b19 1416 ang2 = LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE * ang2 / (3600 * PERIMETRE_ROUE_CODEUSE);
Brand101 0:444ac4067b19 1417 }
Brand101 0:444ac4067b19 1418
Brand101 0:444ac4067b19 1419 //Mode Reculer
Brand101 0:444ac4067b19 1420 else if(sens < 0) {
Brand101 0:444ac4067b19 1421 /*pour avancer, on tourne dans le sens anti-horaire//MOI*\*/
Brand101 0:444ac4067b19 1422 dist = -(short)sqrt((px-Odo_x)*(px-Odo_x)+(py-Odo_y)*(py-Odo_y));
Brand101 0:444ac4067b19 1423 ang1 = (short)(((atan2((double)(py - Odo_y), (double)(px - Odo_x)) * 1800 / PI) - Odo_theta) + 5400) % 3600;
Brand101 0:444ac4067b19 1424 if(ang1 > 1800)
Brand101 0:444ac4067b19 1425
Brand101 0:444ac4067b19 1426 ang1 = (ang1 - 3600);
Brand101 0:444ac4067b19 1427 ang2 = (short)(ptheta - ang1 - Odo_theta + 3600) % 3600;
Brand101 0:444ac4067b19 1428 if(ang2 > 1800)
Brand101 0:444ac4067b19 1429 ang2 = (ang2 - 3600);
Brand101 0:444ac4067b19 1430
Brand101 0:444ac4067b19 1431 dist = dist * RESOLUTION_ROUE_CODEUSE / PERIMETRE_ROUE_CODEUSE;
Brand101 0:444ac4067b19 1432 ang1 = LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE * ang1 / (3600 * PERIMETRE_ROUE_CODEUSE);
Brand101 0:444ac4067b19 1433 ang2 = LARGEUR_ROBOT * PI * RESOLUTION_ROUE_CODEUSE * ang2 / (3600 * PERIMETRE_ROUE_CODEUSE);
Brand101 0:444ac4067b19 1434 }
Brand101 0:444ac4067b19 1435
Brand101 0:444ac4067b19 1436 param_xytheta[0] = ang1;
Brand101 0:444ac4067b19 1437 param_xytheta[1] = dist;
Brand101 0:444ac4067b19 1438 param_xytheta[2] = ang2;
Brand101 0:444ac4067b19 1439
Brand101 0:444ac4067b19 1440 etat_automate_xytheta = 1;
Brand101 0:444ac4067b19 1441 etat_automate_depl = INIT_X_Y_THETA;
Brand101 0:444ac4067b19 1442 break;
Brand101 0:444ac4067b19 1443 }
Brand101 0:444ac4067b19 1444 }
Brand101 0:444ac4067b19 1445
Brand101 0:444ac4067b19 1446 void Odometrie(void) //void Odometrie(void)
Brand101 0:444ac4067b19 1447 {
Brand101 0:444ac4067b19 1448 //Declaration des variables
Brand101 0:444ac4067b19 1449 //char val1[8];
Brand101 0:444ac4067b19 1450 double dist, ang; //distance, angle
Brand101 0:444ac4067b19 1451
Brand101 0:444ac4067b19 1452 //Recuperation des valeurs des compteurs des encodeurs
Brand101 0:444ac4067b19 1453 Odo_val_pos_1 = (double)encoder1.GetCounter();
Brand101 0:444ac4067b19 1454
Brand101 0:444ac4067b19 1455 /* MOI*\
Brand101 0:444ac4067b19 1456 //#elif GROS_ROB == 0
Brand101 0:444ac4067b19 1457 Odo_val_pos_2 = COEF_ROUE_GAUCHE * (double)encoder2.GetCounter();
Brand101 0:444ac4067b19 1458 //#endif
Brand101 0:444ac4067b19 1459 */
Brand101 0:444ac4067b19 1460 Odo_val_pos_2 = COEF_ROUE_GAUCHE * (double)encoder2.GetCounter();
Brand101 0:444ac4067b19 1461
Brand101 0:444ac4067b19 1462
Brand101 0:444ac4067b19 1463 //Calcul de la distance parcourue
Brand101 0:444ac4067b19 1464 dist = 0.5*((Odo_val_pos_1 - Odo_last_val_pos_1) + (Odo_val_pos_2 - Odo_last_val_pos_2));
Brand101 0:444ac4067b19 1465
Brand101 0:444ac4067b19 1466 //Calcul de la valeur de l'angle parcouru
Brand101 0:444ac4067b19 1467 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);
Brand101 0:444ac4067b19 1468
Brand101 0:444ac4067b19 1469 //Determination de la position sur le terrain en X, Y, Theta
Brand101 0:444ac4067b19 1470 Odo_theta += ang;
Brand101 0:444ac4067b19 1471 Odo_x += dist*cos((double)(Odo_theta*PI/1800.0))*PERIMETRE_ROUE_CODEUSE/RESOLUTION_ROUE_CODEUSE;
Brand101 0:444ac4067b19 1472 Odo_y += dist*sin((double)(Odo_theta*PI/1800.0))*PERIMETRE_ROUE_CODEUSE/RESOLUTION_ROUE_CODEUSE;
Brand101 0:444ac4067b19 1473
Brand101 0:444ac4067b19 1474 //Stockage de la derniere valeur de l'odometrie
Brand101 0:444ac4067b19 1475 Odo_last_val_pos_1 = Odo_val_pos_1;
Brand101 0:444ac4067b19 1476 Odo_last_val_pos_2 = Odo_val_pos_2;
Brand101 0:444ac4067b19 1477
Brand101 0:444ac4067b19 1478
Brand101 0:444ac4067b19 1479 //Condition d'envoi des informations de l'odometrie par CAN
Brand101 0:444ac4067b19 1480 Send_odometrie();
Brand101 0:444ac4067b19 1481
Brand101 0:444ac4067b19 1482 /****************************************/
Brand101 0:444ac4067b19 1483 /****************************************/
Brand101 0:444ac4067b19 1484 /* <DEBUG> */
Brand101 0:444ac4067b19 1485 /****************************************/
Brand101 0:444ac4067b19 1486 /****************************************/
Brand101 0:444ac4067b19 1487
Brand101 0:444ac4067b19 1488 Debug_Asserv();
Brand101 0:444ac4067b19 1489
Brand101 0:444ac4067b19 1490 /****************************************/
Brand101 0:444ac4067b19 1491 /****************************************/
Brand101 0:444ac4067b19 1492 /* <DEBUG> */
Brand101 0:444ac4067b19 1493 /****************************************/
Brand101 0:444ac4067b19 1494 /****************************************/
Brand101 0:444ac4067b19 1495
Brand101 0:444ac4067b19 1496
Brand101 0:444ac4067b19 1497 }
Brand101 0:444ac4067b19 1498
Brand101 0:444ac4067b19 1499
Brand101 0:444ac4067b19 1500 void Ralentir(short nouvelle_vitesse)
Brand101 0:444ac4067b19 1501 {
Brand101 0:444ac4067b19 1502 //Si on allait en avant (cpt >= 0), on decremente cpt jusqu'a la nouvelle vitesse
Brand101 0:444ac4067b19 1503 if(cpt >= 0) {
Brand101 0:444ac4067b19 1504 cpt --;
Brand101 0:444ac4067b19 1505 if(cpt <= ((double)nouvelle_vitesse / ((double)AMAX/1000.0))) { //La vitesse souhaitait est atteinte
Brand101 0:444ac4067b19 1506 cpt = (short)((double)nouvelle_vitesse / ((double)AMAX/1000.0));
Brand101 0:444ac4067b19 1507 ralentare = 0;
Brand101 0:444ac4067b19 1508 }
Brand101 0:444ac4067b19 1509 }
Brand101 0:444ac4067b19 1510
Brand101 0:444ac4067b19 1511 //Si on allait en arri�re (cpt < 0), on incr�mente cpt jusqu'a la nouvelle vitesse
Brand101 0:444ac4067b19 1512 else if(cpt < 0) {
Brand101 0:444ac4067b19 1513 cpt ++;
Brand101 0:444ac4067b19 1514 if(cpt >= -((double)nouvelle_vitesse / ((double)AMAX/1000.0))) { //La vitesse souhaitait est atteinte
Brand101 0:444ac4067b19 1515 cpt = (short)-((double)nouvelle_vitesse / ((double)AMAX/1000.0));
Brand101 0:444ac4067b19 1516 ralentare = 0;
Brand101 0:444ac4067b19 1517 }
Brand101 0:444ac4067b19 1518 }
Brand101 0:444ac4067b19 1519
Brand101 0:444ac4067b19 1520 //Ralentir fonctionne comme une acc�l�ration standard, donc on modifie la consigne de position
Brand101 0:444ac4067b19 1521 consigne_pos += (double)(((double)cpt * (double)AMAX)/1000.0);
Brand101 0:444ac4067b19 1522
Brand101 0:444ac4067b19 1523 //Calcul de la valeur de commande 1 et 2
Brand101 0:444ac4067b19 1524 Asser_Pos_Mot1(roue_drt_init + consigne_pos);
Brand101 0:444ac4067b19 1525 Asser_Pos_Mot2(roue_gch_init + consigne_pos);
Brand101 0:444ac4067b19 1526
Brand101 0:444ac4067b19 1527 //Ecriture de la valeur de la commande souhait�e
Brand101 0:444ac4067b19 1528 write_PWM2(commande2);
Brand101 0:444ac4067b19 1529 write_PWM1(commande1);
Brand101 0:444ac4067b19 1530
Brand101 0:444ac4067b19 1531 }
Brand101 0:444ac4067b19 1532
Brand101 0:444ac4067b19 1533 void Arret(void)
Brand101 0:444ac4067b19 1534 {
Brand101 0:444ac4067b19 1535 //Recuperation de la valeur de l'odometrie de la roue de droite et de gauche
Brand101 0:444ac4067b19 1536 roue_drt_init = (double)encoder1.GetCounter();
Brand101 0:444ac4067b19 1537
Brand101 0:444ac4067b19 1538 /* MOI*\
Brand101 0:444ac4067b19 1539 //#elif GROS_ROB == 0
Brand101 0:444ac4067b19 1540 roue_gch_init = COEF_ROUE_GAUCHE * (double)encoder2.GetCounter();
Brand101 0:444ac4067b19 1541 //#endif
Brand101 0:444ac4067b19 1542 */
Brand101 0:444ac4067b19 1543 roue_gch_init = COEF_ROUE_GAUCHE * (double)encoder2.GetCounter();
Brand101 0:444ac4067b19 1544
Brand101 0:444ac4067b19 1545
Brand101 0:444ac4067b19 1546 //Calcul de la valeur des commandes 1 et 2
Brand101 0:444ac4067b19 1547 Asser_Pos_Mot1(roue_drt_init);
Brand101 0:444ac4067b19 1548 Asser_Pos_Mot2(roue_gch_init);
Brand101 0:444ac4067b19 1549
Brand101 0:444ac4067b19 1550 //Ecrire un PWM egal a 0
Brand101 0:444ac4067b19 1551 write_PWM2(0.0);
Brand101 0:444ac4067b19 1552 write_PWM1(0.0);
Brand101 0:444ac4067b19 1553
Brand101 0:444ac4067b19 1554 next_move = 1;
Brand101 0:444ac4067b19 1555
Brand101 0:444ac4067b19 1556 }
Brand101 0:444ac4067b19 1557
Brand101 0:444ac4067b19 1558 void Recalage(int pcons, short vmax, short amax, short dir, short nv_val)
Brand101 0:444ac4067b19 1559 {
Brand101 0:444ac4067b19 1560 //char val1[8];
Brand101 0:444ac4067b19 1561
Brand101 0:444ac4067b19 1562 //Mode AVANCER
Brand101 0:444ac4067b19 1563 if(pcons >= 0) {
Brand101 0:444ac4067b19 1564 switch(etat_automate_depl) {
Brand101 0:444ac4067b19 1565 case INIT_RECALAGE : //etat INIT_RECELAGE
Brand101 0:444ac4067b19 1566 etat_automate_depl = ACCELERATION_RECALAGE; //Passage a l'etat ACCELERATION_RECALAGE
Brand101 0:444ac4067b19 1567 consigne_pos = 0; //Initialisation des differentes variables
Brand101 0:444ac4067b19 1568 cpt = 0;
Brand101 0:444ac4067b19 1569 break;
Brand101 0:444ac4067b19 1570
Brand101 0:444ac4067b19 1571 case ACCELERATION_RECALAGE : //etat ACCELERATION_RECALAGE
Brand101 0:444ac4067b19 1572 // Phase d'acc�leration comme si on faisait une ligne droite
Brand101 0:444ac4067b19 1573
Brand101 0:444ac4067b19 1574 if(consigne_pos >= pcons/2) etat_automate_depl = FIN_RECALAGE; //Passage a l'etat FIN_RECALAGE
Brand101 0:444ac4067b19 1575 cpt ++;
Brand101 0:444ac4067b19 1576
Brand101 0:444ac4067b19 1577 //Calcul de la consigne de position
Brand101 0:444ac4067b19 1578 consigne_pos += (((double)cpt * (double)amax)/1000.0);
Brand101 0:444ac4067b19 1579
Brand101 0:444ac4067b19 1580 if(cpt >= ((double)vmax / ((double)amax/1000.0))) {
Brand101 0:444ac4067b19 1581 etat_automate_depl = VITESSE_CONSTANTE_RECALAGE; //Passage a l'etat VITESSE_CONSTANTE_RECALAGE
Brand101 0:444ac4067b19 1582 }
Brand101 0:444ac4067b19 1583
Brand101 0:444ac4067b19 1584 // Si les 2 erreurs sont sup�rieures � un seuil d�fini, on a fini le recalage
Brand101 0:444ac4067b19 1585 if(ErreurPos1 >= RECALAGE_TH && ErreurPos2 >= RECALAGE_TH) {
Brand101 0:444ac4067b19 1586 etat_automate_depl = FIN_RECALAGE; //Passage a l'etat FIN_RECALAGE
Brand101 0:444ac4067b19 1587 commande1 = 0.0; //Remise a zero des valeurs des commandes
Brand101 0:444ac4067b19 1588 commande2 = 0.0;
Brand101 0:444ac4067b19 1589 }
Brand101 0:444ac4067b19 1590 // Si on d�tecte une augmentation d'erreur sur une roue, on l'arr�te pour �viter de patiner
Brand101 0:444ac4067b19 1591 else if(ErreurPos1 >= RECALAGE_TH) commande1 = 0.0;
Brand101 0:444ac4067b19 1592 else if(ErreurPos2 >= RECALAGE_TH) commande2 = 0.0;
Brand101 0:444ac4067b19 1593 break;
Brand101 0:444ac4067b19 1594
Brand101 0:444ac4067b19 1595 case VITESSE_CONSTANTE_RECALAGE : //etat VITESSE_CONSTANTE_RECALAGE
Brand101 0:444ac4067b19 1596 // Phase de vitesse constante
Brand101 0:444ac4067b19 1597 consigne_pos += vmax;
Brand101 0:444ac4067b19 1598
Brand101 0:444ac4067b19 1599 // Idem que plus haut, on surveille les erreurs des 2 roues
Brand101 0:444ac4067b19 1600 if(ErreurPos1 >= RECALAGE_TH && ErreurPos2 >= RECALAGE_TH) {
Brand101 0:444ac4067b19 1601 etat_automate_depl = FIN_RECALAGE; //Passage a l'etat FIN_RECALAGE
Brand101 0:444ac4067b19 1602 commande1 = 0.0; //Remise a zero des valeurs des commandes
Brand101 0:444ac4067b19 1603 commande2 = 0.0;
Brand101 0:444ac4067b19 1604 } else if(ErreurPos1 >= RECALAGE_TH) commande1 = 0.0; //Mise a zero de la commande 1
Brand101 0:444ac4067b19 1605 else if(ErreurPos2 >= RECALAGE_TH) commande2 = 0.0; //Mise a zero de la commande 2
Brand101 0:444ac4067b19 1606 break;
Brand101 0:444ac4067b19 1607
Brand101 0:444ac4067b19 1608 case FIN_RECALAGE : //etat FIN_RECALAGE
Brand101 0:444ac4067b19 1609 // Fin du recalage, on met � jour les donn�es de position
Brand101 0:444ac4067b19 1610 if(cpt >=20) {
Brand101 0:444ac4067b19 1611 //Recalage de x
Brand101 0:444ac4067b19 1612 if(dir == 1) {
Brand101 0:444ac4067b19 1613 Odo_x = nv_val;
Brand101 0:444ac4067b19 1614 // On met l'angle � jour en fonction de la position sur le terrain
Brand101 0:444ac4067b19 1615 // Si on est dans la partie haute ( > la moiti�), on est dans un sens,
Brand101 0:444ac4067b19 1616 // Si on est dans la partie basse, on met � jour dans l'autre sens
Brand101 0:444ac4067b19 1617 // On prend aussi en compte le sens dans lequel on a fait la ligne droite
Brand101 0:444ac4067b19 1618
Brand101 0:444ac4067b19 1619 if(nv_val > 1000) {
Brand101 0:444ac4067b19 1620 if(pcons >=0)
Brand101 0:444ac4067b19 1621 Odo_theta = 0;
Brand101 0:444ac4067b19 1622 else
Brand101 0:444ac4067b19 1623 Odo_theta = 1800;
Brand101 0:444ac4067b19 1624 }
Brand101 0:444ac4067b19 1625
Brand101 0:444ac4067b19 1626 else {
Brand101 0:444ac4067b19 1627 if(pcons >= 0)
Brand101 0:444ac4067b19 1628 Odo_theta = 1800;
Brand101 0:444ac4067b19 1629 else
Brand101 0:444ac4067b19 1630 Odo_theta = 0;
Brand101 0:444ac4067b19 1631 }
Brand101 0:444ac4067b19 1632 }
Brand101 0:444ac4067b19 1633
Brand101 0:444ac4067b19 1634 //Recalage de y
Brand101 0:444ac4067b19 1635 else {
Brand101 0:444ac4067b19 1636 Odo_y = nv_val;
Brand101 0:444ac4067b19 1637
Brand101 0:444ac4067b19 1638 if(nv_val > 1500) {
Brand101 0:444ac4067b19 1639 if(pcons >=0)
Brand101 0:444ac4067b19 1640 Odo_theta = 900;
Brand101 0:444ac4067b19 1641 else
Brand101 0:444ac4067b19 1642 Odo_theta = -900;
Brand101 0:444ac4067b19 1643 }
Brand101 0:444ac4067b19 1644
Brand101 0:444ac4067b19 1645 else {
Brand101 0:444ac4067b19 1646 if(pcons >= 0)
Brand101 0:444ac4067b19 1647 Odo_theta = -900;
Brand101 0:444ac4067b19 1648 else
Brand101 0:444ac4067b19 1649 Odo_theta = 900;
Brand101 0:444ac4067b19 1650 }
Brand101 0:444ac4067b19 1651 }
Brand101 0:444ac4067b19 1652
Brand101 0:444ac4067b19 1653 etat_automate_depl = INIT_RECALAGE;
Brand101 0:444ac4067b19 1654
Brand101 0:444ac4067b19 1655 next_move = 1;
Brand101 0:444ac4067b19 1656
Brand101 0:444ac4067b19 1657 //Mise a zero des commandes et des moteurs;
Brand101 0:444ac4067b19 1658 commande1 = 0.0;
Brand101 0:444ac4067b19 1659 commande2 = 0.0;
Brand101 0:444ac4067b19 1660 write_PWM1(0.0);
Brand101 0:444ac4067b19 1661 write_PWM2(0.0);
Brand101 0:444ac4067b19 1662 }
Brand101 0:444ac4067b19 1663 cpt ++;
Brand101 0:444ac4067b19 1664 break;
Brand101 0:444ac4067b19 1665 }
Brand101 0:444ac4067b19 1666 }
Brand101 0:444ac4067b19 1667
Brand101 0:444ac4067b19 1668 //Mode RECULER
Brand101 0:444ac4067b19 1669 else if(pcons < 0) {
Brand101 0:444ac4067b19 1670 switch(etat_automate_depl) {
Brand101 0:444ac4067b19 1671 case INIT_RECALAGE : //etat INIT_RECELAGE
Brand101 0:444ac4067b19 1672 etat_automate_depl = ACCELERATION_RECALAGE; //Passage a l'etat ACCELERATION_RECALAGE
Brand101 0:444ac4067b19 1673 consigne_pos = 0; //Initialisation des diff�rentes variables
Brand101 0:444ac4067b19 1674 cpt = 0;
Brand101 0:444ac4067b19 1675 break;
Brand101 0:444ac4067b19 1676
Brand101 0:444ac4067b19 1677 case ACCELERATION_RECALAGE : //etat ACCELERATION_RECALAGE
Brand101 0:444ac4067b19 1678 // Phase d'acc�leration comme si on faisait une ligne droite
Brand101 0:444ac4067b19 1679 if(consigne_pos <= pcons/2) etat_automate_depl = FIN_RECALAGE; //Passage a l'etat FIN_RECALAGE
Brand101 0:444ac4067b19 1680
Brand101 0:444ac4067b19 1681 cpt --;
Brand101 0:444ac4067b19 1682 consigne_pos += (double)(((double)cpt * (double)amax)/1000.0);
Brand101 0:444ac4067b19 1683
Brand101 0:444ac4067b19 1684 if(cpt <= -((double)vmax / ((double)amax/1000.0))) {
Brand101 0:444ac4067b19 1685 etat_automate_depl = VITESSE_CONSTANTE_RECALAGE; //Passage a l'etat VITESSE_CONSTANTE_RECALAGE
Brand101 0:444ac4067b19 1686 }
Brand101 0:444ac4067b19 1687
Brand101 0:444ac4067b19 1688 // Si les 2 erreurs sont inf�rieures � un seuil d�fini, on a fini le recalage
Brand101 0:444ac4067b19 1689 if(ErreurPos1 <= -RECALAGE_TH && ErreurPos2 <= -RECALAGE_TH) {
Brand101 0:444ac4067b19 1690 etat_automate_depl = FIN_RECALAGE; //Passage a l'etat FIN_RECALAGE
Brand101 0:444ac4067b19 1691 commande1 = 0.0;
Brand101 0:444ac4067b19 1692 commande2 = 0.0;
Brand101 0:444ac4067b19 1693 }
Brand101 0:444ac4067b19 1694 // Si on d�tecte une augmentation d'erreur sur une roue, on l'arr�te pour �viter de patiner
Brand101 0:444ac4067b19 1695 else if(ErreurPos1 <= -RECALAGE_TH) commande1 = 0.0;
Brand101 0:444ac4067b19 1696 else if(ErreurPos2 <= -RECALAGE_TH) commande2 = 0.0;
Brand101 0:444ac4067b19 1697 break;
Brand101 0:444ac4067b19 1698
Brand101 0:444ac4067b19 1699 case VITESSE_CONSTANTE_RECALAGE : //etat VITESSE_CONSTANTE_RECALAGE
Brand101 0:444ac4067b19 1700 // Phase de vitesse constante
Brand101 0:444ac4067b19 1701 consigne_pos -= vmax;
Brand101 0:444ac4067b19 1702
Brand101 0:444ac4067b19 1703 // Idem que plus haut, on surveille les erreurs des 2 roues
Brand101 0:444ac4067b19 1704 if(ErreurPos1 <= -RECALAGE_TH && ErreurPos2 <= -RECALAGE_TH) {
Brand101 0:444ac4067b19 1705 etat_automate_depl = FIN_RECALAGE; //Passage a l'etat FIN_RECALAGE
Brand101 0:444ac4067b19 1706 commande1 = 0.0;
Brand101 0:444ac4067b19 1707 commande2 = 0.0;
Brand101 0:444ac4067b19 1708 } else if(ErreurPos1 <= -RECALAGE_TH) commande1 = 0.0;
Brand101 0:444ac4067b19 1709 else if(ErreurPos2 <= -RECALAGE_TH)commande2 = 0.0;
Brand101 0:444ac4067b19 1710 break;
Brand101 0:444ac4067b19 1711
Brand101 0:444ac4067b19 1712 case FIN_RECALAGE : //etat FIN_RECALAGE
Brand101 0:444ac4067b19 1713 if(cpt >=20) {
Brand101 0:444ac4067b19 1714 //Recalage de x
Brand101 0:444ac4067b19 1715 if(dir == 1) {
Brand101 0:444ac4067b19 1716 Odo_x = nv_val;
Brand101 0:444ac4067b19 1717
Brand101 0:444ac4067b19 1718 if(nv_val > 1000) {
Brand101 0:444ac4067b19 1719 if(pcons >=0)
Brand101 0:444ac4067b19 1720 Odo_theta = 0;
Brand101 0:444ac4067b19 1721 else
Brand101 0:444ac4067b19 1722 Odo_theta = 1800;
Brand101 0:444ac4067b19 1723 }
Brand101 0:444ac4067b19 1724
Brand101 0:444ac4067b19 1725 else {
Brand101 0:444ac4067b19 1726 if(pcons >= 0)
Brand101 0:444ac4067b19 1727 Odo_theta = 1800;
Brand101 0:444ac4067b19 1728 else
Brand101 0:444ac4067b19 1729 Odo_theta = 0;
Brand101 0:444ac4067b19 1730 }
Brand101 0:444ac4067b19 1731 }
Brand101 0:444ac4067b19 1732
Brand101 0:444ac4067b19 1733 //Recalage de y
Brand101 0:444ac4067b19 1734 else {
Brand101 0:444ac4067b19 1735 Odo_y = nv_val;
Brand101 0:444ac4067b19 1736
Brand101 0:444ac4067b19 1737 if(nv_val > 1500) {
Brand101 0:444ac4067b19 1738 if(pcons >=0)
Brand101 0:444ac4067b19 1739 Odo_theta = 900;
Brand101 0:444ac4067b19 1740 else
Brand101 0:444ac4067b19 1741 Odo_theta = -900;
Brand101 0:444ac4067b19 1742 }
Brand101 0:444ac4067b19 1743
Brand101 0:444ac4067b19 1744 else {
Brand101 0:444ac4067b19 1745 if(pcons >= 0)
Brand101 0:444ac4067b19 1746 Odo_theta = -900;
Brand101 0:444ac4067b19 1747 else
Brand101 0:444ac4067b19 1748 Odo_theta = 900;
Brand101 0:444ac4067b19 1749 }
Brand101 0:444ac4067b19 1750 }
Brand101 0:444ac4067b19 1751
Brand101 0:444ac4067b19 1752 etat_automate_depl = 0;
Brand101 0:444ac4067b19 1753
Brand101 0:444ac4067b19 1754 next_move = 1;
Brand101 0:444ac4067b19 1755
Brand101 0:444ac4067b19 1756 //Mise a zero des commandes et des moteurs
Brand101 0:444ac4067b19 1757 commande1 = 0.0;
Brand101 0:444ac4067b19 1758 commande2 = 0.0;
Brand101 0:444ac4067b19 1759 write_PWM1(0.0);
Brand101 0:444ac4067b19 1760 write_PWM2(0.0);
Brand101 0:444ac4067b19 1761 }
Brand101 0:444ac4067b19 1762 cpt ++;
Brand101 0:444ac4067b19 1763 break;
Brand101 0:444ac4067b19 1764 }
Brand101 0:444ac4067b19 1765 }
Brand101 0:444ac4067b19 1766
Brand101 0:444ac4067b19 1767 //Calcul des correcteurs a apporter aux moteurs
Brand101 0:444ac4067b19 1768 Asser_Pos_Mot1(roue_drt_init + consigne_pos);
Brand101 0:444ac4067b19 1769 Asser_Pos_Mot2(roue_gch_init + consigne_pos);
Brand101 0:444ac4067b19 1770
Brand101 0:444ac4067b19 1771 //Envoi de la nouvelle valeur de commande aux PWM
Brand101 0:444ac4067b19 1772 write_PWM2(commande2);
Brand101 0:444ac4067b19 1773 write_PWM1(commande1);
Brand101 0:444ac4067b19 1774
Brand101 0:444ac4067b19 1775 }
Brand101 0:444ac4067b19 1776
Brand101 0:444ac4067b19 1777
Brand101 0:444ac4067b19 1778 void Rayon_De_Courbure(short rayon, short theta, short vmax, short amax, short sens, short dmax)
Brand101 0:444ac4067b19 1779 {
Brand101 0:444ac4067b19 1780 //Declaration des variables
Brand101 0:444ac4067b19 1781 //char val1[8];
Brand101 0:444ac4067b19 1782 static double tc, ta, td, vmax_tri, pcons;
Brand101 0:444ac4067b19 1783
Brand101 0:444ac4067b19 1784 //Si l'angle a parcourir est positif
Brand101 0:444ac4067b19 1785 if(theta >= 0) {
Brand101 0:444ac4067b19 1786 switch(etat_automate_depl) {
Brand101 0:444ac4067b19 1787 case INIT_RAYON_COURBURE : //etat d'initialisation
Brand101 0:444ac4067b19 1788 etat_automate_depl = ACCELERATION_TRAPEZE_RAYON_COURBURE; //Passage a l'etat ACCELERATION_TRAPEZE_RAYON_COURBURE
Brand101 0:444ac4067b19 1789
Brand101 0:444ac4067b19 1790 //Mise a zero des variables car on effectue un nouveau deplacement
Brand101 0:444ac4067b19 1791 consigne_pos = 0;
Brand101 0:444ac4067b19 1792 cpt = 0;
Brand101 0:444ac4067b19 1793
Brand101 0:444ac4067b19 1794 //Calcul de la position voulue
Brand101 0:444ac4067b19 1795 pcons = ((rayon + LARGEUR_ROBOT/2) * 2 * PI * theta / 3600) * RESOLUTION_ROUE_CODEUSE / PERIMETRE_ROUE_CODEUSE;
Brand101 0:444ac4067b19 1796
Brand101 0:444ac4067b19 1797 //Elaboration du profil de vitesse
Brand101 0:444ac4067b19 1798 //Calcul du temps a vitesse constante
Brand101 0:444ac4067b19 1799 tc = ((double)pcons / (double)vmax) - (((double)vmax * 500.0) / (double)amax) - (((double)vmax * 500.0) / (double)dmax);
Brand101 0:444ac4067b19 1800
Brand101 0:444ac4067b19 1801 if (tc > 0) { //Resultat positif, il y a une phase de vitesse constante
Brand101 0:444ac4067b19 1802 etat_automate_depl = ACCELERATION_TRAPEZE_RAYON_COURBURE; //Passage a l'etat ACCELERATION_TRAPEZE_RAYON_COURBURE
Brand101 0:444ac4067b19 1803 ta = (((double)vmax * 1000.0) / (double)amax); //Calcul du temps d'acceleration
Brand101 0:444ac4067b19 1804 td = (((double)vmax * 1000.0) / (double)dmax); //Calcul du temps de deceleration
Brand101 0:444ac4067b19 1805
Brand101 0:444ac4067b19 1806 canmsg.id = 0x7F1;
Brand101 0:444ac4067b19 1807 canmsg.len = 0;
Brand101 0:444ac4067b19 1808 can1.write(canmsg);
Brand101 0:444ac4067b19 1809
Brand101 0:444ac4067b19 1810 }
Brand101 0:444ac4067b19 1811
Brand101 0:444ac4067b19 1812 //Temps a vitesse constant inexistant
Brand101 0:444ac4067b19 1813 else if(tc < 0) { //Resultat negatif, il n'y a pas de phase de vitesse constante
Brand101 0:444ac4067b19 1814 etat_automate_depl = ACCELERATION_TRIANGLE_RAYON_COURBURE; //Passage a l'etat ACCELERATION_TRIANGLE_RAYON_COURBURE
Brand101 0:444ac4067b19 1815
Brand101 0:444ac4067b19 1816 //Calcul de la vitesse maximale en profil triangle
Brand101 0:444ac4067b19 1817 vmax_tri = sqrt((2 * pcons) / ((1000.0 / (double)amax) + (1000.0 / (double)dmax)));
Brand101 0:444ac4067b19 1818
Brand101 0:444ac4067b19 1819 canmsg.id = 0x7F5;
Brand101 0:444ac4067b19 1820 canmsg.len = 0;
Brand101 0:444ac4067b19 1821 can1.write(canmsg);
Brand101 0:444ac4067b19 1822
Brand101 0:444ac4067b19 1823 }
Brand101 0:444ac4067b19 1824 break;
Brand101 0:444ac4067b19 1825
Brand101 0:444ac4067b19 1826 case ACCELERATION_TRAPEZE_RAYON_COURBURE : //Etat d'acceleration en profil trapeze
Brand101 0:444ac4067b19 1827 cpt ++;
Brand101 0:444ac4067b19 1828
Brand101 0:444ac4067b19 1829 //Incrementation de la consigne de position
Brand101 0:444ac4067b19 1830 consigne_pos += (((double)cpt * (double)amax) / 1000.0);
Brand101 0:444ac4067b19 1831
Brand101 0:444ac4067b19 1832 if(cpt >= ta) { //Condition pour quitter la phase d'acceleration
Brand101 0:444ac4067b19 1833 etat_automate_depl = VITESSE_CONSTANTE_TRAPEZE_RAYON_COURBURE; //Passage a l'etat VITESSE_CONSTANTE_TRAPEZE_RAYON_COURBURE
Brand101 0:444ac4067b19 1834 cpt = 0;
Brand101 0:444ac4067b19 1835
Brand101 0:444ac4067b19 1836 canmsg.id = 0x7F2;
Brand101 0:444ac4067b19 1837 canmsg.len = 0;
Brand101 0:444ac4067b19 1838 can1.write(canmsg);
Brand101 0:444ac4067b19 1839
Brand101 0:444ac4067b19 1840 }
Brand101 0:444ac4067b19 1841 break;
Brand101 0:444ac4067b19 1842
Brand101 0:444ac4067b19 1843 case VITESSE_CONSTANTE_TRAPEZE_RAYON_COURBURE : //Etat de vitesse constante en profil trapeze
Brand101 0:444ac4067b19 1844 cpt ++;
Brand101 0:444ac4067b19 1845
Brand101 0:444ac4067b19 1846 //Incrementation de la consigne de vitesse par la valeur de la vitesse
Brand101 0:444ac4067b19 1847 consigne_pos += vmax;
Brand101 0:444ac4067b19 1848
Brand101 0:444ac4067b19 1849 //Si il n'y a pas d'enchainements
Brand101 0:444ac4067b19 1850 if(cpt >= tc && liste[cpt_ordre].enchainement != 1) { //Condition pour quitter la phase de vitesse constante
Brand101 0:444ac4067b19 1851 etat_automate_depl = DECELERATION_TRAPEZE_RAYON_COURBURE; //Passage a l'etat DECELERATION_TRAPEZE_RAYON_COURBURE
Brand101 0:444ac4067b19 1852 cpt = 0;
Brand101 0:444ac4067b19 1853
Brand101 0:444ac4067b19 1854 canmsg.id = 0x7F3;
Brand101 0:444ac4067b19 1855 canmsg.len = 0;
Brand101 0:444ac4067b19 1856 can1.write(canmsg);
Brand101 0:444ac4067b19 1857
Brand101 0:444ac4067b19 1858 }
Brand101 0:444ac4067b19 1859
Brand101 0:444ac4067b19 1860 //Si on est dans un enchainement, on passe � l'instruction suivante
Brand101 0:444ac4067b19 1861 else if(consigne_pos >= pcons && liste[cpt_ordre].enchainement == 1) {
Brand101 0:444ac4067b19 1862 consigne_pos = pcons;
Brand101 0:444ac4067b19 1863 cpt_ordre = 0;
Brand101 0:444ac4067b19 1864 }
Brand101 0:444ac4067b19 1865 break;
Brand101 0:444ac4067b19 1866
Brand101 0:444ac4067b19 1867 case DECELERATION_TRAPEZE_RAYON_COURBURE : //Etat de deceleration en profil trapeze
Brand101 0:444ac4067b19 1868 cpt ++;
Brand101 0:444ac4067b19 1869
Brand101 0:444ac4067b19 1870 //Incrementation de la consigne de position
Brand101 0:444ac4067b19 1871 consigne_pos += vmax - (double)(((double)cpt * (double)dmax) / 1000.0);
Brand101 0:444ac4067b19 1872
Brand101 0:444ac4067b19 1873 if(cpt >= td) { //Condition pour quitter la phase de deceleration en profil trapeze
Brand101 0:444ac4067b19 1874 etat_automate_depl = ARRET_RAYON_COURBURE; //Passage a l'etat ARRET_RAYON_COURBURE
Brand101 0:444ac4067b19 1875 consigne_pos = pcons;
Brand101 0:444ac4067b19 1876 cpt = 0;
Brand101 0:444ac4067b19 1877
Brand101 0:444ac4067b19 1878 canmsg.id = 0x7F4;
Brand101 0:444ac4067b19 1879 canmsg.len = 0;
Brand101 0:444ac4067b19 1880 can1.write(canmsg);
Brand101 0:444ac4067b19 1881
Brand101 0:444ac4067b19 1882 }
Brand101 0:444ac4067b19 1883 break;
Brand101 0:444ac4067b19 1884
Brand101 0:444ac4067b19 1885 case ARRET_RAYON_COURBURE : //Etat d'arret
Brand101 0:444ac4067b19 1886 if(cpt >= 20) {
Brand101 0:444ac4067b19 1887 etat_automate_depl = INIT_RAYON_COURBURE; //Passage a l'etat d'INIT_RAYON_COURBURE
Brand101 0:444ac4067b19 1888 next_move = 1;
Brand101 0:444ac4067b19 1889
Brand101 0:444ac4067b19 1890 }
Brand101 0:444ac4067b19 1891 cpt ++;
Brand101 0:444ac4067b19 1892 break;
Brand101 0:444ac4067b19 1893
Brand101 0:444ac4067b19 1894 case ACCELERATION_TRIANGLE_RAYON_COURBURE : //Etat d'acceleration en profil triangle
Brand101 0:444ac4067b19 1895 cpt ++;
Brand101 0:444ac4067b19 1896
Brand101 0:444ac4067b19 1897 //Incrementation de la consigne de position
Brand101 0:444ac4067b19 1898 consigne_pos += (((double)cpt * (double)amax) / 1000.0);
Brand101 0:444ac4067b19 1899
Brand101 0:444ac4067b19 1900 if((cpt * amax / 1000.0) >= vmax_tri) { //Condition pour sortir de la phase d'acceleration en profil triangle
Brand101 0:444ac4067b19 1901 etat_automate_depl = DECELERATION_TRIANGLE_RAYON_COURBURE; //Passage a l'etat DECELERATION_TRIANGLE_RAYON_COURBURE
Brand101 0:444ac4067b19 1902 cpt = 0;
Brand101 0:444ac4067b19 1903
Brand101 0:444ac4067b19 1904 canmsg.id = 0x7F6;
Brand101 0:444ac4067b19 1905 canmsg.len = 0;
Brand101 0:444ac4067b19 1906 can1.write(canmsg);
Brand101 0:444ac4067b19 1907
Brand101 0:444ac4067b19 1908 }
Brand101 0:444ac4067b19 1909 break;
Brand101 0:444ac4067b19 1910
Brand101 0:444ac4067b19 1911 case DECELERATION_TRIANGLE_RAYON_COURBURE : //Etat de deceleration en profil triangle
Brand101 0:444ac4067b19 1912 cpt ++;
Brand101 0:444ac4067b19 1913
Brand101 0:444ac4067b19 1914 //Incrementation de la consigne de position
Brand101 0:444ac4067b19 1915 consigne_pos += vmax_tri - (((double)cpt * (double)dmax) / 1000.0);
Brand101 0:444ac4067b19 1916
Brand101 0:444ac4067b19 1917 if(((double)vmax_tri - (((double)cpt * (double)dmax)) / 1000.0) <= 0) { //Condition pour sortir de la phase de deceleration en profil triangle
Brand101 0:444ac4067b19 1918 etat_automate_depl = ARRET_RAYON_COURBURE;
Brand101 0:444ac4067b19 1919 cpt = 0;
Brand101 0:444ac4067b19 1920
Brand101 0:444ac4067b19 1921 canmsg.id = 0x7F4;
Brand101 0:444ac4067b19 1922 canmsg.len = 0;
Brand101 0:444ac4067b19 1923 can1.write(canmsg);
Brand101 0:444ac4067b19 1924
Brand101 0:444ac4067b19 1925 }
Brand101 0:444ac4067b19 1926 break;
Brand101 0:444ac4067b19 1927 }
Brand101 0:444ac4067b19 1928 }
Brand101 0:444ac4067b19 1929
Brand101 0:444ac4067b19 1930 //Si l'angle a parcourir est negatif
Brand101 0:444ac4067b19 1931 else if(theta < 0) {
Brand101 0:444ac4067b19 1932 switch(etat_automate_depl) {
Brand101 0:444ac4067b19 1933 case 0 :
Brand101 0:444ac4067b19 1934 etat_automate_depl++;
Brand101 0:444ac4067b19 1935 consigne_pos = 0;
Brand101 0:444ac4067b19 1936 cpt = 0;
Brand101 0:444ac4067b19 1937 pcons = ((rayon + LARGEUR_ROBOT/2) * 2 * PI * theta / 3600) * RESOLUTION_ROUE_CODEUSE / PERIMETRE_ROUE_CODEUSE;
Brand101 0:444ac4067b19 1938 tc = ((double)pcons / (double)vmax) - (((double)vmax * 500.0) / (double)amax) - (((double)vmax * 500.0) / (double)dmax);
Brand101 0:444ac4067b19 1939 if (tc > 0) {
Brand101 0:444ac4067b19 1940 etat_automate_depl++;
Brand101 0:444ac4067b19 1941 ta = (((double)vmax * 1000.0) / (double)amax);
Brand101 0:444ac4067b19 1942 td = (((double)vmax * 1000.0) / (double)dmax);
Brand101 0:444ac4067b19 1943
Brand101 0:444ac4067b19 1944 canmsg.id = 0x7F1;
Brand101 0:444ac4067b19 1945 canmsg.len = 0;
Brand101 0:444ac4067b19 1946 can1.write(canmsg);
Brand101 0:444ac4067b19 1947
Brand101 0:444ac4067b19 1948 } else if(tc < 0) {
Brand101 0:444ac4067b19 1949 etat_automate_depl = 5;
Brand101 0:444ac4067b19 1950 vmax_tri = sqrt((2 * -pcons) / ((1000.0 / (double)amax) + (1000.0 / (double)dmax)));
Brand101 0:444ac4067b19 1951
Brand101 0:444ac4067b19 1952 canmsg.id = 0x7F5;
Brand101 0:444ac4067b19 1953 canmsg.len = 0;
Brand101 0:444ac4067b19 1954 can1.write(canmsg);
Brand101 0:444ac4067b19 1955
Brand101 0:444ac4067b19 1956 }
Brand101 0:444ac4067b19 1957 break;
Brand101 0:444ac4067b19 1958 case 1 :
Brand101 0:444ac4067b19 1959 cpt ++;
Brand101 0:444ac4067b19 1960 consigne_pos -= (((double)cpt * (double)amax) / 1000.0);
Brand101 0:444ac4067b19 1961 if(cpt >= ta) {
Brand101 0:444ac4067b19 1962 etat_automate_depl ++;
Brand101 0:444ac4067b19 1963 cpt = 0;
Brand101 0:444ac4067b19 1964
Brand101 0:444ac4067b19 1965 canmsg.id = 0x7F2;
Brand101 0:444ac4067b19 1966 canmsg.len = 0;
Brand101 0:444ac4067b19 1967 can1.write(canmsg);
Brand101 0:444ac4067b19 1968
Brand101 0:444ac4067b19 1969 }
Brand101 0:444ac4067b19 1970 break;
Brand101 0:444ac4067b19 1971 case 2 :
Brand101 0:444ac4067b19 1972 cpt ++;
Brand101 0:444ac4067b19 1973 consigne_pos -= vmax;
Brand101 0:444ac4067b19 1974 if(cpt >= tc && liste[cpt_ordre].enchainement != 1) {
Brand101 0:444ac4067b19 1975 etat_automate_depl ++;
Brand101 0:444ac4067b19 1976 cpt = 0;
Brand101 0:444ac4067b19 1977
Brand101 0:444ac4067b19 1978 canmsg.id = 0x7F3;
Brand101 0:444ac4067b19 1979 canmsg.len = 0;
Brand101 0:444ac4067b19 1980 can1.write(canmsg);
Brand101 0:444ac4067b19 1981
Brand101 0:444ac4067b19 1982 }
Brand101 0:444ac4067b19 1983 //Si on est dans un enchainement, on passe � l'instruction suivante
Brand101 0:444ac4067b19 1984 else if(consigne_pos >= pcons && liste[cpt_ordre].enchainement == 1) {
Brand101 0:444ac4067b19 1985 consigne_pos = pcons;
Brand101 0:444ac4067b19 1986 cpt_ordre = 0;
Brand101 0:444ac4067b19 1987 }
Brand101 0:444ac4067b19 1988 break;
Brand101 0:444ac4067b19 1989 case 3 :
Brand101 0:444ac4067b19 1990 cpt ++;
Brand101 0:444ac4067b19 1991 consigne_pos -= vmax - (double)(((double)cpt * (double)dmax) / 1000.0);
Brand101 0:444ac4067b19 1992 if(cpt >= td) {
Brand101 0:444ac4067b19 1993 etat_automate_depl ++;
Brand101 0:444ac4067b19 1994 consigne_pos = pcons;
Brand101 0:444ac4067b19 1995 cpt = 0;
Brand101 0:444ac4067b19 1996
Brand101 0:444ac4067b19 1997 canmsg.id = 0x7F4;
Brand101 0:444ac4067b19 1998 canmsg.len = 0;
Brand101 0:444ac4067b19 1999 can1.write(canmsg);
Brand101 0:444ac4067b19 2000
Brand101 0:444ac4067b19 2001 }
Brand101 0:444ac4067b19 2002 break;
Brand101 0:444ac4067b19 2003 case 4 :
Brand101 0:444ac4067b19 2004 if(cpt >= 20) {
Brand101 0:444ac4067b19 2005 etat_automate_depl = 0;
Brand101 0:444ac4067b19 2006 next_move = 1;
Brand101 0:444ac4067b19 2007
Brand101 0:444ac4067b19 2008 }
Brand101 0:444ac4067b19 2009 cpt ++;
Brand101 0:444ac4067b19 2010 break;
Brand101 0:444ac4067b19 2011 case 5 :
Brand101 0:444ac4067b19 2012 cpt ++;
Brand101 0:444ac4067b19 2013 consigne_pos -= (((double)cpt * (double)amax) / 1000.0);
Brand101 0:444ac4067b19 2014 if((cpt * amax / 1000.0) >= vmax_tri) {
Brand101 0:444ac4067b19 2015 etat_automate_depl ++;
Brand101 0:444ac4067b19 2016 cpt = 0;
Brand101 0:444ac4067b19 2017
Brand101 0:444ac4067b19 2018 canmsg.id = 0x7F6;
Brand101 0:444ac4067b19 2019 canmsg.len = 0;
Brand101 0:444ac4067b19 2020 can1.write(canmsg);
Brand101 0:444ac4067b19 2021
Brand101 0:444ac4067b19 2022 }
Brand101 0:444ac4067b19 2023 break;
Brand101 0:444ac4067b19 2024 case 6 :
Brand101 0:444ac4067b19 2025 cpt ++;
Brand101 0:444ac4067b19 2026 consigne_pos -= vmax_tri - (((double)cpt * (double)dmax) / 1000.0);
Brand101 0:444ac4067b19 2027 if(((double)vmax_tri - (((double)cpt * (double)dmax)) / 1000.0) <= 0) {
Brand101 0:444ac4067b19 2028 etat_automate_depl = 4;
Brand101 0:444ac4067b19 2029 cpt = 0;
Brand101 0:444ac4067b19 2030
Brand101 0:444ac4067b19 2031 canmsg.id = 0x7F4;
Brand101 0:444ac4067b19 2032 canmsg.len = 0;
Brand101 0:444ac4067b19 2033 can1.write(canmsg);
Brand101 0:444ac4067b19 2034
Brand101 0:444ac4067b19 2035 }
Brand101 0:444ac4067b19 2036 break;
Brand101 0:444ac4067b19 2037 }
Brand101 0:444ac4067b19 2038 }
Brand101 0:444ac4067b19 2039
Brand101 0:444ac4067b19 2040 //Determination des commandes en fonction de la direction de deplacement du robot
Brand101 0:444ac4067b19 2041 if(sens >= 0) {
Brand101 0:444ac4067b19 2042
Brand101 0:444ac4067b19 2043 Asser_Pos_Mot1(roue_drt_init + consigne_pos);
Brand101 0:444ac4067b19 2044 Asser_Pos_Mot2(roue_gch_init + (consigne_pos * ((2 * rayon) - LARGEUR_ROBOT)/((2 * rayon) + LARGEUR_ROBOT)));
Brand101 0:444ac4067b19 2045 } else if(sens < 0) {
Brand101 0:444ac4067b19 2046
Brand101 0:444ac4067b19 2047 Asser_Pos_Mot1(roue_drt_init + (consigne_pos * ((2 * rayon) - LARGEUR_ROBOT)/((2 * rayon) + LARGEUR_ROBOT)));
Brand101 0:444ac4067b19 2048 Asser_Pos_Mot2(roue_gch_init + consigne_pos);
Brand101 0:444ac4067b19 2049 }
Brand101 0:444ac4067b19 2050
Brand101 0:444ac4067b19 2051 //Envoi de la vitesse aux moteurs
Brand101 0:444ac4067b19 2052 write_PWM2(commande2);
Brand101 0:444ac4067b19 2053 write_PWM1(commande1);
Brand101 0:444ac4067b19 2054 }
Brand101 0:444ac4067b19 2055
Brand101 0:444ac4067b19 2056
Brand101 0:444ac4067b19 2057 void MY_ISR()
Brand101 0:444ac4067b19 2058 {
Brand101 0:444ac4067b19 2059 char val1[8];
Brand101 0:444ac4067b19 2060 if(ms_count == 20) {
Brand101 0:444ac4067b19 2061 //pc.printf("vit1=%f vit2=%f\n", test1, test2);
Brand101 0:444ac4067b19 2062 if(asser_actif) {
Brand101 0:444ac4067b19 2063 //Si on a re�u l'instruction de stop...
Brand101 0:444ac4067b19 2064 if(stop_receive) {
Brand101 0:444ac4067b19 2065 stop_receive = 0;
Brand101 0:444ac4067b19 2066 ErreurPos1 = 0;
Brand101 0:444ac4067b19 2067 ErreurPos2 = 0;
Brand101 0:444ac4067b19 2068 Somme_ErreurPos1 = 0;
Brand101 0:444ac4067b19 2069 Somme_ErreurPos2 = 0;
Brand101 0:444ac4067b19 2070 Delta_ErreurPos1 = 0;
Brand101 0:444ac4067b19 2071 Delta_ErreurPos2 = 0;
Brand101 0:444ac4067b19 2072 //On poursuit l'action si on est en rotation
Brand101 0:444ac4067b19 2073 if(liste[cpt_ordre].type == TYPE_DEPLACEMENT_ROTATION);
Brand101 0:444ac4067b19 2074 //On s'arr�te tout de suite si on est en ligne droite
Brand101 0:444ac4067b19 2075 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) {
Brand101 0:444ac4067b19 2076 //On vide le buffer de mouvements et on pr�voit de s'asservir sur la position atteinte
Brand101 0:444ac4067b19 2077 for(i = 0; i<nb_ordres; i++) liste[i] = (struct Ordre_deplacement) {
Brand101 0:444ac4067b19 2078 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
Brand101 0:444ac4067b19 2079 };
Brand101 0:444ac4067b19 2080 liste[0] = (struct Ordre_deplacement) {
Brand101 0:444ac4067b19 2081 TYPE_DEPLACEMENT_STOP,0,0,0,0,0,0,0,0,0,0,0,0,0,0
Brand101 0:444ac4067b19 2082 };
Brand101 0:444ac4067b19 2083 nb_ordres = 0;
Brand101 0:444ac4067b19 2084 etat_automate_depl = 0;
Brand101 0:444ac4067b19 2085 cpt_ordre = 0;
Brand101 0:444ac4067b19 2086 }
Brand101 0:444ac4067b19 2087 //Si on est en X_Y_Theta, on poursuit la rotation, ou on arr�te la ligne droite
Brand101 0:444ac4067b19 2088 else if(liste[cpt_ordre].type == TYPE_DEPLACEMENT_X_Y_THETA) {
Brand101 0:444ac4067b19 2089 if(etat_automate_xytheta == 2) {
Brand101 0:444ac4067b19 2090 //On vide le buffer de mouvements et on pr�voit de s'asservir sur la position atteinte
Brand101 0:444ac4067b19 2091 for(i = 0; i<nb_ordres; i++) liste[i] = (struct Ordre_deplacement) {
Brand101 0:444ac4067b19 2092 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
Brand101 0:444ac4067b19 2093 };
Brand101 0:444ac4067b19 2094 liste[0] = (struct Ordre_deplacement) {
Brand101 0:444ac4067b19 2095 TYPE_DEPLACEMENT_STOP,0,0,0,0,0,0,0,0,0,0,0,0,0,0
Brand101 0:444ac4067b19 2096 };
Brand101 0:444ac4067b19 2097 nb_ordres = 0;
Brand101 0:444ac4067b19 2098 cpt_ordre = 0;
Brand101 0:444ac4067b19 2099 }
Brand101 0:444ac4067b19 2100 }
Brand101 0:444ac4067b19 2101 }
Brand101 0:444ac4067b19 2102 //Si on a re�u un changement de vitesse...
Brand101 0:444ac4067b19 2103 else if(ralentare) {
Brand101 0:444ac4067b19 2104 Ralentir(VMAX);
Brand101 0:444ac4067b19 2105 for(i = 0; i<nb_ordres; i++) {
Brand101 0:444ac4067b19 2106 liste[i].vmax = VMAX;
Brand101 0:444ac4067b19 2107 liste[i].amax = AMAX;
Brand101 0:444ac4067b19 2108 }
Brand101 0:444ac4067b19 2109 }
Brand101 0:444ac4067b19 2110
Brand101 0:444ac4067b19 2111 else if(recalage_debut_receive) {
Brand101 0:444ac4067b19 2112 if(couleur_debut == 1) {
Brand101 0:444ac4067b19 2113
Brand101 0:444ac4067b19 2114 //#if GROS_ROB == 0
Brand101 0:444ac4067b19 2115 Odo_x = 1961;
Brand101 0:444ac4067b19 2116 Odo_y = 2800;
Brand101 0:444ac4067b19 2117 Odo_theta = 1800;
Brand101 0:444ac4067b19 2118 liste[0] = (struct Ordre_deplacement) {
Brand101 0:444ac4067b19 2119 TYPE_DEPLACEMENT_LIGNE_DROITE,0,30,500,(971 * RESOLUTION_ROUE_CODEUSE) / PERIMETRE_ROUE_CODEUSE,0,0,0,0,0,0,0,0,0,0
Brand101 0:444ac4067b19 2120 };
Brand101 0:444ac4067b19 2121 liste[1] = (struct Ordre_deplacement) {
Brand101 0:444ac4067b19 2122 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
Brand101 0:444ac4067b19 2123 };
Brand101 0:444ac4067b19 2124 liste[2] = (struct Ordre_deplacement) {
Brand101 0:444ac4067b19 2125 TYPE_DEPLACEMENT_RECALAGE,0,30,500,(-1000 * RESOLUTION_ROUE_CODEUSE) / PERIMETRE_ROUE_CODEUSE,2,2961,0,0,0,0,0,0,0,0
Brand101 0:444ac4067b19 2126 };
Brand101 0:444ac4067b19 2127 liste[3] = (struct Ordre_deplacement) {
Brand101 0:444ac4067b19 2128 TYPE_DEPLACEMENT_LIGNE_DROITE,0,30,500,(111 * RESOLUTION_ROUE_CODEUSE) / PERIMETRE_ROUE_CODEUSE,0,0,0,0,0,0,0,0,0,0
Brand101 0:444ac4067b19 2129 };
Brand101 0:444ac4067b19 2130 liste[4] = (struct Ordre_deplacement) {
Brand101 0:444ac4067b19 2131 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
Brand101 0:444ac4067b19 2132 };
Brand101 0:444ac4067b19 2133
Brand101 0:444ac4067b19 2134 recalage_debut_receive = 0;
Brand101 0:444ac4067b19 2135 } else if(couleur_debut == 0) {
Brand101 0:444ac4067b19 2136
Brand101 0:444ac4067b19 2137
Brand101 0:444ac4067b19 2138 //#if GROS_ROB == 0
Brand101 0:444ac4067b19 2139 Odo_x = 1961;
Brand101 0:444ac4067b19 2140 Odo_y = 200;
Brand101 0:444ac4067b19 2141 Odo_theta = 1800;
Brand101 0:444ac4067b19 2142 liste[0] = (struct Ordre_deplacement) {
Brand101 0:444ac4067b19 2143 TYPE_DEPLACEMENT_LIGNE_DROITE,0,30,500,(971 * RESOLUTION_ROUE_CODEUSE) / PERIMETRE_ROUE_CODEUSE,0,0,0,0,0,0,0,0,0,0
Brand101 0:444ac4067b19 2144 };
Brand101 0:444ac4067b19 2145 liste[1] = (struct Ordre_deplacement) {
Brand101 0:444ac4067b19 2146 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
Brand101 0:444ac4067b19 2147 };
Brand101 0:444ac4067b19 2148 liste[2] = (struct Ordre_deplacement) {
Brand101 0:444ac4067b19 2149 TYPE_DEPLACEMENT_RECALAGE,0,30,500,(-1000 * RESOLUTION_ROUE_CODEUSE) / PERIMETRE_ROUE_CODEUSE,2,39,0,0,0,0,0,0,0,0
Brand101 0:444ac4067b19 2150 };
Brand101 0:444ac4067b19 2151 liste[3] = (struct Ordre_deplacement) {
Brand101 0:444ac4067b19 2152 TYPE_DEPLACEMENT_LIGNE_DROITE,0,30,500,(111 * RESOLUTION_ROUE_CODEUSE) / PERIMETRE_ROUE_CODEUSE,0,0,0,0,0,0,0,0,0,0
Brand101 0:444ac4067b19 2153 };
Brand101 0:444ac4067b19 2154 liste[4] = (struct Ordre_deplacement) {
Brand101 0:444ac4067b19 2155 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
Brand101 0:444ac4067b19 2156 };
Brand101 0:444ac4067b19 2157
Brand101 0:444ac4067b19 2158 recalage_debut_receive = 0;
Brand101 0:444ac4067b19 2159 }
Brand101 0:444ac4067b19 2160 }
Brand101 0:444ac4067b19 2161
Brand101 0:444ac4067b19 2162 //Si on vient de finir un mouvement...
Brand101 0:444ac4067b19 2163 else if(next_move) {
Brand101 0:444ac4067b19 2164
Brand101 0:444ac4067b19 2165 ErreurPos1 = 0;
Brand101 0:444ac4067b19 2166 ErreurPos2 = 0;
Brand101 0:444ac4067b19 2167 Somme_ErreurPos1 = 0;
Brand101 0:444ac4067b19 2168 Somme_ErreurPos2 = 0;
Brand101 0:444ac4067b19 2169 Delta_ErreurPos1 = 0;
Brand101 0:444ac4067b19 2170 Delta_ErreurPos2 = 0;
Brand101 0:444ac4067b19 2171 //Si on devait s'arr�ter...
Brand101 0:444ac4067b19 2172 if(stop) {
Brand101 0:444ac4067b19 2173 //On vide le buffer de mouvements et on pr�voit de s'asservir sur la position atteinte
Brand101 0:444ac4067b19 2174 for(i = 0; i<nb_ordres; i++) liste[i] = (struct Ordre_deplacement) {
Brand101 0:444ac4067b19 2175 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
Brand101 0:444ac4067b19 2176 };
Brand101 0:444ac4067b19 2177 liste[0] = (struct Ordre_deplacement) {
Brand101 0:444ac4067b19 2178 TYPE_DEPLACEMENT_IMMOBILE,0,0,0,0,0,0,0,0,0,0,0,0,0,0
Brand101 0:444ac4067b19 2179 };
Brand101 0:444ac4067b19 2180 nb_ordres = 0;
Brand101 0:444ac4067b19 2181 cpt_ordre = 0;
Brand101 0:444ac4067b19 2182 stop = 0;
Brand101 0:444ac4067b19 2183 next_move = 0;
Brand101 0:444ac4067b19 2184 cpt = 0;
Brand101 0:444ac4067b19 2185
Brand101 0:444ac4067b19 2186 //On pr�vient qu'on s'est arr�t�
Brand101 0:444ac4067b19 2187 motor_of();
Brand101 0:444ac4067b19 2188
Brand101 0:444ac4067b19 2189 }
Brand101 0:444ac4067b19 2190 //Sinon...
Brand101 0:444ac4067b19 2191 else if(!stop) {
Brand101 0:444ac4067b19 2192 next_move = 0;
Brand101 0:444ac4067b19 2193 //On conserve la position du robot pour la prochaine action
Brand101 0:444ac4067b19 2194 roue_drt_init = encoder1.GetCounter();
Brand101 0:444ac4067b19 2195
Brand101 0:444ac4067b19 2196 //MOI*\*/ // bug Edouard
Brand101 0:444ac4067b19 2197 //#elif GROS_ROB == 0
Brand101 0:444ac4067b19 2198 roue_gch_init = COEF_ROUE_GAUCHE * (double)encoder2.GetCounter();
Brand101 0:444ac4067b19 2199 //#endif
Brand101 0:444ac4067b19 2200
Brand101 0:444ac4067b19 2201 cpt_ordre++;
Brand101 0:444ac4067b19 2202
Brand101 0:444ac4067b19 2203 if(recalage_debut == 1) {
Brand101 0:444ac4067b19 2204
Brand101 0:444ac4067b19 2205 if(cpt_ordre >= 5) {
Brand101 0:444ac4067b19 2206 recalage_debut = 2;
Brand101 0:444ac4067b19 2207 //On vide le buffer de mouvements et on pr�voit de s'asservir sur la position atteinte
Brand101 0:444ac4067b19 2208 for(i = 0; i<nb_ordres; i++) liste[i] = (struct Ordre_deplacement) {
Brand101 0:444ac4067b19 2209 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
Brand101 0:444ac4067b19 2210 };
Brand101 0:444ac4067b19 2211 liste[0] = (struct Ordre_deplacement) {
Brand101 0:444ac4067b19 2212 TYPE_DEPLACEMENT_IMMOBILE,0,0,0,0,0,0,0,0,0,0,0,0,0,0
Brand101 0:444ac4067b19 2213 };
Brand101 0:444ac4067b19 2214 nb_ordres = 0;
Brand101 0:444ac4067b19 2215 cpt_ordre = 0;
Brand101 0:444ac4067b19 2216 }
Brand101 0:444ac4067b19 2217 }
Brand101 0:444ac4067b19 2218 //Si on n'est pas dans un enchainement...
Brand101 0:444ac4067b19 2219 else if(liste[cpt_ordre].enchainement != 1) {
Brand101 0:444ac4067b19 2220 //On envoie le flag de fin d'instruction correspondant sur CAN
Brand101 0:444ac4067b19 2221 switch(liste[cpt_ordre].type) {
Brand101 0:444ac4067b19 2222 case TYPE_DEPLACEMENT_LIGNE_DROITE :
Brand101 0:444ac4067b19 2223 val1[0] = ASSERVISSEMENT_RECALAGE;
Brand101 0:444ac4067b19 2224 break;
Brand101 0:444ac4067b19 2225 case TYPE_DEPLACEMENT_ROTATION :
Brand101 0:444ac4067b19 2226 val1[0] = ASSERVISSEMENT_ROTATION;
Brand101 0:444ac4067b19 2227 break;
Brand101 0:444ac4067b19 2228 case TYPE_DEPLACEMENT_X_Y_THETA :
Brand101 0:444ac4067b19 2229 val1[0] = ASSERVISSEMENT_XYT;
Brand101 0:444ac4067b19 2230 break;
Brand101 0:444ac4067b19 2231 case TYPE_DEPLACEMENT_RAYON_COURBURE :
Brand101 0:444ac4067b19 2232 val1[0] = ASSERVISSEMENT_COURBURE;
Brand101 0:444ac4067b19 2233 break;
Brand101 0:444ac4067b19 2234 case TYPE_DEPLACEMENT_RECALAGE :
Brand101 0:444ac4067b19 2235 val1[0] = ASSERVISSEMENT_RECALAGE;
Brand101 0:444ac4067b19 2236 break;
Brand101 0:444ac4067b19 2237 default :
Brand101 0:444ac4067b19 2238 val1[0] = 0;
Brand101 0:444ac4067b19 2239 break;
Brand101 0:444ac4067b19 2240 }
Brand101 0:444ac4067b19 2241
Brand101 0:444ac4067b19 2242 //On vide le buffer de mouvements et on pr�voit de s'asservir sur la position atteinte
Brand101 0:444ac4067b19 2243 for(i = 0; i<nb_ordres; i++) liste[i] = (struct Ordre_deplacement) {
Brand101 0:444ac4067b19 2244 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
Brand101 0:444ac4067b19 2245 };
Brand101 0:444ac4067b19 2246 liste[0] = (struct Ordre_deplacement) {
Brand101 0:444ac4067b19 2247 TYPE_DEPLACEMENT_IMMOBILE,0,0,0,0,0,0,0,0,0,0,0,0,0,0
Brand101 0:444ac4067b19 2248 };
Brand101 0:444ac4067b19 2249 nb_ordres = 0;
Brand101 0:444ac4067b19 2250 cpt_ordre = 0;
Brand101 0:444ac4067b19 2251 cpt = 0;
Brand101 0:444ac4067b19 2252
Brand101 0:444ac4067b19 2253 //On pr�vient qu'on s'est arr�t�
Brand101 0:444ac4067b19 2254 motor_of();
Brand101 0:444ac4067b19 2255 }
Brand101 0:444ac4067b19 2256 }
Brand101 0:444ac4067b19 2257 } else { //On v�rifie le type d'action � effectuer, en fonction du buffer d'instructions
Brand101 0:444ac4067b19 2258 test3++;
Brand101 0:444ac4067b19 2259 switch(liste[cpt_ordre].type) {
Brand101 0:444ac4067b19 2260 case TYPE_DEPLACEMENT_STOP :
Brand101 0:444ac4067b19 2261 Arret();
Brand101 0:444ac4067b19 2262 break;
Brand101 0:444ac4067b19 2263
Brand101 0:444ac4067b19 2264 case TYPE_DEPLACEMENT_IMMOBILE:
Brand101 0:444ac4067b19 2265 test4++;
Brand101 0:444ac4067b19 2266 Asser_Pos_Mot1(roue_drt_init);
Brand101 0:444ac4067b19 2267 Asser_Pos_Mot2(roue_gch_init);
Brand101 0:444ac4067b19 2268 write_PWM2(commande2);
Brand101 0:444ac4067b19 2269 write_PWM1(commande1);
Brand101 0:444ac4067b19 2270 break;
Brand101 0:444ac4067b19 2271
Brand101 0:444ac4067b19 2272 case TYPE_DEPLACEMENT_LIGNE_DROITE:
Brand101 0:444ac4067b19 2273 Avancer_ligne_droite(liste[cpt_ordre].distance, VMAX, AMAX, DMAX);
Brand101 0:444ac4067b19 2274 break;
Brand101 0:444ac4067b19 2275
Brand101 0:444ac4067b19 2276 case TYPE_DEPLACEMENT_ROTATION:
Brand101 0:444ac4067b19 2277 Rotation(liste[cpt_ordre].angle, VMAX, AMAX, DMAX);
Brand101 0:444ac4067b19 2278 break;
Brand101 0:444ac4067b19 2279
Brand101 0:444ac4067b19 2280 case TYPE_DEPLACEMENT_X_Y_THETA:
Brand101 0:444ac4067b19 2281 X_Y_Theta(liste[cpt_ordre].x, liste[cpt_ordre].y, liste[cpt_ordre].theta,
Brand101 0:444ac4067b19 2282 liste[cpt_ordre].sens, VMAX, AMAX);
Brand101 0:444ac4067b19 2283 break;
Brand101 0:444ac4067b19 2284
Brand101 0:444ac4067b19 2285 case TYPE_DEPLACEMENT_RAYON_COURBURE :
Brand101 0:444ac4067b19 2286 Rayon_De_Courbure(liste[cpt_ordre].rayon, liste[cpt_ordre].theta_ray, VMAX,
Brand101 0:444ac4067b19 2287 AMAX, liste[cpt_ordre].sens, DMAX);
Brand101 0:444ac4067b19 2288 break;
Brand101 0:444ac4067b19 2289
Brand101 0:444ac4067b19 2290 case TYPE_DEPLACEMENT_RECALAGE :
Brand101 0:444ac4067b19 2291 Recalage(liste[cpt_ordre].distance, 10, 500, liste[cpt_ordre].recalage, liste[cpt_ordre].val_recalage);
Brand101 0:444ac4067b19 2292 break;
Brand101 0:444ac4067b19 2293
Brand101 0:444ac4067b19 2294 default :
Brand101 0:444ac4067b19 2295 Asser_Pos_Mot1(roue_drt_init);
Brand101 0:444ac4067b19 2296 Asser_Pos_Mot2(roue_gch_init);
Brand101 0:444ac4067b19 2297 write_PWM2(commande2);
Brand101 0:444ac4067b19 2298 write_PWM1(commande1);
Brand101 0:444ac4067b19 2299 test7++;
Brand101 0:444ac4067b19 2300 break;
Brand101 0:444ac4067b19 2301
Brand101 0:444ac4067b19 2302 }
Brand101 0:444ac4067b19 2303 }
Brand101 0:444ac4067b19 2304 //On calcule l'odom�trie � chaque tour de boucle
Brand101 0:444ac4067b19 2305
Brand101 0:444ac4067b19 2306 //MOI*\
Brand101 0:444ac4067b19 2307 ms_count = 0; // reset ms counter
Brand101 0:444ac4067b19 2308 ms_count1 ++;
Brand101 0:444ac4067b19 2309 //Odometrie();
Brand101 0:444ac4067b19 2310 if(ms_count1 == 5)
Brand101 0:444ac4067b19 2311 ms_count1 = 0;
Brand101 0:444ac4067b19 2312 } else {
Brand101 0:444ac4067b19 2313 ms_count = 0; // reset ms counter
Brand101 0:444ac4067b19 2314 ms_count1 ++;
Brand101 0:444ac4067b19 2315 //Odometrie();
Brand101 0:444ac4067b19 2316 if(ms_count1 == 5)
Brand101 0:444ac4067b19 2317 ms_count1 = 0;
Brand101 0:444ac4067b19 2318 }
Brand101 0:444ac4067b19 2319
Brand101 0:444ac4067b19 2320 } else {
Brand101 0:444ac4067b19 2321 ms_count ++;
Brand101 0:444ac4067b19 2322 } /*else if(ms_count % 10 == 0) {
Brand101 0:444ac4067b19 2323 Odometrie();
Brand101 0:444ac4067b19 2324
Brand101 0:444ac4067b19 2325 }
Brand101 0:444ac4067b19 2326 */
Brand101 0:444ac4067b19 2327 }