CRAC Team / Mbed 2 deprecated code_carte_IHM_avec_modifs

code avec modifs, programme mit dans les robots pour les derniers matchs

Dependencies:   mbed SerialHalfDuplex SDFileSystem liaison_Bluetooth ident_crac DISCO-F469NI_portrait

Strategie/Strategie.cpp@38:9d6a3ccc0582, 2021-07-17 (annotated)

Committer:
goldmas
Date:
Sat Jul 17 11:07:17 2021 +0000
Revision:
38:9d6a3ccc0582
Parent:
36:c37dbe2be916 
Dernier code de la coupe de fracne 2021

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gabrieltetar 1:7e925468f9d9 1 #include "global.h"
gabrieltetar 1:7e925468f9d9 2
gabrieltetar 1:7e925468f9d9 3
gabrieltetar 1:7e925468f9d9 4 #define M_PI 3.14159265358979323846
gabrieltetar 1:7e925468f9d9 5
gabrieltetar 21:d137ec53c3a9 6 E_stratGameEtat gameEtat = ETAT_CHECK_CARTES;
gabrieltetar 21:d137ec53c3a9 7 T_etat strat_etat_s = INIT;
gabrieltetar 1:7e925468f9d9 8
gabrieltetar 1:7e925468f9d9 9 int waitingAckID_FIN;
gabrieltetar 1:7e925468f9d9 10 int waitingAckFrom_FIN;
gabrieltetar 1:7e925468f9d9 11
gabrieltetar 1:7e925468f9d9 12 Ticker ticker;
gabrieltetar 1:7e925468f9d9 13
gabrieltetar 1:7e925468f9d9 14
gabrieltetar 1:7e925468f9d9 15 Ticker chrono;
gabrieltetar 1:7e925468f9d9 16 Timeout AffTime;
goldmas 38:9d6a3ccc0582 17 Ticker timer;
gabrieltetar 1:7e925468f9d9 18 Timer cartesCheker;//Le timer pour le timeout de la vérification des cartes
gabrieltetar 1:7e925468f9d9 19 Timer gameTimer;
gabrieltetar 1:7e925468f9d9 20 Timer debugetatTimer;
gabrieltetar 1:7e925468f9d9 21 Timer timeoutWarning;
gabrieltetar 1:7e925468f9d9 22 Timer timeoutWarningWaitEnd;
gabrieltetar 1:7e925468f9d9 23 Timeout chronoEnd;//permet d'envoyer la trame CAN pour la fin
gabrieltetar 1:7e925468f9d9 24
gabrieltetar 1:7e925468f9d9 25 unsigned char screenChecktry = 0;
gabrieltetar 1:7e925468f9d9 26 unsigned char test[32] = {32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32,32};
gabrieltetar 1:7e925468f9d9 27
goldmas 38:9d6a3ccc0582 28 signed char asser_stop_direction=0;
gabrieltetar 1:7e925468f9d9 29 char counter = 0;
gabrieltetar 1:7e925468f9d9 30 char check;
gabrieltetar 1:7e925468f9d9 31 char Jack = 1;
gabrieltetar 1:7e925468f9d9 32 short SCORE_GLOBAL=0;
gabrieltetar 1:7e925468f9d9 33 short SCORE_GR=0;
gabrieltetar 1:7e925468f9d9 34 short SCORE_PR=0;
gabrieltetar 14:6aa8aa1699ad 35 unsigned short distance_recalage;
gabrieltetar 14:6aa8aa1699ad 36 unsigned short distance_revenir;
gabrieltetar 1:7e925468f9d9 37
gabrieltetar 14:6aa8aa1699ad 38 unsigned short x;
gabrieltetar 14:6aa8aa1699ad 39 unsigned short y;
gabrieltetar 14:6aa8aa1699ad 40 unsigned char isStopEnable = 1;//Permet de savoir si il faut autoriser le stop via les balises
gabrieltetar 24:1a13c998c7ac 41 unsigned short flag_check_carte = 0, flag_strat = 0, flag_timer;
gabrieltetar 1:7e925468f9d9 42 int flagReceptionTelemetres = 0, flagNonRepriseErrorMot = 0;
gabrieltetar 21:d137ec53c3a9 43
goldmas 38:9d6a3ccc0582 44 int Flag_Bras_Re = 0, Flag_Manche_Bas = 0, Flag_Manche_Moy = 0, Flag_pavillon = 0, Flag_bon_port = 0; //Flag utilisé pour compter le score des bras / manches / pavillon
goldmas 38:9d6a3ccc0582 45 unsigned short Flag_num_bras;
gabrieltetar 21:d137ec53c3a9 46
gabrieltetar 1:7e925468f9d9 47 signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
gabrieltetar 1:7e925468f9d9 48
gabrieltetar 1:7e925468f9d9 49 signed short x_robot,y_robot,theta_robot;//La position du robot
gabrieltetar 1:7e925468f9d9 50 signed short target_x_robot, target_y_robot, target_theta_robot;
gabrieltetar 1:7e925468f9d9 51 E_InstructionType actionPrecedente;
gabrieltetar 1:7e925468f9d9 52 //unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
gabrieltetar 1:7e925468f9d9 53 int flagSendCan=1;
gabrieltetar 1:7e925468f9d9 54 unsigned char Cote = 0; //0 -> JAUNE | 1 -> VIOLET
goldmas 38:9d6a3ccc0582 55 unsigned char Hauteur = 0; //Robot du haut -> 1, du bas -> 0
gabrieltetar 1:7e925468f9d9 56 unsigned short angleRecalage = 0;
gabrieltetar 1:7e925468f9d9 57 unsigned char checkCurrent = 0;
gabrieltetar 1:7e925468f9d9 58 unsigned char countAliveCard = 0;
gabrieltetar 1:7e925468f9d9 59 unsigned char ligne=0;
gabrieltetar 1:7e925468f9d9 60 int Fevitement=0;
gabrieltetar 1:7e925468f9d9 61 int EvitEtat= 0;
gabrieltetar 1:7e925468f9d9 62 int stop_evitement=0;
gabrieltetar 19:e70b9d4a319c 63 signed char nbStrat = 0; //N° de la strategie (1-10)
gabrieltetar 1:7e925468f9d9 64 unsigned char ModeDemo = 0; // Si à 1, indique que l'on est dans le mode demo
gabrieltetar 1:7e925468f9d9 65 unsigned char countRobotNear = 0;//Le nombre de robot à proximité
gabrieltetar 1:7e925468f9d9 66 unsigned char ingnorBaliseOnce = 0;//une fois détecté réinitialise
gabrieltetar 1:7e925468f9d9 67 unsigned char ingnorBalise = 0;//0:balise ignore 1:on ecoute la balise
gabrieltetar 1:7e925468f9d9 68 short direction;
goldmas 38:9d6a3ccc0582 69 char val_girou ;
goldmas 38:9d6a3ccc0582 70
goldmas 38:9d6a3ccc0582 71 unsigned char debug_bon_port = 0;
gabrieltetar 1:7e925468f9d9 72
gabrieltetar 1:7e925468f9d9 73 unsigned char ingnorInversionOnce = 0;//Pour ignorer l'inversion des instruction une fois
gabrieltetar 1:7e925468f9d9 74
gabrieltetar 1:7e925468f9d9 75 struct S_Instruction instruction;
gabrieltetar 1:7e925468f9d9 76 struct S_Dodge_queue dodgeq;
gabrieltetar 1:7e925468f9d9 77
gabrieltetar 1:7e925468f9d9 78 char couleur1, couleur2, couleur3;
gabrieltetar 1:7e925468f9d9 79 float cptf;
gabrieltetar 1:7e925468f9d9 80 int cpt,cpt1;
gabrieltetar 1:7e925468f9d9 81
gabrieltetar 21:d137ec53c3a9 82
gabrieltetar 21:d137ec53c3a9 83
gabrieltetar 1:7e925468f9d9 84 E_stratGameEtat memGameEtat= gameEtat;
gabrieltetar 1:7e925468f9d9 85 E_stratGameEtat lastEtat = ETAT_CHECK_CARTES;
gabrieltetar 1:7e925468f9d9 86 E_Stratposdebut etat_pos=RECALAGE_1;
gabrieltetar 1:7e925468f9d9 87
gabrieltetar 21:d137ec53c3a9 88 void SendRawId (unsigned short id);
gabrieltetar 21:d137ec53c3a9 89 void can2Rx_ISR(void);
gabrieltetar 7:44eec996a76e 90
gabrieltetar 1:7e925468f9d9 91 signed char blocage_balise;
gabrieltetar 1:7e925468f9d9 92 void print_segment(int nombre, int decalage);
gabrieltetar 1:7e925468f9d9 93 void affichage_compteur (int nombre);
gabrieltetar 1:7e925468f9d9 94 void effacer_segment(long couleur);
gabrieltetar 14:6aa8aa1699ad 95 unsigned char doAction(unsigned char id, unsigned short arg1, short arg2);
gabrieltetar 1:7e925468f9d9 96 unsigned short telemetreDistance=0;
gabrieltetar 1:7e925468f9d9 97 unsigned short telemetreDistance_avant_gauche=0;
gabrieltetar 1:7e925468f9d9 98 unsigned short telemetreDistance_avant_droite=0;
gabrieltetar 1:7e925468f9d9 99 unsigned short telemetreDistance_arriere_gauche=0;
gabrieltetar 1:7e925468f9d9 100 unsigned short telemetreDistance_arriere_droite=0;
gabrieltetar 1:7e925468f9d9 101
gabrieltetar 1:7e925468f9d9 102 unsigned char DT_AVD_interrupt=0;
gabrieltetar 1:7e925468f9d9 103 unsigned char DT_AVG_interrupt=0;
gabrieltetar 1:7e925468f9d9 104 unsigned char DT_ARD_interrupt=0;
gabrieltetar 1:7e925468f9d9 105 unsigned char DT_ARG_interrupt=0;
gabrieltetar 1:7e925468f9d9 106
gabrieltetar 1:7e925468f9d9 107
gabrieltetar 1:7e925468f9d9 108
gabrieltetar 1:7e925468f9d9 109 unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR,CHECK_BALISE};
gabrieltetar 1:7e925468f9d9 110 unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR,ALIVE_BALISE};
goldmas 36:c37dbe2be916 111 InterruptIn jackB1(PG_11); // entrée numerique en interruption pour le jack (JackB1 sur la carte esclave)
goldmas 36:c37dbe2be916 112 InterruptIn jackA1(PA_6);
gabrieltetar 1:7e925468f9d9 113
gabrieltetar 1:7e925468f9d9 114
gabrieltetar 1:7e925468f9d9 115
gabrieltetar 1:7e925468f9d9 116
gabrieltetar 1:7e925468f9d9 117
gabrieltetar 1:7e925468f9d9 118 /****************************************************************************************/
gabrieltetar 1:7e925468f9d9 119 /* FUNCTION NAME: chronometre_ISR */
gabrieltetar 1:7e925468f9d9 120 /* DESCRIPTION : Interruption à la fin des 90s du match */
gabrieltetar 1:7e925468f9d9 121 /****************************************************************************************/
gabrieltetar 1:7e925468f9d9 122 void chronometre_ISR (void)
gabrieltetar 1:7e925468f9d9 123 {
gabrieltetar 1:7e925468f9d9 124 SendRawId(ASSERVISSEMENT_STOP);//On stope les moteurs
gabrieltetar 1:7e925468f9d9 125 SendRawId(GLOBAL_GAME_END);//Indication fin de match
gabrieltetar 21:d137ec53c3a9 126 strat_etat_s=FIN;
gabrieltetar 1:7e925468f9d9 127 gameTimer.stop();//Arret du timer
gabrieltetar 1:7e925468f9d9 128
gabrieltetar 1:7e925468f9d9 129 while(1);//On bloque la programme dans l'interruption
gabrieltetar 1:7e925468f9d9 130 }
gabrieltetar 1:7e925468f9d9 131
gabrieltetar 1:7e925468f9d9 132
gabrieltetar 1:7e925468f9d9 133
gabrieltetar 1:7e925468f9d9 134 /****************************************************************************************/
gabrieltetar 1:7e925468f9d9 135 /* FUNCTION NAME: jack_ISR */
gabrieltetar 1:7e925468f9d9 136 /* DESCRIPTION : Interruption en changement d'état sur le Jack */
gabrieltetar 1:7e925468f9d9 137 /****************************************************************************************/
gabrieltetar 1:7e925468f9d9 138 void jack_ISR (void)
gabrieltetar 1:7e925468f9d9 139 {
gabrieltetar 1:7e925468f9d9 140 if(gameEtat == ETAT_GAME_WAIT_FOR_JACK) {
gabrieltetar 1:7e925468f9d9 141 gameEtat = ETAT_GAME_START;//On débute le match
gabrieltetar 21:d137ec53c3a9 142 //strat_etat_s=COMPTEUR;
gabrieltetar 1:7e925468f9d9 143 blocage_balise=1;
gabrieltetar 1:7e925468f9d9 144 }
gabrieltetar 1:7e925468f9d9 145 }
gabrieltetar 1:7e925468f9d9 146
goldmas 38:9d6a3ccc0582 147 void wait_ISR(void)
goldmas 38:9d6a3ccc0582 148 {
goldmas 38:9d6a3ccc0582 149 actual_instruction = instruction.nextLineOK;
goldmas 38:9d6a3ccc0582 150 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
goldmas 38:9d6a3ccc0582 151 timer.detach();
goldmas 38:9d6a3ccc0582 152 }
gabrieltetar 1:7e925468f9d9 153
goldmas 38:9d6a3ccc0582 154 void girouette_ISR(void)
goldmas 38:9d6a3ccc0582 155 {
goldmas 38:9d6a3ccc0582 156 actual_instruction = instruction.nextLineOK;
goldmas 38:9d6a3ccc0582 157 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
goldmas 38:9d6a3ccc0582 158 }
goldmas 38:9d6a3ccc0582 159
goldmas 38:9d6a3ccc0582 160 void isr_end_danger(void)
goldmas 38:9d6a3ccc0582 161 {
goldmas 38:9d6a3ccc0582 162 balise_end_danger(&instruction,&dodgeq,target_x_robot,target_y_robot,target_theta_robot, theta_robot,x_robot,y_robot);
goldmas 38:9d6a3ccc0582 163 }
gabrieltetar 1:7e925468f9d9 164
gabrieltetar 1:7e925468f9d9 165 /****************************************************************************************/
gabrieltetar 19:e70b9d4a319c 166 /* FUNCTION NAME: Strategie */
gabrieltetar 1:7e925468f9d9 167 /* DESCRIPTION : Automate de gestion de la stratégie du robot */
gabrieltetar 1:7e925468f9d9 168 /****************************************************************************************/
gabrieltetar 19:e70b9d4a319c 169 void Strategie(void)
gabrieltetar 1:7e925468f9d9 170 {
gabrieltetar 1:7e925468f9d9 171 static unsigned char AX12_enchainement = 0;
gabrieltetar 1:7e925468f9d9 172 static unsigned char MV_enchainement = 0;
gabrieltetar 1:7e925468f9d9 173 signed char localData1 = 0;
gabrieltetar 1:7e925468f9d9 174 signed short localData2 = 0;
gabrieltetar 1:7e925468f9d9 175 unsigned short localData3 = 0;
goldmas 38:9d6a3ccc0582 176 unsigned short localData4 = 0;
gabrieltetar 1:7e925468f9d9 177 unsigned char localData5 = 0;
goldmas 38:9d6a3ccc0582 178 int unique = 0 ;
gabrieltetar 1:7e925468f9d9 179
goldmas 38:9d6a3ccc0582 180 if(gameTimer.read_ms() >= 95000)
goldmas 38:9d6a3ccc0582 181 {
goldmas 38:9d6a3ccc0582 182 int unique = 0 ;
goldmas 38:9d6a3ccc0582 183 if (unique == 0)
goldmas 38:9d6a3ccc0582 184 {
goldmas 38:9d6a3ccc0582 185 SendRawId(PAVILLON_DEPLOYE) ;
goldmas 38:9d6a3ccc0582 186 Flag_pavillon=1;
goldmas 38:9d6a3ccc0582 187 unique = 1;
goldmas 38:9d6a3ccc0582 188 }
goldmas 38:9d6a3ccc0582 189 }
goldmas 38:9d6a3ccc0582 190
gabrieltetar 1:7e925468f9d9 191 if(gameTimer.read_ms() >= 99000) {//Fin du match (On autorise 2s pour déposer des éléments
gabrieltetar 1:7e925468f9d9 192 gameTimer.stop();
gabrieltetar 1:7e925468f9d9 193 gameTimer.reset();
gabrieltetar 1:7e925468f9d9 194 gameEtat = ETAT_END;//Fin du temps
gabrieltetar 21:d137ec53c3a9 195 strat_etat_s=FIN;
gabrieltetar 1:7e925468f9d9 196 }
gabrieltetar 1:7e925468f9d9 197
gabrieltetar 1:7e925468f9d9 198 if(lastEtat != gameEtat || debugetatTimer.read_ms() >= 1000) {
gabrieltetar 1:7e925468f9d9 199 lastEtat = gameEtat;
gabrieltetar 1:7e925468f9d9 200 debugetatTimer.reset();
gabrieltetar 1:7e925468f9d9 201 sendStratEtat((unsigned char)gameEtat, (unsigned char)actual_instruction);
gabrieltetar 1:7e925468f9d9 202 }
gabrieltetar 1:7e925468f9d9 203
gabrieltetar 1:7e925468f9d9 204
gabrieltetar 1:7e925468f9d9 205
gabrieltetar 1:7e925468f9d9 206 switch(gameEtat) {
gabrieltetar 1:7e925468f9d9 207
gabrieltetar 1:7e925468f9d9 208 case ETAT_CHECK_CARTES:
gabrieltetar 1:7e925468f9d9 209 /*
gabrieltetar 1:7e925468f9d9 210 Il faut faire une boucle pour verifier toutes les cartes les une apres les autres
gabrieltetar 1:7e925468f9d9 211 */
gabrieltetar 1:7e925468f9d9 212 waitingAckFrom = id_alive[checkCurrent];//On indique que l'on attend un ack de la carte IHM
gabrieltetar 1:7e925468f9d9 213 SendRawId(id_check[checkCurrent]);//On demande à la carte d'indiquer ça présence
gabrieltetar 1:7e925468f9d9 214
gabrieltetar 1:7e925468f9d9 215 screenChecktry++;//On incrèment le conteur de tentative de 1
gabrieltetar 1:7e925468f9d9 216 cartesCheker.reset();//On reset le timeOut
gabrieltetar 1:7e925468f9d9 217 cartesCheker.start();//On lance le timer pour le timeout
gabrieltetar 1:7e925468f9d9 218 gameEtat = ETAT_CHECK_CARTES_WAIT_ACK;
gabrieltetar 1:7e925468f9d9 219 break;
gabrieltetar 1:7e925468f9d9 220
gabrieltetar 1:7e925468f9d9 221 case ETAT_CHECK_CARTES_WAIT_ACK:
gabrieltetar 1:7e925468f9d9 222 /*
gabrieltetar 1:7e925468f9d9 223 On attend l'ack de la carte en cours de vérification
gabrieltetar 1:7e925468f9d9 224 */
gabrieltetar 1:7e925468f9d9 225 //printf("cartesCheker = %d waitingAckFrom = %d\n",cartesCheker.read_ms(), waitingAckFrom);
gabrieltetar 1:7e925468f9d9 226 if(waitingAckFrom == 0) {//C'est bon la carte est en ligne
gabrieltetar 1:7e925468f9d9 227 cartesCheker.stop();
gabrieltetar 1:7e925468f9d9 228 screenChecktry = 0;
gabrieltetar 1:7e925468f9d9 229 countAliveCard++;
gabrieltetar 1:7e925468f9d9 230 checkCurrent++;
gabrieltetar 1:7e925468f9d9 231 if(checkCurrent >= NOMBRE_CARTES) {
gabrieltetar 1:7e925468f9d9 232 printf("all card check, missing %d cards\n",(NOMBRE_CARTES-countAliveCard));
gabrieltetar 1:7e925468f9d9 233 if(countAliveCard >= NOMBRE_CARTES) {
gabrieltetar 1:7e925468f9d9 234 gameEtat = ETAT_CONFIG;
gabrieltetar 1:7e925468f9d9 235 SendRawId(ECRAN_ALL_CHECK);
gabrieltetar 24:1a13c998c7ac 236 flag_check_carte=1;
gabrieltetar 1:7e925468f9d9 237
gabrieltetar 1:7e925468f9d9 238 //tactile_printf("Selection couleur et strategie");
gabrieltetar 1:7e925468f9d9 239 } else {
gabrieltetar 1:7e925468f9d9 240 gameEtat = ETAT_WAIT_FORCE;//Passage en attente de forçage du lancement
gabrieltetar 1:7e925468f9d9 241 waitingAckFrom = ECRAN_ALL_CHECK;
gabrieltetar 1:7e925468f9d9 242 }
gabrieltetar 1:7e925468f9d9 243 } else
gabrieltetar 1:7e925468f9d9 244 gameEtat = ETAT_CHECK_CARTES;
gabrieltetar 1:7e925468f9d9 245 } else if(cartesCheker.read_ms () > 100) {
gabrieltetar 1:7e925468f9d9 246 cartesCheker.stop();
gabrieltetar 1:7e925468f9d9 247 if(screenChecktry >=3) {
gabrieltetar 1:7e925468f9d9 248 //printf("missing card %d\n",id_check[checkCurrent]);
gabrieltetar 1:7e925468f9d9 249 screenChecktry = 0;
gabrieltetar 1:7e925468f9d9 250 checkCurrent++;
gabrieltetar 1:7e925468f9d9 251
gabrieltetar 1:7e925468f9d9 252 if(checkCurrent >= NOMBRE_CARTES) {
gabrieltetar 1:7e925468f9d9 253 if(countAliveCard == NOMBRE_CARTES) {
gabrieltetar 1:7e925468f9d9 254 gameEtat = ETAT_CONFIG;
gabrieltetar 24:1a13c998c7ac 255 flag_check_carte=1;
gabrieltetar 1:7e925468f9d9 256 } else {
gabrieltetar 1:7e925468f9d9 257 gameEtat = ETAT_WAIT_FORCE;
gabrieltetar 1:7e925468f9d9 258 waitingAckFrom = ECRAN_ALL_CHECK;
gabrieltetar 1:7e925468f9d9 259 }
gabrieltetar 1:7e925468f9d9 260 } else
gabrieltetar 1:7e925468f9d9 261 gameEtat = ETAT_CHECK_CARTES;
gabrieltetar 1:7e925468f9d9 262
gabrieltetar 1:7e925468f9d9 263 } else
gabrieltetar 1:7e925468f9d9 264 gameEtat = ETAT_CHECK_CARTES;
gabrieltetar 1:7e925468f9d9 265
gabrieltetar 1:7e925468f9d9 266 }
gabrieltetar 1:7e925468f9d9 267 break;
gabrieltetar 1:7e925468f9d9 268 case ETAT_WAIT_FORCE:
gabrieltetar 1:7e925468f9d9 269 /*
gabrieltetar 1:7e925468f9d9 270 Attente du forçage de la part de la carte IHM
gabrieltetar 1:7e925468f9d9 271 */
gabrieltetar 1:7e925468f9d9 272 if(waitingAckFrom == 0) {
gabrieltetar 1:7e925468f9d9 273 gameEtat = ETAT_CONFIG;
gabrieltetar 1:7e925468f9d9 274 }
gabrieltetar 1:7e925468f9d9 275 break;
gabrieltetar 1:7e925468f9d9 276 case ETAT_CONFIG:
gabrieltetar 1:7e925468f9d9 277 /*
gabrieltetar 1:7e925468f9d9 278 Attente de l'odre de choix de mode,
gabrieltetar 1:7e925468f9d9 279 Il est possible de modifier la couleur et l'id de la stratégie
gabrieltetar 1:7e925468f9d9 280 Il est aussi possible d'envoyer les ordres de debug
gabrieltetar 1:7e925468f9d9 281 */
gabrieltetar 1:7e925468f9d9 282 modeTelemetre = 0;
gabrieltetar 1:7e925468f9d9 283 break;
gabrieltetar 1:7e925468f9d9 284 case ETAT_GAME_INIT:
gabrieltetar 1:7e925468f9d9 285 //On charge la liste des instructions
gabrieltetar 1:7e925468f9d9 286
gabrieltetar 19:e70b9d4a319c 287 loadAllInstruction(nbStrat);//Mise en cache de toute les instructions
gabrieltetar 1:7e925468f9d9 288 led3=1;
gabrieltetar 1:7e925468f9d9 289
gabrieltetar 1:7e925468f9d9 290 SendRawId(GLOBAL_START);
gabrieltetar 1:7e925468f9d9 291
gabrieltetar 1:7e925468f9d9 292 gameEtat = ETAT_GAME_WAIT_FOR_JACK;
gabrieltetar 10:1964bb91b925 293 Debug_Audio(3,7);
gabrieltetar 21:d137ec53c3a9 294 if (strat_etat_s == TEST_MOTEUR|| strat_etat_s ==TEST_VENTOUSE || strat_etat_s == TEST_COULEUR || strat_etat_s ==TEST_SERVO_BRAS) {
gabrieltetar 1:7e925468f9d9 295 SendRawId(DEBUG_FAKE_JAKE);
gabrieltetar 1:7e925468f9d9 296 } else {
gabrieltetar 21:d137ec53c3a9 297 strat_etat_s = AFF_WAIT_JACK;
gabrieltetar 1:7e925468f9d9 298 }
gabrieltetar 1:7e925468f9d9 299 //tactile_printf("Attente du JACK.");
gabrieltetar 1:7e925468f9d9 300 setAsservissementEtat(1);//On réactive l'asservissement
goldmas 36:c37dbe2be916 301 jackB1.mode(PullDown); // désactivation de la résistance interne du jack
goldmas 36:c37dbe2be916 302 jackB1.fall(&jack_ISR); // création de l'interrupt attachée au changement d'état (front descendant) sur le jack
goldmas 36:c37dbe2be916 303 jackA1.mode(PullDown); // désactivation de la résistance interne du jack
goldmas 36:c37dbe2be916 304 jackA1.fall(&jack_ISR); // création de l'interrupt attachée au changement d'état (front descendant) sur le jack
gabrieltetar 1:7e925468f9d9 305
gabrieltetar 1:7e925468f9d9 306 localData2 = POSITION_DEBUT_T;
gabrieltetar 1:7e925468f9d9 307 localData3 = POSITION_DEBUT_Y;
gabrieltetar 1:7e925468f9d9 308 if(InversStrat == 1) {
gabrieltetar 1:7e925468f9d9 309 localData2 = -localData2;//Inversion theta
gabrieltetar 1:7e925468f9d9 310 localData3 = 3000 - POSITION_DEBUT_Y;//Inversion du Y
gabrieltetar 1:7e925468f9d9 311 }
gabrieltetar 1:7e925468f9d9 312 SetOdometrie(ODOMETRIE_SMALL_POSITION, POSITION_DEBUT_X,1800,localData2);
gabrieltetar 1:7e925468f9d9 313
gabrieltetar 1:7e925468f9d9 314 instruction = strat_instructions[actual_instruction];
gabrieltetar 1:7e925468f9d9 315 //On effectue le traitement de l'instruction
gabrieltetar 1:7e925468f9d9 316
gabrieltetar 1:7e925468f9d9 317 break;
gabrieltetar 1:7e925468f9d9 318 case ETAT_GAME_WAIT_FOR_JACK:
gabrieltetar 1:7e925468f9d9 319 if(instruction.order==POSITION_DEBUT) {
gabrieltetar 1:7e925468f9d9 320 switch(etat_pos) { // AUTOMATE PERMETTANT AU ROBOT DE SE POSITIONNER TOUT SEUL AU DEBUT DE LA PARTIE (Ne PAS RETIRER LE JACK PENDANT CE TEMPS !!!)
gabrieltetar 1:7e925468f9d9 321 case RECALAGE_1:
gabrieltetar 1:7e925468f9d9 322 SendRawId(RECALAGE_START);
gabrieltetar 1:7e925468f9d9 323 waitingAckID = ASSERVISSEMENT_RECALAGE;
gabrieltetar 1:7e925468f9d9 324 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
goldmas 36:c37dbe2be916 325 if(Hauteur==0)
goldmas 36:c37dbe2be916 326 localData3=2000-(MOITIEE_ROBOT);
goldmas 36:c37dbe2be916 327 else
goldmas 36:c37dbe2be916 328 localData3=MOITIEE_ROBOT;
goldmas 36:c37dbe2be916 329 GoStraight(1000, 1, localData3, 0); //on se recale contre le mur donc il faut donner la valeur du centre du robot (les -5 qui trainent sont dus au tables pourraves sur place)
gabrieltetar 1:7e925468f9d9 330 while(waitingAckID !=0 && waitingAckFrom !=0)
gabrieltetar 1:7e925468f9d9 331 canProcessRx();
gabrieltetar 1:7e925468f9d9 332 waitingAckID_FIN=ASSERVISSEMENT_RECALAGE;
gabrieltetar 1:7e925468f9d9 333 waitingAckFrom_FIN= INSTRUCTION_END_MOTEUR;
gabrieltetar 1:7e925468f9d9 334 while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
gabrieltetar 1:7e925468f9d9 335 canProcessRx();
gabrieltetar 1:7e925468f9d9 336 etat_pos=RECULER_1;
gabrieltetar 1:7e925468f9d9 337 break;
gabrieltetar 1:7e925468f9d9 338
gabrieltetar 1:7e925468f9d9 339 case RECULER_1:
gabrieltetar 1:7e925468f9d9 340 waitingAckID = ASSERVISSEMENT_RECALAGE;
gabrieltetar 1:7e925468f9d9 341 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
gabrieltetar 1:7e925468f9d9 342 GoStraight(-100, 0, 0, 0);//-450
gabrieltetar 1:7e925468f9d9 343 while(waitingAckID !=0 && waitingAckFrom !=0)
gabrieltetar 1:7e925468f9d9 344 canProcessRx();
gabrieltetar 1:7e925468f9d9 345 waitingAckID_FIN=ASSERVISSEMENT_RECALAGE;
gabrieltetar 1:7e925468f9d9 346 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
gabrieltetar 1:7e925468f9d9 347 while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
gabrieltetar 1:7e925468f9d9 348 canProcessRx();
gabrieltetar 1:7e925468f9d9 349 etat_pos=TOURNER;
gabrieltetar 1:7e925468f9d9 350 break;
gabrieltetar 1:7e925468f9d9 351
gabrieltetar 1:7e925468f9d9 352 case TOURNER:
gabrieltetar 1:7e925468f9d9 353 waitingAckID = ASSERVISSEMENT_ROTATION;
gabrieltetar 1:7e925468f9d9 354 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
goldmas 36:c37dbe2be916 355 if(Cote==0 && Hauteur==1) localData2 = 900;
goldmas 36:c37dbe2be916 356 else if(Cote == 0 && Hauteur==0 )localData2=-900;
goldmas 36:c37dbe2be916 357 else if(Cote==1 && Hauteur==1 )localData2=-900;
goldmas 36:c37dbe2be916 358 else if(Cote==1 && Hauteur==0 )localData2=900;
goldmas 36:c37dbe2be916 359
gabrieltetar 1:7e925468f9d9 360 Rotate(localData2);
gabrieltetar 1:7e925468f9d9 361 while(waitingAckID !=0 && waitingAckFrom !=0)
gabrieltetar 1:7e925468f9d9 362 canProcessRx();
gabrieltetar 1:7e925468f9d9 363 waitingAckID_FIN=ASSERVISSEMENT_ROTATION;
gabrieltetar 1:7e925468f9d9 364 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
gabrieltetar 1:7e925468f9d9 365 while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
gabrieltetar 1:7e925468f9d9 366 canProcessRx();
gabrieltetar 1:7e925468f9d9 367 etat_pos=RECALAGE_2;
gabrieltetar 1:7e925468f9d9 368 break;
gabrieltetar 1:7e925468f9d9 369
gabrieltetar 1:7e925468f9d9 370 case RECALAGE_2:
gabrieltetar 1:7e925468f9d9 371 waitingAckID = ASSERVISSEMENT_RECALAGE;
gabrieltetar 1:7e925468f9d9 372 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
gabrieltetar 1:7e925468f9d9 373 if(Cote==1) {
gabrieltetar 1:7e925468f9d9 374 localData3=3000-(MOITIEE_ROBOT);
gabrieltetar 1:7e925468f9d9 375 } else {
gabrieltetar 1:7e925468f9d9 376 localData3=MOITIEE_ROBOT;
gabrieltetar 1:7e925468f9d9 377 }
goldmas 36:c37dbe2be916 378 GoStraight(1000, 2,localData3, 0); //on se recale contre le mur donc il faut donner la valeur du centre du robot
gabrieltetar 1:7e925468f9d9 379 while(waitingAckID !=0 && waitingAckFrom !=0)
gabrieltetar 1:7e925468f9d9 380 canProcessRx();
gabrieltetar 1:7e925468f9d9 381 waitingAckID_FIN=ASSERVISSEMENT_RECALAGE;
gabrieltetar 1:7e925468f9d9 382 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
gabrieltetar 1:7e925468f9d9 383 while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
gabrieltetar 1:7e925468f9d9 384 canProcessRx();
gabrieltetar 1:7e925468f9d9 385 etat_pos=RECULER_2;
gabrieltetar 1:7e925468f9d9 386 break;
gabrieltetar 1:7e925468f9d9 387
gabrieltetar 1:7e925468f9d9 388 case RECULER_2:
gabrieltetar 1:7e925468f9d9 389 waitingAckID = ASSERVISSEMENT_RECALAGE;
gabrieltetar 1:7e925468f9d9 390 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
gabrieltetar 1:7e925468f9d9 391 GoStraight(-100, 0, 0, 0);
gabrieltetar 1:7e925468f9d9 392 while(waitingAckID !=0 && waitingAckFrom !=0)
gabrieltetar 1:7e925468f9d9 393 canProcessRx();
gabrieltetar 1:7e925468f9d9 394 waitingAckID_FIN=ASSERVISSEMENT_RECALAGE;
gabrieltetar 1:7e925468f9d9 395 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
gabrieltetar 1:7e925468f9d9 396 while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
gabrieltetar 1:7e925468f9d9 397 canProcessRx();
gabrieltetar 1:7e925468f9d9 398 etat_pos=GOTOPOS;
gabrieltetar 1:7e925468f9d9 399 break;
gabrieltetar 1:7e925468f9d9 400
gabrieltetar 1:7e925468f9d9 401 case GOTOPOS:
goldmas 38:9d6a3ccc0582 402 if(Hauteur == 0)
goldmas 38:9d6a3ccc0582 403 {
goldmas 38:9d6a3ccc0582 404 localData1 = -1;
goldmas 38:9d6a3ccc0582 405
goldmas 38:9d6a3ccc0582 406 if(InversStrat == 1 && ingnorInversionOnce == 0) {
goldmas 38:9d6a3ccc0582 407 localData2 = -instruction.arg3;
goldmas 38:9d6a3ccc0582 408 localData3 = 3000 - instruction.arg2;//Inversion du Y
goldmas 38:9d6a3ccc0582 409 } else {
goldmas 38:9d6a3ccc0582 410 localData3 = instruction.arg2;
goldmas 38:9d6a3ccc0582 411 localData2 = instruction.arg3;
goldmas 38:9d6a3ccc0582 412 }
goldmas 38:9d6a3ccc0582 413
goldmas 38:9d6a3ccc0582 414 GoToPosition(instruction.arg1,localData3,localData2,localData1);
goldmas 38:9d6a3ccc0582 415 waitingAckID = ASSERVISSEMENT_XYT;
goldmas 38:9d6a3ccc0582 416 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
goldmas 38:9d6a3ccc0582 417
goldmas 38:9d6a3ccc0582 418 while(waitingAckID !=0 && waitingAckFrom !=0)
goldmas 38:9d6a3ccc0582 419 canProcessRx();
goldmas 38:9d6a3ccc0582 420 waitingAckID_FIN=ASSERVISSEMENT_XYT;
goldmas 38:9d6a3ccc0582 421 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
goldmas 38:9d6a3ccc0582 422 while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
goldmas 38:9d6a3ccc0582 423 canProcessRx();
goldmas 38:9d6a3ccc0582 424 etat_pos=FIN_POS;
gabrieltetar 1:7e925468f9d9 425 }
goldmas 38:9d6a3ccc0582 426 else if(Hauteur == 1)
goldmas 38:9d6a3ccc0582 427 {
goldmas 38:9d6a3ccc0582 428 localData1 = 1;
goldmas 38:9d6a3ccc0582 429
goldmas 38:9d6a3ccc0582 430 if(InversStrat == 1 && ingnorInversionOnce == 0) {
goldmas 38:9d6a3ccc0582 431 localData2 = -instruction.arg3;
goldmas 38:9d6a3ccc0582 432 localData3 = 3000 - 600;//Inversion du Y
goldmas 38:9d6a3ccc0582 433 } else {
goldmas 38:9d6a3ccc0582 434 localData3 = 600;
goldmas 38:9d6a3ccc0582 435 localData2 = instruction.arg3;
goldmas 38:9d6a3ccc0582 436 }
goldmas 38:9d6a3ccc0582 437
goldmas 38:9d6a3ccc0582 438 GoToPosition(instruction.arg1,localData3,localData2,localData1);
goldmas 38:9d6a3ccc0582 439 waitingAckID = ASSERVISSEMENT_XYT;
goldmas 38:9d6a3ccc0582 440 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
goldmas 38:9d6a3ccc0582 441
goldmas 38:9d6a3ccc0582 442 while(waitingAckID !=0 && waitingAckFrom !=0)
goldmas 38:9d6a3ccc0582 443 canProcessRx();
goldmas 38:9d6a3ccc0582 444 waitingAckID_FIN=ASSERVISSEMENT_XYT;
goldmas 38:9d6a3ccc0582 445 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
goldmas 38:9d6a3ccc0582 446 while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
goldmas 38:9d6a3ccc0582 447 canProcessRx();
goldmas 38:9d6a3ccc0582 448
goldmas 38:9d6a3ccc0582 449 localData1 = -1;
goldmas 38:9d6a3ccc0582 450
goldmas 38:9d6a3ccc0582 451 if(InversStrat == 1 && ingnorInversionOnce == 0) {
goldmas 38:9d6a3ccc0582 452 localData2 = -instruction.arg3;
goldmas 38:9d6a3ccc0582 453 localData3 = 3000 - instruction.arg2;//Inversion du Y
goldmas 38:9d6a3ccc0582 454 } else {
goldmas 38:9d6a3ccc0582 455 localData3 = instruction.arg2;
goldmas 38:9d6a3ccc0582 456 localData2 = instruction.arg3;
goldmas 38:9d6a3ccc0582 457 }
goldmas 38:9d6a3ccc0582 458
goldmas 38:9d6a3ccc0582 459 GoToPosition(instruction.arg1,localData3,localData2,localData1);
goldmas 38:9d6a3ccc0582 460 waitingAckID = ASSERVISSEMENT_XYT;
goldmas 38:9d6a3ccc0582 461 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
goldmas 38:9d6a3ccc0582 462
goldmas 38:9d6a3ccc0582 463 while(waitingAckID !=0 && waitingAckFrom !=0)
goldmas 38:9d6a3ccc0582 464 canProcessRx();
goldmas 38:9d6a3ccc0582 465 waitingAckID_FIN=ASSERVISSEMENT_XYT;
goldmas 38:9d6a3ccc0582 466 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
goldmas 38:9d6a3ccc0582 467 while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
goldmas 38:9d6a3ccc0582 468 canProcessRx();
goldmas 38:9d6a3ccc0582 469 etat_pos=FIN_POS;
goldmas 38:9d6a3ccc0582 470 }
goldmas 38:9d6a3ccc0582 471
gabrieltetar 1:7e925468f9d9 472 break;
gabrieltetar 1:7e925468f9d9 473 case FIN_POS:
gabrieltetar 1:7e925468f9d9 474 actual_instruction = instruction.nextLineOK;
goldmas 38:9d6a3ccc0582 475 target_x_robot=x_robot;
goldmas 38:9d6a3ccc0582 476 target_y_robot=y_robot;
goldmas 38:9d6a3ccc0582 477 target_theta_robot=theta_robot;
gabrieltetar 1:7e925468f9d9 478 break;
gabrieltetar 1:7e925468f9d9 479 }
gabrieltetar 1:7e925468f9d9 480 }
gabrieltetar 1:7e925468f9d9 481
gabrieltetar 1:7e925468f9d9 482
gabrieltetar 1:7e925468f9d9 483 break;
gabrieltetar 1:7e925468f9d9 484 case ETAT_GAME_START:
gabrieltetar 10:1964bb91b925 485 Debug_Audio(3,3);
gabrieltetar 1:7e925468f9d9 486 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
gabrieltetar 1:7e925468f9d9 487
gabrieltetar 1:7e925468f9d9 488 if (ModeDemo == 0) {
gabrieltetar 1:7e925468f9d9 489 chronoEnd.attach(&chronometre_ISR,100);//On lance le chrono de 90s
gabrieltetar 1:7e925468f9d9 490 gameTimer.start();
gabrieltetar 1:7e925468f9d9 491 }
gabrieltetar 1:7e925468f9d9 492 gameTimer.reset();
goldmas 36:c37dbe2be916 493 jackB1.fall(NULL);//On désactive l'interruption du jack
goldmas 36:c37dbe2be916 494 jackA1.fall(NULL);//On désactive l'interruption du jack
gabrieltetar 1:7e925468f9d9 495 //SendRawId(GLOBAL_START);
gabrieltetar 1:7e925468f9d9 496 Jack=0; //à envoyer sur le CAN et en direct pour l'automate de l'ihm ou sur CANV
gabrieltetar 1:7e925468f9d9 497 //tactile_printf("Start");//Pas vraiment utile mais bon
gabrieltetar 1:7e925468f9d9 498 break;
gabrieltetar 1:7e925468f9d9 499 case ETAT_GAME_LOAD_NEXT_INSTRUCTION:
gabrieltetar 1:7e925468f9d9 500 flagNonRepriseErrorMot = 0;
gabrieltetar 1:7e925468f9d9 501 /*
gabrieltetar 1:7e925468f9d9 502 Chargement de l'instruction suivante ou arret du robot si il n'y a plus d'instruction
gabrieltetar 1:7e925468f9d9 503 */
gabrieltetar 1:7e925468f9d9 504 //printf("load next instruction\n");
gabrieltetar 1:7e925468f9d9 505 if(dodgeq.nb > 0){//dodge q
gabrieltetar 1:7e925468f9d9 506 instruction.order=dodgeq.inst[dodgeq.nb-1].order;
gabrieltetar 1:7e925468f9d9 507 instruction.arg1=dodgeq.inst[dodgeq.nb-1].arg1;
gabrieltetar 1:7e925468f9d9 508 instruction.arg2=dodgeq.inst[dodgeq.nb-1].arg2;
gabrieltetar 1:7e925468f9d9 509 instruction.arg3=dodgeq.inst[dodgeq.nb-1].arg3;
gabrieltetar 1:7e925468f9d9 510 gameEtat=ETAT_GAME_PROCESS_INSTRUCTION;
gabrieltetar 1:7e925468f9d9 511 dodgeq.nb--;
gabrieltetar 1:7e925468f9d9 512 }//end dodge q
gabrieltetar 1:7e925468f9d9 513 else{// no dodge q
gabrieltetar 1:7e925468f9d9 514 if(actual_instruction >= nb_instructions || actual_instruction == 255) {
gabrieltetar 1:7e925468f9d9 515 gameEtat = ETAT_END;
gabrieltetar 1:7e925468f9d9 516 //Il n'y a plus d'instruction, fin du jeu
gabrieltetar 1:7e925468f9d9 517 } else {
gabrieltetar 1:7e925468f9d9 518 instruction = strat_instructions[actual_instruction];
gabrieltetar 1:7e925468f9d9 519 //On effectue le traitement de l'instruction
gabrieltetar 1:7e925468f9d9 520 gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;
gabrieltetar 1:7e925468f9d9 521 }
gabrieltetar 1:7e925468f9d9 522 }//end no dodge q
gabrieltetar 1:7e925468f9d9 523 screenChecktry = 0;
gabrieltetar 1:7e925468f9d9 524 ingnorInversionOnce = 0;
gabrieltetar 1:7e925468f9d9 525 break;
gabrieltetar 1:7e925468f9d9 526 case ETAT_GAME_PROCESS_INSTRUCTION:
gabrieltetar 1:7e925468f9d9 527 /*
gabrieltetar 1:7e925468f9d9 528 Traitement de l'instruction, envoie de la trame CAN
gabrieltetar 1:7e925468f9d9 529 */
gabrieltetar 1:7e925468f9d9 530 //debug_Instruction(instruction);
gabrieltetar 1:7e925468f9d9 531 actionPrecedente = instruction.order;
goldmas 38:9d6a3ccc0582 532 switch(instruction.order)
goldmas 38:9d6a3ccc0582 533 {
gabrieltetar 1:7e925468f9d9 534 case MV_COURBURE://C'est un rayon de courbure
goldmas 38:9d6a3ccc0582 535 // Debug_Audio(3,6);
gabrieltetar 1:7e925468f9d9 536 float alpha=0, theta=0;
goldmas 38:9d6a3ccc0582 537 short alph=0;
gabrieltetar 1:7e925468f9d9 538 actionPrecedente = MV_COURBURE;
gabrieltetar 1:7e925468f9d9 539 waitingAckID = ASSERVISSEMENT_COURBURE;
gabrieltetar 1:7e925468f9d9 540 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
gabrieltetar 1:7e925468f9d9 541 if(instruction.nextActionType == ENCHAINEMENT) {
gabrieltetar 1:7e925468f9d9 542 MV_enchainement++;
gabrieltetar 1:7e925468f9d9 543 localData5 = 1;
gabrieltetar 1:7e925468f9d9 544 } else {
gabrieltetar 1:7e925468f9d9 545 if(MV_enchainement > 0) {
gabrieltetar 1:7e925468f9d9 546 localData5 = 2;
gabrieltetar 1:7e925468f9d9 547 MV_enchainement = 0;
gabrieltetar 1:7e925468f9d9 548 } else {
gabrieltetar 1:7e925468f9d9 549 localData5 = 0;
gabrieltetar 1:7e925468f9d9 550 }
gabrieltetar 1:7e925468f9d9 551 }
gabrieltetar 1:7e925468f9d9 552 if(InversStrat == 1 && ingnorInversionOnce == 0) {
gabrieltetar 1:7e925468f9d9 553 if(instruction.direction == LEFT) instruction.direction = RIGHT;
gabrieltetar 1:7e925468f9d9 554 else instruction.direction = LEFT;
gabrieltetar 1:7e925468f9d9 555 }
gabrieltetar 1:7e925468f9d9 556
gabrieltetar 1:7e925468f9d9 557 localData1 = ((instruction.direction == LEFT)?1:-1);
gabrieltetar 1:7e925468f9d9 558 localData2 = instruction.arg3;
gabrieltetar 1:7e925468f9d9 559 /*if(InversStrat == 1 && ingnorInversionOnce == 0) {
gabrieltetar 1:7e925468f9d9 560 localData1 = -localData1;//Inversion de la direction
gabrieltetar 1:7e925468f9d9 561 }*/
goldmas 38:9d6a3ccc0582 562 enum E_InstructionDirection directionxyt;
gabrieltetar 1:7e925468f9d9 563 BendRadius(instruction.arg1, localData2, localData1, localData5);
goldmas 38:9d6a3ccc0582 564 alph=localData2;
gabrieltetar 1:7e925468f9d9 565 if(localData2>0) {
gabrieltetar 1:7e925468f9d9 566 direction=1;
gabrieltetar 1:7e925468f9d9 567 } else {
gabrieltetar 1:7e925468f9d9 568 direction=-1;
gabrieltetar 1:7e925468f9d9 569 }
goldmas 38:9d6a3ccc0582 570 if(localData2>0)
goldmas 38:9d6a3ccc0582 571 {
goldmas 38:9d6a3ccc0582 572 directionxyt=FORWARD;
goldmas 38:9d6a3ccc0582 573 asser_stop_direction=1;
goldmas 38:9d6a3ccc0582 574 }
goldmas 38:9d6a3ccc0582 575 else
goldmas 38:9d6a3ccc0582 576 {
goldmas 38:9d6a3ccc0582 577 directionxyt=BACKWARD;
goldmas 38:9d6a3ccc0582 578 asser_stop_direction=-1;
goldmas 38:9d6a3ccc0582 579 }
gabrieltetar 1:7e925468f9d9 580 alpha = localData2*M_PI/1800.0f;
gabrieltetar 1:7e925468f9d9 581 theta = theta_robot*M_PI/1800.0f;
goldmas 38:9d6a3ccc0582 582 int nbre;
goldmas 38:9d6a3ccc0582 583 nbre=abs(alph)/450;
goldmas 38:9d6a3ccc0582 584 for(int c=0;c<nbre+1;c++)
goldmas 38:9d6a3ccc0582 585 {
goldmas 38:9d6a3ccc0582 586 dodgeq.inst[c].order = MV_XYT;
goldmas 38:9d6a3ccc0582 587 dodgeq.inst[c].direction=directionxyt;
goldmas 38:9d6a3ccc0582 588 if(instruction.direction == LEFT) //-------------LEFT
goldmas 38:9d6a3ccc0582 589 {
goldmas 38:9d6a3ccc0582 590 target_x_robot = x_robot + instruction.arg1*(sin(theta+alpha)-sin(theta));// X
goldmas 38:9d6a3ccc0582 591 target_y_robot = y_robot + instruction.arg1*(cos(theta)-cos(theta+alpha));// Y
goldmas 38:9d6a3ccc0582 592 target_theta_robot = theta_robot + alph;// T
goldmas 38:9d6a3ccc0582 593 dodgeq.inst[c].arg1 = target_x_robot;// X
goldmas 38:9d6a3ccc0582 594 dodgeq.inst[c].arg2 = target_y_robot;// Y
goldmas 38:9d6a3ccc0582 595 dodgeq.inst[c].arg3 = target_theta_robot;// T
gabrieltetar 1:7e925468f9d9 596 }
goldmas 38:9d6a3ccc0582 597 else
goldmas 38:9d6a3ccc0582 598 {
goldmas 38:9d6a3ccc0582 599 target_x_robot = x_robot + instruction.arg1*(sin(theta)-sin(theta-alpha));// X
goldmas 38:9d6a3ccc0582 600 target_y_robot = y_robot + instruction.arg1*(cos(theta-alpha)-cos(theta));// Y
goldmas 38:9d6a3ccc0582 601 target_theta_robot = theta_robot - alph;// T
goldmas 38:9d6a3ccc0582 602 dodgeq.inst[c].arg1 = target_x_robot;// X
goldmas 38:9d6a3ccc0582 603 dodgeq.inst[c].arg2 = target_y_robot;// Y
goldmas 38:9d6a3ccc0582 604 dodgeq.inst[c].arg3 = target_theta_robot;// T
gabrieltetar 1:7e925468f9d9 605 }
goldmas 38:9d6a3ccc0582 606 alpha-=alpha/((float)nbre);
goldmas 38:9d6a3ccc0582 607 alph-=alph/(nbre+1);
gabrieltetar 1:7e925468f9d9 608 }
gabrieltetar 1:7e925468f9d9 609 break;
gabrieltetar 1:7e925468f9d9 610
gabrieltetar 1:7e925468f9d9 611
gabrieltetar 1:7e925468f9d9 612
gabrieltetar 1:7e925468f9d9 613 case MV_LINE://Ligne droite
gabrieltetar 1:7e925468f9d9 614 waitingAckID = ASSERVISSEMENT_RECALAGE;
gabrieltetar 1:7e925468f9d9 615 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
gabrieltetar 1:7e925468f9d9 616 if(instruction.nextActionType == ENCHAINEMENT) {
gabrieltetar 1:7e925468f9d9 617 MV_enchainement++;
gabrieltetar 1:7e925468f9d9 618 localData5 = 1;
gabrieltetar 1:7e925468f9d9 619 } else {
gabrieltetar 1:7e925468f9d9 620 if(MV_enchainement > 0) {//Utilisé en cas d'enchainement,
gabrieltetar 1:7e925468f9d9 621 localData5 = 2;
gabrieltetar 1:7e925468f9d9 622 MV_enchainement = 0;
gabrieltetar 1:7e925468f9d9 623 } else {
gabrieltetar 1:7e925468f9d9 624 localData5 = 0;
gabrieltetar 1:7e925468f9d9 625 }
gabrieltetar 1:7e925468f9d9 626 }
gabrieltetar 1:7e925468f9d9 627 localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1);
goldmas 38:9d6a3ccc0582 628 if(instruction.direction == FORWARD)
goldmas 38:9d6a3ccc0582 629 asser_stop_direction=1;
goldmas 38:9d6a3ccc0582 630 else
goldmas 38:9d6a3ccc0582 631 asser_stop_direction=-1;
gabrieltetar 1:7e925468f9d9 632 GoStraight(localData2, 0, 0, localData5);
gabrieltetar 1:7e925468f9d9 633
gabrieltetar 1:7e925468f9d9 634 target_x_robot = x_robot + localData2*cos((double)theta_robot*M_PI/1800);
gabrieltetar 1:7e925468f9d9 635 target_y_robot = y_robot + localData2*sin((double)theta_robot*M_PI/1800);
gabrieltetar 1:7e925468f9d9 636 target_theta_robot = theta_robot;
gabrieltetar 1:7e925468f9d9 637
gabrieltetar 1:7e925468f9d9 638 break;
gabrieltetar 1:7e925468f9d9 639 case MV_TURN: //Rotation sur place
gabrieltetar 1:7e925468f9d9 640 target_x_robot = x_robot;
gabrieltetar 1:7e925468f9d9 641 target_y_robot = y_robot;
gabrieltetar 1:7e925468f9d9 642 target_theta_robot = theta_robot + localData2;
gabrieltetar 1:7e925468f9d9 643 localData2 = instruction.arg3;
gabrieltetar 1:7e925468f9d9 644
gabrieltetar 1:7e925468f9d9 645 if(InversStrat == 1 && ingnorInversionOnce == 0) {
gabrieltetar 1:7e925468f9d9 646 localData2 = -localData2;
gabrieltetar 1:7e925468f9d9 647 }
gabrieltetar 1:7e925468f9d9 648
gabrieltetar 1:7e925468f9d9 649
gabrieltetar 1:7e925468f9d9 650 if(instruction.direction == ABSOLUTE) {
gabrieltetar 1:7e925468f9d9 651 //C'est un rotation absolu, il faut la convertir en relative
gabrieltetar 1:7e925468f9d9 652
gabrieltetar 1:7e925468f9d9 653 localData2 = (localData2 - theta_robot)%3600;
gabrieltetar 1:7e925468f9d9 654 if(localData2 > 1800) localData2 = localData2-3600;
gabrieltetar 1:7e925468f9d9 655
gabrieltetar 1:7e925468f9d9 656 else if(localData2 <-1800) localData2 = localData2+3600;
gabrieltetar 1:7e925468f9d9 657 }
gabrieltetar 1:7e925468f9d9 658
gabrieltetar 1:7e925468f9d9 659
gabrieltetar 1:7e925468f9d9 660 waitingAckID = ASSERVISSEMENT_ROTATION;
gabrieltetar 1:7e925468f9d9 661 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
gabrieltetar 1:7e925468f9d9 662 Rotate(localData2);
gabrieltetar 1:7e925468f9d9 663
gabrieltetar 1:7e925468f9d9 664 break;
goldmas 38:9d6a3ccc0582 665
gabrieltetar 1:7e925468f9d9 666 case MV_XYT:
goldmas 38:9d6a3ccc0582 667
goldmas 38:9d6a3ccc0582 668 if(Flag_bon_port ==1) //XYT pour la rentrée dans le port
goldmas 38:9d6a3ccc0582 669 {
gabrieltetar 10:1964bb91b925 670 Debug_Audio(3,10);
gabrieltetar 1:7e925468f9d9 671 if(instruction.direction == BACKWARD) {
gabrieltetar 1:7e925468f9d9 672 localData1 = -1;
goldmas 38:9d6a3ccc0582 673 asser_stop_direction=-1;
gabrieltetar 1:7e925468f9d9 674 } else {
gabrieltetar 1:7e925468f9d9 675 localData1 = 1;
goldmas 38:9d6a3ccc0582 676 asser_stop_direction=1;
goldmas 38:9d6a3ccc0582 677 }
goldmas 38:9d6a3ccc0582 678
goldmas 38:9d6a3ccc0582 679 if(val_girou == 0) //Si c'est Sud on déplace le X
goldmas 38:9d6a3ccc0582 680 {
goldmas 38:9d6a3ccc0582 681 localData4 = 1300;
goldmas 38:9d6a3ccc0582 682 }
goldmas 38:9d6a3ccc0582 683 else
goldmas 38:9d6a3ccc0582 684 {
goldmas 38:9d6a3ccc0582 685 localData4 = instruction.arg1;
goldmas 38:9d6a3ccc0582 686 }
goldmas 38:9d6a3ccc0582 687
goldmas 38:9d6a3ccc0582 688 if(InversStrat == 1 && ingnorInversionOnce == 0)
goldmas 38:9d6a3ccc0582 689 {
goldmas 38:9d6a3ccc0582 690 localData2 = -instruction.arg3;
goldmas 38:9d6a3ccc0582 691 localData3 = 3000 - instruction.arg2;//Inversion du Y
goldmas 38:9d6a3ccc0582 692 }
goldmas 38:9d6a3ccc0582 693 else
goldmas 38:9d6a3ccc0582 694 {
goldmas 38:9d6a3ccc0582 695 localData3 = instruction.arg2;
goldmas 38:9d6a3ccc0582 696 localData2 = instruction.arg3;
goldmas 38:9d6a3ccc0582 697 }
goldmas 38:9d6a3ccc0582 698 if((instruction.arg1<=0)||(localData3<=0))
goldmas 38:9d6a3ccc0582 699 GoToPosition(target_x_robot,target_y_robot,target_theta_robot,localData1);
goldmas 38:9d6a3ccc0582 700 else
goldmas 38:9d6a3ccc0582 701 GoToPosition(localData4,localData3,localData2,localData1);
goldmas 38:9d6a3ccc0582 702 waitingAckID = ASSERVISSEMENT_XYT;
goldmas 38:9d6a3ccc0582 703 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
goldmas 38:9d6a3ccc0582 704
goldmas 38:9d6a3ccc0582 705 target_x_robot = instruction.arg1;
goldmas 38:9d6a3ccc0582 706 target_y_robot = localData3;
goldmas 38:9d6a3ccc0582 707 target_theta_robot = localData2;
goldmas 38:9d6a3ccc0582 708
goldmas 38:9d6a3ccc0582 709 Flag_bon_port = 0;
goldmas 38:9d6a3ccc0582 710 }
goldmas 38:9d6a3ccc0582 711 else //Sinon on effectue XYT normalement selon les instructions
goldmas 38:9d6a3ccc0582 712 {
goldmas 38:9d6a3ccc0582 713 Debug_Audio(3,10);
goldmas 38:9d6a3ccc0582 714 if(instruction.direction == BACKWARD) {
goldmas 38:9d6a3ccc0582 715 localData1 = -1;
goldmas 38:9d6a3ccc0582 716 asser_stop_direction=-1;
goldmas 38:9d6a3ccc0582 717 } else {
goldmas 38:9d6a3ccc0582 718 localData1 = 1;
goldmas 38:9d6a3ccc0582 719 asser_stop_direction=1;
gabrieltetar 1:7e925468f9d9 720 }
gabrieltetar 1:7e925468f9d9 721
gabrieltetar 1:7e925468f9d9 722 if(InversStrat == 1 && ingnorInversionOnce == 0) {
gabrieltetar 1:7e925468f9d9 723 localData2 = -instruction.arg3;
gabrieltetar 1:7e925468f9d9 724 localData3 = 3000 - instruction.arg2;//Inversion du Y
gabrieltetar 1:7e925468f9d9 725 } else {
gabrieltetar 1:7e925468f9d9 726 localData3 = instruction.arg2;
gabrieltetar 1:7e925468f9d9 727 localData2 = instruction.arg3;
gabrieltetar 1:7e925468f9d9 728 }
goldmas 38:9d6a3ccc0582 729 if((instruction.arg1<=0)||(localData3<=0))
goldmas 38:9d6a3ccc0582 730 GoToPosition(target_x_robot,target_y_robot,target_theta_robot,localData1);
goldmas 38:9d6a3ccc0582 731 else
goldmas 38:9d6a3ccc0582 732 GoToPosition(instruction.arg1,localData3,localData2,localData1);
gabrieltetar 1:7e925468f9d9 733 waitingAckID = ASSERVISSEMENT_XYT;
gabrieltetar 1:7e925468f9d9 734 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
gabrieltetar 1:7e925468f9d9 735
gabrieltetar 1:7e925468f9d9 736 target_x_robot = instruction.arg1;
gabrieltetar 1:7e925468f9d9 737 target_y_robot = localData3;
gabrieltetar 1:7e925468f9d9 738 target_theta_robot = localData2;
goldmas 38:9d6a3ccc0582 739 }
goldmas 38:9d6a3ccc0582 740 break;
goldmas 38:9d6a3ccc0582 741
gabrieltetar 1:7e925468f9d9 742 case MV_RECALAGE:
goldmas 38:9d6a3ccc0582 743 //Effectuer un recalage si on a aucun gobelet ou au moins 2
goldmas 38:9d6a3ccc0582 744 /*
goldmas 38:9d6a3ccc0582 745 if( ((instruction.direction == FORWARD)&&(etat_groupe[0]==5&&etat_groupe[1]==5) || (etat_groupe[0]==5&&etat_groupe[2]==5) || (etat_groupe[1]==5&&etat_groupe[2]==5)) ||
goldmas 38:9d6a3ccc0582 746 ((instruction.direction == BACKWARD)&&(etat_groupe[3]==5&&etat_groupe[4]==5) || (etat_groupe[3]==5&&etat_groupe[5]==5) || (etat_groupe[4]==5&&etat_groupe[5]==5)) ||
goldmas 38:9d6a3ccc0582 747 (etat_groupe[0]!=5 && etat_groupe[1]!=5 && etat_groupe[2]!=5 && etat_groupe[3]!=5 && etat_groupe[4]!=5 && etat_groupe[5]!=5))
goldmas 38:9d6a3ccc0582 748 {
goldmas 38:9d6a3ccc0582 749 */
goldmas 38:9d6a3ccc0582 750 if(instruction.nextActionType == MECANIQUE)
goldmas 38:9d6a3ccc0582 751 {
goldmas 38:9d6a3ccc0582 752 instruction.nextActionType = WAIT;
goldmas 38:9d6a3ccc0582 753 waitingAckID = ASSERVISSEMENT_RECALAGE;
goldmas 38:9d6a3ccc0582 754 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
goldmas 38:9d6a3ccc0582 755 localData2 = (((instruction.direction == FORWARD)?1:-1)*1000);//On indique une distance de 3000 pour etre sur que le robot va ce recaler
goldmas 38:9d6a3ccc0582 756 if(instruction.precision == RECALAGE_Y)
goldmas 38:9d6a3ccc0582 757 {
goldmas 38:9d6a3ccc0582 758 localData5 = 2;
goldmas 38:9d6a3ccc0582 759 if(InversStrat == 1 && ingnorInversionOnce == 0)
goldmas 38:9d6a3ccc0582 760 {
goldmas 38:9d6a3ccc0582 761 localData3 = 3000 - instruction.arg1;//Inversion du Y
goldmas 38:9d6a3ccc0582 762 }
goldmas 38:9d6a3ccc0582 763 else
goldmas 38:9d6a3ccc0582 764 {
goldmas 38:9d6a3ccc0582 765 localData3 = instruction.arg1;
goldmas 38:9d6a3ccc0582 766 }
goldmas 38:9d6a3ccc0582 767 }
goldmas 38:9d6a3ccc0582 768 else
goldmas 38:9d6a3ccc0582 769 {
goldmas 38:9d6a3ccc0582 770 localData5 = 1;
gabrieltetar 1:7e925468f9d9 771 localData3 = instruction.arg1;
gabrieltetar 1:7e925468f9d9 772 }
goldmas 38:9d6a3ccc0582 773 GoStraight(localData2, localData5, localData3, 0);
goldmas 38:9d6a3ccc0582 774 }
goldmas 38:9d6a3ccc0582 775 else
goldmas 38:9d6a3ccc0582 776 { //CAPTEUR
goldmas 38:9d6a3ccc0582 777 SendRawId(DATA_RECALAGE);
goldmas 38:9d6a3ccc0582 778 waitingAckID = RECEPTION_RECALAGE;
goldmas 38:9d6a3ccc0582 779 waitingAckFrom = ACKNOWLEDGE_TELEMETRE;
goldmas 38:9d6a3ccc0582 780
goldmas 38:9d6a3ccc0582 781 // On attend que les variables soient actualisé
goldmas 38:9d6a3ccc0582 782 while(!(waitingAckID == 0 && waitingAckFrom == 0))
goldmas 38:9d6a3ccc0582 783 canProcessRx();
goldmas 38:9d6a3ccc0582 784 while(!(waitingAckID_FIN==0 && waitingAckFrom_FIN==0))
goldmas 38:9d6a3ccc0582 785 canProcessRx();
goldmas 38:9d6a3ccc0582 786
goldmas 38:9d6a3ccc0582 787 if(instruction.precision == RECALAGE_Y) // ((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600)) (theta_robot < 900 && theta_robot > -900)
goldmas 38:9d6a3ccc0582 788 {
goldmas 38:9d6a3ccc0582 789 SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, recalageDistanceY(), theta_robot);
goldmas 38:9d6a3ccc0582 790 }
goldmas 38:9d6a3ccc0582 791 else if(instruction.precision == RECALAGE_X)
goldmas 38:9d6a3ccc0582 792 {
goldmas 38:9d6a3ccc0582 793 SetOdometrie(ODOMETRIE_SMALL_POSITION, recalageDistanceX(), y_robot, theta_robot);
goldmas 38:9d6a3ccc0582 794 }
goldmas 38:9d6a3ccc0582 795 else if(instruction.precision == RECALAGE_T)
goldmas 38:9d6a3ccc0582 796 {
goldmas 38:9d6a3ccc0582 797 SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, y_robot, recalageAngulaireCapteur() );
goldmas 38:9d6a3ccc0582 798 }
gabrieltetar 1:7e925468f9d9 799 }
goldmas 38:9d6a3ccc0582 800 /*}
goldmas 38:9d6a3ccc0582 801 //Ou effectuer une ligne droite jusqu'à la nouvelle valeur de recalage
goldmas 38:9d6a3ccc0582 802
goldmas 38:9d6a3ccc0582 803 else
goldmas 38:9d6a3ccc0582 804 {
goldmas 38:9d6a3ccc0582 805 Debug_Audio(3,8);
goldmas 38:9d6a3ccc0582 806 instruction.nextActionType = WAIT;
goldmas 38:9d6a3ccc0582 807 waitingAckID = ASSERVISSEMENT_RECALAGE;
goldmas 38:9d6a3ccc0582 808 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
goldmas 38:9d6a3ccc0582 809
goldmas 38:9d6a3ccc0582 810 //Calcul de la distance à parcourir pour se retrouver au même point que si on avait fait un recalage
goldmas 38:9d6a3ccc0582 811 localData2 = instruction.arg1;
goldmas 38:9d6a3ccc0582 812 if(instruction.precision == RECALAGE_Y)
goldmas 38:9d6a3ccc0582 813 {
goldmas 38:9d6a3ccc0582 814 if(localData2 == 2832) localData2 = 2832 - y_robot; //Gob+ en y
goldmas 38:9d6a3ccc0582 815 else if(localData2 == 168) localData2 = y_robot - 168; //Gob- en y
gabrieltetar 1:7e925468f9d9 816 }
goldmas 38:9d6a3ccc0582 817 else if(instruction.precision == RECALAGE_X)
goldmas 38:9d6a3ccc0582 818 {
goldmas 38:9d6a3ccc0582 819 if(localData2 == 1832) localData2 = 1832 - x_robot; //Gob+ en x
goldmas 38:9d6a3ccc0582 820 else if(localData2 == 168) localData2 = x_robot - 168; //Gob- en x
goldmas 38:9d6a3ccc0582 821 }
goldmas 38:9d6a3ccc0582 822 localData2 = ((instruction.direction == FORWARD)?1:-1)*localData2;
goldmas 38:9d6a3ccc0582 823 GoStraight(localData2, 0, 0, 0);//x,x,x,localData5
goldmas 38:9d6a3ccc0582 824
goldmas 38:9d6a3ccc0582 825 target_x_robot = x_robot + localData2*cos((double)theta_robot*M_PI/1800);
goldmas 38:9d6a3ccc0582 826 target_y_robot = y_robot + localData2*sin((double)theta_robot*M_PI/1800);
goldmas 38:9d6a3ccc0582 827 target_theta_robot = theta_robot;
gabrieltetar 1:7e925468f9d9 828 }
goldmas 38:9d6a3ccc0582 829 */
gabrieltetar 1:7e925468f9d9 830 break;
gabrieltetar 1:7e925468f9d9 831
gabrieltetar 1:7e925468f9d9 832 case ACTION:
gabrieltetar 1:7e925468f9d9 833 waitingAckID_FIN = 0;
gabrieltetar 1:7e925468f9d9 834 waitingAckFrom_FIN = 0;
gabrieltetar 1:7e925468f9d9 835 int tempo = 0;
gabrieltetar 1:7e925468f9d9 836 waitingAckID= ACK_ACTION; //On veut un ack de type action
gabrieltetar 1:7e925468f9d9 837 waitingAckFrom = ACKNOWLEDGE_HERKULEX; //de la part des herkulex
goldmas 38:9d6a3ccc0582 838 if(InversStrat==1 && ingnorInversionOnce == 0)
goldmas 38:9d6a3ccc0582 839 {
goldmas 38:9d6a3ccc0582 840 if((instruction.arg1 >= 151 && instruction.arg1 <= 154) || (instruction.arg1 >= 163 && instruction.arg1 <= 164))
goldmas 38:9d6a3ccc0582 841 {
goldmas 38:9d6a3ccc0582 842 if(instruction.arg2 == 0) instruction.arg2 = 2;
goldmas 38:9d6a3ccc0582 843 else if(instruction.arg2 == 2) instruction.arg2 = 0;
goldmas 38:9d6a3ccc0582 844 else if(instruction.arg2 == 10) instruction.arg2 = 21;
goldmas 38:9d6a3ccc0582 845 else if(instruction.arg2 == 21) instruction.arg2 = 10;
goldmas 38:9d6a3ccc0582 846 else if(instruction.arg2 == 3) instruction.arg2 = 5;
goldmas 38:9d6a3ccc0582 847 else if(instruction.arg2 == 5) instruction.arg2 = 3;
goldmas 38:9d6a3ccc0582 848 else if(instruction.arg2 == 43) instruction.arg2 = 54;
goldmas 38:9d6a3ccc0582 849 else if(instruction.arg2 == 54) instruction.arg2 = 43;
goldmas 38:9d6a3ccc0582 850 }
goldmas 38:9d6a3ccc0582 851 else if(instruction.arg1 >= 157 && instruction.arg1 <= 159)
goldmas 38:9d6a3ccc0582 852 {
goldmas 38:9d6a3ccc0582 853 if(instruction.arg2 == 0) instruction.arg2 = 1;
goldmas 38:9d6a3ccc0582 854 else if(instruction.arg2 == 1) instruction.arg2 = 0;
goldmas 38:9d6a3ccc0582 855 }
goldmas 38:9d6a3ccc0582 856 }
gabrieltetar 14:6aa8aa1699ad 857 tempo = doAction(instruction.arg1,instruction.arg2,instruction.arg3);
gabrieltetar 1:7e925468f9d9 858 // unsigned char test=(unsigned char) tempo;
gabrieltetar 1:7e925468f9d9 859 // SendMsgCan(0x5BD, &test,1);
gabrieltetar 1:7e925468f9d9 860 if(tempo == 1) {
gabrieltetar 1:7e925468f9d9 861 //L'action est spécifique
gabrieltetar 1:7e925468f9d9 862 if((waitingAckFrom == 0 && waitingAckID == 0) && instruction.nextActionType == ENCHAINEMENT) {
gabrieltetar 1:7e925468f9d9 863 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
gabrieltetar 1:7e925468f9d9 864 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
gabrieltetar 1:7e925468f9d9 865 } else {
gabrieltetar 1:7e925468f9d9 866 gameEtat = ETAT_GAME_WAIT_ACK;
gabrieltetar 1:7e925468f9d9 867 }
gabrieltetar 1:7e925468f9d9 868 return;
gabrieltetar 1:7e925468f9d9 869 } else if (tempo == 2) {
gabrieltetar 1:7e925468f9d9 870 // on est dans le cas de l'avance selon le telemetre
gabrieltetar 1:7e925468f9d9 871 waitingAckID = ASSERVISSEMENT_RECALAGE;
gabrieltetar 1:7e925468f9d9 872 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
gabrieltetar 1:7e925468f9d9 873
gabrieltetar 1:7e925468f9d9 874 localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1);
gabrieltetar 1:7e925468f9d9 875 GoStraight(telemetreDistance, 0, 0, 0);
gabrieltetar 1:7e925468f9d9 876 // on reset la distance du telemetre à 0
gabrieltetar 1:7e925468f9d9 877 telemetreDistance = 5000;
gabrieltetar 1:7e925468f9d9 878 } else {
gabrieltetar 1:7e925468f9d9 879 //C'est un AX12 qu'il faut bouger
gabrieltetar 1:7e925468f9d9 880 //AX12_setGoal(instruction.arg1,instruction.arg3/10,instruction.arg2);
gabrieltetar 1:7e925468f9d9 881 //AX12_enchainement++;
gabrieltetar 1:7e925468f9d9 882
gabrieltetar 1:7e925468f9d9 883 }
goldmas 38:9d6a3ccc0582 884 /*target_x_robot=x_robot;
goldmas 38:9d6a3ccc0582 885 target_y_robot=y_robot;
goldmas 38:9d6a3ccc0582 886 target_theta_robot=theta_robot;
goldmas 38:9d6a3ccc0582 887 */break;
gabrieltetar 1:7e925468f9d9 888 default:
gabrieltetar 1:7e925468f9d9 889 //Instruction inconnue, on l'ignore
gabrieltetar 1:7e925468f9d9 890 break;
gabrieltetar 1:7e925468f9d9 891 }
gabrieltetar 1:7e925468f9d9 892
gabrieltetar 1:7e925468f9d9 893 if(instruction.nextActionType == JUMP || instruction.nextActionType == WAIT) {
gabrieltetar 1:7e925468f9d9 894 gameEtat = ETAT_GAME_WAIT_ACK;//Il faut attendre que la carte est bien reçu l'acknowledge
gabrieltetar 1:7e925468f9d9 895 screenChecktry++;//On incrèment le conteur de tentative de 1
gabrieltetar 1:7e925468f9d9 896 cartesCheker.reset();//On reset le timeOut
gabrieltetar 1:7e925468f9d9 897 cartesCheker.start();
gabrieltetar 1:7e925468f9d9 898 if(AX12_enchainement > 0) {
gabrieltetar 1:7e925468f9d9 899 //AX12_processChange();//Il faut lancer le déplacement des AX12
gabrieltetar 1:7e925468f9d9 900 //AX12_enchainement = 0;
gabrieltetar 1:7e925468f9d9 901 }
gabrieltetar 1:7e925468f9d9 902 } else { //C'est un enchainement
gabrieltetar 1:7e925468f9d9 903 if(instruction.order == MV_LINE) {
gabrieltetar 1:7e925468f9d9 904 gameEtat = ETAT_GAME_WAIT_ACK;
gabrieltetar 1:7e925468f9d9 905 } else {
gabrieltetar 1:7e925468f9d9 906 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
gabrieltetar 1:7e925468f9d9 907 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//C'est un enchainement, on charge directement l'instruction suivante
gabrieltetar 1:7e925468f9d9 908 }
gabrieltetar 1:7e925468f9d9 909 }
gabrieltetar 1:7e925468f9d9 910
gabrieltetar 1:7e925468f9d9 911 break;
gabrieltetar 1:7e925468f9d9 912 case ETAT_GAME_WAIT_ACK:
gabrieltetar 1:7e925468f9d9 913 canProcessRx();
gabrieltetar 1:7e925468f9d9 914 //SendSpeed(200);//--------------------------------------------------MODE RALENTI !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
goldmas 38:9d6a3ccc0582 915 if(waitingAckID == 0 && waitingAckFrom == 0) //Les ack ont été reset, c'est bon on continue
goldmas 38:9d6a3ccc0582 916 {
gabrieltetar 1:7e925468f9d9 917 //if(true) {
gabrieltetar 1:7e925468f9d9 918 cartesCheker.stop();
goldmas 38:9d6a3ccc0582 919 if(instruction.nextActionType == JUMP)
goldmas 38:9d6a3ccc0582 920 {
goldmas 38:9d6a3ccc0582 921 if(instruction.jumpAction == JUMP_POSITION)
goldmas 38:9d6a3ccc0582 922 {
gabrieltetar 1:7e925468f9d9 923 gameEtat = ETAT_GAME_JUMP_POSITION;
goldmas 38:9d6a3ccc0582 924 }
goldmas 38:9d6a3ccc0582 925 else
goldmas 38:9d6a3ccc0582 926 { //Pour eviter les erreurs, on dit que c'est par défaut un jump time
gabrieltetar 1:7e925468f9d9 927 gameEtat = ETAT_GAME_JUMP_TIME;
gabrieltetar 1:7e925468f9d9 928 cartesCheker.reset();//On reset le timeOut
gabrieltetar 1:7e925468f9d9 929 cartesCheker.start();
gabrieltetar 1:7e925468f9d9 930 }
goldmas 38:9d6a3ccc0582 931 }
goldmas 38:9d6a3ccc0582 932 else if(instruction.nextActionType == WAIT) ///Actualisation des waiting ack afin d'attendre la fin des actions
goldmas 38:9d6a3ccc0582 933 {
gabrieltetar 1:7e925468f9d9 934 /*wait_ms(200);
gabrieltetar 1:7e925468f9d9 935 #ifdef ROBOT_BIG
gabrieltetar 1:7e925468f9d9 936 SetOdometrie(ODOMETRIE_BIG_POSITION, x_robot, y_robot, theta_robot);
gabrieltetar 1:7e925468f9d9 937 #else
gabrieltetar 1:7e925468f9d9 938 SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, y_robot, theta_robot);
gabrieltetar 1:7e925468f9d9 939 #endif
gabrieltetar 1:7e925468f9d9 940 wait_ms(200);*/
gabrieltetar 1:7e925468f9d9 941
gabrieltetar 1:7e925468f9d9 942 gameEtat = ETAT_GAME_WAIT_END_INSTRUCTION;
gabrieltetar 1:7e925468f9d9 943 switch(instruction.order) {
gabrieltetar 1:7e925468f9d9 944 case MV_BEZIER:
gabrieltetar 1:7e925468f9d9 945 waitingAckID_FIN = ASSERVISSEMENT_BEZIER;
gabrieltetar 1:7e925468f9d9 946 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
gabrieltetar 1:7e925468f9d9 947 break;
gabrieltetar 1:7e925468f9d9 948 case MV_COURBURE:
gabrieltetar 1:7e925468f9d9 949 waitingAckID_FIN = ASSERVISSEMENT_COURBURE;
gabrieltetar 1:7e925468f9d9 950 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
gabrieltetar 1:7e925468f9d9 951 break;
gabrieltetar 1:7e925468f9d9 952 case MV_LINE:
gabrieltetar 1:7e925468f9d9 953 waitingAckID_FIN = ASSERVISSEMENT_RECALAGE;
gabrieltetar 1:7e925468f9d9 954 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
gabrieltetar 1:7e925468f9d9 955 break;
gabrieltetar 1:7e925468f9d9 956 case MV_TURN:
gabrieltetar 1:7e925468f9d9 957 waitingAckID_FIN = ASSERVISSEMENT_ROTATION;
gabrieltetar 1:7e925468f9d9 958 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
gabrieltetar 1:7e925468f9d9 959 break;
gabrieltetar 1:7e925468f9d9 960 case MV_XYT:
gabrieltetar 1:7e925468f9d9 961 waitingAckID_FIN = ASSERVISSEMENT_XYT;
gabrieltetar 1:7e925468f9d9 962 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
gabrieltetar 1:7e925468f9d9 963 break;
gabrieltetar 1:7e925468f9d9 964 case MV_RECALAGE:
gabrieltetar 1:7e925468f9d9 965 waitingAckID_FIN = ASSERVISSEMENT_RECALAGE;
gabrieltetar 1:7e925468f9d9 966 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
gabrieltetar 1:7e925468f9d9 967 break;
gabrieltetar 1:7e925468f9d9 968 case ACTION:
gabrieltetar 1:7e925468f9d9 969
goldmas 38:9d6a3ccc0582 970 if (modeTelemetre == 0)
goldmas 38:9d6a3ccc0582 971 {
goldmas 38:9d6a3ccc0582 972 if (telemetreDistance == 0)
goldmas 38:9d6a3ccc0582 973 {
gabrieltetar 1:7e925468f9d9 974 waitingAckID_FIN = ACK_FIN_ACTION;// ack de type action
gabrieltetar 1:7e925468f9d9 975 waitingAckFrom_FIN = ACKNOWLEDGE_HERKULEX; //de la part des herkulex/actionneurs
goldmas 38:9d6a3ccc0582 976 }
goldmas 38:9d6a3ccc0582 977 else if(telemetreDistance == 5000)
goldmas 38:9d6a3ccc0582 978 {
gabrieltetar 1:7e925468f9d9 979 // on est dans le cas ou l'on fait une ligne suivant la distance du telemetre
gabrieltetar 1:7e925468f9d9 980 //waitingAckID_FIN = ASSERVISSEMENT_RECALAGE;
gabrieltetar 1:7e925468f9d9 981 //waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
gabrieltetar 1:7e925468f9d9 982 telemetreDistance = 0;
gabrieltetar 1:7e925468f9d9 983 }
goldmas 38:9d6a3ccc0582 984 }
goldmas 38:9d6a3ccc0582 985 else // si on attend la reponse du telemetre
goldmas 38:9d6a3ccc0582 986 {
gabrieltetar 1:7e925468f9d9 987 //modeTelemetre = 1;
gabrieltetar 1:7e925468f9d9 988 waitingAckID_FIN = OBJET_SUR_TABLE;
gabrieltetar 1:7e925468f9d9 989 waitingAckFrom_FIN = 0;
gabrieltetar 1:7e925468f9d9 990 }
gabrieltetar 1:7e925468f9d9 991 break;
gabrieltetar 1:7e925468f9d9 992 default:
gabrieltetar 1:7e925468f9d9 993 break;
gabrieltetar 1:7e925468f9d9 994 }
goldmas 38:9d6a3ccc0582 995 }
goldmas 38:9d6a3ccc0582 996 else
goldmas 38:9d6a3ccc0582 997 {
gabrieltetar 1:7e925468f9d9 998 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
gabrieltetar 1:7e925468f9d9 999 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
gabrieltetar 1:7e925468f9d9 1000 }
goldmas 38:9d6a3ccc0582 1001 }
goldmas 38:9d6a3ccc0582 1002 else if(cartesCheker.read_ms () > 1000)
goldmas 38:9d6a3ccc0582 1003 {
gabrieltetar 1:7e925468f9d9 1004 cartesCheker.stop();
goldmas 38:9d6a3ccc0582 1005 if(screenChecktry >=2) //La carte n'a pas reçus l'information, on passe à l'instruction d'erreur
goldmas 38:9d6a3ccc0582 1006 {
gabrieltetar 1:7e925468f9d9 1007 actual_instruction = instruction.nextLineError;
gabrieltetar 1:7e925468f9d9 1008 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
goldmas 38:9d6a3ccc0582 1009 }
goldmas 38:9d6a3ccc0582 1010 else
goldmas 38:9d6a3ccc0582 1011 {
gabrieltetar 21:d137ec53c3a9 1012 gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;//On retourne dans l'strat_etat_s d'envois de l'instruction
gabrieltetar 1:7e925468f9d9 1013 }
gabrieltetar 1:7e925468f9d9 1014 }
gabrieltetar 1:7e925468f9d9 1015 break;
gabrieltetar 1:7e925468f9d9 1016
gabrieltetar 1:7e925468f9d9 1017 case ETAT_GAME_JUMP_TIME:
gabrieltetar 1:7e925468f9d9 1018 if(cartesCheker.read_ms () >= instruction.JumpTimeOrX) {
gabrieltetar 1:7e925468f9d9 1019 cartesCheker.stop();//On arrete le timer
gabrieltetar 1:7e925468f9d9 1020 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
gabrieltetar 1:7e925468f9d9 1021 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
gabrieltetar 1:7e925468f9d9 1022 }
gabrieltetar 1:7e925468f9d9 1023 break;
gabrieltetar 1:7e925468f9d9 1024
gabrieltetar 1:7e925468f9d9 1025 case ETAT_GAME_JUMP_CONFIG:
gabrieltetar 1:7e925468f9d9 1026 signed int depasX = 1, depasY = 1; // servent à indiquer le sens de dépassement des coordonnées
gabrieltetar 1:7e925468f9d9 1027 // 1 si l'instruction est plus grande que la position du robot
gabrieltetar 1:7e925468f9d9 1028 // -1 si l'instruction est plus petite que la position du robot
gabrieltetar 1:7e925468f9d9 1029 // 0 si l'instruction et position du robot sont proche de moins de 1cm
gabrieltetar 1:7e925468f9d9 1030 if (abs(x_robot-instruction.JumpTimeOrX)<10) {
gabrieltetar 1:7e925468f9d9 1031 depasX = 0;
gabrieltetar 1:7e925468f9d9 1032 } else if(x_robot > instruction.JumpTimeOrX) {
gabrieltetar 1:7e925468f9d9 1033 depasX = -1;
gabrieltetar 1:7e925468f9d9 1034 }
gabrieltetar 1:7e925468f9d9 1035
gabrieltetar 1:7e925468f9d9 1036 if(abs(y_robot-instruction.JumpY)<10) {
gabrieltetar 1:7e925468f9d9 1037 depasY = 0;
gabrieltetar 1:7e925468f9d9 1038 } else if(y_robot > instruction.JumpY) {
gabrieltetar 1:7e925468f9d9 1039 depasY = -1;
gabrieltetar 1:7e925468f9d9 1040 }
gabrieltetar 1:7e925468f9d9 1041
gabrieltetar 1:7e925468f9d9 1042 gameEtat = ETAT_GAME_JUMP_POSITION;
gabrieltetar 1:7e925468f9d9 1043 break;
gabrieltetar 1:7e925468f9d9 1044 case ETAT_GAME_JUMP_POSITION:
gabrieltetar 1:7e925468f9d9 1045 bool Xok = false, Yok = false;
gabrieltetar 1:7e925468f9d9 1046
gabrieltetar 1:7e925468f9d9 1047 if (depasX == 0) {
gabrieltetar 1:7e925468f9d9 1048 Xok = true;
gabrieltetar 1:7e925468f9d9 1049 } else if ((instruction.JumpTimeOrX - x_robot)*depasX < -5) {
gabrieltetar 1:7e925468f9d9 1050 Xok = true;
gabrieltetar 1:7e925468f9d9 1051 }
gabrieltetar 1:7e925468f9d9 1052
gabrieltetar 1:7e925468f9d9 1053 if (depasY == 0) {
gabrieltetar 1:7e925468f9d9 1054 Yok = true;
gabrieltetar 1:7e925468f9d9 1055 } else if ((instruction.JumpY - y_robot)*depasY < -5) {
gabrieltetar 1:7e925468f9d9 1056 Yok = true;
gabrieltetar 1:7e925468f9d9 1057 }
gabrieltetar 1:7e925468f9d9 1058
gabrieltetar 1:7e925468f9d9 1059 // on teste si les deux coordonnées ont été dépassées, si oui on lance l'instruction suivante
gabrieltetar 1:7e925468f9d9 1060 if (Xok && Yok) {
gabrieltetar 1:7e925468f9d9 1061 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
gabrieltetar 1:7e925468f9d9 1062 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
gabrieltetar 1:7e925468f9d9 1063 }
gabrieltetar 1:7e925468f9d9 1064
gabrieltetar 1:7e925468f9d9 1065 break;
gabrieltetar 1:7e925468f9d9 1066 case ETAT_GAME_WAIT_END_INSTRUCTION:
gabrieltetar 1:7e925468f9d9 1067 canProcessRx();
gabrieltetar 1:7e925468f9d9 1068 if(waitingAckID_FIN == 0 && waitingAckFrom_FIN ==0) {//On attend que la carte nous indique que l'instruction est terminée
gabrieltetar 1:7e925468f9d9 1069 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
gabrieltetar 1:7e925468f9d9 1070 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
gabrieltetar 1:7e925468f9d9 1071 }
gabrieltetar 1:7e925468f9d9 1072
gabrieltetar 1:7e925468f9d9 1073 break;
gabrieltetar 1:7e925468f9d9 1074
gabrieltetar 1:7e925468f9d9 1075 case ETAT_END:
gabrieltetar 10:1964bb91b925 1076 Debug_Audio(3,4);
gabrieltetar 1:7e925468f9d9 1077 if (ModeDemo) {
gabrieltetar 1:7e925468f9d9 1078 gameEtat = ETAT_CHECK_CARTE_SCREEN;
gabrieltetar 1:7e925468f9d9 1079 ModeDemo = 1;
gabrieltetar 1:7e925468f9d9 1080 } else {
gabrieltetar 1:7e925468f9d9 1081 gameEtat = ETAT_END_LOOP;
gabrieltetar 1:7e925468f9d9 1082 }
gabrieltetar 1:7e925468f9d9 1083 break;
gabrieltetar 1:7e925468f9d9 1084 case ETAT_END_LOOP:
gabrieltetar 1:7e925468f9d9 1085 //Rien, on tourne en rond
gabrieltetar 1:7e925468f9d9 1086
gabrieltetar 1:7e925468f9d9 1087 break;
gabrieltetar 1:7e925468f9d9 1088 default:
gabrieltetar 1:7e925468f9d9 1089
gabrieltetar 1:7e925468f9d9 1090 break;
gabrieltetar 1:7e925468f9d9 1091 }
gabrieltetar 1:7e925468f9d9 1092 }
gabrieltetar 1:7e925468f9d9 1093
gabrieltetar 1:7e925468f9d9 1094
gabrieltetar 1:7e925468f9d9 1095
gabrieltetar 1:7e925468f9d9 1096
gabrieltetar 1:7e925468f9d9 1097
gabrieltetar 1:7e925468f9d9 1098 /****************************************************************************************/
gabrieltetar 1:7e925468f9d9 1099 /* FUNCTION NAME: canProcessRx */
gabrieltetar 1:7e925468f9d9 1100 /* DESCRIPTION : Fait évoluer l'automate de l'IHM en fonction des receptions sur le CAN*/
gabrieltetar 1:7e925468f9d9 1101 /****************************************************************************************/
gabrieltetar 1:7e925468f9d9 1102 void canProcessRx(void)
gabrieltetar 1:7e925468f9d9 1103 {
gabrieltetar 1:7e925468f9d9 1104 static signed char FIFO_occupation=0,FIFO_max_occupation=0;
gabrieltetar 1:7e925468f9d9 1105 char message[10]="toto";
gabrieltetar 1:7e925468f9d9 1106 char message1[10]="toto";
gabrieltetar 1:7e925468f9d9 1107 char message2[10]="toto";
gabrieltetar 1:7e925468f9d9 1108 char message3[10]="toto";
gabrieltetar 1:7e925468f9d9 1109 char message4[10]="toto";
gabrieltetar 1:7e925468f9d9 1110 FIFO_occupation=FIFO_ecriture-FIFO_lecture;
gabrieltetar 1:7e925468f9d9 1111
gabrieltetar 1:7e925468f9d9 1112 if(FIFO_occupation<0)
gabrieltetar 1:7e925468f9d9 1113 FIFO_occupation=FIFO_occupation+SIZE_FIFO;
gabrieltetar 1:7e925468f9d9 1114
gabrieltetar 1:7e925468f9d9 1115 if(FIFO_max_occupation<FIFO_occupation) {
gabrieltetar 1:7e925468f9d9 1116 FIFO_max_occupation=FIFO_occupation;
gabrieltetar 1:7e925468f9d9 1117 //SendRawId(
gabrieltetar 1:7e925468f9d9 1118 }
gabrieltetar 1:7e925468f9d9 1119
gabrieltetar 1:7e925468f9d9 1120 if(FIFO_occupation!=0) {
gabrieltetar 1:7e925468f9d9 1121 int identifiant=msgRxBuffer[FIFO_lecture].id;
gabrieltetar 1:7e925468f9d9 1122
gabrieltetar 1:7e925468f9d9 1123 if (waitingId == identifiant) waitingId = 0;
gabrieltetar 1:7e925468f9d9 1124 switch(identifiant) {
gabrieltetar 1:7e925468f9d9 1125
gabrieltetar 1:7e925468f9d9 1126 case ALIVE_MOTEUR:
gabrieltetar 24:1a13c998c7ac 1127 if (strat_etat_s == ATT) {
gabrieltetar 27:ff70537a7619 1128 DrawCheck(30,30,50,350,1);
gabrieltetar 24:1a13c998c7ac 1129 }
gabrieltetar 1:7e925468f9d9 1130 break;
gabrieltetar 1:7e925468f9d9 1131
gabrieltetar 1:7e925468f9d9 1132 case ALIVE_BALISE:
gabrieltetar 24:1a13c998c7ac 1133 if (strat_etat_s == ATT) {
gabrieltetar 27:ff70537a7619 1134 DrawCheck(30,30,50,400,1);
gabrieltetar 24:1a13c998c7ac 1135 }
gabrieltetar 1:7e925468f9d9 1136 break;
gabrieltetar 1:7e925468f9d9 1137
gabrieltetar 1:7e925468f9d9 1138 case RESET_IHM:
gabrieltetar 21:d137ec53c3a9 1139 strat_etat_s = CHOIX;
gabrieltetar 1:7e925468f9d9 1140 break;
gabrieltetar 1:7e925468f9d9 1141
gabrieltetar 1:7e925468f9d9 1142 case DEBUG_FAKE_JAKE://Permet de lancer le match à distance
gabrieltetar 1:7e925468f9d9 1143 case GLOBAL_JACK:
gabrieltetar 1:7e925468f9d9 1144 if(gameEtat == ETAT_GAME_WAIT_FOR_JACK) {
gabrieltetar 1:7e925468f9d9 1145 gameEtat = ETAT_GAME_START;
gabrieltetar 1:7e925468f9d9 1146 SendRawId(ACKNOWLEDGE_JACK);
gabrieltetar 1:7e925468f9d9 1147 }
gabrieltetar 1:7e925468f9d9 1148 break;
gabrieltetar 1:7e925468f9d9 1149
gabrieltetar 1:7e925468f9d9 1150 case ALIVE_ACTIONNEURS_AVANT: //pas de break donc passe directement dans ECRAN_ALL_CHECK mais conserve l'ident initial
gabrieltetar 1:7e925468f9d9 1151 case ALIVE_ACTIONNEURS_ARRIERE:
gabrieltetar 1:7e925468f9d9 1152 case ALIVE_HERKULEX:
gabrieltetar 1:7e925468f9d9 1153 case ECRAN_ALL_CHECK:
gabrieltetar 1:7e925468f9d9 1154 if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id) {
gabrieltetar 1:7e925468f9d9 1155 waitingAckFrom = 0;//C'est la bonne carte qui indique qu'elle est en ligne
gabrieltetar 1:7e925468f9d9 1156 }
gabrieltetar 24:1a13c998c7ac 1157 flag_check_carte=1;
gabrieltetar 1:7e925468f9d9 1158 break;
gabrieltetar 1:7e925468f9d9 1159
gabrieltetar 1:7e925468f9d9 1160 case ASSERVISSEMENT_ERROR_MOTEUR://erreur asservissement
gabrieltetar 1:7e925468f9d9 1161 {
gabrieltetar 1:7e925468f9d9 1162
gabrieltetar 1:7e925468f9d9 1163 unsigned short recieveAckID;// = (unsigned short)msgRxBuffer[FIFO_lecture].data[0] | ( ((unsigned short)msgRxBuffer[FIFO_lecture].data[1]) <<8);
gabrieltetar 1:7e925468f9d9 1164 memcpy(&recieveAckID, msgRxBuffer[FIFO_lecture].data, 2);
gabrieltetar 1:7e925468f9d9 1165 if(recieveAckID == waitingAckID_FIN && waitingAckFrom_FIN == INSTRUCTION_END_MOTEUR)
gabrieltetar 1:7e925468f9d9 1166 {
gabrieltetar 1:7e925468f9d9 1167 if(flagNonRepriseErrorMot) {
gabrieltetar 1:7e925468f9d9 1168 actual_instruction = instruction.nextLineError;
gabrieltetar 1:7e925468f9d9 1169 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
gabrieltetar 1:7e925468f9d9 1170 flagNonRepriseErrorMot = 0;
gabrieltetar 1:7e925468f9d9 1171 } else {
gabrieltetar 1:7e925468f9d9 1172 flagNonRepriseErrorMot = 1;
gabrieltetar 1:7e925468f9d9 1173 timeoutWarningWaitEnd.reset();
gabrieltetar 1:7e925468f9d9 1174 timeoutWarningWaitEnd.start();
gabrieltetar 1:7e925468f9d9 1175 //gameEtat = ETAT_WARING_END_BALISE_WAIT;
gabrieltetar 1:7e925468f9d9 1176 }
gabrieltetar 1:7e925468f9d9 1177 }
gabrieltetar 1:7e925468f9d9 1178 /*
gabrieltetar 1:7e925468f9d9 1179 if(flagNonRepriseErrorMot) {
gabrieltetar 1:7e925468f9d9 1180 actual_instruction = instruction.nextLineError;
gabrieltetar 1:7e925468f9d9 1181 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
gabrieltetar 1:7e925468f9d9 1182 flagNonRepriseErrorMot = 0;
gabrieltetar 1:7e925468f9d9 1183 } else {
gabrieltetar 1:7e925468f9d9 1184 flagNonRepriseErrorMot = 1;
gabrieltetar 1:7e925468f9d9 1185 gameEtat = ETAT_WARNING_END_LAST_INSTRUCTION;
gabrieltetar 1:7e925468f9d9 1186 }*/
gabrieltetar 1:7e925468f9d9 1187 } break;
gabrieltetar 1:7e925468f9d9 1188
gabrieltetar 1:7e925468f9d9 1189 /////////////////////////////////////Acknowledges de Reception de la demande d'action////////////////////////////////////////
gabrieltetar 1:7e925468f9d9 1190 case ACKNOWLEDGE_HERKULEX:
gabrieltetar 1:7e925468f9d9 1191 case ACKNOWLEDGE_BALISE: //pas de break donc passe directement dans ACK_FIN_ACTION mais conserve l'ident initial
gabrieltetar 1:7e925468f9d9 1192
gabrieltetar 1:7e925468f9d9 1193 case ACKNOWLEDGE_TELEMETRE:
gabrieltetar 1:7e925468f9d9 1194 /////////////////////////////////////////////Acknowledges de la fin d'action/////////////////////////////////////////////////
gabrieltetar 1:7e925468f9d9 1195 case ACKNOWLEDGE_MOTEUR:
gabrieltetar 1:7e925468f9d9 1196 case INSTRUCTION_END_BALISE:
gabrieltetar 1:7e925468f9d9 1197 case ACK_FIN_ACTION:
gabrieltetar 1:7e925468f9d9 1198 case INSTRUCTION_END_MOTEUR:
gabrieltetar 1:7e925468f9d9 1199 unsigned short recieveAckID;// = (unsigned short)msgRxBuffer[FIFO_lecture].data[0] | ( ((unsigned short)msgRxBuffer[FIFO_lecture].data[1]) <<8);
gabrieltetar 1:7e925468f9d9 1200 memcpy(&recieveAckID, msgRxBuffer[FIFO_lecture].data, 2);
gabrieltetar 1:7e925468f9d9 1201 /*
gabrieltetar 1:7e925468f9d9 1202 //on desactive la balise dans les rotations XYT
gabrieltetar 1:7e925468f9d9 1203 if(msgRxBuffer[FIFO_lecture].id==ACKNOWLEDGE_MOTEUR && ASSERVISSEMENT_XYT==recieveAckID)ingnorBalise=1;
gabrieltetar 1:7e925468f9d9 1204 if(msgRxBuffer[FIFO_lecture].id==INSTRUCTION_END_MOTEUR && ASSERVISSEMENT_XYT_ROTATE==recieveAckID)ingnorBalise=0;
gabrieltetar 1:7e925468f9d9 1205
gabrieltetar 1:7e925468f9d9 1206 //on desactive la balise dans les rotations
gabrieltetar 1:7e925468f9d9 1207 if(msgRxBuffer[FIFO_lecture].id==ACKNOWLEDGE_MOTEUR && ASSERVISSEMENT_ROTATION==recieveAckID)ingnorBalise=1;
gabrieltetar 1:7e925468f9d9 1208 if(msgRxBuffer[FIFO_lecture].id==INSTRUCTION_END_MOTEUR && ASSERVISSEMENT_ROTATION==recieveAckID)ingnorBalise=0;
gabrieltetar 1:7e925468f9d9 1209 */
gabrieltetar 1:7e925468f9d9 1210
gabrieltetar 1:7e925468f9d9 1211 // SendMsgCan(0x666,&ingnorBalise,1);
gabrieltetar 1:7e925468f9d9 1212
gabrieltetar 1:7e925468f9d9 1213 if( waitingAckFrom == msgRxBuffer[FIFO_lecture].id && recieveAckID == waitingAckID ) {
gabrieltetar 1:7e925468f9d9 1214 waitingAckFrom = 0;
gabrieltetar 1:7e925468f9d9 1215 waitingAckID = 0;
gabrieltetar 1:7e925468f9d9 1216 }
gabrieltetar 1:7e925468f9d9 1217 if( waitingAckFrom_FIN == msgRxBuffer[FIFO_lecture].id && recieveAckID == waitingAckID_FIN ) {
gabrieltetar 1:7e925468f9d9 1218 waitingAckFrom_FIN = 0;
gabrieltetar 1:7e925468f9d9 1219 waitingAckID_FIN = 0;
gabrieltetar 1:7e925468f9d9 1220 }
gabrieltetar 1:7e925468f9d9 1221
gabrieltetar 1:7e925468f9d9 1222 /*
gabrieltetar 1:7e925468f9d9 1223 if((waitingAckFrom == msgRxBuffer[FIFO_lecture].id) &&
gabrieltetar 1:7e925468f9d9 1224 ((unsigned short)msgRxBuffer[FIFO_lecture].data[0] | (((unsigned short)msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID) )
gabrieltetar 1:7e925468f9d9 1225 {
gabrieltetar 1:7e925468f9d9 1226 waitingAckFrom = 0;
gabrieltetar 1:7e925468f9d9 1227 waitingAckID = 0;
gabrieltetar 1:7e925468f9d9 1228 }
gabrieltetar 1:7e925468f9d9 1229 if(waitingAckFrom_FIN == msgRxBuffer[FIFO_lecture].id && ((unsigned short)msgRxBuffer[FIFO_lecture].data[0]
gabrieltetar 1:7e925468f9d9 1230 |(((unsigned short)msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID_FIN))
gabrieltetar 1:7e925468f9d9 1231 {
gabrieltetar 1:7e925468f9d9 1232 waitingAckFrom_FIN = 0;
gabrieltetar 1:7e925468f9d9 1233 waitingAckID_FIN = 0;
gabrieltetar 1:7e925468f9d9 1234 }
gabrieltetar 1:7e925468f9d9 1235 */
gabrieltetar 1:7e925468f9d9 1236 break;
gabrieltetar 1:7e925468f9d9 1237 case ODOMETRIE_BIG_POSITION:
gabrieltetar 24:1a13c998c7ac 1238 x_robot=msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8);
gabrieltetar 24:1a13c998c7ac 1239 y_robot=msgRxBuffer[FIFO_lecture].data[2]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[3])<<8);
gabrieltetar 24:1a13c998c7ac 1240 theta_robot=msgRxBuffer[FIFO_lecture].data[4]|((signed short)(msgRxBuffer[FIFO_lecture].data[5])<<8);
gabrieltetar 24:1a13c998c7ac 1241 break;
gabrieltetar 1:7e925468f9d9 1242 case ODOMETRIE_SMALL_POSITION:
gabrieltetar 1:7e925468f9d9 1243
gabrieltetar 1:7e925468f9d9 1244 x_robot=msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8);
gabrieltetar 1:7e925468f9d9 1245 y_robot=msgRxBuffer[FIFO_lecture].data[2]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[3])<<8);
gabrieltetar 1:7e925468f9d9 1246 theta_robot=msgRxBuffer[FIFO_lecture].data[4]|((signed short)(msgRxBuffer[FIFO_lecture].data[5])<<8);
gabrieltetar 1:7e925468f9d9 1247 break;
gabrieltetar 1:7e925468f9d9 1248
gabrieltetar 1:7e925468f9d9 1249 case ACK_ACTION:
gabrieltetar 1:7e925468f9d9 1250 if(waitingAckID == msgRxBuffer[FIFO_lecture].id) {
gabrieltetar 1:7e925468f9d9 1251 waitingAckFrom = 0;
gabrieltetar 1:7e925468f9d9 1252 waitingAckID = 0;
gabrieltetar 1:7e925468f9d9 1253 }
gabrieltetar 1:7e925468f9d9 1254 break;
gabrieltetar 1:7e925468f9d9 1255
gabrieltetar 1:7e925468f9d9 1256 case BALISE_DANGER :
gabrieltetar 1:7e925468f9d9 1257 SendAck(ACKNOWLEDGE_BALISE, BALISE_END_DANGER);
goldmas 38:9d6a3ccc0582 1258 balise_danger(FIFO_lecture);
gabrieltetar 1:7e925468f9d9 1259 break;
gabrieltetar 1:7e925468f9d9 1260
gabrieltetar 1:7e925468f9d9 1261 case BALISE_STOP:
gabrieltetar 1:7e925468f9d9 1262 SendAck(ACKNOWLEDGE_BALISE, BALISE_STOP);
gabrieltetar 1:7e925468f9d9 1263 balise_stop(FIFO_lecture);
gabrieltetar 1:7e925468f9d9 1264 break;
gabrieltetar 1:7e925468f9d9 1265
gabrieltetar 1:7e925468f9d9 1266 case BALISE_END_DANGER:
gabrieltetar 1:7e925468f9d9 1267 SendAck(ACKNOWLEDGE_BALISE, BALISE_END_DANGER);
gabrieltetar 21:d137ec53c3a9 1268 balise_end_danger(&instruction,&dodgeq,target_x_robot,target_y_robot,target_theta_robot, theta_robot,x_robot,y_robot);
gabrieltetar 1:7e925468f9d9 1269 break;
gabrieltetar 1:7e925468f9d9 1270
gabrieltetar 1:7e925468f9d9 1271 case RECEPTION_DATA:
gabrieltetar 1:7e925468f9d9 1272 telemetreDistance=char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[0], msgRxBuffer[FIFO_lecture].data[1]);
gabrieltetar 1:7e925468f9d9 1273 telemetreDistance= (float)telemetreDistance*100.0f*35.5f+50.0f;
gabrieltetar 1:7e925468f9d9 1274 waitingAckFrom = 0;
gabrieltetar 1:7e925468f9d9 1275 waitingAckID = 0;
gabrieltetar 1:7e925468f9d9 1276 break;
gabrieltetar 1:7e925468f9d9 1277
gabrieltetar 1:7e925468f9d9 1278 case RECEPTION_RECALAGE:
gabrieltetar 1:7e925468f9d9 1279 wait_us(150);
gabrieltetar 1:7e925468f9d9 1280 flagReceptionTelemetres = 1;
gabrieltetar 1:7e925468f9d9 1281 // telemetreDistance_avant_droite = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[0], msgRxBuffer[FIFO_lecture].data[1]); //on récupère la distance traité par l'autre micro
gabrieltetar 1:7e925468f9d9 1282 memcpy(&telemetreDistance_avant_droite,msgRxBuffer[FIFO_lecture].data,2);
gabrieltetar 1:7e925468f9d9 1283 telemetreDistance_avant_gauche = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[2], msgRxBuffer[FIFO_lecture].data[3]);
gabrieltetar 1:7e925468f9d9 1284 telemetreDistance_arriere_gauche = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[4], msgRxBuffer[FIFO_lecture].data[5]);
gabrieltetar 1:7e925468f9d9 1285 telemetreDistance_arriere_droite = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[6], msgRxBuffer[FIFO_lecture].data[7]);
gabrieltetar 1:7e925468f9d9 1286
gabrieltetar 1:7e925468f9d9 1287
gabrieltetar 1:7e925468f9d9 1288 if(ModeDemo==1) {
gabrieltetar 1:7e925468f9d9 1289 sprintf(message,"%04dmm L:%d",telemetreDistance_avant_droite,DT_AVD_interrupt);
gabrieltetar 1:7e925468f9d9 1290 lcd.SetBackColor(LCD_COLOR_WHITE);
gabrieltetar 1:7e925468f9d9 1291 lcd.DisplayStringAt(0, LINE(8),(unsigned char *)"LASER AVD : ",LEFT_MODE);
gabrieltetar 1:7e925468f9d9 1292 lcd.DisplayStringAt(200, LINE(8),(unsigned char *)message, LEFT_MODE);
gabrieltetar 1:7e925468f9d9 1293
gabrieltetar 1:7e925468f9d9 1294 sprintf(message1,"%04dmm L:%d",telemetreDistance_avant_gauche,DT_AVG_interrupt);
gabrieltetar 1:7e925468f9d9 1295 lcd.SetBackColor(LCD_COLOR_WHITE);
gabrieltetar 1:7e925468f9d9 1296 lcd.DisplayStringAt(0, LINE(10),(unsigned char *)"LASER AVG : ",LEFT_MODE);
gabrieltetar 1:7e925468f9d9 1297 lcd.DisplayStringAt(200, LINE(10),(unsigned char *)message1, LEFT_MODE);
gabrieltetar 1:7e925468f9d9 1298
gabrieltetar 1:7e925468f9d9 1299
gabrieltetar 1:7e925468f9d9 1300 sprintf(message4,"%04d",theta_robot);
gabrieltetar 1:7e925468f9d9 1301 lcd.SetBackColor(LCD_COLOR_WHITE);
gabrieltetar 1:7e925468f9d9 1302 lcd.DisplayStringAt(0, LINE(13),(unsigned char *)"THETA: ",LEFT_MODE);
gabrieltetar 1:7e925468f9d9 1303 lcd.DisplayStringAt(200, LINE(13),(unsigned char *)message4, LEFT_MODE);
gabrieltetar 1:7e925468f9d9 1304
gabrieltetar 1:7e925468f9d9 1305
gabrieltetar 1:7e925468f9d9 1306 sprintf(message2,"%04dmm L:%d",telemetreDistance_arriere_gauche,DT_ARG_interrupt);
gabrieltetar 1:7e925468f9d9 1307 lcd.SetBackColor(LCD_COLOR_WHITE);
gabrieltetar 1:7e925468f9d9 1308 lcd.DisplayStringAt(0, LINE(16),(unsigned char *)"LASER ARG : ",LEFT_MODE);
gabrieltetar 1:7e925468f9d9 1309 lcd.DisplayStringAt(200, LINE(16),(unsigned char *)message2, LEFT_MODE);
gabrieltetar 1:7e925468f9d9 1310
gabrieltetar 1:7e925468f9d9 1311 sprintf(message3,"%04dmm L:%d",telemetreDistance_arriere_droite,DT_ARD_interrupt);
gabrieltetar 1:7e925468f9d9 1312 lcd.SetBackColor(LCD_COLOR_WHITE);
gabrieltetar 1:7e925468f9d9 1313 lcd.DisplayStringAt(0, LINE(18),(unsigned char *)"LASER ARD : ",LEFT_MODE);
gabrieltetar 1:7e925468f9d9 1314 lcd.DisplayStringAt(200, LINE(18),(unsigned char *)message3, LEFT_MODE);
gabrieltetar 1:7e925468f9d9 1315 }
gabrieltetar 1:7e925468f9d9 1316 break;
gabrieltetar 1:7e925468f9d9 1317 case RECEPTION_TELEMETRE_LOGIQUE:
gabrieltetar 1:7e925468f9d9 1318
gabrieltetar 1:7e925468f9d9 1319 DT_AVD_interrupt=msgRxBuffer[FIFO_lecture].data[0];
gabrieltetar 1:7e925468f9d9 1320 DT_AVG_interrupt=msgRxBuffer[FIFO_lecture].data[1];
gabrieltetar 1:7e925468f9d9 1321 DT_ARG_interrupt=msgRxBuffer[FIFO_lecture].data[2];
gabrieltetar 1:7e925468f9d9 1322 DT_ARD_interrupt=msgRxBuffer[FIFO_lecture].data[3];
gabrieltetar 1:7e925468f9d9 1323
gabrieltetar 1:7e925468f9d9 1324 break;
gabrieltetar 1:7e925468f9d9 1325 case RECEPTION_COULEUR:
gabrieltetar 1:7e925468f9d9 1326 if (blocage_balise==0) {
gabrieltetar 1:7e925468f9d9 1327 couleur1=msgRxBuffer[FIFO_lecture].data[0];
gabrieltetar 1:7e925468f9d9 1328 couleur2=msgRxBuffer[FIFO_lecture].data[1];
gabrieltetar 1:7e925468f9d9 1329 couleur3=msgRxBuffer[FIFO_lecture].data[2];
gabrieltetar 1:7e925468f9d9 1330
gabrieltetar 1:7e925468f9d9 1331 /*lcd.DisplayStringAt(0,LINE(16),(unsigned char *)couleur1+'0',LEFT_MODE);
gabrieltetar 1:7e925468f9d9 1332 lcd.DisplayStringAt(0,LINE(16+1),(unsigned char *)couleur2+'0',LEFT_MODE);
gabrieltetar 1:7e925468f9d9 1333 lcd.DisplayStringAt(0,LINE(16+2),(unsigned char *)couleur3+'0',LEFT_MODE);*/
gabrieltetar 1:7e925468f9d9 1334 }
gabrieltetar 1:7e925468f9d9 1335
gabrieltetar 1:7e925468f9d9 1336 break;
goldmas 38:9d6a3ccc0582 1337
goldmas 38:9d6a3ccc0582 1338 case VALEUR_GIROUETTE :
goldmas 38:9d6a3ccc0582 1339 val_girou = msgRxBuffer[FIFO_lecture].data[0] ;
goldmas 38:9d6a3ccc0582 1340 girouette_ISR();
goldmas 38:9d6a3ccc0582 1341
goldmas 38:9d6a3ccc0582 1342 break ;
goldmas 38:9d6a3ccc0582 1343
gabrieltetar 10:1964bb91b925 1344 default:
gabrieltetar 10:1964bb91b925 1345 break;
gabrieltetar 1:7e925468f9d9 1346 /*
gabrieltetar 1:7e925468f9d9 1347 case NO_BLOC: //il n'y a pas de bloc, on saute les étapes liées à l'attrape bloc
gabrieltetar 1:7e925468f9d9 1348 actual_instruction = instruction.nextLineError;
gabrieltetar 1:7e925468f9d9 1349 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
gabrieltetar 1:7e925468f9d9 1350 // waitingAckID_FIN=0;
gabrieltetar 1:7e925468f9d9 1351 // waitingAckFrom_FIN=0;
gabrieltetar 1:7e925468f9d9 1352 SendRawId(0x40);
gabrieltetar 1:7e925468f9d9 1353 break;*/
goldmas 38:9d6a3ccc0582 1354
goldmas 38:9d6a3ccc0582 1355
goldmas 38:9d6a3ccc0582 1356
goldmas 38:9d6a3ccc0582 1357
goldmas 38:9d6a3ccc0582 1358
gabrieltetar 1:7e925468f9d9 1359 }
gabrieltetar 1:7e925468f9d9 1360 FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
gabrieltetar 1:7e925468f9d9 1361 }
gabrieltetar 1:7e925468f9d9 1362 }
gabrieltetar 1:7e925468f9d9 1363
gabrieltetar 1:7e925468f9d9 1364
gabrieltetar 1:7e925468f9d9 1365
gabrieltetar 1:7e925468f9d9 1366
gabrieltetar 1:7e925468f9d9 1367
gabrieltetar 1:7e925468f9d9 1368
gabrieltetar 1:7e925468f9d9 1369
gabrieltetar 1:7e925468f9d9 1370 void affichage_compteur (int nombre)
gabrieltetar 1:7e925468f9d9 1371 {
gabrieltetar 1:7e925468f9d9 1372 int dizaine=0,unite=0,centaine=0;
gabrieltetar 1:7e925468f9d9 1373 centaine = nombre/100;
gabrieltetar 1:7e925468f9d9 1374 dizaine = nombre/10;
gabrieltetar 1:7e925468f9d9 1375 unite = nombre-(10*dizaine);
gabrieltetar 1:7e925468f9d9 1376 print_segment(unite,-50);
gabrieltetar 1:7e925468f9d9 1377 print_segment(dizaine,100);
gabrieltetar 1:7e925468f9d9 1378 if(centaine!=0) {
gabrieltetar 1:7e925468f9d9 1379 print_segment(centaine,350);
gabrieltetar 1:7e925468f9d9 1380 }
gabrieltetar 1:7e925468f9d9 1381
gabrieltetar 1:7e925468f9d9 1382 }
gabrieltetar 1:7e925468f9d9 1383
gabrieltetar 1:7e925468f9d9 1384
gabrieltetar 1:7e925468f9d9 1385 //****print_segment***
gabrieltetar 1:7e925468f9d9 1386 //Dessine en 7 segment le nombre en parametre
gabrieltetar 1:7e925468f9d9 1387 // A
gabrieltetar 1:7e925468f9d9 1388 // =====
gabrieltetar 1:7e925468f9d9 1389 // | |
gabrieltetar 1:7e925468f9d9 1390 // B | G | E
gabrieltetar 1:7e925468f9d9 1391 // |=====|
gabrieltetar 1:7e925468f9d9 1392 // C | | F
gabrieltetar 1:7e925468f9d9 1393 // | |
gabrieltetar 1:7e925468f9d9 1394 // =====
gabrieltetar 1:7e925468f9d9 1395 // D
gabrieltetar 1:7e925468f9d9 1396 /*
gabrieltetar 1:7e925468f9d9 1397 position pour le chiffre des unites
gabrieltetar 1:7e925468f9d9 1398 lcd.FillRect(460,75,120,25);// A
gabrieltetar 1:7e925468f9d9 1399 lcd.FillRect(435,100,25,120);// B
gabrieltetar 1:7e925468f9d9 1400 lcd.FillRect(435,245,25,120);// C
gabrieltetar 1:7e925468f9d9 1401 lcd.FillRect(460,365,120,25);// D
gabrieltetar 1:7e925468f9d9 1402 lcd.FillRect(580,100,25,120);// E
gabrieltetar 1:7e925468f9d9 1403 lcd.FillRect(580,245,25,120);// F
gabrieltetar 1:7e925468f9d9 1404 lcd.FillRect(460,220,120,25);// G
gabrieltetar 1:7e925468f9d9 1405
gabrieltetar 1:7e925468f9d9 1406 position pour le chiffre des dizaines
gabrieltetar 1:7e925468f9d9 1407 lcd.FillRect(260,75,120,25);// A
gabrieltetar 1:7e925468f9d9 1408 lcd.FillRect(235,100,25,120);// B
gabrieltetar 1:7e925468f9d9 1409 lcd.FillRect(235,245,25,120);// C
gabrieltetar 1:7e925468f9d9 1410 lcd.FillRect(260,365,120,25);// D
gabrieltetar 1:7e925468f9d9 1411 lcd.FillRect(380,100,25,120);// E
gabrieltetar 1:7e925468f9d9 1412 lcd.FillRect(380,245,25,120);// F
gabrieltetar 1:7e925468f9d9 1413 lcd.FillRect(260,220,120,25);// G
gabrieltetar 1:7e925468f9d9 1414 */
gabrieltetar 1:7e925468f9d9 1415
gabrieltetar 1:7e925468f9d9 1416 void print_segment(int nombre, int decalage)
gabrieltetar 1:7e925468f9d9 1417 {
gabrieltetar 1:7e925468f9d9 1418
gabrieltetar 1:7e925468f9d9 1419 switch(nombre) {
gabrieltetar 1:7e925468f9d9 1420 case 0:
gabrieltetar 1:7e925468f9d9 1421 lcd.FillRect(240-decalage,75,120,25);
gabrieltetar 1:7e925468f9d9 1422 lcd.FillRect(215-decalage,100,25,120);
gabrieltetar 1:7e925468f9d9 1423 lcd.FillRect(215-decalage,245,25,120);
gabrieltetar 1:7e925468f9d9 1424 lcd.FillRect(360-decalage,245,25,120);
gabrieltetar 1:7e925468f9d9 1425 lcd.FillRect(360-decalage,100,25,120);
gabrieltetar 1:7e925468f9d9 1426 lcd.FillRect(240-decalage,365,120,25);
gabrieltetar 1:7e925468f9d9 1427 break;
gabrieltetar 1:7e925468f9d9 1428
gabrieltetar 1:7e925468f9d9 1429 case 1:
gabrieltetar 1:7e925468f9d9 1430 lcd.FillRect(360-decalage,100,25,120);// E
gabrieltetar 1:7e925468f9d9 1431 lcd.FillRect(360-decalage,245,25,120);// F
gabrieltetar 1:7e925468f9d9 1432 break;
gabrieltetar 1:7e925468f9d9 1433
gabrieltetar 1:7e925468f9d9 1434 case 2:
gabrieltetar 1:7e925468f9d9 1435 lcd.FillRect(240-decalage,75,120,25);// A
gabrieltetar 1:7e925468f9d9 1436 lcd.FillRect(215-decalage,245,25,120);// C
gabrieltetar 1:7e925468f9d9 1437 lcd.FillRect(240-decalage,365,120,25);// D
gabrieltetar 1:7e925468f9d9 1438 lcd.FillRect(360-decalage,100,25,120);// E
gabrieltetar 1:7e925468f9d9 1439 lcd.FillRect(240-decalage,220,120,25);// G
gabrieltetar 1:7e925468f9d9 1440 break;
gabrieltetar 1:7e925468f9d9 1441
gabrieltetar 1:7e925468f9d9 1442 case 3:
gabrieltetar 1:7e925468f9d9 1443 lcd.FillRect(240-decalage,75,120,25);// A
gabrieltetar 1:7e925468f9d9 1444 lcd.FillRect(360-decalage,100,25,120);// E
gabrieltetar 1:7e925468f9d9 1445 lcd.FillRect(240-decalage,220,120,25);// G
gabrieltetar 1:7e925468f9d9 1446 lcd.FillRect(240-decalage,365,120,25);// D
gabrieltetar 1:7e925468f9d9 1447 lcd.FillRect(360-decalage,245,25,120);// F
gabrieltetar 1:7e925468f9d9 1448 break;
gabrieltetar 1:7e925468f9d9 1449
gabrieltetar 1:7e925468f9d9 1450 case 4:
gabrieltetar 1:7e925468f9d9 1451 lcd.FillRect(215-decalage,100,25,120);// B
gabrieltetar 1:7e925468f9d9 1452 lcd.FillRect(360-decalage,100,25,120);// E
gabrieltetar 1:7e925468f9d9 1453 lcd.FillRect(360-decalage,245,25,120);// F
gabrieltetar 1:7e925468f9d9 1454 lcd.FillRect(240-decalage,220,120,25);// G
gabrieltetar 1:7e925468f9d9 1455 break;
gabrieltetar 1:7e925468f9d9 1456
gabrieltetar 1:7e925468f9d9 1457 case 5:
gabrieltetar 1:7e925468f9d9 1458 lcd.FillRect(240-decalage,75,120,25);// A
gabrieltetar 1:7e925468f9d9 1459 lcd.FillRect(215-decalage,100,25,120);// B
gabrieltetar 1:7e925468f9d9 1460 lcd.FillRect(240-decalage,220,120,25);// G
gabrieltetar 1:7e925468f9d9 1461 lcd.FillRect(240-decalage,365,120,25);// D
gabrieltetar 1:7e925468f9d9 1462 lcd.FillRect(360-decalage,245,25,120);// F
gabrieltetar 1:7e925468f9d9 1463 break;
gabrieltetar 1:7e925468f9d9 1464
gabrieltetar 1:7e925468f9d9 1465 case 6:
gabrieltetar 1:7e925468f9d9 1466 lcd.FillRect(240-decalage,75,120,25);// A
gabrieltetar 1:7e925468f9d9 1467 lcd.FillRect(215-decalage,100,25,120);// B
gabrieltetar 1:7e925468f9d9 1468 lcd.FillRect(215-decalage,245,25,120);// C
gabrieltetar 1:7e925468f9d9 1469 lcd.FillRect(240-decalage,365,120,25);// D
gabrieltetar 1:7e925468f9d9 1470 lcd.FillRect(360-decalage,245,25,120);// F
gabrieltetar 1:7e925468f9d9 1471 lcd.FillRect(240-decalage,220,120,25);// G
gabrieltetar 1:7e925468f9d9 1472 break;
gabrieltetar 1:7e925468f9d9 1473
gabrieltetar 1:7e925468f9d9 1474 case 7:
gabrieltetar 1:7e925468f9d9 1475 lcd.FillRect(240-decalage,75,120,25);// A
gabrieltetar 1:7e925468f9d9 1476 lcd.FillRect(360-decalage,100,25,120);// E
gabrieltetar 1:7e925468f9d9 1477 lcd.FillRect(360-decalage,245,25,120);// F
gabrieltetar 1:7e925468f9d9 1478 break;
gabrieltetar 1:7e925468f9d9 1479
gabrieltetar 1:7e925468f9d9 1480 case 8:
gabrieltetar 1:7e925468f9d9 1481 lcd.FillRect(240-decalage,75,120,25); // A
gabrieltetar 1:7e925468f9d9 1482 lcd.FillRect(215-decalage,100,25,120);
gabrieltetar 1:7e925468f9d9 1483 lcd.FillRect(215-decalage,245,25,120);
gabrieltetar 1:7e925468f9d9 1484 lcd.FillRect(360-decalage,245,25,120);//...
gabrieltetar 1:7e925468f9d9 1485 lcd.FillRect(360-decalage,100,25,120);
gabrieltetar 1:7e925468f9d9 1486 lcd.FillRect(240-decalage,365,120,25);
gabrieltetar 1:7e925468f9d9 1487 lcd.FillRect(240-decalage,220,120,25);// G
gabrieltetar 1:7e925468f9d9 1488 break;
gabrieltetar 1:7e925468f9d9 1489
gabrieltetar 1:7e925468f9d9 1490 case 9:
gabrieltetar 1:7e925468f9d9 1491 lcd.FillRect(240-decalage,75,120,25);// A
gabrieltetar 1:7e925468f9d9 1492 lcd.FillRect(215-decalage,100,25,120);// B
gabrieltetar 1:7e925468f9d9 1493 lcd.FillRect(240-decalage,365,120,25);// D
gabrieltetar 1:7e925468f9d9 1494 lcd.FillRect(360-decalage,100,25,120);// E
gabrieltetar 1:7e925468f9d9 1495 lcd.FillRect(360-decalage,245,25,120);// F
gabrieltetar 1:7e925468f9d9 1496 lcd.FillRect(240-decalage,220,120,25);// G
gabrieltetar 1:7e925468f9d9 1497 break;
gabrieltetar 1:7e925468f9d9 1498 }
gabrieltetar 1:7e925468f9d9 1499 }
gabrieltetar 1:7e925468f9d9 1500
gabrieltetar 1:7e925468f9d9 1501 void effacer_segment(long couleur)
gabrieltetar 1:7e925468f9d9 1502 {
gabrieltetar 1:7e925468f9d9 1503 lcd.SetTextColor(couleur);
gabrieltetar 1:7e925468f9d9 1504 lcd.FillRect(240-200,75,120,25); // A
gabrieltetar 1:7e925468f9d9 1505 lcd.FillRect(215-200,100,25,120);
gabrieltetar 1:7e925468f9d9 1506 lcd.FillRect(215-200,245,25,120);
gabrieltetar 1:7e925468f9d9 1507 lcd.FillRect(360-200,245,25,120);//...
gabrieltetar 1:7e925468f9d9 1508 lcd.FillRect(360-200,100,25,120);
gabrieltetar 1:7e925468f9d9 1509 lcd.FillRect(240-200,365,120,25);
gabrieltetar 1:7e925468f9d9 1510 lcd.FillRect(240-200,220,120,25);// G
gabrieltetar 1:7e925468f9d9 1511
gabrieltetar 1:7e925468f9d9 1512 lcd.FillRect(240,75,120,25); // A
gabrieltetar 1:7e925468f9d9 1513 lcd.FillRect(215,100,25,120);
gabrieltetar 1:7e925468f9d9 1514 lcd.FillRect(215,245,25,120);
gabrieltetar 1:7e925468f9d9 1515 lcd.FillRect(360,245,25,120);//...
gabrieltetar 1:7e925468f9d9 1516 lcd.FillRect(360,100,25,120);
gabrieltetar 1:7e925468f9d9 1517 lcd.FillRect(240,365,120,25);
gabrieltetar 1:7e925468f9d9 1518 lcd.FillRect(240,220,120,25);// G
gabrieltetar 1:7e925468f9d9 1519 }
gabrieltetar 1:7e925468f9d9 1520
gabrieltetar 1:7e925468f9d9 1521 short recalageAngulaireCapteur(void)
gabrieltetar 1:7e925468f9d9 1522 {
gabrieltetar 1:7e925468f9d9 1523 unsigned char nombresDeMesuresAuxTelemetresQuiSontCoherentes = 0;
gabrieltetar 1:7e925468f9d9 1524 unsigned int moyennageTelemetre = 0;
gabrieltetar 1:7e925468f9d9 1525 unsigned short angleAvant = 0;
gabrieltetar 1:7e925468f9d9 1526 unsigned short angleArriere = 0;
gabrieltetar 1:7e925468f9d9 1527 unsigned short orientationArrondie = 0;
gabrieltetar 1:7e925468f9d9 1528
gabrieltetar 1:7e925468f9d9 1529 unsigned short position_avant_gauche=0;
gabrieltetar 1:7e925468f9d9 1530 unsigned short position_avant_droite=0;
gabrieltetar 1:7e925468f9d9 1531 unsigned short position_arriere_gauche=0;
gabrieltetar 1:7e925468f9d9 1532 unsigned short position_arriere_droite=0;
gabrieltetar 1:7e925468f9d9 1533
gabrieltetar 1:7e925468f9d9 1534 unsigned short tempo= telemetreDistance_arriere_gauche;
gabrieltetar 1:7e925468f9d9 1535 telemetreDistance_arriere_gauche=telemetreDistance_arriere_droite;
gabrieltetar 1:7e925468f9d9 1536 telemetreDistance_arriere_droite=tempo;
gabrieltetar 1:7e925468f9d9 1537
gabrieltetar 1:7e925468f9d9 1538
gabrieltetar 1:7e925468f9d9 1539
gabrieltetar 1:7e925468f9d9 1540 if(theta_robot >= 450 && theta_robot <= 1350)
gabrieltetar 1:7e925468f9d9 1541 orientationArrondie = 90;
gabrieltetar 1:7e925468f9d9 1542 else if(theta_robot <= -450 && theta_robot >= -1350)
gabrieltetar 1:7e925468f9d9 1543 orientationArrondie = 270;
gabrieltetar 1:7e925468f9d9 1544 else if(theta_robot <= 450 && theta_robot >= -450)
gabrieltetar 1:7e925468f9d9 1545 orientationArrondie = 0;
gabrieltetar 1:7e925468f9d9 1546 else if(theta_robot >= 1350 && theta_robot <= -1350)
gabrieltetar 1:7e925468f9d9 1547 orientationArrondie = 180;
gabrieltetar 1:7e925468f9d9 1548
gabrieltetar 1:7e925468f9d9 1549 // Calcul de position pour faire la vérification de cohérence
gabrieltetar 1:7e925468f9d9 1550 if(orientationArrondie == 90 || orientationArrondie == 270) {
gabrieltetar 1:7e925468f9d9 1551 position_avant_gauche = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_avant_gauche;
gabrieltetar 1:7e925468f9d9 1552 position_avant_droite = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_avant_droite;
gabrieltetar 1:7e925468f9d9 1553 position_arriere_gauche = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?0:3000) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?1:-1)*telemetreDistance_arriere_gauche;
gabrieltetar 1:7e925468f9d9 1554 position_arriere_droite = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?0:3000) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?1:-1)*telemetreDistance_arriere_droite;
gabrieltetar 1:7e925468f9d9 1555
gabrieltetar 1:7e925468f9d9 1556 } else if(orientationArrondie == 0 || orientationArrondie == 180) {
gabrieltetar 1:7e925468f9d9 1557 position_avant_gauche = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_gauche;
gabrieltetar 1:7e925468f9d9 1558 position_avant_droite = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_droite;
gabrieltetar 1:7e925468f9d9 1559 position_arriere_gauche = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_gauche;
gabrieltetar 1:7e925468f9d9 1560 position_arriere_droite = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_droite;
gabrieltetar 1:7e925468f9d9 1561 }
gabrieltetar 1:7e925468f9d9 1562
gabrieltetar 1:7e925468f9d9 1563
gabrieltetar 1:7e925468f9d9 1564 if(orientationArrondie == 90 || orientationArrondie == 270) { // Si il est en axe Y
gabrieltetar 1:7e925468f9d9 1565 if(position_arriere_droite >= y_robot-instruction.arg1 && position_arriere_droite <= y_robot+instruction.arg1) { // Et que les mesures sont cohérentes
gabrieltetar 1:7e925468f9d9 1566 if(position_arriere_gauche >= y_robot-instruction.arg1 && position_arriere_gauche <= y_robot+instruction.arg1) {
gabrieltetar 1:7e925468f9d9 1567 if(telemetreDistance_arriere_droite > telemetreDistance_arriere_gauche)
gabrieltetar 1:7e925468f9d9 1568 angleArriere =900+(1800 * atan2((double)(telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche), (double)ESPACE_INTER_TELEMETRE ))/M_PI;
gabrieltetar 1:7e925468f9d9 1569 else
gabrieltetar 1:7e925468f9d9 1570 angleArriere =(1800 * atan2( (double) ESPACE_INTER_TELEMETRE,(double) (telemetreDistance_arriere_gauche-telemetreDistance_arriere_droite) ))/M_PI;
gabrieltetar 1:7e925468f9d9 1571
gabrieltetar 1:7e925468f9d9 1572 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
gabrieltetar 1:7e925468f9d9 1573 moyennageTelemetre += angleArriere;
gabrieltetar 1:7e925468f9d9 1574 }
gabrieltetar 1:7e925468f9d9 1575 }
gabrieltetar 1:7e925468f9d9 1576 } else if(orientationArrondie == 0 || orientationArrondie == 180) { // Si il est en axe X
gabrieltetar 1:7e925468f9d9 1577 if(position_arriere_droite >= x_robot-instruction.arg1 && position_arriere_droite <= x_robot+instruction.arg1) { // Et que les mesures sont cohérentes
gabrieltetar 1:7e925468f9d9 1578 if(position_arriere_gauche >= x_robot-instruction.arg1 && position_arriere_gauche <= x_robot+instruction.arg1) {
gabrieltetar 1:7e925468f9d9 1579 if(telemetreDistance_arriere_droite > telemetreDistance_arriere_gauche)
gabrieltetar 1:7e925468f9d9 1580 angleArriere =900+(1800 * atan2( (double) (telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche), (double) ESPACE_INTER_TELEMETRE ))/M_PI;
gabrieltetar 1:7e925468f9d9 1581 else
gabrieltetar 1:7e925468f9d9 1582 angleArriere =(1800 * atan2( (double) ESPACE_INTER_TELEMETRE,(double) (telemetreDistance_arriere_gauche-telemetreDistance_arriere_droite) ))/M_PI;
gabrieltetar 1:7e925468f9d9 1583
gabrieltetar 1:7e925468f9d9 1584 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
gabrieltetar 1:7e925468f9d9 1585 moyennageTelemetre += angleArriere;
gabrieltetar 1:7e925468f9d9 1586 }
gabrieltetar 1:7e925468f9d9 1587 }
gabrieltetar 1:7e925468f9d9 1588 }
gabrieltetar 1:7e925468f9d9 1589
gabrieltetar 1:7e925468f9d9 1590 if(orientationArrondie == 90 || orientationArrondie == 270) { // Si il est en axe Y
gabrieltetar 1:7e925468f9d9 1591 if(position_avant_droite >= y_robot-instruction.arg1 && position_avant_droite <= y_robot+instruction.arg1) { // Et que les mesures sont cohérentes
gabrieltetar 1:7e925468f9d9 1592 if(position_avant_gauche >= y_robot-instruction.arg1 && position_avant_gauche <= y_robot+instruction.arg1) {
gabrieltetar 1:7e925468f9d9 1593 if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche)
gabrieltetar 1:7e925468f9d9 1594 angleAvant = (1800 * atan2( (double) ESPACE_INTER_TELEMETRE,(double) (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) ))/M_PI;
gabrieltetar 1:7e925468f9d9 1595 else
gabrieltetar 1:7e925468f9d9 1596 angleAvant = 900 + (1800 * atan2( (double)( telemetreDistance_avant_gauche-telemetreDistance_avant_droite),(double) ESPACE_INTER_TELEMETRE ))/M_PI;
gabrieltetar 1:7e925468f9d9 1597
gabrieltetar 1:7e925468f9d9 1598 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
gabrieltetar 1:7e925468f9d9 1599 moyennageTelemetre += angleAvant;
gabrieltetar 1:7e925468f9d9 1600 }
gabrieltetar 1:7e925468f9d9 1601 }
gabrieltetar 1:7e925468f9d9 1602 } else if(orientationArrondie == 0 || orientationArrondie == 180) { // Si il est en axe X
gabrieltetar 1:7e925468f9d9 1603 if(position_avant_droite >= x_robot-instruction.arg1 && position_avant_droite <= x_robot+instruction.arg1) { // Et que les mesures sont cohérentes
gabrieltetar 1:7e925468f9d9 1604 if(position_avant_gauche >= x_robot-instruction.arg1 && position_avant_gauche <= x_robot+instruction.arg1) {
gabrieltetar 1:7e925468f9d9 1605 if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche)
gabrieltetar 1:7e925468f9d9 1606 angleAvant = (1800 * atan2((double) ESPACE_INTER_TELEMETRE, (double) (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) ))/M_PI;
gabrieltetar 1:7e925468f9d9 1607 else
gabrieltetar 1:7e925468f9d9 1608 angleAvant = 900 + (1800 * atan2( (double) (telemetreDistance_avant_gauche-telemetreDistance_avant_droite),(double) ESPACE_INTER_TELEMETRE ))/M_PI;
gabrieltetar 1:7e925468f9d9 1609
gabrieltetar 1:7e925468f9d9 1610 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
gabrieltetar 1:7e925468f9d9 1611 moyennageTelemetre += angleAvant;
gabrieltetar 1:7e925468f9d9 1612 }
gabrieltetar 1:7e925468f9d9 1613 }
gabrieltetar 1:7e925468f9d9 1614 }
gabrieltetar 1:7e925468f9d9 1615
gabrieltetar 1:7e925468f9d9 1616 angleRecalage = moyennageTelemetre/nombresDeMesuresAuxTelemetresQuiSontCoherentes;
gabrieltetar 1:7e925468f9d9 1617
gabrieltetar 1:7e925468f9d9 1618 if(nombresDeMesuresAuxTelemetresQuiSontCoherentes) {
gabrieltetar 1:7e925468f9d9 1619 if(orientationArrondie == 0) {
gabrieltetar 1:7e925468f9d9 1620 angleRecalage -= 900;
gabrieltetar 1:7e925468f9d9 1621
gabrieltetar 1:7e925468f9d9 1622 /*if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche)
gabrieltetar 1:7e925468f9d9 1623 distanceRecalage = *);
gabrieltetar 1:7e925468f9d9 1624 else
gabrieltetar 1:7e925468f9d9 1625 distanceRecalage = 900 + (1800 * atan( (double)( (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;*/
gabrieltetar 1:7e925468f9d9 1626 } else if(orientationArrondie == 90) {
gabrieltetar 1:7e925468f9d9 1627 angleRecalage += 0;
gabrieltetar 1:7e925468f9d9 1628 } else if(orientationArrondie == 180) {
gabrieltetar 1:7e925468f9d9 1629 angleRecalage += 900;
gabrieltetar 1:7e925468f9d9 1630 } else if(orientationArrondie == 270) {
gabrieltetar 1:7e925468f9d9 1631 angleRecalage += 1800;
gabrieltetar 1:7e925468f9d9 1632 }
gabrieltetar 1:7e925468f9d9 1633 }
gabrieltetar 1:7e925468f9d9 1634
gabrieltetar 1:7e925468f9d9 1635 return (nombresDeMesuresAuxTelemetresQuiSontCoherentes && (angleAvant-angleArriere<80 && angleAvant-angleArriere>-80)) ? angleRecalage : theta_robot;
gabrieltetar 1:7e925468f9d9 1636 }
gabrieltetar 1:7e925468f9d9 1637
gabrieltetar 1:7e925468f9d9 1638 short recalageDistanceX(void)
gabrieltetar 1:7e925468f9d9 1639 {
gabrieltetar 1:7e925468f9d9 1640 unsigned char nombresDeMesuresAuxTelemetresQuiSontCoherentes = 0;
gabrieltetar 1:7e925468f9d9 1641 unsigned int moyennageTelemetre = 0;
gabrieltetar 1:7e925468f9d9 1642
gabrieltetar 1:7e925468f9d9 1643 unsigned short tempo= telemetreDistance_arriere_gauche;
gabrieltetar 1:7e925468f9d9 1644 telemetreDistance_arriere_gauche=telemetreDistance_arriere_droite;
gabrieltetar 1:7e925468f9d9 1645 telemetreDistance_arriere_droite=tempo;
gabrieltetar 1:7e925468f9d9 1646
gabrieltetar 1:7e925468f9d9 1647 telemetreDistance_avant_gauche = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_gauche;
gabrieltetar 1:7e925468f9d9 1648 telemetreDistance_avant_droite = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_droite;
gabrieltetar 1:7e925468f9d9 1649 telemetreDistance_arriere_gauche = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_gauche;
gabrieltetar 1:7e925468f9d9 1650 telemetreDistance_arriere_droite = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_droite;
gabrieltetar 1:7e925468f9d9 1651
gabrieltetar 1:7e925468f9d9 1652 if(telemetreDistance_avant_gauche >= x_robot-instruction.arg1 && telemetreDistance_avant_gauche <= x_robot+instruction.arg1) {
gabrieltetar 1:7e925468f9d9 1653 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
gabrieltetar 1:7e925468f9d9 1654 moyennageTelemetre += telemetreDistance_avant_gauche;
gabrieltetar 1:7e925468f9d9 1655 }
gabrieltetar 1:7e925468f9d9 1656 if(telemetreDistance_avant_droite >= x_robot-instruction.arg1 && telemetreDistance_avant_droite <= x_robot+instruction.arg1) {
gabrieltetar 1:7e925468f9d9 1657 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
gabrieltetar 1:7e925468f9d9 1658 moyennageTelemetre += telemetreDistance_avant_droite;
gabrieltetar 1:7e925468f9d9 1659 }
gabrieltetar 1:7e925468f9d9 1660 if(telemetreDistance_arriere_gauche >= x_robot-instruction.arg1 && telemetreDistance_arriere_gauche <= x_robot+instruction.arg1) {
gabrieltetar 1:7e925468f9d9 1661 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
gabrieltetar 1:7e925468f9d9 1662 moyennageTelemetre += telemetreDistance_arriere_gauche;
gabrieltetar 1:7e925468f9d9 1663 }
gabrieltetar 1:7e925468f9d9 1664 if(telemetreDistance_arriere_droite >= x_robot-instruction.arg1 && telemetreDistance_arriere_droite <= x_robot+instruction.arg1) {
gabrieltetar 1:7e925468f9d9 1665 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
gabrieltetar 1:7e925468f9d9 1666 moyennageTelemetre += telemetreDistance_arriere_droite;
gabrieltetar 1:7e925468f9d9 1667 }
gabrieltetar 1:7e925468f9d9 1668
gabrieltetar 1:7e925468f9d9 1669 moyennageTelemetre /= nombresDeMesuresAuxTelemetresQuiSontCoherentes;
gabrieltetar 1:7e925468f9d9 1670
gabrieltetar 1:7e925468f9d9 1671 return (nombresDeMesuresAuxTelemetresQuiSontCoherentes)? moyennageTelemetre : x_robot; //SetOdometrie(ODOMETRIE_SMALL_POSITION, moyennageTelemetre, y_robot, theta_robot);
gabrieltetar 1:7e925468f9d9 1672 }
gabrieltetar 1:7e925468f9d9 1673
gabrieltetar 1:7e925468f9d9 1674 short recalageDistanceY(void)
gabrieltetar 1:7e925468f9d9 1675 {
gabrieltetar 1:7e925468f9d9 1676 unsigned char nombresDeMesuresAuxTelemetresQuiSontCoherentes = 0;
gabrieltetar 1:7e925468f9d9 1677 unsigned int moyennageTelemetre = 0;
gabrieltetar 1:7e925468f9d9 1678
gabrieltetar 1:7e925468f9d9 1679 unsigned short tempo= telemetreDistance_arriere_gauche;
gabrieltetar 1:7e925468f9d9 1680 telemetreDistance_arriere_gauche=telemetreDistance_arriere_droite;
gabrieltetar 1:7e925468f9d9 1681 telemetreDistance_arriere_droite=tempo;
gabrieltetar 1:7e925468f9d9 1682
gabrieltetar 1:7e925468f9d9 1683 telemetreDistance_avant_gauche = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_avant_gauche;
gabrieltetar 1:7e925468f9d9 1684 telemetreDistance_avant_droite = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?3000:0) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?-1:1)*telemetreDistance_avant_droite;
gabrieltetar 1:7e925468f9d9 1685 telemetreDistance_arriere_gauche = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?0:3000) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?1:-1)*telemetreDistance_arriere_gauche;
gabrieltetar 1:7e925468f9d9 1686 telemetreDistance_arriere_droite = (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?0:3000) + (((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600))?1:-1)*telemetreDistance_arriere_droite;
gabrieltetar 1:7e925468f9d9 1687
gabrieltetar 1:7e925468f9d9 1688 if(telemetreDistance_avant_gauche >= y_robot-instruction.arg1 && telemetreDistance_avant_gauche <= y_robot+instruction.arg1) {
gabrieltetar 1:7e925468f9d9 1689 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
gabrieltetar 1:7e925468f9d9 1690 moyennageTelemetre += telemetreDistance_avant_gauche;
gabrieltetar 1:7e925468f9d9 1691 }
gabrieltetar 1:7e925468f9d9 1692 if(telemetreDistance_avant_droite >= y_robot-instruction.arg1 && telemetreDistance_avant_droite <= y_robot+instruction.arg1) {
gabrieltetar 1:7e925468f9d9 1693 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
gabrieltetar 1:7e925468f9d9 1694 moyennageTelemetre += telemetreDistance_avant_droite;
gabrieltetar 1:7e925468f9d9 1695 }
gabrieltetar 1:7e925468f9d9 1696 if(telemetreDistance_arriere_gauche >= y_robot-instruction.arg1 && telemetreDistance_arriere_gauche <= y_robot+instruction.arg1) {
gabrieltetar 1:7e925468f9d9 1697 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
gabrieltetar 1:7e925468f9d9 1698 moyennageTelemetre += telemetreDistance_arriere_gauche;
gabrieltetar 1:7e925468f9d9 1699 }
gabrieltetar 1:7e925468f9d9 1700 if(telemetreDistance_arriere_droite >= y_robot-instruction.arg1 && telemetreDistance_arriere_droite <= y_robot+instruction.arg1) {
gabrieltetar 1:7e925468f9d9 1701 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
gabrieltetar 1:7e925468f9d9 1702 moyennageTelemetre += telemetreDistance_arriere_droite;
gabrieltetar 1:7e925468f9d9 1703 }
gabrieltetar 1:7e925468f9d9 1704
gabrieltetar 1:7e925468f9d9 1705 moyennageTelemetre /= nombresDeMesuresAuxTelemetresQuiSontCoherentes;
gabrieltetar 1:7e925468f9d9 1706
gabrieltetar 1:7e925468f9d9 1707 return (nombresDeMesuresAuxTelemetresQuiSontCoherentes)? moyennageTelemetre : y_robot ; // SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, moyennageTelemetre, theta_robot);
gabrieltetar 1:7e925468f9d9 1708 }
gabrieltetar 1:7e925468f9d9 1709
gabrieltetar 14:6aa8aa1699ad 1710 /*************************************************************************************************/
gabrieltetar 14:6aa8aa1699ad 1711 /* FUNCTION NAME: doAction */
gabrieltetar 14:6aa8aa1699ad 1712 /* DESCRIPTION : Effectuer une action specifique correspondant au numéro dans le fichier strat */
gabrieltetar 14:6aa8aa1699ad 1713 /*************************************************************************************************/
gabrieltetar 14:6aa8aa1699ad 1714
gabrieltetar 14:6aa8aa1699ad 1715 unsigned char doAction(unsigned char id, unsigned short arg1, short arg2)
gabrieltetar 14:6aa8aa1699ad 1716 {
gabrieltetar 14:6aa8aa1699ad 1717 int retour = 1;
gabrieltetar 14:6aa8aa1699ad 1718 CANMessage msgTx=CANMessage();
gabrieltetar 14:6aa8aa1699ad 1719 msgTx.format=CANStandard;
gabrieltetar 14:6aa8aa1699ad 1720 msgTx.type=CANData;
gabrieltetar 14:6aa8aa1699ad 1721 switch(id)
maximilienlv 17:1cf2043c5c01 1722 {
gabrieltetar 14:6aa8aa1699ad 1723 case 120:
maximilienlv 28:7f2990747e09 1724 wait_ms(500);
gabrieltetar 14:6aa8aa1699ad 1725 break;
gabrieltetar 14:6aa8aa1699ad 1726
gabrieltetar 14:6aa8aa1699ad 1727 case 121:
gabrieltetar 14:6aa8aa1699ad 1728 //SendRawId(ACCELERATEUR_INSERTION_ARRIERE_GAUCHE);
gabrieltetar 14:6aa8aa1699ad 1729 break;
gabrieltetar 14:6aa8aa1699ad 1730
gabrieltetar 14:6aa8aa1699ad 1731 case 150:
gabrieltetar 14:6aa8aa1699ad 1732 SCORE_PR+=arg1;
gabrieltetar 14:6aa8aa1699ad 1733 waitingAckFrom = 0;
gabrieltetar 14:6aa8aa1699ad 1734 waitingAckID = 0;
gabrieltetar 14:6aa8aa1699ad 1735 break;
gabrieltetar 14:6aa8aa1699ad 1736
gabrieltetar 14:6aa8aa1699ad 1737 case 200 :
gabrieltetar 14:6aa8aa1699ad 1738 SendRawId(DATA_TELEMETRE);
gabrieltetar 14:6aa8aa1699ad 1739 /*telemetreDistance = dataTelemetre();
gabrieltetar 14:6aa8aa1699ad 1740 wait_ms(1);
gabrieltetar 14:6aa8aa1699ad 1741 telemetreDistance = dataTelemetre();
gabrieltetar 14:6aa8aa1699ad 1742 telemetreDistance = telemetreDistance - 170;*/
gabrieltetar 14:6aa8aa1699ad 1743 break;
gabrieltetar 14:6aa8aa1699ad 1744
gabrieltetar 14:6aa8aa1699ad 1745 case 201 :
gabrieltetar 14:6aa8aa1699ad 1746 SendRawId(0x99);//
gabrieltetar 14:6aa8aa1699ad 1747 retour = 2;
gabrieltetar 14:6aa8aa1699ad 1748 break;
gabrieltetar 14:6aa8aa1699ad 1749
gabrieltetar 14:6aa8aa1699ad 1750
gabrieltetar 14:6aa8aa1699ad 1751 case 11://0 Désactiver le stop,1 Activer le stop saut de strat,2 Activer le stop avec evitement
gabrieltetar 14:6aa8aa1699ad 1752 isStopEnable =(unsigned char) arg1;
gabrieltetar 14:6aa8aa1699ad 1753 // SendMsgCan(0x5BC, &isStopEnable,1);
gabrieltetar 14:6aa8aa1699ad 1754 waitingAckFrom = 0;
gabrieltetar 14:6aa8aa1699ad 1755 waitingAckID =0;
gabrieltetar 14:6aa8aa1699ad 1756 break;
maximilienlv 20:2f0b1c48a35f 1757
maximilienlv 20:2f0b1c48a35f 1758 case 19: // CHANGER LA VITESSE + DECELERATION
maximilienlv 20:2f0b1c48a35f 1759 //SendSpeedDecel(arg1,(unsigned short) arg2);
maximilienlv 20:2f0b1c48a35f 1760 wait_us(200);
maximilienlv 20:2f0b1c48a35f 1761 waitingAckFrom = 0;
maximilienlv 20:2f0b1c48a35f 1762 waitingAckID =0;
maximilienlv 20:2f0b1c48a35f 1763 break;
maximilienlv 20:2f0b1c48a35f 1764
gabrieltetar 14:6aa8aa1699ad 1765 case 20://Désactiver l'asservissement
gabrieltetar 14:6aa8aa1699ad 1766 setAsservissementEtat(0);
gabrieltetar 14:6aa8aa1699ad 1767 break;
gabrieltetar 14:6aa8aa1699ad 1768
gabrieltetar 14:6aa8aa1699ad 1769 case 21://Activer l'asservissement
gabrieltetar 14:6aa8aa1699ad 1770 setAsservissementEtat(1);
gabrieltetar 14:6aa8aa1699ad 1771 break;
gabrieltetar 14:6aa8aa1699ad 1772
gabrieltetar 14:6aa8aa1699ad 1773 case 22://Changer la vitesse du robot
gabrieltetar 14:6aa8aa1699ad 1774 SendSpeed(arg1);//,(unsigned short)arg2, (unsigned short)arg2);
gabrieltetar 14:6aa8aa1699ad 1775 wait_us(200);
gabrieltetar 14:6aa8aa1699ad 1776 waitingAckFrom = 0;
gabrieltetar 14:6aa8aa1699ad 1777 waitingAckID = 0;
gabrieltetar 14:6aa8aa1699ad 1778 break;
maximilienlv 17:1cf2043c5c01 1779
gabrieltetar 14:6aa8aa1699ad 1780 case 23:
gabrieltetar 14:6aa8aa1699ad 1781 SendAccel(arg1,(unsigned short)arg2);//,(unsigned short)arg2, (unsigned short)arg2);
gabrieltetar 14:6aa8aa1699ad 1782 wait_us(200);
gabrieltetar 14:6aa8aa1699ad 1783 waitingAckFrom = 0;
gabrieltetar 14:6aa8aa1699ad 1784 waitingAckID = 0;
gabrieltetar 14:6aa8aa1699ad 1785 break;
maximilienlv 20:2f0b1c48a35f 1786
goldmas 38:9d6a3ccc0582 1787 case 151: //Bras attraper
goldmas 33:c54841d11156 1788 unsigned char argu_at_bras = arg1;
goldmas 35:2a745eeb7922 1789 if(arg1 == 543) argu_at_bras = 66;
goldmas 33:c54841d11156 1790 SendMsgCan(BRAS_AT, &argu_at_bras,sizeof(arg1));
goldmas 33:c54841d11156 1791 waitingAckFrom = 0;
goldmas 33:c54841d11156 1792 waitingAckID =0;
gabrieltetar 14:6aa8aa1699ad 1793 break;
maximilienlv 16:c6c765e8b982 1794
goldmas 38:9d6a3ccc0582 1795 case 152: //Bras relacher
goldmas 33:c54841d11156 1796 unsigned char argu_re_bras = arg1;
goldmas 35:2a745eeb7922 1797 if(arg1 == 543) argu_re_bras = 66;
goldmas 33:c54841d11156 1798 SendMsgCan(BRAS_RE, &argu_re_bras,sizeof(arg1));
goldmas 38:9d6a3ccc0582 1799 SendRawId(0x050);
goldmas 38:9d6a3ccc0582 1800 Flag_Bras_Re = 1;
goldmas 38:9d6a3ccc0582 1801 Flag_num_bras = argu_re_bras;
goldmas 35:2a745eeb7922 1802 waitingAckFrom = 0;
goldmas 35:2a745eeb7922 1803 waitingAckID =0;
goldmas 35:2a745eeb7922 1804 break;
maximilienlv 20:2f0b1c48a35f 1805
goldmas 38:9d6a3ccc0582 1806 case 153: //Ventouse attraper
maximilienlv 20:2f0b1c48a35f 1807 unsigned char argu_at_vent = arg1;
goldmas 35:2a745eeb7922 1808 if(arg1 == 543) argu_at_vent = 66;
maximilienlv 20:2f0b1c48a35f 1809 SendMsgCan(VENT_AT, &argu_at_vent,sizeof(arg1));
maximilienlv 16:c6c765e8b982 1810 waitingAckFrom = 0;
maximilienlv 16:c6c765e8b982 1811 waitingAckID =0;
maximilienlv 20:2f0b1c48a35f 1812 break;
maximilienlv 20:2f0b1c48a35f 1813
goldmas 38:9d6a3ccc0582 1814 case 154: //Ventouse Relacher
maximilienlv 20:2f0b1c48a35f 1815 unsigned char argu_re_vent = arg1;
goldmas 35:2a745eeb7922 1816 if(arg1 == 543) argu_re_vent = 66;
maximilienlv 20:2f0b1c48a35f 1817 SendMsgCan(VENT_RE, &argu_re_vent,sizeof(arg1));
maximilienlv 20:2f0b1c48a35f 1818 waitingAckFrom = 0;
maximilienlv 20:2f0b1c48a35f 1819 waitingAckID =0;
maximilienlv 20:2f0b1c48a35f 1820 break;
gabrieltetar 14:6aa8aa1699ad 1821
goldmas 38:9d6a3ccc0582 1822 case 157: //Manche à air bas
goldmas 35:2a745eeb7922 1823 unsigned char argu_manche_bas = arg1;
goldmas 35:2a745eeb7922 1824 SendMsgCan(AUTOMATE_MANCHE_BAS, &argu_manche_bas,sizeof(arg1));
goldmas 38:9d6a3ccc0582 1825 Flag_Manche_Bas = 1;
goldmas 38:9d6a3ccc0582 1826 Flag_Manche_Moy = 1;
goldmas 35:2a745eeb7922 1827 waitingAckFrom = 0;
goldmas 35:2a745eeb7922 1828 waitingAckID =0;
goldmas 35:2a745eeb7922 1829 break;
goldmas 35:2a745eeb7922 1830
goldmas 38:9d6a3ccc0582 1831 case 158: //Manche à air moyen
goldmas 38:9d6a3ccc0582 1832 unsigned char argu_manche_moy = arg1;
goldmas 38:9d6a3ccc0582 1833 SendMsgCan(AUTOMATE_MANCHE_MOY,&argu_manche_moy,sizeof(arg1));
goldmas 38:9d6a3ccc0582 1834 Flag_Manche_Bas = 1;
goldmas 38:9d6a3ccc0582 1835 Flag_Manche_Moy = 1;
goldmas 38:9d6a3ccc0582 1836 waitingAckFrom = 0;
goldmas 38:9d6a3ccc0582 1837 waitingAckID =0;
goldmas 38:9d6a3ccc0582 1838 break;
goldmas 38:9d6a3ccc0582 1839
goldmas 38:9d6a3ccc0582 1840 case 159: //Manche à air haut
goldmas 33:c54841d11156 1841 unsigned char argu_manche_haut = arg1;
goldmas 33:c54841d11156 1842 SendMsgCan(AUTOMATE_MANCHE_HAUT, &argu_manche_haut,sizeof(arg1));
goldmas 33:c54841d11156 1843 waitingAckFrom = 0;
goldmas 33:c54841d11156 1844 waitingAckID =0;
goldmas 35:2a745eeb7922 1845 break;
goldmas 33:c54841d11156 1846
goldmas 38:9d6a3ccc0582 1847 case 160 : //lecture de la girouette
goldmas 38:9d6a3ccc0582 1848 SendRawId(LECTURE_GIROUETTE) ;
goldmas 38:9d6a3ccc0582 1849 debug_bon_port = 1;
goldmas 38:9d6a3ccc0582 1850 wait_ms(100);
goldmas 38:9d6a3ccc0582 1851 break;
goldmas 38:9d6a3ccc0582 1852
goldmas 38:9d6a3ccc0582 1853 case 161 : //aller dans le bon port de fin de match
goldmas 38:9d6a3ccc0582 1854 Flag_bon_port = 1;
goldmas 38:9d6a3ccc0582 1855 waitingAckFrom = 0;
goldmas 38:9d6a3ccc0582 1856 waitingAckID =0;
goldmas 38:9d6a3ccc0582 1857 break ;
goldmas 38:9d6a3ccc0582 1858
goldmas 38:9d6a3ccc0582 1859 case 162 : //attendre le temps indiqué en seconde
goldmas 38:9d6a3ccc0582 1860 debug_bon_port = 2;
goldmas 38:9d6a3ccc0582 1861 timer.attach(&wait_ISR, arg1);
goldmas 38:9d6a3ccc0582 1862 break ;
goldmas 38:9d6a3ccc0582 1863
goldmas 38:9d6a3ccc0582 1864 case 163 : //bras prépare à poser
goldmas 38:9d6a3ccc0582 1865 unsigned char argu_prepa_bras = arg1;
goldmas 38:9d6a3ccc0582 1866 if(arg1 == 543) argu_prepa_bras = 66;
goldmas 38:9d6a3ccc0582 1867 SendMsgCan(BRAS_PREPA, &argu_prepa_bras,sizeof(arg1));
goldmas 38:9d6a3ccc0582 1868 waitingAckFrom = 0;
goldmas 38:9d6a3ccc0582 1869 waitingAckID =0;
goldmas 38:9d6a3ccc0582 1870 break ;
goldmas 38:9d6a3ccc0582 1871
goldmas 38:9d6a3ccc0582 1872 case 164 : //bras pose les gobelets en préparation
goldmas 38:9d6a3ccc0582 1873 unsigned char argu_pose_bras = arg1;
goldmas 38:9d6a3ccc0582 1874 if(arg1 == 543) argu_pose_bras = 66;
goldmas 38:9d6a3ccc0582 1875 SendMsgCan(BRAS_POSE, &argu_pose_bras,sizeof(arg1));
goldmas 38:9d6a3ccc0582 1876 Flag_Bras_Re = 1;
goldmas 38:9d6a3ccc0582 1877 Flag_num_bras = argu_re_bras;
goldmas 38:9d6a3ccc0582 1878 waitingAckFrom = 0;
goldmas 38:9d6a3ccc0582 1879 waitingAckID =0;
goldmas 38:9d6a3ccc0582 1880 break ;
goldmas 38:9d6a3ccc0582 1881
gabrieltetar 14:6aa8aa1699ad 1882 default:
gabrieltetar 14:6aa8aa1699ad 1883 retour = 0;//L'action n'existe pas, il faut utiliser le CAN
maximilienlv 16:c6c765e8b982 1884 break;
gabrieltetar 14:6aa8aa1699ad 1885 }
gabrieltetar 14:6aa8aa1699ad 1886 return retour;//L'action est spécifique.
gabrieltetar 14:6aa8aa1699ad 1887
maximilienlv 17:1cf2043c5c01 1888 }