CRAC Team / Mbed 2 deprecated code_carte_IHM_sans_bug

code de la carte IHM avant les bugs et avant le travail effectué avec Melchior

Dependencies:   mbed SerialHalfDuplex SDFileSystem liaison_Bluetooth ident_crac DISCO-F469NI_portrait

Strategie/Strategie.cpp@37:f1a8734c193d, 2021-07-17 (annotated)

Committer:
goldmas
Date:
Sat Jul 17 16:07:29 2021 +0000
Revision:
37:f1a8734c193d
Parent:
36:c37dbe2be916 
code de l'IHM avant bug

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