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@22:c7763a7ec6c7, 2020-06-26 (annotated)

Committer:
gabrieltetar
Date:
Fri Jun 26 13:08:54 2020 +0000
Revision:
22:c7763a7ec6c7
Parent:
21:d137ec53c3a9
Child:
24:1a13c998c7ac
IHM VERSION TETAR

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