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@36:c37dbe2be916, 2021-05-31 (annotated)

Committer:
goldmas
Date:
Mon May 31 13:36:03 2021 +0000
Revision:
36:c37dbe2be916
Parent:
35:2a745eeb7922
Child:
37:f1a8734c193d
Child:
38:9d6a3ccc0582
dernier code vallable;

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