Programme carte strategie (disco)

Dependencies:   mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac

Committer:
Artiom
Date:
Tue May 21 14:38:09 2019 +0000
Revision:
52:a47350923b5e
Parent:
51:aa6e09f2cfec
Child:
53:e96acb11a51f

        

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Artiom 44:badcbe8766e9 1 #include "global.h"
Sitkah 31:833fc481b002 2 #include <string.h>
Artiom 44:badcbe8766e9 3 #include <sstream>
Artiom 52:a47350923b5e 4 #include <math.h>
Sitkah 29:41e02746041d 5 //#include "StrategieManager.h"
Sitkah 29:41e02746041d 6
Sitkah 29:41e02746041d 7
Sitkah 29:41e02746041d 8
Sitkah 29:41e02746041d 9 #define M_PI 3.14159265358979323846
Sitkah 35:742dc6b200b0 10 #define VERT 0xFF00FF00
Sitkah 29:41e02746041d 11 #define ROUGE 0xFFFF0000
Sitkah 35:742dc6b200b0 12 #define BLEU 0xFF0000FF
Sitkah 29:41e02746041d 13 #define JAUNE 0xFFFEFE00
Sitkah 29:41e02746041d 14 #define BLANC 0xFF000000
Sitkah 31:833fc481b002 15 #define ORANGE 0xFFFFA500
Sitkah 31:833fc481b002 16 #define NOIR 0xFF000000
Artiom 44:badcbe8766e9 17 #define DIY_GREY 0xFFDFDFDF
antbig 0:ad97421fb1fb 18
Sitkah 29:41e02746041d 19 char tableau_aff[10][50];
Artiom 44:badcbe8766e9 20 char tableau_etat[22][50]= {
Sitkah 29:41e02746041d 21 "Check_carte_screen",
Sitkah 29:41e02746041d 22 "Check_carte_screen_wait_ack",
Sitkah 29:41e02746041d 23 "Check_cartes",
Sitkah 29:41e02746041d 24 "Check_cartes_wait_ack",
Sitkah 29:41e02746041d 25 "Wait_force",
Sitkah 29:41e02746041d 26 "Config",
Sitkah 29:41e02746041d 27 "Game_init",
Sitkah 29:41e02746041d 28 "Game_wait_for_jack",
Sitkah 29:41e02746041d 29 "Game_start",
Sitkah 29:41e02746041d 30 "Game_next_instruction",
Sitkah 29:41e02746041d 31 "Game_instruction",
Sitkah 29:41e02746041d 32 "Game_wait_ack",
Sitkah 29:41e02746041d 33 "Game_jump_time",
Sitkah 29:41e02746041d 34 "Game_jump_config",
Sitkah 29:41e02746041d 35 "Game_jump_position",
Sitkah 29:41e02746041d 36 "Game_wait_end_instruction",
Sitkah 29:41e02746041d 37 "Warning_timeout",
Sitkah 29:41e02746041d 38 "Waring_end_balise_wait",
Sitkah 29:41e02746041d 39 "Warning_end_last_instruction",
Sitkah 29:41e02746041d 40 "Warning_switch_strategie",
Sitkah 29:41e02746041d 41 "End",
Sitkah 29:41e02746041d 42 "End_loop",
Sitkah 29:41e02746041d 43 };
Sitkah 29:41e02746041d 44
Sitkah 34:6aa4b46b102e 45 int waitingAckID_FIN;
Sitkah 34:6aa4b46b102e 46 int waitingAckFrom_FIN;
Sitkah 29:41e02746041d 47
Sitkah 29:41e02746041d 48 Ticker ticker;
Sitkah 29:41e02746041d 49 TS_DISCO_F469NI ts;
Sitkah 29:41e02746041d 50 LCD_DISCO_F469NI lcd;
Sitkah 29:41e02746041d 51
Sitkah 29:41e02746041d 52 TS_StateTypeDef TS_State;
Sitkah 29:41e02746041d 53
Sitkah 29:41e02746041d 54 Ticker chrono;
Sitkah 29:41e02746041d 55 Timeout AffTime;
Sitkah 29:41e02746041d 56 Timer timer;
antbig 0:ad97421fb1fb 57 Timer cartesCheker;//Le timer pour le timeout de la vérification des cartes
antbig 0:ad97421fb1fb 58 Timer gameTimer;
antbig 0:ad97421fb1fb 59 Timer debugetatTimer;
antbig 5:dcd817534b57 60 Timer timeoutWarning;
antbig 5:dcd817534b57 61 Timer timeoutWarningWaitEnd;
Artiom 44:badcbe8766e9 62 Timeout chronoEnd;//permet d'envoyer la trame CAN pour la fin
antbig 0:ad97421fb1fb 63
Sitkah 29:41e02746041d 64 unsigned char screenChecktry = 0;
Sitkah 29:41e02746041d 65 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};
Sitkah 29:41e02746041d 66
Sitkah 29:41e02746041d 67 char counter = 0;
Sitkah 29:41e02746041d 68 char check;
Sitkah 29:41e02746041d 69 char Jack = 1;
Sitkah 40:21bb685b553b 70 short SCORE_GLOBAL=0;
Sitkah 40:21bb685b553b 71 short SCORE_GR=20;
Sitkah 38:76f886a1c8e6 72 short SCORE_PR=0;
Sitkah 29:41e02746041d 73
Sitkah 29:41e02746041d 74 int flag = 0, flag_strat = 0, flag_timer;
Sitkah 29:41e02746041d 75 char Ack_strat = 0;
Sitkah 29:41e02746041d 76 signed char Strat = 0;
antbig 0:ad97421fb1fb 77 signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
antbig 0:ad97421fb1fb 78
antbig 0:ad97421fb1fb 79 signed short x_robot,y_robot,theta_robot;//La position du robot
ClementBreteau 25:f140c93a8666 80 signed short target_x_robot, target_y_robot, target_theta_robot;
ClementBreteau 25:f140c93a8666 81 E_InstructionType actionPrecedente;
antbig 12:14729d584500 82 signed short start_move_x,start_move_y,start_move_theta;//La position du robot lors du début d'un mouvement, utilisé pour reprendre le mouvement apres stop balise
Sitkah 29:41e02746041d 83 //unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
Sitkah 29:41e02746041d 84 int flagSendCan=1;
Sitkah 34:6aa4b46b102e 85 unsigned char Cote = 0; //0 -> VERT | 1 -> jaune
Sitkah 35:742dc6b200b0 86 unsigned short angleRecalage = 0;
antbig 0:ad97421fb1fb 87 unsigned char checkCurrent = 0;
Artiom 44:badcbe8766e9 88 unsigned char countAliveCard = 0;
Sitkah 35:742dc6b200b0 89 unsigned char ligne=0;
antbig 0:ad97421fb1fb 90
Artiom 44:badcbe8766e9 91
Artiom 44:badcbe8766e9 92 int Fevitement=0;
Artiom 44:badcbe8766e9 93 int EvitEtat= 0;
Artiom 44:badcbe8766e9 94 int stop_evitement=0;
Artiom 44:badcbe8766e9 95
Artiom 44:badcbe8766e9 96
Sitkah 37:fca332b64b42 97 float angle_moyen_balise_IR = 0.0;
Sitkah 37:fca332b64b42 98
Sitkah 37:fca332b64b42 99
Sitkah 29:41e02746041d 100 signed char Strategie = 0; //N° de la strategie (1-10)
antbig 1:116040d14164 101
ClementBreteau 14:c8fc06c4887f 102 unsigned char ModeDemo = 0; // Si à 1, indique que l'on est dans le mode demo
ClementBreteau 14:c8fc06c4887f 103
antbig 5:dcd817534b57 104 unsigned char countRobotNear = 0;//Le nombre de robot à proximité
antbig 5:dcd817534b57 105
Artiom 44:badcbe8766e9 106 unsigned char ingnorBaliseOnce = 0;//une fois détecté réinitialise
Artiom 44:badcbe8766e9 107 unsigned char ingnorBalise = 0;//0:balise ignore 1:on ecoute la balise
Artiom 44:badcbe8766e9 108 unsigned char robot_arrete = 0;
antbig 12:14729d584500 109
antbig 28:acd18776ed2d 110 unsigned char ingnorInversionOnce = 0;//Pour ignorer l'inversion des instruction une fois
antbig 28:acd18776ed2d 111
antbig 28:acd18776ed2d 112 struct S_Instruction instruction;
antbig 28:acd18776ed2d 113
Sitkah 30:a1e37af4bbde 114 char couleur1, couleur2, couleur3;
Artiom 44:badcbe8766e9 115 float cptf;
Artiom 44:badcbe8766e9 116 int cpt,cpt1;
Sitkah 29:41e02746041d 117
Sitkah 32:1c9ab15c740e 118 typedef enum {INIT, ATT, CHOIX, DEMO, TEST_TELEMETRE, TEST_CAPTEURS, TEST_SERVO, TEST_TIR, DEMO_IMMEUBLE,DEMO_TRIEUR, SELECT_SIDE, TACTIQUE, DETAILS,LECTURE, LAUNCH, AFF_WAIT_JACK, WAIT_JACK, COMPTEUR, FIN} T_etat;
Sitkah 29:41e02746041d 119 T_etat etat = INIT;
Sitkah 29:41e02746041d 120 E_stratGameEtat gameEtat = ETAT_CHECK_CARTES;
Sitkah 29:41e02746041d 121 E_stratGameEtat lastEtat = ETAT_CHECK_CARTES;
Sitkah 38:76f886a1c8e6 122 E_Stratposdebut etat_pos=RECALAGE_1;
Sitkah 29:41e02746041d 123
Sitkah 29:41e02746041d 124 /////////////////DEFINITION DES BOUTONS////////////////////
Villanut 45:4f93e99bac6e 125 Button COTE_VERT(0, 25, 400, 300, "JAUNE");
Villanut 45:4f93e99bac6e 126 Button COTE_ORANGE(0, 350, 400, 300, "VIOLET");
Artiom 44:badcbe8766e9 127 Button RETOUR (0, 680, 400, 110, "--Precedent--");
Artiom 44:badcbe8766e9 128 Button LANCER (0, 200, 400, 200, "--LANCER--");
Artiom 44:badcbe8766e9 129 Button CHECK (0, 420, 400, 200, "Valider");
Artiom 44:badcbe8766e9 130 Button MATCH (0, 50, 400, 320, "Match");
Artiom 44:badcbe8766e9 131 Button DEMONSTRATION (0, 400, 400, 320, "Demo");
Artiom 44:badcbe8766e9 132 Button TEST_HERKULEX(0, 25, 400, 100, "Test servos");
Artiom 44:badcbe8766e9 133 Button TEST_LASER(0, 135, 400, 100, "Test telemetre");
Artiom 44:badcbe8766e9 134 Button TEST_COULEURS(0,245,400,100,"Test capteurs");
Artiom 44:badcbe8766e9 135 Button TEST_TIR_BALLE(0,355,400,100,"Test Lanceur");
Artiom 44:badcbe8766e9 136 Button TEST_IMMEUBLE(0,465,400,100,"Test immeuble");
Artiom 44:badcbe8766e9 137 Button TEST_TRIEUR(0,575,400,100,"Test aiguilleur");
Artiom 44:badcbe8766e9 138 Button TIR_CHATEAU(0, 25, 400, 100, "Tir chateau");
Artiom 44:badcbe8766e9 139 Button EPURATION(0, 150, 400, 100, "epuration");
Artiom 44:badcbe8766e9 140 Button LANCEUR_ON(0,275,400,100,"allumer le lanceur");
Artiom 44:badcbe8766e9 141 Button LANCEUR_OFF(0,400,400,100,"eteindre le lanceur");
Artiom 44:badcbe8766e9 142 Button ABAISSE_BLOC(0, 25, 400, 100, "Ramasser blocs");
Artiom 44:badcbe8766e9 143 Button RELEVE_BLOC(0, 135, 400, 100, "lacher blocs");
Artiom 44:badcbe8766e9 144 Button BRAS_ABEILLE_ON(0,245,400,100,"bras abeille");
Artiom 44:badcbe8766e9 145 Button BRAS_ABEILLE_OFF(0,355,400,100,"baisser bras abeille");
Artiom 44:badcbe8766e9 146 Button INTERRUPTEUR_ON(0,465,400,100,"baisser bras interrupt");
Artiom 44:badcbe8766e9 147 Button INTERRUPTEUR_OFF(0,575,400,100,"baisser bras interrupt");
Artiom 44:badcbe8766e9 148 Button FORCE_LAUNCH(0, 50, 400, 320, "Force Launch");
Artiom 44:badcbe8766e9 149 Button TRI(0, 25, 400, 100, "Test tri");
Artiom 44:badcbe8766e9 150 Button AIGUILLEUR_D(0, 150, 400, 100, "aiguilleur droite");
Artiom 44:badcbe8766e9 151 Button AIGUILLEUR_G(0,275,400,100,"aiguilleur gauche");
Artiom 44:badcbe8766e9 152 Button AIGUILLEUR_CTRE(0,400,400,100,"aiguilleur centre");
Artiom 44:badcbe8766e9 153 Button SUIVANT(0,380,200,100,"Suivant");
Artiom 44:badcbe8766e9 154 Button COLOR_ORANGE (0, 230, 190, 110,"");
Artiom 44:badcbe8766e9 155 Button COLOR_JAUNE (210, 230, 190, 110,"");
Artiom 44:badcbe8766e9 156 Button COLOR_VERT (0, 350, 190, 110,"");
Artiom 44:badcbe8766e9 157 Button COLOR_BLEU (210, 350, 190, 110,"");
Artiom 44:badcbe8766e9 158 Button COLOR_NOIR (105, 470, 190, 110,"");
Artiom 44:badcbe8766e9 159 ////////////////////////////////////////////////////////////
Sitkah 29:41e02746041d 160
Sitkah 29:41e02746041d 161 void SendRawId (unsigned short id);
Sitkah 29:41e02746041d 162 void SelectionStrat (unsigned char numeroStrat);
Sitkah 29:41e02746041d 163 void Setflag(void);
Sitkah 29:41e02746041d 164 void can2Rx_ISR(void);
Sitkah 29:41e02746041d 165 signed char Bouton_Strat (void);
Sitkah 41:b029ddc4d60e 166 signed char blocage_balise;
Sitkah 29:41e02746041d 167 void print_segment(int nombre, int decalage);
Sitkah 29:41e02746041d 168 void affichage_compteur (int nombre);
Sitkah 29:41e02746041d 169 void effacer_segment(long couleur);
Sitkah 29:41e02746041d 170
Sitkah 30:a1e37af4bbde 171 unsigned short telemetreDistance=0;
Sitkah 34:6aa4b46b102e 172 unsigned short telemetreDistance_avant_gauche=0;
Sitkah 34:6aa4b46b102e 173 unsigned short telemetreDistance_avant_droite=0;
Sitkah 34:6aa4b46b102e 174 unsigned short telemetreDistance_arriere_gauche=0;
Sitkah 34:6aa4b46b102e 175 unsigned short telemetreDistance_arriere_droite=0;
Sitkah 29:41e02746041d 176
Sitkah 29:41e02746041d 177 #ifdef ROBOT_BIG
Sitkah 29:41e02746041d 178
Sitkah 29:41e02746041d 179
Sitkah 38:76f886a1c8e6 180 unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR,CHECK_BALISE};
Sitkah 38:76f886a1c8e6 181 unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR,ALIVE_BALISE};
Sitkah 29:41e02746041d 182
Artiom 50:a5361ffeefc8 183 InterruptIn jack(PG_11); // entrée numerique en interruption pour le jack
Sitkah 29:41e02746041d 184 #else
Sitkah 29:41e02746041d 185
Sitkah 29:41e02746041d 186
Sitkah 38:76f886a1c8e6 187 unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR,CHECK_BALISE};
Sitkah 38:76f886a1c8e6 188 unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR,ALIVE_BALISE};
Artiom 50:a5361ffeefc8 189 InterruptIn jack(PG_11); // entrée numerique en interruption pour le jack
Sitkah 29:41e02746041d 190
Sitkah 29:41e02746041d 191
Sitkah 29:41e02746041d 192 #endif
Sitkah 29:41e02746041d 193
Sitkah 29:41e02746041d 194
Artiom 44:badcbe8766e9 195
Artiom 44:badcbe8766e9 196
Sitkah 29:41e02746041d 197
ClementBreteau 14:c8fc06c4887f 198
antbig 4:88431b537477 199 /****************************************************************************************/
antbig 4:88431b537477 200 /* FUNCTION NAME: chronometre_ISR */
antbig 4:88431b537477 201 /* DESCRIPTION : Interruption à la fin des 90s du match */
antbig 4:88431b537477 202 /****************************************************************************************/
antbig 0:ad97421fb1fb 203 void chronometre_ISR (void)
antbig 0:ad97421fb1fb 204 {
antbig 0:ad97421fb1fb 205 SendRawId(ASSERVISSEMENT_STOP);//On stope les moteurs
antbig 0:ad97421fb1fb 206 SendRawId(GLOBAL_GAME_END);//Indication fin de match
Sitkah 29:41e02746041d 207 etat=FIN;
antbig 0:ad97421fb1fb 208 gameTimer.stop();//Arret du timer
Artiom 44:badcbe8766e9 209
antbig 1:116040d14164 210 while(1);//On bloque la programme dans l'interruption
antbig 0:ad97421fb1fb 211 }
antbig 0:ad97421fb1fb 212
Sitkah 29:41e02746041d 213
Sitkah 29:41e02746041d 214
antbig 4:88431b537477 215 /****************************************************************************************/
antbig 8:0edc7dfb7f7e 216 /* FUNCTION NAME: jack_ISR */
antbig 8:0edc7dfb7f7e 217 /* DESCRIPTION : Interruption en changement d'état sur le Jack */
antbig 8:0edc7dfb7f7e 218 /****************************************************************************************/
antbig 8:0edc7dfb7f7e 219 void jack_ISR (void)
antbig 8:0edc7dfb7f7e 220 {
antbig 8:0edc7dfb7f7e 221 if(gameEtat == ETAT_GAME_WAIT_FOR_JACK) {
antbig 8:0edc7dfb7f7e 222 gameEtat = ETAT_GAME_START;//On débute le match
Sitkah 29:41e02746041d 223 etat=COMPTEUR;
Sitkah 41:b029ddc4d60e 224 blocage_balise=1;
antbig 8:0edc7dfb7f7e 225 }
antbig 8:0edc7dfb7f7e 226 }
antbig 8:0edc7dfb7f7e 227
antbig 8:0edc7dfb7f7e 228 /****************************************************************************************/
Sitkah 29:41e02746041d 229 /* FUNCTION NAME: SelectionStrat */
Sitkah 29:41e02746041d 230 /* DESCRIPTION : Affiche la Stratégie sélectionnée sur l'ihm */
Sitkah 29:41e02746041d 231 /****************************************************************************************/
Sitkah 29:41e02746041d 232
Sitkah 29:41e02746041d 233
Sitkah 29:41e02746041d 234 void SelectionStrat (unsigned char Strategie)
Sitkah 29:41e02746041d 235 {
Sitkah 29:41e02746041d 236 lcd.SetBackColor(LCD_COLOR_WHITE);
Sitkah 29:41e02746041d 237 lcd.SetTextColor(LCD_COLOR_BLACK);
Artiom 44:badcbe8766e9 238
Artiom 44:badcbe8766e9 239 switch (Strategie+1) {
Sitkah 29:41e02746041d 240 case 0x1 :
Sitkah 29:41e02746041d 241 //description de Strategie n°1
Sitkah 29:41e02746041d 242 lcd.DisplayStringAt(150, 0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
Sitkah 29:41e02746041d 243 break;
Artiom 44:badcbe8766e9 244
Sitkah 29:41e02746041d 245 case 0x2 :
Sitkah 29:41e02746041d 246 //description de Strategie n°2
Sitkah 29:41e02746041d 247 lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
Sitkah 29:41e02746041d 248 break;
Artiom 44:badcbe8766e9 249
Sitkah 29:41e02746041d 250 case 0x3 :
Sitkah 29:41e02746041d 251 //description de Strategie n°3
Sitkah 29:41e02746041d 252 lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
Sitkah 29:41e02746041d 253 break;
Artiom 44:badcbe8766e9 254
Sitkah 29:41e02746041d 255 case 0x4 :
Sitkah 29:41e02746041d 256 //description de Strategie n°4
Sitkah 29:41e02746041d 257 lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
Sitkah 29:41e02746041d 258 break;
Artiom 44:badcbe8766e9 259
Sitkah 29:41e02746041d 260 case 0x5 :
Sitkah 29:41e02746041d 261 //description de Strategie n°5
Sitkah 29:41e02746041d 262 lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
Sitkah 29:41e02746041d 263 break;
Artiom 44:badcbe8766e9 264
Sitkah 29:41e02746041d 265 case 0x6 :
Sitkah 29:41e02746041d 266 //description de Strategie n°5
Sitkah 29:41e02746041d 267 lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
Sitkah 29:41e02746041d 268 break;
Artiom 44:badcbe8766e9 269
Sitkah 29:41e02746041d 270 case 0x7 :
Sitkah 29:41e02746041d 271 //description de Strategie n°5
Sitkah 29:41e02746041d 272 lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
Sitkah 29:41e02746041d 273 break;
Artiom 44:badcbe8766e9 274
Sitkah 29:41e02746041d 275 case 0x8 :
Sitkah 29:41e02746041d 276 //description de Strategie n°5
Sitkah 29:41e02746041d 277 lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
Sitkah 29:41e02746041d 278 break;
Artiom 44:badcbe8766e9 279
Sitkah 29:41e02746041d 280 case 0x9 :
Sitkah 29:41e02746041d 281 //description de Strategie n°5
Sitkah 29:41e02746041d 282 lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
Sitkah 29:41e02746041d 283 break;
Artiom 44:badcbe8766e9 284
Sitkah 29:41e02746041d 285 case 0xA :
Sitkah 29:41e02746041d 286 //description de Strategie n°5
Sitkah 29:41e02746041d 287 lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE);
Sitkah 29:41e02746041d 288 break;
Artiom 44:badcbe8766e9 289 }
Sitkah 29:41e02746041d 290 }
Sitkah 29:41e02746041d 291
Sitkah 29:41e02746041d 292 void Setflag(void)
Sitkah 29:41e02746041d 293 {
Sitkah 29:41e02746041d 294 flagSendCan = 1;
Sitkah 29:41e02746041d 295 }
Sitkah 29:41e02746041d 296
Sitkah 42:657b6a573e11 297
Artiom 44:badcbe8766e9 298 //Affiche une variable sur l'écran tactile//
Artiom 44:badcbe8766e9 299 void affichage_var(double Var)
Artiom 44:badcbe8766e9 300 {
Sitkah 35:742dc6b200b0 301 if(ligne==7)
Sitkah 35:742dc6b200b0 302 ligne=0;
Sitkah 35:742dc6b200b0 303 char aff[10]="toto";
Sitkah 35:742dc6b200b0 304 sprintf(aff,"%lf ",Var);
Sitkah 40:21bb685b553b 305 lcd.DisplayStringAt(120, LINE(20+(ligne)), (uint8_t *)aff, LEFT_MODE);
Sitkah 38:76f886a1c8e6 306 //ligne++;
Artiom 44:badcbe8766e9 307
Sitkah 35:742dc6b200b0 308 }
Sitkah 42:657b6a573e11 309
Sitkah 42:657b6a573e11 310
Sitkah 42:657b6a573e11 311 /****************************************************************************************/
Sitkah 42:657b6a573e11 312 /* FUNCTION NAME: affichage_debug */
Sitkah 42:657b6a573e11 313 /* DESCRIPTION : Affiche l'état de gameEtat sur l'écran lcd */
Artiom 44:badcbe8766e9 314 /****************************************************************************************/
Artiom 44:badcbe8766e9 315 void affichage_debug(int Var)
Artiom 44:badcbe8766e9 316 {
Sitkah 29:41e02746041d 317 int i;
Sitkah 29:41e02746041d 318 int conv=(int)Var;
Sitkah 29:41e02746041d 319 SUIVANT.Draw(ROUGE, 0);
Artiom 44:badcbe8766e9 320 for(i=0; i<9; i++) {
Sitkah 29:41e02746041d 321 strcpy(tableau_aff[i],"");
Sitkah 29:41e02746041d 322 strcpy(tableau_aff[i],tableau_aff[i+1]);
Sitkah 29:41e02746041d 323 }
Sitkah 29:41e02746041d 324 strcpy(tableau_aff[9],tableau_etat[conv]);
Artiom 44:badcbe8766e9 325 for(i=0; i<10; i++) {
Sitkah 34:6aa4b46b102e 326 lcd.SetBackColor(VERT);
Sitkah 29:41e02746041d 327 lcd.DisplayStringAt(0, LINE(20+i), (uint8_t *)tableau_aff[i], LEFT_MODE);
Artiom 44:badcbe8766e9 328 }
Sitkah 42:657b6a573e11 329 /*while(!ack_bluetooth){ // mode pas à pas en bluetooth ou via écran
Sitkah 30:a1e37af4bbde 330 //liaison_bluetooth();
Sitkah 29:41e02746041d 331 }
Sitkah 30:a1e37af4bbde 332 ack_bluetooth=0;*/
Sitkah 30:a1e37af4bbde 333 /*while(SUIVANT.Touched()==0);
Sitkah 30:a1e37af4bbde 334 while(SUIVANT.Touched());*/
Artiom 44:badcbe8766e9 335 }
Sitkah 29:41e02746041d 336
Sitkah 42:657b6a573e11 337 /****************************************************************************************/
Sitkah 42:657b6a573e11 338 /* FUNCTION NAME: automate_etat_ihm */
Sitkah 42:657b6a573e11 339 /* DESCRIPTION : Automate de gestion de l'affichage */
Artiom 44:badcbe8766e9 340 /****************************************************************************************/
Sitkah 29:41e02746041d 341 void automate_etat_ihm(void)
Sitkah 29:41e02746041d 342 {
Sitkah 29:41e02746041d 343 int j;
Artiom 44:badcbe8766e9 344 if (j==0) {
Sitkah 29:41e02746041d 345 ts.Init(lcd.GetXSize(), lcd.GetYSize());
Sitkah 29:41e02746041d 346 j++;
Sitkah 29:41e02746041d 347 }
Artiom 44:badcbe8766e9 348 ts.GetState(&TS_State);
Artiom 44:badcbe8766e9 349 switch (etat) {
Sitkah 42:657b6a573e11 350 case INIT : //intialise l'écran et passe à l'attente d'initialisation des cartes
Artiom 44:badcbe8766e9 351 ts.GetState(&TS_State);
Sitkah 29:41e02746041d 352 canProcessRx();
Artiom 44:badcbe8766e9 353
Artiom 44:badcbe8766e9 354
Artiom 44:badcbe8766e9 355
Artiom 44:badcbe8766e9 356
Sitkah 29:41e02746041d 357 lcd.SetBackColor(LCD_COLOR_WHITE);
Sitkah 29:41e02746041d 358 lcd.SetTextColor(LCD_COLOR_BLACK);
Sitkah 29:41e02746041d 359 lcd.Clear (LCD_COLOR_WHITE);
Sitkah 29:41e02746041d 360 wait(0.15);
Sitkah 29:41e02746041d 361 lcd.DisplayStringAt(0, 10, (uint8_t *)"Verification des cartes", LEFT_MODE);
Sitkah 29:41e02746041d 362 //cartes non verifiées////////////////
Sitkah 29:41e02746041d 363 lcd.SetTextColor(DIY_GREY);
Sitkah 29:41e02746041d 364 lcd.FillRect(0,400,400,150); //carte moteur
Sitkah 29:41e02746041d 365 lcd.FillRect(0,600,400,150); //Balise
Artiom 44:badcbe8766e9 366
Sitkah 29:41e02746041d 367 lcd.SetTextColor(LCD_COLOR_BLACK);
Sitkah 29:41e02746041d 368 lcd.SetBackColor(DIY_GREY);
Sitkah 29:41e02746041d 369 lcd.DisplayStringAt(80, 450, (uint8_t *)"Carte Moteur", LEFT_MODE);
Artiom 44:badcbe8766e9 370 lcd.DisplayStringAt(110,650, (uint8_t *)"Balise", LEFT_MODE);
Sitkah 29:41e02746041d 371 ////////////////////////////////////////
Artiom 44:badcbe8766e9 372
Artiom 44:badcbe8766e9 373 FORCE_LAUNCH.Draw(0xFFFF0000, 0);
Artiom 44:badcbe8766e9 374
Sitkah 29:41e02746041d 375 etat=ATT;
Sitkah 29:41e02746041d 376 break;
Artiom 44:badcbe8766e9 377
Sitkah 42:657b6a573e11 378 case ATT : //Si les cartes sont présentes passe directement à choix sinon attente de force Launch (cette partie est encore buggée mais les cartes affichent bien leur présence donc faut juste force launch tout le temps...)
Artiom 44:badcbe8766e9 379 if (flag==1) {
Sitkah 29:41e02746041d 380 etat = CHOIX;
Sitkah 29:41e02746041d 381 gameEtat = ETAT_CONFIG;
Artiom 44:badcbe8766e9 382 } else if (FORCE_LAUNCH.Touched()) {
Sitkah 29:41e02746041d 383 etat = CHOIX;
Sitkah 29:41e02746041d 384 gameEtat = ETAT_CONFIG;
Sitkah 30:a1e37af4bbde 385 while(FORCE_LAUNCH.Touched());
Sitkah 29:41e02746041d 386 }
Artiom 44:badcbe8766e9 387
Sitkah 29:41e02746041d 388 break;
Artiom 44:badcbe8766e9 389
Artiom 44:badcbe8766e9 390
Sitkah 42:657b6a573e11 391 case CHOIX : //Match ou DEMO
Sitkah 29:41e02746041d 392 lcd.SetBackColor(LCD_COLOR_WHITE);
Sitkah 29:41e02746041d 393 lcd.SetTextColor(LCD_COLOR_BLACK);
Sitkah 29:41e02746041d 394 lcd.Clear (LCD_COLOR_WHITE);
Sitkah 29:41e02746041d 395 lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Match ou demonstration ?", LEFT_MODE);
Sitkah 29:41e02746041d 396 DEMONSTRATION.Draw(LCD_COLOR_LIGHTGREEN, 0);
Sitkah 29:41e02746041d 397 MATCH.Draw(0xFFF01010, 0);
Artiom 44:badcbe8766e9 398 while(etat == CHOIX) {
Sitkah 29:41e02746041d 399 canProcessRx();
Artiom 44:badcbe8766e9 400 if(DEMONSTRATION.Touched()) {
Sitkah 29:41e02746041d 401 etat = DEMO;
Sitkah 29:41e02746041d 402 while(DEMONSTRATION.Touched());
Sitkah 29:41e02746041d 403 }
Artiom 44:badcbe8766e9 404
Artiom 44:badcbe8766e9 405 if(MATCH.Touched()) {
Sitkah 29:41e02746041d 406 etat = SELECT_SIDE;
Sitkah 29:41e02746041d 407 while(MATCH.Touched());
Sitkah 29:41e02746041d 408 }
Sitkah 29:41e02746041d 409
Sitkah 29:41e02746041d 410 }
Sitkah 29:41e02746041d 411 break;
Artiom 44:badcbe8766e9 412
Artiom 44:badcbe8766e9 413 case DEMO :
Sitkah 29:41e02746041d 414 lcd.Clear(LCD_COLOR_WHITE);
Sitkah 29:41e02746041d 415 RETOUR.Draw(0xFFFF0000, 0);
Sitkah 34:6aa4b46b102e 416 TEST_HERKULEX.Draw(VERT, 0);
Sitkah 34:6aa4b46b102e 417 TEST_LASER.Draw(VERT, 0);
Sitkah 34:6aa4b46b102e 418 TEST_COULEURS.Draw(VERT, 0);
Sitkah 34:6aa4b46b102e 419 TEST_TIR_BALLE.Draw(VERT, 0);
Sitkah 34:6aa4b46b102e 420 TEST_IMMEUBLE.Draw(VERT,0);
Sitkah 34:6aa4b46b102e 421 TEST_TRIEUR.Draw(VERT,0);
Sitkah 42:657b6a573e11 422 if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config//
Artiom 44:badcbe8766e9 423 InversStrat = 0;//Pas d'inversion de la couleur
Sitkah 29:41e02746041d 424 }
Artiom 44:badcbe8766e9 425 while (etat == DEMO) { ////////////////////////////LISTE DES DIFFERENTES DEMOS POSSIBLES///////////////////////////////////////////
Sitkah 29:41e02746041d 426 canProcessRx();
Artiom 44:badcbe8766e9 427 if(TEST_HERKULEX.Touched()) {
Sitkah 30:a1e37af4bbde 428 //Strat = 0x10;
Sitkah 31:833fc481b002 429 while(TEST_HERKULEX.Touched());
Sitkah 32:1c9ab15c740e 430 CANMessage trame_Tx = CANMessage();
Sitkah 32:1c9ab15c740e 431 trame_Tx.len = 1;
Sitkah 32:1c9ab15c740e 432 trame_Tx.format = CANStandard;
Sitkah 32:1c9ab15c740e 433 trame_Tx.type = CANData;
Sitkah 31:833fc481b002 434 trame_Tx.id=CHOICE_COLOR;
Sitkah 32:1c9ab15c740e 435 trame_Tx.data[0]=0x2;
Sitkah 31:833fc481b002 436 can2.write(trame_Tx);
Sitkah 31:833fc481b002 437 TEST_HERKULEX.Draw(0xFFF0F0F0, 0);
Sitkah 31:833fc481b002 438 etat = TEST_SERVO;
Artiom 44:badcbe8766e9 439 lcd.Clear(LCD_COLOR_WHITE);
Sitkah 30:a1e37af4bbde 440 ModeDemo=1;
Artiom 44:badcbe8766e9 441 } else if(TEST_LASER.Touched()) {
Artiom 44:badcbe8766e9 442 //Strat = 0x11;
Artiom 44:badcbe8766e9 443 while(TEST_LASER.Touched());
Sitkah 31:833fc481b002 444 TEST_LASER.Draw(0xFFF0F0F0, 0);
Sitkah 31:833fc481b002 445 etat = TEST_TELEMETRE;
Artiom 44:badcbe8766e9 446 } else if (TEST_COULEURS.Touched()) {
Sitkah 31:833fc481b002 447 while(TEST_COULEURS.Touched());
Artiom 44:badcbe8766e9 448 TEST_LASER.Draw(0xFFF0F0F0, 0);
Artiom 44:badcbe8766e9 449 etat =TEST_CAPTEURS ;
Sitkah 31:833fc481b002 450 }
Artiom 44:badcbe8766e9 451
Artiom 44:badcbe8766e9 452 else if (TEST_TIR_BALLE.Touched()) {
Sitkah 31:833fc481b002 453 while(TEST_TIR_BALLE.Touched());
Artiom 44:badcbe8766e9 454 TEST_TIR_BALLE.Draw(0xFFF0F0F0, 0);
Artiom 44:badcbe8766e9 455 etat =TEST_TIR ;
Artiom 44:badcbe8766e9 456 lcd.Clear(LCD_COLOR_WHITE);
Sitkah 32:1c9ab15c740e 457 CANMessage trame_Tx = CANMessage();
Sitkah 32:1c9ab15c740e 458 trame_Tx.len = 1;
Sitkah 32:1c9ab15c740e 459 trame_Tx.format = CANStandard;
Sitkah 32:1c9ab15c740e 460 trame_Tx.type = CANData;
Sitkah 31:833fc481b002 461 trame_Tx.id=CHOICE_COLOR;
Sitkah 32:1c9ab15c740e 462 trame_Tx.data[0]=0x2;
Sitkah 31:833fc481b002 463 can2.write(trame_Tx);
Artiom 44:badcbe8766e9 464 ModeDemo=1;
Artiom 44:badcbe8766e9 465 } else if(TEST_IMMEUBLE.Touched()) {
Artiom 44:badcbe8766e9 466 while(TEST_IMMEUBLE.Touched());
Artiom 44:badcbe8766e9 467 TEST_IMMEUBLE.Draw(0xFFF0F0F0, 0);
Artiom 44:badcbe8766e9 468 etat =DEMO_IMMEUBLE;
Artiom 44:badcbe8766e9 469 lcd.Clear(LCD_COLOR_WHITE);
Artiom 44:badcbe8766e9 470 } else if(TEST_TRIEUR.Touched()) {
Sitkah 32:1c9ab15c740e 471 while(TEST_TRIEUR.Touched());
Sitkah 32:1c9ab15c740e 472 etat=DEMO_TRIEUR;
Sitkah 32:1c9ab15c740e 473 lcd.Clear(LCD_COLOR_WHITE);
Artiom 44:badcbe8766e9 474 }
Artiom 44:badcbe8766e9 475 if(RETOUR.Touched()) {
Sitkah 29:41e02746041d 476 etat = CHOIX;
Sitkah 29:41e02746041d 477 while(RETOUR.Touched());
Artiom 44:badcbe8766e9 478
Sitkah 29:41e02746041d 479 }
Sitkah 29:41e02746041d 480 if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config
Sitkah 29:41e02746041d 481 Ack_strat = 1;
Sitkah 29:41e02746041d 482 wait_ms(10);
Sitkah 29:41e02746041d 483 }
Sitkah 29:41e02746041d 484 }
Sitkah 29:41e02746041d 485 break;
Artiom 44:badcbe8766e9 486 ///////////////////////////////TESTE LES SERVOS LIES AU TRI DES BALLES///////////////////////////////
Artiom 44:badcbe8766e9 487 case DEMO_TRIEUR:
Sitkah 32:1c9ab15c740e 488 lcd.SetBackColor(LCD_COLOR_WHITE);
Sitkah 32:1c9ab15c740e 489 lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"DEMONSTRATION COURS", LEFT_MODE);
Sitkah 32:1c9ab15c740e 490 TRI.Draw(VERT, 0);
Sitkah 32:1c9ab15c740e 491 AIGUILLEUR_D.Draw(VERT, 0);
Sitkah 32:1c9ab15c740e 492 AIGUILLEUR_G.Draw(VERT, 0);
Sitkah 32:1c9ab15c740e 493 AIGUILLEUR_CTRE.Draw(VERT, 0);
Artiom 44:badcbe8766e9 494 while(etat==DEMO_TRIEUR) {
Artiom 44:badcbe8766e9 495 if(RETOUR.Touched()) {
Sitkah 32:1c9ab15c740e 496 while (RETOUR.Touched());
Sitkah 32:1c9ab15c740e 497 etat=DEMO;
Artiom 44:badcbe8766e9 498 } else if(TRI.Touched()) {
Artiom 44:badcbe8766e9 499 while (TRI.Touched());
Sitkah 32:1c9ab15c740e 500 SendRawId(AIGUILLEUR_CENTRE);
Sitkah 32:1c9ab15c740e 501 wait(0.5);
Sitkah 32:1c9ab15c740e 502 SendRawId(AIGUILLEUR_DROITE);
Sitkah 32:1c9ab15c740e 503 wait(0.5);
Sitkah 32:1c9ab15c740e 504 SendRawId(AIGUILLEUR_GAUCHE);
Sitkah 32:1c9ab15c740e 505 wait(0.5);
Sitkah 32:1c9ab15c740e 506 SendRawId(AIGUILLEUR_CENTRE);
Artiom 44:badcbe8766e9 507
Sitkah 32:1c9ab15c740e 508 break;
Artiom 44:badcbe8766e9 509 } else if(AIGUILLEUR_D.Touched()) {
Artiom 44:badcbe8766e9 510 while (AIGUILLEUR_D.Touched());
Sitkah 32:1c9ab15c740e 511 SendRawId(AIGUILLEUR_DROITE);
Sitkah 32:1c9ab15c740e 512 break;
Artiom 44:badcbe8766e9 513 } else if(AIGUILLEUR_G.Touched()) {
Sitkah 32:1c9ab15c740e 514 while (AIGUILLEUR_G.Touched());
Artiom 44:badcbe8766e9 515 SendRawId(AIGUILLEUR_GAUCHE);
Sitkah 32:1c9ab15c740e 516 break;
Artiom 44:badcbe8766e9 517
Artiom 44:badcbe8766e9 518 } else if(BRAS_ABEILLE_OFF.Touched()) {
Artiom 44:badcbe8766e9 519 while (BRAS_ABEILLE_OFF.Touched());
Sitkah 32:1c9ab15c740e 520 SendRawId(BRAS_ABEILLE_DOWN);
Artiom 44:badcbe8766e9 521 break;
Artiom 44:badcbe8766e9 522 } else if(AIGUILLEUR_CTRE.Touched()) {
Artiom 44:badcbe8766e9 523 while (AIGUILLEUR_CTRE.Touched());
Artiom 44:badcbe8766e9 524 SendRawId(AIGUILLEUR_CENTRE);
Artiom 44:badcbe8766e9 525 break;
Artiom 44:badcbe8766e9 526 }
Artiom 44:badcbe8766e9 527
Sitkah 32:1c9ab15c740e 528 }
Sitkah 32:1c9ab15c740e 529 break;
Sitkah 42:657b6a573e11 530 case DEMO_IMMEUBLE: //TESTE LE MONTE IMMEUBLE SUIVANT UN CODE COULEUR CHOISI
Sitkah 31:833fc481b002 531 int color=0;
Sitkah 31:833fc481b002 532 lcd.SetBackColor(LCD_COLOR_WHITE);
Sitkah 31:833fc481b002 533 lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"Choix du code couleur", LEFT_MODE);
Artiom 44:badcbe8766e9 534
Sitkah 31:833fc481b002 535 CANMessage msgTx=CANMessage();
Sitkah 31:833fc481b002 536 msgTx.id=MONTER_IMMEUBLE; // Monter immeuble
Sitkah 31:833fc481b002 537 msgTx.len=3;
Sitkah 31:833fc481b002 538 msgTx.format=CANStandard;
Sitkah 31:833fc481b002 539 msgTx.type=CANData;
Artiom 44:badcbe8766e9 540
Artiom 44:badcbe8766e9 541
Artiom 44:badcbe8766e9 542 while(etat==DEMO_IMMEUBLE) {
Artiom 44:badcbe8766e9 543 switch(color) {
Sitkah 31:833fc481b002 544 case 0:
Artiom 44:badcbe8766e9 545
Sitkah 31:833fc481b002 546 RETOUR.Draw(ROUGE,0);
Sitkah 31:833fc481b002 547 COLOR_NOIR.Draw(NOIR,1);
Sitkah 31:833fc481b002 548 COLOR_ORANGE.Draw(ORANGE,0);
Sitkah 31:833fc481b002 549 COLOR_JAUNE.Draw(JAUNE,0);
Sitkah 31:833fc481b002 550 COLOR_VERT.Draw(VERT,0);
Sitkah 38:76f886a1c8e6 551 COLOR_BLEU.Draw(BLEU,0);
Artiom 44:badcbe8766e9 552
Sitkah 31:833fc481b002 553 lcd.SetBackColor(LCD_COLOR_WHITE);
Sitkah 31:833fc481b002 554 lcd.SetTextColor(NOIR);
Sitkah 31:833fc481b002 555 lcd.DisplayStringAt(100, LINE(4), (uint8_t *)"COULEUR 1", LEFT_MODE);
Artiom 44:badcbe8766e9 556 while(color==0) {
Artiom 44:badcbe8766e9 557 if(COLOR_ORANGE.Touched()) {
Sitkah 31:833fc481b002 558 while(COLOR_ORANGE.Touched());
Sitkah 31:833fc481b002 559 COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
Artiom 44:badcbe8766e9 560 msgTx.data[color]=1;
Sitkah 31:833fc481b002 561 color++;
Artiom 44:badcbe8766e9 562 } else if (COLOR_NOIR.Touched()) {
Sitkah 31:833fc481b002 563 while(COLOR_NOIR.Touched());
Sitkah 31:833fc481b002 564 COLOR_NOIR.Draw(LCD_COLOR_WHITE);
Artiom 44:badcbe8766e9 565 msgTx.data[color]=2;
Sitkah 31:833fc481b002 566 color++;
Artiom 44:badcbe8766e9 567 } else if (COLOR_VERT.Touched()) {
Sitkah 31:833fc481b002 568 while(COLOR_VERT.Touched());
Sitkah 31:833fc481b002 569 COLOR_VERT.Draw(LCD_COLOR_WHITE);
Artiom 44:badcbe8766e9 570
Sitkah 31:833fc481b002 571 msgTx.data[color]=3;
Sitkah 31:833fc481b002 572 color++;
Artiom 44:badcbe8766e9 573 } else if (COLOR_JAUNE.Touched()) {
Sitkah 31:833fc481b002 574 while(COLOR_JAUNE.Touched());
Sitkah 31:833fc481b002 575 COLOR_JAUNE.Draw(LCD_COLOR_WHITE);
Sitkah 31:833fc481b002 576 msgTx.data[color]=4;
Sitkah 31:833fc481b002 577 color++;
Artiom 44:badcbe8766e9 578 } else if (COLOR_BLEU.Touched()) {
Sitkah 31:833fc481b002 579 while(COLOR_BLEU.Touched());
Sitkah 31:833fc481b002 580 COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
Sitkah 31:833fc481b002 581 msgTx.data[color]=5;
Sitkah 31:833fc481b002 582 color++;
Sitkah 31:833fc481b002 583 }
Sitkah 31:833fc481b002 584 }
Sitkah 31:833fc481b002 585 break;
Artiom 44:badcbe8766e9 586
Sitkah 31:833fc481b002 587 case 1:
Sitkah 31:833fc481b002 588 lcd.SetBackColor(LCD_COLOR_WHITE);
Sitkah 31:833fc481b002 589 lcd.SetTextColor(NOIR);
Sitkah 31:833fc481b002 590 lcd.DisplayStringAt(100, LINE(4), (uint8_t *)"COULEUR 2", LEFT_MODE);
Artiom 44:badcbe8766e9 591 if(COLOR_ORANGE.Touched()) {
Artiom 44:badcbe8766e9 592 while(COLOR_ORANGE.Touched());
Artiom 44:badcbe8766e9 593 COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
Artiom 44:badcbe8766e9 594 msgTx.data[color]=1;
Artiom 44:badcbe8766e9 595 color++;
Artiom 44:badcbe8766e9 596 } else if (COLOR_NOIR.Touched()) {
Artiom 44:badcbe8766e9 597 while(COLOR_NOIR.Touched());
Artiom 44:badcbe8766e9 598 COLOR_NOIR.Draw(LCD_COLOR_WHITE);
Artiom 44:badcbe8766e9 599 msgTx.data[color]=2;
Artiom 44:badcbe8766e9 600 color++;
Artiom 44:badcbe8766e9 601 } else if (COLOR_VERT.Touched()) {
Artiom 44:badcbe8766e9 602 while(COLOR_VERT.Touched());
Artiom 44:badcbe8766e9 603 COLOR_VERT.Draw(LCD_COLOR_WHITE);
Artiom 44:badcbe8766e9 604
Artiom 44:badcbe8766e9 605 msgTx.data[color]=3;
Artiom 44:badcbe8766e9 606 color++;
Artiom 44:badcbe8766e9 607 } else if (COLOR_JAUNE.Touched()) {
Artiom 44:badcbe8766e9 608 while(COLOR_JAUNE.Touched());
Artiom 44:badcbe8766e9 609 COLOR_JAUNE.Draw(LCD_COLOR_WHITE);
Artiom 44:badcbe8766e9 610 msgTx.data[color]=4;
Artiom 44:badcbe8766e9 611 color++;
Artiom 44:badcbe8766e9 612 } else if (COLOR_BLEU.Touched()) {
Artiom 44:badcbe8766e9 613 while(COLOR_BLEU.Touched());
Artiom 44:badcbe8766e9 614 COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
Artiom 44:badcbe8766e9 615 msgTx.data[color]=5;
Artiom 44:badcbe8766e9 616 color++;
Artiom 44:badcbe8766e9 617 }
Sitkah 31:833fc481b002 618 break;
Artiom 44:badcbe8766e9 619
Sitkah 31:833fc481b002 620 case 2:
Sitkah 31:833fc481b002 621 lcd.SetBackColor(LCD_COLOR_WHITE);
Sitkah 31:833fc481b002 622 lcd.SetTextColor(NOIR);
Sitkah 31:833fc481b002 623 lcd.DisplayStringAt(100, LINE(4), (uint8_t *)"COULEUR 3", LEFT_MODE);
Artiom 44:badcbe8766e9 624 if(COLOR_ORANGE.Touched()) {
Artiom 44:badcbe8766e9 625 while(COLOR_ORANGE.Touched());
Artiom 44:badcbe8766e9 626 COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
Artiom 44:badcbe8766e9 627 msgTx.data[color]=1;
Artiom 44:badcbe8766e9 628 color++;
Artiom 44:badcbe8766e9 629 } else if (COLOR_NOIR.Touched()) {
Artiom 44:badcbe8766e9 630 while(COLOR_NOIR.Touched());
Artiom 44:badcbe8766e9 631 COLOR_NOIR.Draw(LCD_COLOR_WHITE);
Artiom 44:badcbe8766e9 632 msgTx.data[color]=2;
Artiom 44:badcbe8766e9 633 color++;
Artiom 44:badcbe8766e9 634 } else if (COLOR_VERT.Touched()) {
Artiom 44:badcbe8766e9 635 while(COLOR_VERT.Touched());
Artiom 44:badcbe8766e9 636 COLOR_VERT.Draw(LCD_COLOR_WHITE);
Artiom 44:badcbe8766e9 637
Artiom 44:badcbe8766e9 638 msgTx.data[color]=3;
Artiom 44:badcbe8766e9 639 color++;
Artiom 44:badcbe8766e9 640 } else if (COLOR_JAUNE.Touched()) {
Artiom 44:badcbe8766e9 641 while(COLOR_JAUNE.Touched());
Artiom 44:badcbe8766e9 642 COLOR_JAUNE.Draw(LCD_COLOR_WHITE);
Artiom 44:badcbe8766e9 643 msgTx.data[color]=4;
Artiom 44:badcbe8766e9 644 color++;
Artiom 44:badcbe8766e9 645 } else if (COLOR_BLEU.Touched()) {
Artiom 44:badcbe8766e9 646 while(COLOR_BLEU.Touched());
Artiom 44:badcbe8766e9 647 COLOR_ORANGE.Draw(LCD_COLOR_WHITE);
Artiom 44:badcbe8766e9 648 msgTx.data[color]=5;
Artiom 44:badcbe8766e9 649 color++;
Artiom 44:badcbe8766e9 650 }
Sitkah 31:833fc481b002 651 break;
Sitkah 31:833fc481b002 652 case 3:
Sitkah 31:833fc481b002 653 lcd.Clear(LCD_COLOR_WHITE);
Sitkah 31:833fc481b002 654 lcd.SetBackColor(LCD_COLOR_WHITE);
Sitkah 31:833fc481b002 655 lcd.SetTextColor(NOIR);
Artiom 44:badcbe8766e9 656
Sitkah 31:833fc481b002 657 lcd.DisplayStringAt(0, LINE(4), (uint8_t *)"Immeuble en construction", LEFT_MODE);
Sitkah 31:833fc481b002 658 RETOUR.Draw(ROUGE,0);
Sitkah 31:833fc481b002 659 can2.write(msgTx);
Sitkah 31:833fc481b002 660 color++;
Sitkah 31:833fc481b002 661 break;
Artiom 44:badcbe8766e9 662
Sitkah 31:833fc481b002 663 case 4:
Artiom 44:badcbe8766e9 664 if(RETOUR.Touched()) {
Sitkah 31:833fc481b002 665 while(RETOUR.Touched());
Artiom 44:badcbe8766e9 666 etat=DEMO;
Sitkah 31:833fc481b002 667 }
Sitkah 31:833fc481b002 668 break;
Sitkah 29:41e02746041d 669 }
Artiom 44:badcbe8766e9 670 if(RETOUR.Touched()) {
Sitkah 31:833fc481b002 671 while(RETOUR.Touched());
Sitkah 30:a1e37af4bbde 672 etat=DEMO;
Sitkah 30:a1e37af4bbde 673 }
Sitkah 31:833fc481b002 674 }
Sitkah 31:833fc481b002 675 break;
Artiom 44:badcbe8766e9 676
Artiom 44:badcbe8766e9 677
Artiom 44:badcbe8766e9 678
Artiom 44:badcbe8766e9 679
Artiom 44:badcbe8766e9 680
Sitkah 42:657b6a573e11 681 case TEST_SERVO: //TEST DU RESTE DES SERVOS DISPO HORS TIR
Sitkah 31:833fc481b002 682 lcd.SetBackColor(LCD_COLOR_WHITE);
Sitkah 31:833fc481b002 683 lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"DEMONSTRATION COURS", LEFT_MODE);
Sitkah 31:833fc481b002 684 ABAISSE_BLOC.Draw(VERT, 0);
Sitkah 31:833fc481b002 685 RELEVE_BLOC.Draw(VERT, 0);
Sitkah 31:833fc481b002 686 BRAS_ABEILLE_ON.Draw(VERT, 0);
Sitkah 31:833fc481b002 687 BRAS_ABEILLE_OFF.Draw(VERT, 0);
Sitkah 31:833fc481b002 688 INTERRUPTEUR_ON.Draw(VERT, 0);
Sitkah 31:833fc481b002 689 INTERRUPTEUR_OFF.Draw(VERT, 0);
Sitkah 31:833fc481b002 690 RETOUR.Draw(0xFFFF0000,0);
Artiom 44:badcbe8766e9 691 while(etat==TEST_SERVO) {
Artiom 44:badcbe8766e9 692 if(RETOUR.Touched()) {
Sitkah 31:833fc481b002 693 while (RETOUR.Touched());
Sitkah 31:833fc481b002 694 etat=DEMO;
Artiom 44:badcbe8766e9 695 } else if(ABAISSE_BLOC.Touched()) {
Artiom 44:badcbe8766e9 696 while (ABAISSE_BLOC.Touched());
Villanut 45:4f93e99bac6e 697 SendRawId(GABARIT_PETIT_ROBOT);
Sitkah 31:833fc481b002 698 break;
Artiom 44:badcbe8766e9 699 } else if(RELEVE_BLOC.Touched()) {
Artiom 44:badcbe8766e9 700 while (RELEVE_BLOC.Touched());
Villanut 45:4f93e99bac6e 701 SendRawId(PRESENTOIR_AVANT);
Sitkah 31:833fc481b002 702 break;
Artiom 44:badcbe8766e9 703 } else if(BRAS_ABEILLE_ON.Touched()) {
Sitkah 31:833fc481b002 704 while (BRAS_ABEILLE_ON.Touched());
Artiom 44:badcbe8766e9 705 SendRawId(BRAS_ABEILLE_UP);
Sitkah 31:833fc481b002 706 break;
Artiom 44:badcbe8766e9 707
Artiom 44:badcbe8766e9 708 } else if(BRAS_ABEILLE_OFF.Touched()) {
Artiom 44:badcbe8766e9 709 while (BRAS_ABEILLE_OFF.Touched());
Sitkah 31:833fc481b002 710 SendRawId(BRAS_ABEILLE_DOWN);
Artiom 44:badcbe8766e9 711 break;
Artiom 44:badcbe8766e9 712 } else if(INTERRUPTEUR_ON.Touched()) {
Artiom 44:badcbe8766e9 713 while (INTERRUPTEUR_ON.Touched());
Artiom 44:badcbe8766e9 714 SendRawId(ALLUMER_PANNEAU_UP);
Artiom 44:badcbe8766e9 715 break;
Artiom 44:badcbe8766e9 716 } else if(INTERRUPTEUR_OFF.Touched()) {
Artiom 44:badcbe8766e9 717 while (INTERRUPTEUR_OFF.Touched());
Artiom 44:badcbe8766e9 718 SendRawId(ALLUMER_PANNEAU_DOWN);
Sitkah 31:833fc481b002 719 break;
Sitkah 31:833fc481b002 720 }
Artiom 44:badcbe8766e9 721 }
Sitkah 31:833fc481b002 722 break;
Artiom 44:badcbe8766e9 723
Sitkah 42:657b6a573e11 724 case TEST_TIR: // TEST DES FONCTIONS LIEES AUX TIRS
Sitkah 31:833fc481b002 725 lcd.SetBackColor(LCD_COLOR_WHITE);
Sitkah 31:833fc481b002 726 lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"DEMONSTRATION COURS", LEFT_MODE);
Sitkah 31:833fc481b002 727 TIR_CHATEAU.Draw(VERT, 0);
Sitkah 31:833fc481b002 728 EPURATION.Draw(VERT, 0);
Sitkah 31:833fc481b002 729 LANCEUR_ON.Draw(VERT, 0);
Sitkah 31:833fc481b002 730 LANCEUR_OFF.Draw(VERT, 0);
Sitkah 31:833fc481b002 731 RETOUR.Draw(ROUGE, 0);
Artiom 44:badcbe8766e9 732 while(etat==TEST_TIR) {
Artiom 44:badcbe8766e9 733 if(TIR_CHATEAU.Touched()) {
Sitkah 31:833fc481b002 734 while (TIR_CHATEAU.Touched());
Sitkah 31:833fc481b002 735 SendRawId(INCLINAISON_CHATEAU);
Sitkah 31:833fc481b002 736 break;
Artiom 44:badcbe8766e9 737 } else if (EPURATION.Touched()) {
Sitkah 31:833fc481b002 738 while (EPURATION.Touched());
Sitkah 31:833fc481b002 739 SendRawId(INCLINAISON_EPURATION);
Sitkah 31:833fc481b002 740 break;
Artiom 44:badcbe8766e9 741 } else if(LANCEUR_ON.Touched()) {
Sitkah 31:833fc481b002 742 while (LANCEUR_ON.Touched());
Sitkah 36:6dd30780bd8e 743 CANMessage msgTx=CANMessage();
Sitkah 36:6dd30780bd8e 744 msgTx.format=CANStandard;
Sitkah 36:6dd30780bd8e 745 msgTx.type=CANData;
Sitkah 36:6dd30780bd8e 746 msgTx.id=LANCEMENT_MOTEUR_TIR_ON;
Artiom 44:badcbe8766e9 747
Sitkah 36:6dd30780bd8e 748 msgTx.len=1;
Sitkah 36:6dd30780bd8e 749 msgTx.data[0]=0;
Sitkah 36:6dd30780bd8e 750 can2.write(msgTx);
Sitkah 31:833fc481b002 751 break;
Artiom 44:badcbe8766e9 752 } else if(LANCEUR_OFF.Touched()) {
Sitkah 31:833fc481b002 753 while (LANCEUR_OFF.Touched());
Sitkah 31:833fc481b002 754 SendRawId(LANCEMENT_MOTEUR_TIR_OFF);
Sitkah 31:833fc481b002 755 break;
Artiom 44:badcbe8766e9 756 } else if (RETOUR.Touched()) {
Sitkah 31:833fc481b002 757 while (RETOUR.Touched());
Sitkah 31:833fc481b002 758 etat=DEMO;
Artiom 44:badcbe8766e9 759
Sitkah 31:833fc481b002 760 }
Sitkah 31:833fc481b002 761 }
Artiom 44:badcbe8766e9 762 break;
Artiom 44:badcbe8766e9 763
Artiom 44:badcbe8766e9 764
Artiom 44:badcbe8766e9 765
Sitkah 42:657b6a573e11 766 case TEST_TELEMETRE: //AFFICHAGE DE LA VALEUR LUE PAR LES 4 TELEMETRES
Sitkah 31:833fc481b002 767 ModeDemo=1;
Sitkah 31:833fc481b002 768 lcd.Clear(LCD_COLOR_WHITE);
Sitkah 31:833fc481b002 769 lcd.SetBackColor(LCD_COLOR_WHITE);
Sitkah 31:833fc481b002 770 lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"DEMONSTRATION COURS", LEFT_MODE);
Sitkah 31:833fc481b002 771 RETOUR.Draw(0xFFFF0000, 0);
Artiom 44:badcbe8766e9 772 while(etat==TEST_TELEMETRE) {
Sitkah 31:833fc481b002 773 SendRawId(DATA_RECALAGE);
Artiom 50:a5361ffeefc8 774 wait_ms(100);
Sitkah 31:833fc481b002 775 canProcessRx();
Artiom 44:badcbe8766e9 776 if(RETOUR.Touched()) {
Artiom 44:badcbe8766e9 777 while( RETOUR.Touched());
Artiom 44:badcbe8766e9 778 etat=DEMO;
Artiom 44:badcbe8766e9 779 lcd.Clear(LCD_COLOR_WHITE);
Sitkah 31:833fc481b002 780 }
Sitkah 29:41e02746041d 781 }
Sitkah 42:657b6a573e11 782 break; ///////////////////////////////////////////FIN DES DEMOS/////////////////////////////////////////////////
Artiom 44:badcbe8766e9 783
Artiom 44:badcbe8766e9 784
Sitkah 42:657b6a573e11 785 case SELECT_SIDE : // CHOIX DU COTE DU TERRAIN + INVERSION DE LA STRAT SI COTE ORANGE+ ENVOI DU COTE A LA CARTE CAPTEUR/ACTIONNEURS
Sitkah 29:41e02746041d 786 lcd.Clear(LCD_COLOR_WHITE);
Sitkah 29:41e02746041d 787 lcd.SetBackColor(LCD_COLOR_WHITE);
Sitkah 29:41e02746041d 788 lcd.SetTextColor(LCD_COLOR_BLACK);
Artiom 44:badcbe8766e9 789
Sitkah 29:41e02746041d 790 lcd.DisplayStringAt(70, LINE(0), (uint8_t *)"Choisir le cote", LEFT_MODE);
Villanut 45:4f93e99bac6e 791 COTE_VERT.Draw(LCD_COLOR_YELLOW, 0);
Villanut 45:4f93e99bac6e 792 COTE_ORANGE.Draw(LCD_COLOR_DARKMAGENTA, 0);
Sitkah 29:41e02746041d 793 RETOUR.Draw(LCD_COLOR_RED, 0);
Artiom 44:badcbe8766e9 794
Artiom 44:badcbe8766e9 795
Artiom 44:badcbe8766e9 796 while (etat == SELECT_SIDE) {
Sitkah 29:41e02746041d 797 canProcessRx();
Artiom 44:badcbe8766e9 798 if(COTE_VERT.Touched()) {
Sitkah 29:41e02746041d 799 Cote = 0x0;
Sitkah 29:41e02746041d 800 InversStrat = Cote;
Sitkah 29:41e02746041d 801 etat = TACTIQUE;
Sitkah 32:1c9ab15c740e 802 CANMessage trame_Tx = CANMessage();
Sitkah 32:1c9ab15c740e 803 trame_Tx.len = 1;
Sitkah 32:1c9ab15c740e 804 trame_Tx.format = CANStandard;
Sitkah 32:1c9ab15c740e 805 trame_Tx.type = CANData;
Sitkah 30:a1e37af4bbde 806 trame_Tx.id=CHOICE_COLOR;
Sitkah 30:a1e37af4bbde 807 trame_Tx.data[0]=Cote;
Sitkah 30:a1e37af4bbde 808 can2.write(trame_Tx);
Sitkah 34:6aa4b46b102e 809 while(COTE_VERT.Touched());
Artiom 44:badcbe8766e9 810
Sitkah 29:41e02746041d 811 }
Artiom 44:badcbe8766e9 812
Artiom 44:badcbe8766e9 813 if(COTE_ORANGE.Touched()) {
Sitkah 29:41e02746041d 814 Cote = 0x1;
Sitkah 29:41e02746041d 815 InversStrat= Cote;
Sitkah 29:41e02746041d 816 etat = TACTIQUE;
Sitkah 32:1c9ab15c740e 817 CANMessage trame_Tx = CANMessage();
Sitkah 32:1c9ab15c740e 818 trame_Tx.len = 1;
Sitkah 32:1c9ab15c740e 819 trame_Tx.format = CANStandard;
Sitkah 32:1c9ab15c740e 820 trame_Tx.type = CANData;
Sitkah 30:a1e37af4bbde 821 trame_Tx.id=CHOICE_COLOR;
Sitkah 30:a1e37af4bbde 822 trame_Tx.data[0]=Cote;
Sitkah 30:a1e37af4bbde 823 can2.write(trame_Tx);
Sitkah 34:6aa4b46b102e 824 while(COTE_ORANGE.Touched());
Sitkah 29:41e02746041d 825 }
Artiom 44:badcbe8766e9 826
Artiom 44:badcbe8766e9 827 if(RETOUR.Touched()) {
Sitkah 29:41e02746041d 828 etat = CHOIX;
Sitkah 29:41e02746041d 829 while(RETOUR.Touched());
Sitkah 29:41e02746041d 830 }
Sitkah 29:41e02746041d 831 }
Artiom 44:badcbe8766e9 832
Sitkah 29:41e02746041d 833 break;
Artiom 44:badcbe8766e9 834
Sitkah 42:657b6a573e11 835 case TACTIQUE : //AFFICHE LA LISTE DES STRATS AFIN DE SELECTIONNER CELLE VOULUE
Artiom 44:badcbe8766e9 836 if (Cote == 0) {
Sitkah 34:6aa4b46b102e 837 lcd.Clear(VERT);
Sitkah 34:6aa4b46b102e 838 lcd.SetBackColor(VERT);
Artiom 44:badcbe8766e9 839 } else if (Cote == 1) {
Sitkah 38:76f886a1c8e6 840 lcd.Clear(ORANGE);
Sitkah 38:76f886a1c8e6 841 lcd.SetBackColor(ORANGE);
Artiom 44:badcbe8766e9 842 } else {
Sitkah 38:76f886a1c8e6 843 lcd.Clear(BLEU);
Sitkah 38:76f886a1c8e6 844 lcd.SetBackColor(BLEU);
Artiom 44:badcbe8766e9 845 }
Artiom 44:badcbe8766e9 846
Artiom 44:badcbe8766e9 847 lcd.SetTextColor(LCD_COLOR_BLACK);
Artiom 44:badcbe8766e9 848
Sitkah 29:41e02746041d 849 lcd.DisplayStringAt(20, LINE(0), (uint8_t *)"Choisir une strategie", LEFT_MODE);
Artiom 44:badcbe8766e9 850
Sitkah 29:41e02746041d 851 Strategie = Bouton_Strat(); // retourne valeur de Strategie si bouton strat renvoi -1 on reviens en arriere
Artiom 44:badcbe8766e9 852 if (Strategie == -1) {
Sitkah 29:41e02746041d 853 etat = SELECT_SIDE;
Artiom 44:badcbe8766e9 854 } else {
Artiom 44:badcbe8766e9 855 etat = DETAILS;
Sitkah 29:41e02746041d 856 }
Sitkah 29:41e02746041d 857 wait(0.1);
Sitkah 29:41e02746041d 858 break;
Artiom 44:badcbe8766e9 859
Sitkah 42:657b6a573e11 860 case DETAILS : //SECONDE VALIDATION DE LA STRAT
Sitkah 29:41e02746041d 861 lcd.Clear(LCD_COLOR_WHITE);
Sitkah 29:41e02746041d 862 lcd.SetBackColor(LCD_COLOR_WHITE);
Sitkah 29:41e02746041d 863 lcd.SetTextColor(LCD_COLOR_BLACK);
Sitkah 29:41e02746041d 864 CHECK.Draw(VERT);
Sitkah 29:41e02746041d 865 RETOUR.Draw(LCD_COLOR_RED);
Artiom 44:badcbe8766e9 866
Sitkah 42:657b6a573e11 867 SelectionStrat(Strategie); //affiche la stratégie selectionnée
Artiom 44:badcbe8766e9 868
Artiom 44:badcbe8766e9 869 while (etat == DETAILS) {
Sitkah 29:41e02746041d 870 canProcessRx();
Artiom 44:badcbe8766e9 871 if (CHECK.Touched()) {
Artiom 44:badcbe8766e9 872 if(gameEtat == ETAT_CONFIG) {
Artiom 44:badcbe8766e9 873 gameEtat = ETAT_GAME_INIT;
Artiom 44:badcbe8766e9 874 etat=LECTURE;
Artiom 44:badcbe8766e9 875
Sitkah 29:41e02746041d 876 }
Artiom 44:badcbe8766e9 877 while(CHECK.Touched());
Artiom 44:badcbe8766e9 878 }
Artiom 44:badcbe8766e9 879
Artiom 44:badcbe8766e9 880 if(RETOUR.Touched()) {
Artiom 44:badcbe8766e9 881 etat = TACTIQUE;
Artiom 44:badcbe8766e9 882 while(RETOUR.Touched());
Artiom 44:badcbe8766e9 883 }
Artiom 44:badcbe8766e9 884 }
Sitkah 29:41e02746041d 885 break;
Artiom 44:badcbe8766e9 886
Artiom 44:badcbe8766e9 887
Sitkah 29:41e02746041d 888 case LECTURE :
Artiom 44:badcbe8766e9 889 break;
Sitkah 42:657b6a573e11 890 case AFF_WAIT_JACK : //FONCTIONS D'AFFICHAGE DE L'ATTENTE DU JACK
Sitkah 29:41e02746041d 891 lcd.Clear(BLANC);
Sitkah 29:41e02746041d 892 lcd.SetBackColor(LCD_COLOR_WHITE);
Sitkah 29:41e02746041d 893 lcd.SetTextColor(LCD_COLOR_BLACK);
Artiom 44:badcbe8766e9 894
Artiom 44:badcbe8766e9 895 if (Cote == 0) {
Sitkah 34:6aa4b46b102e 896 lcd.Clear(VERT);
Sitkah 34:6aa4b46b102e 897 lcd.SetBackColor(VERT);
Artiom 44:badcbe8766e9 898 } else if (Cote == 1) {
Sitkah 38:76f886a1c8e6 899 lcd.Clear(ORANGE);
Sitkah 38:76f886a1c8e6 900 lcd.SetBackColor(ORANGE);
Artiom 44:badcbe8766e9 901 } else {
Sitkah 34:6aa4b46b102e 902 lcd.Clear(VERT);
Sitkah 34:6aa4b46b102e 903 lcd.SetBackColor(VERT);
Sitkah 29:41e02746041d 904 }
Sitkah 29:41e02746041d 905 canProcessRx();
Artiom 44:badcbe8766e9 906 lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"En attente du Jack", CENTER_MODE);
Sitkah 29:41e02746041d 907 etat=WAIT_JACK;
Sitkah 29:41e02746041d 908 break;
Artiom 44:badcbe8766e9 909
Sitkah 42:657b6a573e11 910 case WAIT_JACK: //VERITABLE ATTENTE DU JACK
Artiom 44:badcbe8766e9 911 break;
Artiom 44:badcbe8766e9 912
Sitkah 42:657b6a573e11 913 case COMPTEUR: //PEUT AFFICHER UN COMPTEUR DU TEMPS RESTANT AVANT LA FIN DE LA PARTIE OU BIEN TRES UTILE POUR PRINT DES VARIABLES CHAQUE SEC EX: gameEtat
Sitkah 38:76f886a1c8e6 914 cptf=gameTimer.read();
Sitkah 29:41e02746041d 915 lcd.SetTextColor(LCD_COLOR_BLACK);
Sitkah 38:76f886a1c8e6 916 cpt=(int)cptf;
Artiom 44:badcbe8766e9 917 if(cpt != cpt1) {
Sitkah 34:6aa4b46b102e 918 lcd.Clear(VERT);
Artiom 44:badcbe8766e9 919 // affichage_compteur(100-cpt);
Sitkah 40:21bb685b553b 920 //affichage_compteur(SCORE_PR);
Artiom 44:badcbe8766e9 921 #ifdef ROBOT_BIG
Artiom 44:badcbe8766e9 922 affichage_var(SCORE_GR);
Artiom 44:badcbe8766e9 923 #else
Artiom 44:badcbe8766e9 924 affichage_var(SCORE_PR);
Artiom 44:badcbe8766e9 925 #endif
Artiom 44:badcbe8766e9 926 if(liaison_pr.paquet_en_attente()) {
Sitkah 38:76f886a1c8e6 927 PaquetDomotique *paquet=liaison_pr.lire();
Artiom 44:badcbe8766e9 928 if(paquet->identifiant==PAQUET_IDENTIFIANT_AJOUTERSCORE) {
Sitkah 38:76f886a1c8e6 929 SCORE_PR+=convertir_score(paquet);
Sitkah 38:76f886a1c8e6 930 }
Sitkah 38:76f886a1c8e6 931 delete paquet;
Sitkah 38:76f886a1c8e6 932 }
Sitkah 36:6dd30780bd8e 933 }
Sitkah 29:41e02746041d 934 cpt1=cpt;
Sitkah 29:41e02746041d 935 flag_timer=0;
Sitkah 29:41e02746041d 936
Sitkah 38:76f886a1c8e6 937 //affichage_debug(gameEtat);
Sitkah 34:6aa4b46b102e 938 lcd.SetBackColor(LCD_COLOR_WHITE);
Artiom 44:badcbe8766e9 939
Sitkah 29:41e02746041d 940 break;
Artiom 44:badcbe8766e9 941
Sitkah 42:657b6a573e11 942 case FIN : //AFFICHAGE DE FIN AVEC LE SCORE FINAL
Sitkah 29:41e02746041d 943 lcd.Clear (LCD_COLOR_WHITE);
Sitkah 29:41e02746041d 944 lcd.SetBackColor(LCD_COLOR_WHITE);
Artiom 44:badcbe8766e9 945 #ifdef ROBOT_BIG
Artiom 44:badcbe8766e9 946 // affichage_compteur(SCORE_GR);
Artiom 44:badcbe8766e9 947 affichage_var(SCORE_GR);
Artiom 44:badcbe8766e9 948 //liaison_Tx.envoyer_short(PAQUET_IDENTIFIANT_FINMATCH,SCORE_GLOBAL);
Artiom 44:badcbe8766e9 949 #else
Artiom 44:badcbe8766e9 950 //affichage_compteur(SCORE_PR);
Artiom 44:badcbe8766e9 951 affichage_var(SCORE_PR);
Artiom 44:badcbe8766e9 952 #endif
Artiom 44:badcbe8766e9 953 while(1); // force le redemarage du robot
Sitkah 29:41e02746041d 954 //break;
Artiom 44:badcbe8766e9 955
Artiom 44:badcbe8766e9 956 }
Sitkah 29:41e02746041d 957 }
Sitkah 29:41e02746041d 958
Sitkah 29:41e02746041d 959
Sitkah 29:41e02746041d 960
Sitkah 29:41e02746041d 961 /****************************************************************************************/
antbig 4:88431b537477 962 /* FUNCTION NAME: automate_process */
antbig 4:88431b537477 963 /* DESCRIPTION : Automate de gestion de la stratégie du robot */
antbig 4:88431b537477 964 /****************************************************************************************/
Artiom 44:badcbe8766e9 965 void automate_process(void)
Artiom 44:badcbe8766e9 966 {
antbig 1:116040d14164 967 static unsigned char AX12_enchainement = 0;
antbig 1:116040d14164 968 static unsigned char MV_enchainement = 0;
antbig 0:ad97421fb1fb 969 signed char localData1 = 0;
antbig 0:ad97421fb1fb 970 signed short localData2 = 0;
antbig 0:ad97421fb1fb 971 unsigned short localData3 = 0;
ClementBreteau 14:c8fc06c4887f 972 //signed short localData4 = 0;
antbig 1:116040d14164 973 unsigned char localData5 = 0;
Artiom 44:badcbe8766e9 974
Artiom 44:badcbe8766e9 975
Sitkah 34:6aa4b46b102e 976 if(gameTimer.read_ms() >= 99000) {//Fin du match (On autorise 2s pour déposer des éléments
antbig 0:ad97421fb1fb 977 gameTimer.stop();
antbig 0:ad97421fb1fb 978 gameTimer.reset();
antbig 0:ad97421fb1fb 979 gameEtat = ETAT_END;//Fin du temps
Sitkah 29:41e02746041d 980 etat=FIN;
antbig 0:ad97421fb1fb 981 }
Artiom 44:badcbe8766e9 982
antbig 0:ad97421fb1fb 983 if(lastEtat != gameEtat || debugetatTimer.read_ms() >= 1000) {
antbig 0:ad97421fb1fb 984 lastEtat = gameEtat;
antbig 0:ad97421fb1fb 985 debugetatTimer.reset();
antbig 11:ed13a480ddca 986 sendStratEtat((unsigned char)gameEtat, (unsigned char)actual_instruction);
antbig 0:ad97421fb1fb 987 }
Artiom 44:badcbe8766e9 988
Artiom 44:badcbe8766e9 989 switch(gameEtat) {
Artiom 44:badcbe8766e9 990
Artiom 44:badcbe8766e9 991 case ETAT_CHECK_CARTES:
antbig 0:ad97421fb1fb 992 /*
antbig 0:ad97421fb1fb 993 Il faut faire une boucle pour verifier toutes les cartes les une apres les autres
antbig 0:ad97421fb1fb 994 */
antbig 0:ad97421fb1fb 995 waitingAckFrom = id_alive[checkCurrent];//On indique que l'on attend un ack de la carte IHM
antbig 11:ed13a480ddca 996 SendRawId(id_check[checkCurrent]);//On demande à la carte d'indiquer ça présence
Artiom 44:badcbe8766e9 997
antbig 0:ad97421fb1fb 998 screenChecktry++;//On incrèment le conteur de tentative de 1
antbig 0:ad97421fb1fb 999 cartesCheker.reset();//On reset le timeOut
antbig 0:ad97421fb1fb 1000 cartesCheker.start();//On lance le timer pour le timeout
antbig 0:ad97421fb1fb 1001 gameEtat = ETAT_CHECK_CARTES_WAIT_ACK;
Sitkah 29:41e02746041d 1002 break;
Artiom 44:badcbe8766e9 1003
antbig 0:ad97421fb1fb 1004 case ETAT_CHECK_CARTES_WAIT_ACK:
antbig 0:ad97421fb1fb 1005 /*
antbig 0:ad97421fb1fb 1006 On attend l'ack de la carte en cours de vérification
antbig 0:ad97421fb1fb 1007 */
antbig 0:ad97421fb1fb 1008 //printf("cartesCheker = %d waitingAckFrom = %d\n",cartesCheker.read_ms(), waitingAckFrom);
antbig 0:ad97421fb1fb 1009 if(waitingAckFrom == 0) {//C'est bon la carte est en ligne
antbig 0:ad97421fb1fb 1010 cartesCheker.stop();
antbig 0:ad97421fb1fb 1011 screenChecktry = 0;
antbig 0:ad97421fb1fb 1012 countAliveCard++;
antbig 11:ed13a480ddca 1013 checkCurrent++;
antbig 0:ad97421fb1fb 1014 if(checkCurrent >= NOMBRE_CARTES) {
Sitkah 29:41e02746041d 1015 printf("all card check, missing %d cards\n",(NOMBRE_CARTES-countAliveCard));
antbig 0:ad97421fb1fb 1016 if(countAliveCard >= NOMBRE_CARTES) {
antbig 0:ad97421fb1fb 1017 gameEtat = ETAT_CONFIG;
Sitkah 38:76f886a1c8e6 1018 SendRawId(ECRAN_ALL_CHECK);
Sitkah 29:41e02746041d 1019 flag=1;
Artiom 44:badcbe8766e9 1020
Sitkah 30:a1e37af4bbde 1021 //tactile_printf("Selection couleur et strategie");
Artiom 44:badcbe8766e9 1022 } else {
antbig 0:ad97421fb1fb 1023 gameEtat = ETAT_WAIT_FORCE;//Passage en attente de forçage du lancement
antbig 0:ad97421fb1fb 1024 waitingAckFrom = ECRAN_ALL_CHECK;
antbig 0:ad97421fb1fb 1025 }
Artiom 44:badcbe8766e9 1026 } else
antbig 0:ad97421fb1fb 1027 gameEtat = ETAT_CHECK_CARTES;
Artiom 44:badcbe8766e9 1028 } else if(cartesCheker.read_ms () > 100) {
antbig 0:ad97421fb1fb 1029 cartesCheker.stop();
antbig 0:ad97421fb1fb 1030 if(screenChecktry >=3) {
antbig 12:14729d584500 1031 //printf("missing card %d\n",id_check[checkCurrent]);
antbig 0:ad97421fb1fb 1032 screenChecktry = 0;
antbig 11:ed13a480ddca 1033 checkCurrent++;
Artiom 44:badcbe8766e9 1034
Artiom 44:badcbe8766e9 1035 if(checkCurrent >= NOMBRE_CARTES) {
Artiom 44:badcbe8766e9 1036 if(countAliveCard == NOMBRE_CARTES) {
antbig 0:ad97421fb1fb 1037 gameEtat = ETAT_CONFIG;
Artiom 44:badcbe8766e9 1038 flag=1;
Artiom 44:badcbe8766e9 1039 } else {
Sitkah 29:41e02746041d 1040 gameEtat = ETAT_WAIT_FORCE;
antbig 0:ad97421fb1fb 1041 waitingAckFrom = ECRAN_ALL_CHECK;
antbig 0:ad97421fb1fb 1042 }
Artiom 44:badcbe8766e9 1043 } else
antbig 0:ad97421fb1fb 1044 gameEtat = ETAT_CHECK_CARTES;
Artiom 44:badcbe8766e9 1045
Artiom 44:badcbe8766e9 1046 } else
antbig 0:ad97421fb1fb 1047 gameEtat = ETAT_CHECK_CARTES;
Artiom 44:badcbe8766e9 1048
antbig 0:ad97421fb1fb 1049 }
Sitkah 29:41e02746041d 1050 break;
antbig 0:ad97421fb1fb 1051 case ETAT_WAIT_FORCE:
antbig 0:ad97421fb1fb 1052 /*
antbig 0:ad97421fb1fb 1053 Attente du forçage de la part de la carte IHM
antbig 0:ad97421fb1fb 1054 */
antbig 0:ad97421fb1fb 1055 if(waitingAckFrom == 0) {
antbig 0:ad97421fb1fb 1056 gameEtat = ETAT_CONFIG;
antbig 0:ad97421fb1fb 1057 }
Sitkah 29:41e02746041d 1058 break;
antbig 0:ad97421fb1fb 1059 case ETAT_CONFIG:
antbig 0:ad97421fb1fb 1060 /*
antbig 0:ad97421fb1fb 1061 Attente de l'odre de choix de mode,
antbig 0:ad97421fb1fb 1062 Il est possible de modifier la couleur et l'id de la stratégie
antbig 0:ad97421fb1fb 1063 Il est aussi possible d'envoyer les ordres de debug
antbig 0:ad97421fb1fb 1064 */
ClementBreteau 14:c8fc06c4887f 1065 modeTelemetre = 0;
Sitkah 29:41e02746041d 1066 break;
antbig 1:116040d14164 1067 case ETAT_GAME_INIT:
antbig 0:ad97421fb1fb 1068 //On charge la liste des instructions
Artiom 44:badcbe8766e9 1069
Sitkah 29:41e02746041d 1070 loadAllInstruction(Strategie);//Mise en cache de toute les instructions
Sitkah 29:41e02746041d 1071 led3=1;
Artiom 44:badcbe8766e9 1072
Sitkah 37:fca332b64b42 1073 SendRawId(GLOBAL_START);
Artiom 44:badcbe8766e9 1074
antbig 0:ad97421fb1fb 1075 gameEtat = ETAT_GAME_WAIT_FOR_JACK;
Artiom 44:badcbe8766e9 1076 if (etat == TEST_TELEMETRE|| etat ==TEST_CAPTEURS || etat == TEST_SERVO || etat ==TEST_TIR || etat == DEMO_IMMEUBLE) {
Sitkah 29:41e02746041d 1077 SendRawId(DEBUG_FAKE_JAKE);
Artiom 44:badcbe8766e9 1078 } else {
Artiom 44:badcbe8766e9 1079 etat = AFF_WAIT_JACK;
Sitkah 29:41e02746041d 1080 }
Sitkah 30:a1e37af4bbde 1081 //tactile_printf("Attente du JACK.");
antbig 12:14729d584500 1082 setAsservissementEtat(1);//On réactive l'asservissement
antbig 12:14729d584500 1083 jack.mode(PullDown); // désactivation de la résistance interne du jack
antbig 8:0edc7dfb7f7e 1084 jack.fall(&jack_ISR); // création de l'interrupt attachée au changement d'état (front descendant) sur le jack
Artiom 44:badcbe8766e9 1085
clementlignie 22:a466d08ac42b 1086 localData2 = POSITION_DEBUT_T;
clementlignie 22:a466d08ac42b 1087 localData3 = POSITION_DEBUT_Y;
clementlignie 22:a466d08ac42b 1088 if(InversStrat == 1) {
clementlignie 22:a466d08ac42b 1089 localData2 = -localData2;//Inversion theta
clementlignie 22:a466d08ac42b 1090 localData3 = 3000 - POSITION_DEBUT_Y;//Inversion du Y
clementlignie 22:a466d08ac42b 1091 }
Sitkah 38:76f886a1c8e6 1092 SetOdometrie(ODOMETRIE_SMALL_POSITION, POSITION_DEBUT_X,1800,localData2);
Artiom 44:badcbe8766e9 1093
Sitkah 38:76f886a1c8e6 1094 instruction = strat_instructions[actual_instruction];
Artiom 44:badcbe8766e9 1095 //On effectue le traitement de l'instruction
Artiom 44:badcbe8766e9 1096
Artiom 44:badcbe8766e9 1097 break;
antbig 0:ad97421fb1fb 1098 case ETAT_GAME_WAIT_FOR_JACK:
Artiom 44:badcbe8766e9 1099 if(instruction.order==POSITION_DEBUT) {
Artiom 44:badcbe8766e9 1100 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 !!!)
Sitkah 38:76f886a1c8e6 1101 case RECALAGE_1:
Sitkah 38:76f886a1c8e6 1102 waitingAckID = ASSERVISSEMENT_RECALAGE;
Artiom 44:badcbe8766e9 1103 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 44:badcbe8766e9 1104 #ifdef ROBOT_SMALL
Artiom 46:a9b6bcb30b1c 1105 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)
Artiom 44:badcbe8766e9 1106 #else
Artiom 46:a9b6bcb30b1c 1107 GoStraight(-3000, 1,MOITIEE_ROBOT, 0);
Artiom 44:badcbe8766e9 1108 #endif
Sitkah 38:76f886a1c8e6 1109 while(waitingAckID !=0 && waitingAckFrom !=0)
Sitkah 38:76f886a1c8e6 1110 canProcessRx();
Sitkah 38:76f886a1c8e6 1111 waitingAckID_FIN=ASSERVISSEMENT_RECALAGE;
Artiom 44:badcbe8766e9 1112 waitingAckFrom_FIN= INSTRUCTION_END_MOTEUR;
Sitkah 38:76f886a1c8e6 1113 while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
Sitkah 38:76f886a1c8e6 1114 canProcessRx();
Sitkah 38:76f886a1c8e6 1115 etat_pos=RECULER_1;
Sitkah 38:76f886a1c8e6 1116 break;
Artiom 44:badcbe8766e9 1117
Sitkah 38:76f886a1c8e6 1118 case RECULER_1:
Sitkah 38:76f886a1c8e6 1119 waitingAckID = ASSERVISSEMENT_RECALAGE;
Sitkah 38:76f886a1c8e6 1120 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 44:badcbe8766e9 1121 #ifdef ROBOT_SMALL
Artiom 46:a9b6bcb30b1c 1122 GoStraight(-100, 0, 0, 0);//-450
Artiom 44:badcbe8766e9 1123 #else
Sitkah 41:b029ddc4d60e 1124 GoStraight(150, 0, 0, 0);
Artiom 44:badcbe8766e9 1125 #endif
Sitkah 38:76f886a1c8e6 1126 while(waitingAckID !=0 && waitingAckFrom !=0)
Sitkah 38:76f886a1c8e6 1127 canProcessRx();
Sitkah 38:76f886a1c8e6 1128 waitingAckID_FIN=ASSERVISSEMENT_RECALAGE;
Artiom 44:badcbe8766e9 1129 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Sitkah 38:76f886a1c8e6 1130 while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
Sitkah 38:76f886a1c8e6 1131 canProcessRx();
Sitkah 38:76f886a1c8e6 1132 etat_pos=TOURNER;
Artiom 44:badcbe8766e9 1133 break;
Artiom 44:badcbe8766e9 1134
Sitkah 38:76f886a1c8e6 1135 case TOURNER:
Sitkah 38:76f886a1c8e6 1136 waitingAckID = ASSERVISSEMENT_ROTATION;
Artiom 44:badcbe8766e9 1137 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 44:badcbe8766e9 1138 if(Cote==0) {
Sitkah 38:76f886a1c8e6 1139 localData2 = 900;
Artiom 44:badcbe8766e9 1140 } else {
Sitkah 38:76f886a1c8e6 1141 localData2=-900;
Sitkah 38:76f886a1c8e6 1142 }
Sitkah 38:76f886a1c8e6 1143 Rotate(localData2);
Sitkah 38:76f886a1c8e6 1144 while(waitingAckID !=0 && waitingAckFrom !=0)
Sitkah 38:76f886a1c8e6 1145 canProcessRx();
Sitkah 38:76f886a1c8e6 1146 waitingAckID_FIN=ASSERVISSEMENT_ROTATION;
Artiom 44:badcbe8766e9 1147 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Sitkah 38:76f886a1c8e6 1148 while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
Sitkah 38:76f886a1c8e6 1149 canProcessRx();
Sitkah 38:76f886a1c8e6 1150 etat_pos=RECALAGE_2;
Artiom 44:badcbe8766e9 1151 break;
Artiom 44:badcbe8766e9 1152
Sitkah 38:76f886a1c8e6 1153 case RECALAGE_2:
Sitkah 38:76f886a1c8e6 1154 waitingAckID = ASSERVISSEMENT_RECALAGE;
Artiom 44:badcbe8766e9 1155 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Sitkah 38:76f886a1c8e6 1156 if(Cote==1)
Artiom 46:a9b6bcb30b1c 1157 localData3=3000-(MOITIEE_ROBOT);
Sitkah 38:76f886a1c8e6 1158 else
Sitkah 38:76f886a1c8e6 1159 localData3=MOITIEE_ROBOT;
Artiom 44:badcbe8766e9 1160 #ifdef ROBOT_SMALL
Sitkah 38:76f886a1c8e6 1161 GoStraight(3000, 2,localData3, 0); //on se recale contre le mur donc il faut donner la valeur du centre du robot
Artiom 44:badcbe8766e9 1162 #else
Sitkah 38:76f886a1c8e6 1163 GoStraight(-3000, 2,localData3, 0);
Artiom 44:badcbe8766e9 1164 #endif
Sitkah 38:76f886a1c8e6 1165 while(waitingAckID !=0 && waitingAckFrom !=0)
Sitkah 38:76f886a1c8e6 1166 canProcessRx();
Sitkah 38:76f886a1c8e6 1167 waitingAckID_FIN=ASSERVISSEMENT_RECALAGE;
Artiom 44:badcbe8766e9 1168 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Sitkah 38:76f886a1c8e6 1169 while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
Sitkah 38:76f886a1c8e6 1170 canProcessRx();
Sitkah 38:76f886a1c8e6 1171 etat_pos=RECULER_2;
Artiom 44:badcbe8766e9 1172 break;
Artiom 44:badcbe8766e9 1173
Artiom 44:badcbe8766e9 1174 case RECULER_2:
Sitkah 38:76f886a1c8e6 1175 waitingAckID = ASSERVISSEMENT_RECALAGE;
Artiom 44:badcbe8766e9 1176 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 44:badcbe8766e9 1177 #ifdef ROBOT_SMALL
Artiom 46:a9b6bcb30b1c 1178 GoStraight(-100, 0, 0, 0);
Artiom 44:badcbe8766e9 1179 #else
Sitkah 38:76f886a1c8e6 1180 GoStraight(200, 0, 0, 0);
Artiom 44:badcbe8766e9 1181 #endif
Sitkah 38:76f886a1c8e6 1182 while(waitingAckID !=0 && waitingAckFrom !=0)
Sitkah 38:76f886a1c8e6 1183 canProcessRx();
Sitkah 38:76f886a1c8e6 1184 waitingAckID_FIN=ASSERVISSEMENT_RECALAGE;
Artiom 44:badcbe8766e9 1185 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Sitkah 38:76f886a1c8e6 1186 while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
Sitkah 38:76f886a1c8e6 1187 canProcessRx();
Sitkah 38:76f886a1c8e6 1188 etat_pos=GOTOPOS;
Sitkah 38:76f886a1c8e6 1189 break;
Artiom 44:badcbe8766e9 1190
Sitkah 38:76f886a1c8e6 1191 case GOTOPOS:
Sitkah 38:76f886a1c8e6 1192 localData1 = -1;
Artiom 44:badcbe8766e9 1193
Sitkah 38:76f886a1c8e6 1194 if(InversStrat == 1 && ingnorInversionOnce == 0) {
Sitkah 38:76f886a1c8e6 1195 localData2 = -instruction.arg3;
Sitkah 38:76f886a1c8e6 1196 localData3 = 3000 - instruction.arg2;//Inversion du Y
Sitkah 38:76f886a1c8e6 1197 } else {
Sitkah 38:76f886a1c8e6 1198 localData3 = instruction.arg2;
Sitkah 38:76f886a1c8e6 1199 localData2 = instruction.arg3;
Sitkah 38:76f886a1c8e6 1200 }
Artiom 44:badcbe8766e9 1201
Sitkah 38:76f886a1c8e6 1202 GoToPosition(instruction.arg1,localData3,localData2,localData1);
Sitkah 38:76f886a1c8e6 1203 waitingAckID = ASSERVISSEMENT_XYT;
Sitkah 38:76f886a1c8e6 1204 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 44:badcbe8766e9 1205
Sitkah 38:76f886a1c8e6 1206 while(waitingAckID !=0 && waitingAckFrom !=0)
Sitkah 38:76f886a1c8e6 1207 canProcessRx();
Sitkah 38:76f886a1c8e6 1208 waitingAckID_FIN=ASSERVISSEMENT_XYT;
Artiom 44:badcbe8766e9 1209 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Sitkah 38:76f886a1c8e6 1210 while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
Sitkah 38:76f886a1c8e6 1211 canProcessRx();
Sitkah 38:76f886a1c8e6 1212 etat_pos=FIN_POS;
Artiom 44:badcbe8766e9 1213 break;
Sitkah 38:76f886a1c8e6 1214 case FIN_POS:
Sitkah 38:76f886a1c8e6 1215 actual_instruction = instruction.nextLineOK;
Artiom 44:badcbe8766e9 1216 break;
Sitkah 38:76f886a1c8e6 1217 }
Sitkah 38:76f886a1c8e6 1218 }
Artiom 44:badcbe8766e9 1219
Artiom 44:badcbe8766e9 1220
Artiom 44:badcbe8766e9 1221 break;
antbig 1:116040d14164 1222 case ETAT_GAME_START:
Artiom 44:badcbe8766e9 1223
antbig 1:116040d14164 1224 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
Artiom 44:badcbe8766e9 1225
Artiom 44:badcbe8766e9 1226 if (ModeDemo == 0) {
Sitkah 34:6aa4b46b102e 1227 chronoEnd.attach(&chronometre_ISR,100);//On lance le chrono de 90s
ClementBreteau 14:c8fc06c4887f 1228 gameTimer.start();
Artiom 44:badcbe8766e9 1229 }
antbig 1:116040d14164 1230 gameTimer.reset();
antbig 12:14729d584500 1231 jack.fall(NULL);//On désactive l'interruption du jack
Artiom 44:badcbe8766e9 1232 //SendRawId(GLOBAL_START);
Sitkah 29:41e02746041d 1233 Jack=0; //à envoyer sur le CAN et en direct pour l'automate de l'ihm ou sur CANV
Sitkah 30:a1e37af4bbde 1234 //tactile_printf("Start");//Pas vraiment utile mais bon
Artiom 44:badcbe8766e9 1235 break;
antbig 0:ad97421fb1fb 1236 case ETAT_GAME_LOAD_NEXT_INSTRUCTION:
antbig 0:ad97421fb1fb 1237 /*
antbig 0:ad97421fb1fb 1238 Chargement de l'instruction suivante ou arret du robot si il n'y a plus d'instruction
antbig 0:ad97421fb1fb 1239 */
antbig 0:ad97421fb1fb 1240 //printf("load next instruction\n");
Artiom 44:badcbe8766e9 1241
antbig 0:ad97421fb1fb 1242 if(actual_instruction >= nb_instructions || actual_instruction == 255) {
antbig 0:ad97421fb1fb 1243 gameEtat = ETAT_END;
antbig 0:ad97421fb1fb 1244 //Il n'y a plus d'instruction, fin du jeu
antbig 0:ad97421fb1fb 1245 } else {
antbig 0:ad97421fb1fb 1246 instruction = strat_instructions[actual_instruction];
Artiom 44:badcbe8766e9 1247 //On effectue le traitement de l'instruction
antbig 0:ad97421fb1fb 1248 gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;
antbig 0:ad97421fb1fb 1249 }
antbig 0:ad97421fb1fb 1250 screenChecktry = 0;
antbig 28:acd18776ed2d 1251 ingnorInversionOnce = 0;
Artiom 44:badcbe8766e9 1252 break;
antbig 0:ad97421fb1fb 1253 case ETAT_GAME_PROCESS_INSTRUCTION:
antbig 0:ad97421fb1fb 1254 /*
antbig 0:ad97421fb1fb 1255 Traitement de l'instruction, envoie de la trame CAN
antbig 0:ad97421fb1fb 1256 */
antbig 0:ad97421fb1fb 1257 //debug_Instruction(instruction);
Sitkah 38:76f886a1c8e6 1258 //affichage_debug(gameEtat);
ClementBreteau 25:f140c93a8666 1259 actionPrecedente = instruction.order;
Artiom 44:badcbe8766e9 1260 switch(instruction.order) {
antbig 12:14729d584500 1261 case MV_COURBURE://C'est un rayon de courbure
ClementBreteau 26:2f4fcc2354f3 1262 actionPrecedente = MV_COURBURE;
antbig 6:eddfa414fd11 1263 waitingAckID = ASSERVISSEMENT_COURBURE;
antbig 6:eddfa414fd11 1264 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 44:badcbe8766e9 1265 if(instruction.nextActionType == ENCHAINEMENT) {
antbig 6:eddfa414fd11 1266 MV_enchainement++;
antbig 6:eddfa414fd11 1267 localData5 = 1;
antbig 6:eddfa414fd11 1268 } else {
antbig 6:eddfa414fd11 1269 if(MV_enchainement > 0) {
antbig 6:eddfa414fd11 1270 localData5 = 2;
antbig 6:eddfa414fd11 1271 MV_enchainement = 0;
antbig 6:eddfa414fd11 1272 } else {
antbig 6:eddfa414fd11 1273 localData5 = 0;
antbig 6:eddfa414fd11 1274 }
antbig 0:ad97421fb1fb 1275 }
antbig 11:ed13a480ddca 1276 localData1 = ((instruction.direction == LEFT)?1:-1);
ClementBreteau 23:ab87d308eaf9 1277 localData2 = instruction.arg3;
Artiom 44:badcbe8766e9 1278 if(InversStrat == 1 && ingnorInversionOnce == 0) {
antbig 12:14729d584500 1279 localData1 = -localData1;//Inversion de la direction
antbig 12:14729d584500 1280 }
ClementBreteau 25:f140c93a8666 1281 BendRadius(instruction.arg1, localData2, localData1, localData5);
Artiom 44:badcbe8766e9 1282
Artiom 44:badcbe8766e9 1283
antbig 28:acd18776ed2d 1284 target_theta_robot = localData2 - theta_robot;
antbig 28:acd18776ed2d 1285 /*
ClementBreteau 26:2f4fcc2354f3 1286 if(instruction.direction == LEFT){
Artiom 44:badcbe8766e9 1287
ClementBreteau 26:2f4fcc2354f3 1288 }else{
antbig 28:acd18776ed2d 1289 target_theta_robot = theta_robot + localData2;
antbig 28:acd18776ed2d 1290 }*/
Artiom 44:badcbe8766e9 1291
Artiom 44:badcbe8766e9 1292 break;
antbig 12:14729d584500 1293 case MV_LINE://Ligne droite
antbig 0:ad97421fb1fb 1294 waitingAckID = ASSERVISSEMENT_RECALAGE;
antbig 0:ad97421fb1fb 1295 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
antbig 0:ad97421fb1fb 1296 if(instruction.nextActionType == ENCHAINEMENT) {
antbig 1:116040d14164 1297 MV_enchainement++;
antbig 1:116040d14164 1298 localData5 = 1;
antbig 1:116040d14164 1299 } else {
Artiom 44:badcbe8766e9 1300 if(MV_enchainement > 0) {//Utilisé en cas d'enchainement,
antbig 1:116040d14164 1301 localData5 = 2;
antbig 1:116040d14164 1302 MV_enchainement = 0;
antbig 1:116040d14164 1303 } else {
antbig 1:116040d14164 1304 localData5 = 0;
antbig 1:116040d14164 1305 }
Artiom 44:badcbe8766e9 1306 }
antbig 1:116040d14164 1307 localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1);
antbig 1:116040d14164 1308 GoStraight(localData2, 0, 0, localData5);
Artiom 44:badcbe8766e9 1309
Artiom 44:badcbe8766e9 1310 target_x_robot = x_robot + localData2*cos((double)theta_robot*M_PI/1800);
ClementBreteau 26:2f4fcc2354f3 1311 target_y_robot = y_robot + localData2*sin((double)theta_robot*M_PI/1800);
ClementBreteau 25:f140c93a8666 1312 target_theta_robot = theta_robot;
Artiom 44:badcbe8766e9 1313
Artiom 44:badcbe8766e9 1314 break;
antbig 12:14729d584500 1315 case MV_TURN: //Rotation sur place
antbig 0:ad97421fb1fb 1316 if(instruction.direction == RELATIVE) {
antbig 0:ad97421fb1fb 1317 localData2 = instruction.arg3;
antbig 12:14729d584500 1318 } else {//C'est un rotation absolu, il faut la convertir en relative
antbig 12:14729d584500 1319 localData2 = instruction.arg3;
Artiom 52:a47350923b5e 1320
antbig 12:14729d584500 1321 localData2 = (localData2 - theta_robot)%3600;
antbig 12:14729d584500 1322 if(localData2 > 1800) {
antbig 12:14729d584500 1323 localData2 = localData2-3600;
antbig 12:14729d584500 1324 }
Artiom 52:a47350923b5e 1325 else if(localData2 <-1800){
Artiom 52:a47350923b5e 1326 localData2 = localData2+3600;
Artiom 52:a47350923b5e 1327 }
antbig 0:ad97421fb1fb 1328 }
antbig 28:acd18776ed2d 1329 if(InversStrat == 1 && ingnorInversionOnce == 0) {
Artiom 44:badcbe8766e9 1330 localData2 = -localData2;
Artiom 44:badcbe8766e9 1331 }
antbig 0:ad97421fb1fb 1332 waitingAckID = ASSERVISSEMENT_ROTATION;
antbig 0:ad97421fb1fb 1333 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Sitkah 34:6aa4b46b102e 1334 Rotate(localData2);
Artiom 44:badcbe8766e9 1335
Artiom 44:badcbe8766e9 1336
Artiom 44:badcbe8766e9 1337 break;
antbig 0:ad97421fb1fb 1338 case MV_XYT:
antbig 0:ad97421fb1fb 1339 if(instruction.direction == BACKWARD) {
antbig 0:ad97421fb1fb 1340 localData1 = -1;
antbig 0:ad97421fb1fb 1341 } else {
antbig 0:ad97421fb1fb 1342 localData1 = 1;
antbig 0:ad97421fb1fb 1343 }
Artiom 44:badcbe8766e9 1344
antbig 28:acd18776ed2d 1345 if(InversStrat == 1 && ingnorInversionOnce == 0) {
antbig 12:14729d584500 1346 localData2 = -instruction.arg3;
antbig 2:8d8e2cf798a3 1347 localData3 = 3000 - instruction.arg2;//Inversion du Y
antbig 2:8d8e2cf798a3 1348 } else {
antbig 2:8d8e2cf798a3 1349 localData3 = instruction.arg2;
antbig 12:14729d584500 1350 localData2 = instruction.arg3;
antbig 2:8d8e2cf798a3 1351 }
Artiom 44:badcbe8766e9 1352
antbig 12:14729d584500 1353 GoToPosition(instruction.arg1,localData3,localData2,localData1);
antbig 0:ad97421fb1fb 1354 waitingAckID = ASSERVISSEMENT_XYT;
antbig 0:ad97421fb1fb 1355 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 44:badcbe8766e9 1356
Artiom 44:badcbe8766e9 1357 target_x_robot = instruction.arg1;
ClementBreteau 25:f140c93a8666 1358 target_y_robot = localData3;
ClementBreteau 25:f140c93a8666 1359 target_theta_robot = localData2;
Artiom 44:badcbe8766e9 1360
Artiom 44:badcbe8766e9 1361 break;
antbig 0:ad97421fb1fb 1362 case MV_RECALAGE:
Artiom 44:badcbe8766e9 1363 if(instruction.nextActionType == MECANIQUE) {
Sitkah 34:6aa4b46b102e 1364 instruction.nextActionType = WAIT;
Artiom 44:badcbe8766e9 1365
Sitkah 34:6aa4b46b102e 1366 waitingAckID = ASSERVISSEMENT_RECALAGE;
Sitkah 34:6aa4b46b102e 1367 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 44:badcbe8766e9 1368
Sitkah 34:6aa4b46b102e 1369 localData2 = (((instruction.direction == FORWARD)?1:-1)*3000);//On indique une distance de 3000 pour etre sur que le robot va ce recaler
Artiom 44:badcbe8766e9 1370
Sitkah 34:6aa4b46b102e 1371 if(instruction.precision == RECALAGE_Y) {
Sitkah 34:6aa4b46b102e 1372 localData5 = 2;
Sitkah 34:6aa4b46b102e 1373 if(InversStrat == 1 && ingnorInversionOnce == 0) {
Sitkah 34:6aa4b46b102e 1374 localData3 = 3000 - instruction.arg1;//Inversion du Y
Sitkah 34:6aa4b46b102e 1375 } else {
Sitkah 34:6aa4b46b102e 1376 localData3 = instruction.arg1;
Sitkah 34:6aa4b46b102e 1377 }
antbig 11:ed13a480ddca 1378 } else {
Sitkah 34:6aa4b46b102e 1379 localData5 = 1;
antbig 11:ed13a480ddca 1380 localData3 = instruction.arg1;
antbig 11:ed13a480ddca 1381 }
Sitkah 34:6aa4b46b102e 1382 GoStraight(localData2, localData5, localData3, 0);
Artiom 44:badcbe8766e9 1383 } else { //CAPTEUR
Sitkah 34:6aa4b46b102e 1384 SendRawId(DATA_RECALAGE);
Sitkah 34:6aa4b46b102e 1385 waitingAckID = RECEPTION_RECALAGE;
Sitkah 34:6aa4b46b102e 1386 waitingAckFrom = ACKNOWLEDGE_TELEMETRE;
Artiom 44:badcbe8766e9 1387
Sitkah 34:6aa4b46b102e 1388 // On attend que les variables soient actualisé
Sitkah 34:6aa4b46b102e 1389 while(!(waitingAckID == 0 && waitingAckFrom == 0))
Sitkah 34:6aa4b46b102e 1390 canProcessRx();
Sitkah 35:742dc6b200b0 1391 while(!(waitingAckID_FIN==0 && waitingAckFrom_FIN==0))
Sitkah 35:742dc6b200b0 1392 canProcessRx();
Artiom 44:badcbe8766e9 1393
Artiom 44:badcbe8766e9 1394 if(instruction.precision == RECALAGE_Y) { // ((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600)) (theta_robot < 900 && theta_robot > -900)
Sitkah 36:6dd30780bd8e 1395 SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, recalageDistanceY(), theta_robot);
Artiom 44:badcbe8766e9 1396 } else if(instruction.precision == RECALAGE_X) {
Artiom 44:badcbe8766e9 1397 SetOdometrie(ODOMETRIE_SMALL_POSITION, recalageDistanceX(), y_robot, theta_robot);
Artiom 44:badcbe8766e9 1398 } else if(instruction.precision == RECALAGE_T) {
Artiom 44:badcbe8766e9 1399 SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, y_robot, recalageAngulaireCapteur() );
Sitkah 34:6aa4b46b102e 1400 }
Sitkah 34:6aa4b46b102e 1401 }
Artiom 44:badcbe8766e9 1402 break;
Artiom 44:badcbe8766e9 1403
antbig 0:ad97421fb1fb 1404 case ACTION:
ClementBreteau 18:cc5fec34ed9c 1405 int tempo = 0;
Sitkah 30:a1e37af4bbde 1406 waitingAckID= ACK_ACTION; //On veut un ack de type action
Sitkah 30:a1e37af4bbde 1407 waitingAckFrom = ACKNOWLEDGE_HERKULEX; //de la part des herkulex
ClementBreteau 18:cc5fec34ed9c 1408 tempo = doAction(instruction.arg1,instruction.arg2,instruction.arg3);
Artiom 44:badcbe8766e9 1409 if(tempo == 1) {
antbig 0:ad97421fb1fb 1410 //L'action est spécifique
Sitkah 34:6aa4b46b102e 1411 if((waitingAckFrom == 0 && waitingAckID == 0) && instruction.nextActionType == ENCHAINEMENT) {
antbig 11:ed13a480ddca 1412 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
antbig 11:ed13a480ddca 1413 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
Artiom 44:badcbe8766e9 1414 } else {
ClementBreteau 15:c2fc239e85df 1415 gameEtat = ETAT_GAME_WAIT_ACK;
antbig 11:ed13a480ddca 1416 }
Artiom 44:badcbe8766e9 1417 #ifdef ROBOT_SMALL
Artiom 44:badcbe8766e9 1418 /*actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
Artiom 44:badcbe8766e9 1419 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;*/
Artiom 44:badcbe8766e9 1420 #endif
antbig 11:ed13a480ddca 1421 return;
Artiom 44:badcbe8766e9 1422 #ifdef ROBOT_SMALL
ClementBreteau 18:cc5fec34ed9c 1423 } else if (tempo == 2) {
ClementBreteau 18:cc5fec34ed9c 1424 // on est dans le cas de l'avance selon le telemetre
ClementBreteau 18:cc5fec34ed9c 1425 waitingAckID = ASSERVISSEMENT_RECALAGE;
ClementBreteau 18:cc5fec34ed9c 1426 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 44:badcbe8766e9 1427
ClementBreteau 18:cc5fec34ed9c 1428 localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1);
ClementBreteau 18:cc5fec34ed9c 1429 GoStraight(telemetreDistance, 0, 0, 0);
ClementBreteau 18:cc5fec34ed9c 1430 // on reset la distance du telemetre à 0
ClementBreteau 18:cc5fec34ed9c 1431 telemetreDistance = 5000;
Artiom 44:badcbe8766e9 1432 #endif
Artiom 44:badcbe8766e9 1433 } else {
antbig 0:ad97421fb1fb 1434 //C'est un AX12 qu'il faut bouger
ClementBreteau 15:c2fc239e85df 1435 //AX12_setGoal(instruction.arg1,instruction.arg3/10,instruction.arg2);
ClementBreteau 15:c2fc239e85df 1436 //AX12_enchainement++;
Artiom 44:badcbe8766e9 1437
antbig 0:ad97421fb1fb 1438 }
Artiom 44:badcbe8766e9 1439 break;
antbig 0:ad97421fb1fb 1440 default:
antbig 0:ad97421fb1fb 1441 //Instruction inconnue, on l'ignore
Artiom 44:badcbe8766e9 1442 break;
Artiom 44:badcbe8766e9 1443 }
Artiom 44:badcbe8766e9 1444
Artiom 44:badcbe8766e9 1445
Artiom 44:badcbe8766e9 1446
antbig 0:ad97421fb1fb 1447 if(instruction.nextActionType == JUMP || instruction.nextActionType == WAIT) {
antbig 0:ad97421fb1fb 1448 gameEtat = ETAT_GAME_WAIT_ACK;//Il faut attendre que la carte est bien reçu l'acknowledge
antbig 0:ad97421fb1fb 1449 screenChecktry++;//On incrèment le conteur de tentative de 1
antbig 0:ad97421fb1fb 1450 cartesCheker.reset();//On reset le timeOut
antbig 0:ad97421fb1fb 1451 cartesCheker.start();
antbig 1:116040d14164 1452 if(AX12_enchainement > 0) {
ClementBreteau 15:c2fc239e85df 1453 //AX12_processChange();//Il faut lancer le déplacement des AX12
ClementBreteau 15:c2fc239e85df 1454 //AX12_enchainement = 0;
antbig 1:116040d14164 1455 }
Artiom 44:badcbe8766e9 1456 } else { //C'est un enchainement
Artiom 44:badcbe8766e9 1457 if(instruction.order == MV_LINE) {
Artiom 44:badcbe8766e9 1458 gameEtat = ETAT_GAME_WAIT_ACK;
Artiom 44:badcbe8766e9 1459 } else {
ClementBreteau 15:c2fc239e85df 1460 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
ClementBreteau 15:c2fc239e85df 1461 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//C'est un enchainement, on charge directement l'instruction suivante
ClementBreteau 15:c2fc239e85df 1462 }
antbig 0:ad97421fb1fb 1463 }
Artiom 44:badcbe8766e9 1464
Artiom 44:badcbe8766e9 1465 break;
antbig 0:ad97421fb1fb 1466 case ETAT_GAME_WAIT_ACK:
Sitkah 36:6dd30780bd8e 1467 canProcessRx();
Artiom 44:badcbe8766e9 1468
Sitkah 30:a1e37af4bbde 1469 if(waitingAckID == 0 && waitingAckFrom == 0) {//Les ack ont été reset, c'est bon on continue
Artiom 44:badcbe8766e9 1470 //if(true) {
antbig 0:ad97421fb1fb 1471 cartesCheker.stop();
antbig 0:ad97421fb1fb 1472 if(instruction.nextActionType == JUMP) {
antbig 11:ed13a480ddca 1473 if(instruction.jumpAction == JUMP_POSITION) {
antbig 11:ed13a480ddca 1474 gameEtat = ETAT_GAME_JUMP_POSITION;
Artiom 44:badcbe8766e9 1475 } else { //Pour eviter les erreurs, on dit que c'est par défaut un jump time
antbig 0:ad97421fb1fb 1476 gameEtat = ETAT_GAME_JUMP_TIME;
antbig 0:ad97421fb1fb 1477 cartesCheker.reset();//On reset le timeOut
Artiom 44:badcbe8766e9 1478 cartesCheker.start();
antbig 0:ad97421fb1fb 1479 }
Artiom 44:badcbe8766e9 1480 } else if(instruction.nextActionType == WAIT) { ///Actualisation des waiting ack afin d'attendre la fin des actions
Sitkah 41:b029ddc4d60e 1481 /*wait_ms(200);
Sitkah 40:21bb685b553b 1482 #ifdef ROBOT_BIG
Sitkah 40:21bb685b553b 1483 SetOdometrie(ODOMETRIE_BIG_POSITION, x_robot, y_robot, theta_robot);
Sitkah 40:21bb685b553b 1484 #else
Sitkah 40:21bb685b553b 1485 SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, y_robot, theta_robot);
Sitkah 40:21bb685b553b 1486 #endif
Sitkah 41:b029ddc4d60e 1487 wait_ms(200);*/
Artiom 44:badcbe8766e9 1488
Sitkah 40:21bb685b553b 1489 gameEtat = ETAT_GAME_WAIT_END_INSTRUCTION;
Artiom 44:badcbe8766e9 1490 switch(instruction.order) {
antbig 0:ad97421fb1fb 1491 case MV_COURBURE:
Sitkah 34:6aa4b46b102e 1492 waitingAckID_FIN = ASSERVISSEMENT_COURBURE;
Sitkah 34:6aa4b46b102e 1493 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Artiom 44:badcbe8766e9 1494 break;
antbig 0:ad97421fb1fb 1495 case MV_LINE:
Sitkah 34:6aa4b46b102e 1496 waitingAckID_FIN = ASSERVISSEMENT_RECALAGE;
Sitkah 34:6aa4b46b102e 1497 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Artiom 44:badcbe8766e9 1498 break;
antbig 0:ad97421fb1fb 1499 case MV_TURN:
Sitkah 34:6aa4b46b102e 1500 waitingAckID_FIN = ASSERVISSEMENT_ROTATION;
Sitkah 34:6aa4b46b102e 1501 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Artiom 44:badcbe8766e9 1502 break;
antbig 0:ad97421fb1fb 1503 case MV_XYT:
Sitkah 34:6aa4b46b102e 1504 waitingAckID_FIN = ASSERVISSEMENT_XYT;
Sitkah 34:6aa4b46b102e 1505 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Artiom 44:badcbe8766e9 1506 break;
antbig 0:ad97421fb1fb 1507 case MV_RECALAGE:
Sitkah 34:6aa4b46b102e 1508 waitingAckID_FIN = ASSERVISSEMENT_RECALAGE;
Sitkah 34:6aa4b46b102e 1509 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Artiom 44:badcbe8766e9 1510 break;
antbig 0:ad97421fb1fb 1511 case ACTION:
Artiom 44:badcbe8766e9 1512
Artiom 44:badcbe8766e9 1513 if (modeTelemetre == 0) {
Artiom 44:badcbe8766e9 1514 if (telemetreDistance == 0) {
Artiom 44:badcbe8766e9 1515 waitingAckID_FIN = ACK_FIN_ACTION;// ack de type action
Sitkah 34:6aa4b46b102e 1516 waitingAckFrom_FIN = ACKNOWLEDGE_HERKULEX; //de la part des herkulex/actionneurs
Artiom 44:badcbe8766e9 1517 } else if(telemetreDistance == 5000) {
ClementBreteau 18:cc5fec34ed9c 1518 // on est dans le cas ou l'on fait une ligne suivant la distance du telemetre
Sitkah 34:6aa4b46b102e 1519 waitingAckID_FIN = ASSERVISSEMENT_RECALAGE;
Sitkah 34:6aa4b46b102e 1520 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
ClementBreteau 18:cc5fec34ed9c 1521 telemetreDistance = 0;
ClementBreteau 18:cc5fec34ed9c 1522 }
Artiom 44:badcbe8766e9 1523 } else { // si on attend la reponse du telemetre
Artiom 44:badcbe8766e9 1524 //modeTelemetre = 1;
Sitkah 34:6aa4b46b102e 1525 waitingAckID_FIN = OBJET_SUR_TABLE;
Artiom 44:badcbe8766e9 1526 waitingAckFrom_FIN = 0;
ClementBreteau 15:c2fc239e85df 1527 }
Artiom 44:badcbe8766e9 1528 break;
antbig 0:ad97421fb1fb 1529 default:
Artiom 44:badcbe8766e9 1530 break;
Artiom 44:badcbe8766e9 1531 }
Artiom 44:badcbe8766e9 1532 } else {
antbig 0:ad97421fb1fb 1533 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
antbig 0:ad97421fb1fb 1534 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
antbig 0:ad97421fb1fb 1535 }
Artiom 44:badcbe8766e9 1536 } else if(cartesCheker.read_ms () > 1000) {
antbig 0:ad97421fb1fb 1537 cartesCheker.stop();
antbig 0:ad97421fb1fb 1538 if(screenChecktry >=2) {//La carte n'a pas reçus l'information, on passe à l'instruction d'erreur
antbig 0:ad97421fb1fb 1539 actual_instruction = instruction.nextLineError;
antbig 0:ad97421fb1fb 1540 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
Artiom 44:badcbe8766e9 1541 } else {
Sitkah 34:6aa4b46b102e 1542 gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;//On retourne dans l'etat d'envois de l'instruction
antbig 0:ad97421fb1fb 1543 }
antbig 0:ad97421fb1fb 1544 }
Artiom 44:badcbe8766e9 1545 break;
Artiom 44:badcbe8766e9 1546
antbig 0:ad97421fb1fb 1547 case ETAT_GAME_JUMP_TIME:
antbig 0:ad97421fb1fb 1548 if(cartesCheker.read_ms () >= instruction.JumpTimeOrX) {
antbig 0:ad97421fb1fb 1549 cartesCheker.stop();//On arrete le timer
antbig 0:ad97421fb1fb 1550 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
antbig 0:ad97421fb1fb 1551 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
antbig 0:ad97421fb1fb 1552 }
Artiom 44:badcbe8766e9 1553 break;
Artiom 44:badcbe8766e9 1554
ClementBreteau 15:c2fc239e85df 1555 case ETAT_GAME_JUMP_CONFIG:
ClementBreteau 15:c2fc239e85df 1556 signed int depasX = 1, depasY = 1; // servent à indiquer le sens de dépassement des coordonnées
Artiom 44:badcbe8766e9 1557 // 1 si l'instruction est plus grande que la position du robot
Artiom 44:badcbe8766e9 1558 // -1 si l'instruction est plus petite que la position du robot
Artiom 44:badcbe8766e9 1559 // 0 si l'instruction et position du robot sont proche de moins de 1cm
Artiom 44:badcbe8766e9 1560 if (abs(x_robot-instruction.JumpTimeOrX)<10) {
Artiom 44:badcbe8766e9 1561 depasX = 0;
Artiom 44:badcbe8766e9 1562 } else if(x_robot > instruction.JumpTimeOrX) {
Artiom 44:badcbe8766e9 1563 depasX = -1;
ClementBreteau 15:c2fc239e85df 1564 }
Artiom 44:badcbe8766e9 1565
Artiom 44:badcbe8766e9 1566 if(abs(y_robot-instruction.JumpY)<10) {
Artiom 44:badcbe8766e9 1567 depasY = 0;
Artiom 44:badcbe8766e9 1568 } else if(y_robot > instruction.JumpY) {
Artiom 44:badcbe8766e9 1569 depasY = -1;
ClementBreteau 15:c2fc239e85df 1570 }
Artiom 44:badcbe8766e9 1571
ClementBreteau 15:c2fc239e85df 1572 gameEtat = ETAT_GAME_JUMP_POSITION;
Artiom 44:badcbe8766e9 1573 break;
antbig 0:ad97421fb1fb 1574 case ETAT_GAME_JUMP_POSITION:
ClementBreteau 15:c2fc239e85df 1575 bool Xok = false, Yok = false;
Artiom 44:badcbe8766e9 1576
Artiom 44:badcbe8766e9 1577 if (depasX == 0) {
Artiom 44:badcbe8766e9 1578 Xok = true;
Artiom 44:badcbe8766e9 1579 } else if ((instruction.JumpTimeOrX - x_robot)*depasX < -5) {
Artiom 44:badcbe8766e9 1580 Xok = true;
Artiom 44:badcbe8766e9 1581 }
Artiom 44:badcbe8766e9 1582
Artiom 44:badcbe8766e9 1583 if (depasY == 0) {
Artiom 44:badcbe8766e9 1584 Yok = true;
Artiom 44:badcbe8766e9 1585 } else if ((instruction.JumpY - y_robot)*depasY < -5) {
Artiom 44:badcbe8766e9 1586 Yok = true;
Artiom 44:badcbe8766e9 1587 }
Artiom 44:badcbe8766e9 1588
Artiom 44:badcbe8766e9 1589 // on teste si les deux coordonnées ont été dépassées, si oui on lance l'instruction suivante
Artiom 44:badcbe8766e9 1590 if (Xok && Yok) {
Artiom 44:badcbe8766e9 1591 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
Artiom 44:badcbe8766e9 1592 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
Artiom 44:badcbe8766e9 1593 }
Artiom 44:badcbe8766e9 1594
Artiom 44:badcbe8766e9 1595 break;
antbig 0:ad97421fb1fb 1596 case ETAT_GAME_WAIT_END_INSTRUCTION:
Artiom 44:badcbe8766e9 1597 canProcessRx();
Sitkah 35:742dc6b200b0 1598 if(waitingAckID_FIN == 0 && waitingAckFrom_FIN ==0) {//On attend que la carte nous indique que l'instruction est terminée
antbig 0:ad97421fb1fb 1599 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
antbig 0:ad97421fb1fb 1600 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
antbig 0:ad97421fb1fb 1601 }
Artiom 44:badcbe8766e9 1602
Artiom 44:badcbe8766e9 1603 break;
Artiom 44:badcbe8766e9 1604
Artiom 44:badcbe8766e9 1605
antbig 5:dcd817534b57 1606 case ETAT_WARNING_TIMEOUT://Attente de la trame fin de danger ou du timeout de 2s
Artiom 44:badcbe8766e9 1607 if(timeoutWarning.read_ms() >= BALISE_TIMEOUT) { //ça fait plus de 2s, il faut changer de stratégie
Artiom 44:badcbe8766e9 1608 gameEtat = ETAT_EVITEMENT;
Artiom 51:aa6e09f2cfec 1609 /* if(Fevitement==1) {
Artiom 44:badcbe8766e9 1610 EvitEtat= 0;
Artiom 44:badcbe8766e9 1611 Fevitement=0;
Artiom 51:aa6e09f2cfec 1612 }*/
Artiom 44:badcbe8766e9 1613
Artiom 44:badcbe8766e9 1614 /*-------------------------------------
Artiom 44:badcbe8766e9 1615 code origine
Artiom 44:badcbe8766e9 1616
Sitkah 40:21bb685b553b 1617 if(instruction.nextLineOK != instruction.nextLineError)
Sitkah 38:76f886a1c8e6 1618 {
Sitkah 38:76f886a1c8e6 1619 actual_instruction = instruction.nextLineError;
Sitkah 38:76f886a1c8e6 1620 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
Artiom 44:badcbe8766e9 1621 }----------------------------------------*/
Artiom 44:badcbe8766e9 1622
antbig 5:dcd817534b57 1623 }
Artiom 44:badcbe8766e9 1624 break;
Artiom 44:badcbe8766e9 1625
Artiom 44:badcbe8766e9 1626
Artiom 44:badcbe8766e9 1627
Artiom 44:badcbe8766e9 1628
antbig 5:dcd817534b57 1629 case ETAT_WARING_END_BALISE_WAIT://Attente d'une seconde apres la fin d'un End Balise pour etre sur que c'est bon
antbig 12:14729d584500 1630 if(timeoutWarningWaitEnd.read_ms() >= 1000) {//c'est bon, on repart
antbig 12:14729d584500 1631 //actual_instruction = instruction.nextLineError;
antbig 12:14729d584500 1632 gameEtat = ETAT_WARNING_END_LAST_INSTRUCTION;
antbig 12:14729d584500 1633 }
Artiom 44:badcbe8766e9 1634 break;
antbig 5:dcd817534b57 1635 case ETAT_WARNING_END_LAST_INSTRUCTION://trouver le meilleur moyen de reprendre l'instruction en cours
Artiom 44:badcbe8766e9 1636
Artiom 44:badcbe8766e9 1637
Artiom 44:badcbe8766e9 1638 switch(actionPrecedente) {
ClementBreteau 25:f140c93a8666 1639 case MV_LINE:
Artiom 44:badcbe8766e9 1640
ClementBreteau 25:f140c93a8666 1641 if(instruction.direction == BACKWARD) {
ClementBreteau 25:f140c93a8666 1642 localData1 = -1;
ClementBreteau 25:f140c93a8666 1643 } else {
ClementBreteau 25:f140c93a8666 1644 localData1 = 1;
ClementBreteau 25:f140c93a8666 1645 }
Artiom 44:badcbe8766e9 1646
ClementBreteau 25:f140c93a8666 1647 GoToPosition(target_x_robot,target_y_robot,target_theta_robot,localData1);
antbig 27:76ead555a63d 1648 debugXYTTarget(target_x_robot,target_y_robot,target_theta_robot);
antbig 27:76ead555a63d 1649 waitingAckID = ASSERVISSEMENT_XYT;
antbig 27:76ead555a63d 1650 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 44:badcbe8766e9 1651 gameEtat = ETAT_GAME_WAIT_ACK;
antbig 27:76ead555a63d 1652 instruction.order = MV_XYT;
antbig 28:acd18776ed2d 1653 instruction.arg1 = target_x_robot;
antbig 28:acd18776ed2d 1654 instruction.arg2 = target_y_robot;
antbig 28:acd18776ed2d 1655 instruction.arg3 = target_theta_robot;
antbig 28:acd18776ed2d 1656 instruction.direction = (localData1)?FORWARD:BACKWARD;
antbig 28:acd18776ed2d 1657 ingnorInversionOnce = 1;//Pour éviter que l'ago recalcul l'inversion
antbig 27:76ead555a63d 1658 return;
Artiom 44:badcbe8766e9 1659
ClementBreteau 25:f140c93a8666 1660 case MV_XYT:
Artiom 44:badcbe8766e9 1661 gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;
Sitkah 33:388aa0bf6af4 1662 ///////cv
ClementBreteau 25:f140c93a8666 1663 break;
Artiom 44:badcbe8766e9 1664
ClementBreteau 26:2f4fcc2354f3 1665 case MV_COURBURE:
antbig 28:acd18776ed2d 1666 //target_theta_robot = theta_robot - target_theta_robot;
antbig 28:acd18776ed2d 1667 //instruction.arg3 = instruction.arg3 - target_theta_robot;
Artiom 44:badcbe8766e9 1668 if(instruction.direction == LEFT) {
ClementBreteau 26:2f4fcc2354f3 1669 target_theta_robot = target_theta_robot - theta_robot;
Artiom 44:badcbe8766e9 1670 } else {
antbig 28:acd18776ed2d 1671 target_theta_robot = theta_robot + target_theta_robot;
antbig 28:acd18776ed2d 1672 }
Artiom 44:badcbe8766e9 1673
Artiom 44:badcbe8766e9 1674
antbig 28:acd18776ed2d 1675 target_theta_robot = (target_theta_robot)%3600;
antbig 28:acd18776ed2d 1676 if(target_theta_robot > 1800) {
antbig 28:acd18776ed2d 1677 target_theta_robot = target_theta_robot-3600;
antbig 28:acd18776ed2d 1678 }
Artiom 44:badcbe8766e9 1679 if(InversStrat == 1) {
antbig 28:acd18776ed2d 1680 target_theta_robot = -target_theta_robot;
antbig 28:acd18776ed2d 1681 }
antbig 28:acd18776ed2d 1682 instruction.arg3 = target_theta_robot;
Artiom 44:badcbe8766e9 1683
Artiom 44:badcbe8766e9 1684
Artiom 44:badcbe8766e9 1685 gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;
Artiom 44:badcbe8766e9 1686
ClementBreteau 25:f140c93a8666 1687 break;
antbig 27:76ead555a63d 1688 default:
antbig 27:76ead555a63d 1689 actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
antbig 27:76ead555a63d 1690 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
Artiom 44:badcbe8766e9 1691 break;
ClementBreteau 25:f140c93a8666 1692 }
Artiom 44:badcbe8766e9 1693
antbig 27:76ead555a63d 1694 //actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
antbig 27:76ead555a63d 1695 //gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
Artiom 44:badcbe8766e9 1696 break;
antbig 5:dcd817534b57 1697 case ETAT_WARNING_SWITCH_STRATEGIE://Si à la fin du timeout il y a toujours un robot, passer à l'instruction d'erreur
antbig 5:dcd817534b57 1698 actual_instruction = instruction.nextLineError;
antbig 5:dcd817534b57 1699 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
antbig 12:14729d584500 1700 ingnorBaliseOnce = 1;
Artiom 44:badcbe8766e9 1701 break;
Artiom 44:badcbe8766e9 1702
Sitkah 37:fca332b64b42 1703 case ETAT_EVITEMENT :
Artiom 51:aa6e09f2cfec 1704 /*
Artiom 51:aa6e09f2cfec 1705
Artiom 51:aa6e09f2cfec 1706 90°
Artiom 51:aa6e09f2cfec 1707 |
Artiom 51:aa6e09f2cfec 1708 |Violet
Artiom 51:aa6e09f2cfec 1709 |
Artiom 51:aa6e09f2cfec 1710 |
Artiom 51:aa6e09f2cfec 1711 |
Artiom 51:aa6e09f2cfec 1712 |
Artiom 51:aa6e09f2cfec 1713 |Jaune
Artiom 51:aa6e09f2cfec 1714 |
Artiom 51:aa6e09f2cfec 1715 |________________ 0° */
Artiom 51:aa6e09f2cfec 1716
Artiom 50:a5361ffeefc8 1717 char message[10]="toto";
Artiom 50:a5361ffeefc8 1718 char message1[10]="toto";
Artiom 50:a5361ffeefc8 1719 char message2[10]="toto";
Artiom 50:a5361ffeefc8 1720 char message3[10]="toto";
Artiom 50:a5361ffeefc8 1721
Artiom 44:badcbe8766e9 1722 static short x_terrain=3000;
Artiom 44:badcbe8766e9 1723 static short y_terrain=1500;
Artiom 44:badcbe8766e9 1724
Artiom 44:badcbe8766e9 1725 static float x_cote_droit[3]= {0};
Artiom 44:badcbe8766e9 1726 static float y_cote_droit[3]= {0};
Artiom 50:a5361ffeefc8 1727
Artiom 44:badcbe8766e9 1728 static float x_cote_gauche[3]= {0};
Artiom 44:badcbe8766e9 1729 static float y_cote_gauche[3]= {0};
Artiom 44:badcbe8766e9 1730 static short cote=0;
Artiom 44:badcbe8766e9 1731 //--------------------------
Artiom 50:a5361ffeefc8 1732 static float dist_robot_adversaire=650;//distance à laquelle on s'arrete grace à la balise
Artiom 51:aa6e09f2cfec 1733 int proxy=300;//distance entre point de controle et obstacle/adversaire
Artiom 51:aa6e09f2cfec 1734 int proximity=300;//distance entre l'objectif et obstacle/adversaire
Artiom 51:aa6e09f2cfec 1735 short taille_petit=200;// distance proxymité max mur
Artiom 44:badcbe8766e9 1736 //---------------------------*
Artiom 44:badcbe8766e9 1737 static unsigned short distance=50000;//valeur impossible
Artiom 44:badcbe8766e9 1738 static unsigned short distance_prev=50000;
Artiom 50:a5361ffeefc8 1739 static signed short theta_adversaire;
Artiom 51:aa6e09f2cfec 1740
Artiom 44:badcbe8766e9 1741 switch(EvitEtat) {
Artiom 44:badcbe8766e9 1742 case 0:
Artiom 51:aa6e09f2cfec 1743
Artiom 51:aa6e09f2cfec 1744
Artiom 50:a5361ffeefc8 1745 lcd.SetBackColor(LCD_COLOR_WHITE);
Artiom 50:a5361ffeefc8 1746 lcd.DisplayStringAt(0, LINE(2),(unsigned char *)"EVITEMENT ",LEFT_MODE);
Artiom 50:a5361ffeefc8 1747
Artiom 44:badcbe8766e9 1748 ingnorBalise=1;
Artiom 50:a5361ffeefc8 1749 Rotate(450); //on tourne a gauche pour scanner
Artiom 44:badcbe8766e9 1750 waitingAckID = ASSERVISSEMENT_ROTATION;
Artiom 44:badcbe8766e9 1751 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 44:badcbe8766e9 1752 while(waitingAckID !=0 && waitingAckFrom !=0)
Artiom 44:badcbe8766e9 1753 canProcessRx();
Artiom 44:badcbe8766e9 1754
Artiom 44:badcbe8766e9 1755 waitingAckID_FIN = ASSERVISSEMENT_ROTATION;
Artiom 44:badcbe8766e9 1756 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Artiom 50:a5361ffeefc8 1757 while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0)
Artiom 44:badcbe8766e9 1758 canProcessRx();
Artiom 50:a5361ffeefc8 1759
Artiom 50:a5361ffeefc8 1760 SendSpeed(50,1500,1500);
Artiom 50:a5361ffeefc8 1761 waitingAckID = ASSERVISSEMENT_CONFIG;
Artiom 44:badcbe8766e9 1762 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 44:badcbe8766e9 1763 while(waitingAckID !=0 && waitingAckFrom !=0)
Artiom 44:badcbe8766e9 1764 canProcessRx();
Artiom 50:a5361ffeefc8 1765
Artiom 50:a5361ffeefc8 1766 Rotate(-900);//on tourne a droite pour scanner
Artiom 44:badcbe8766e9 1767 waitingAckID = ASSERVISSEMENT_ROTATION;
Artiom 44:badcbe8766e9 1768 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 50:a5361ffeefc8 1769 wait_us(150);
Artiom 44:badcbe8766e9 1770 while(waitingAckID !=0 && waitingAckFrom !=0)
Artiom 44:badcbe8766e9 1771 canProcessRx();
Artiom 44:badcbe8766e9 1772
Artiom 44:badcbe8766e9 1773 waitingAckID_FIN = ASSERVISSEMENT_ROTATION;
Artiom 44:badcbe8766e9 1774 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Artiom 50:a5361ffeefc8 1775 waitingId = RECEPTION_RECALAGE;
Artiom 50:a5361ffeefc8 1776 SendRawId(DATA_RECALAGE);
Artiom 51:aa6e09f2cfec 1777 wait_us(150);
Artiom 44:badcbe8766e9 1778 while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0) {
Artiom 44:badcbe8766e9 1779 canProcessRx();
Artiom 51:aa6e09f2cfec 1780 wait_ms(10);
Artiom 50:a5361ffeefc8 1781 if(waitingId == 0) {
Artiom 50:a5361ffeefc8 1782 distance=telemetreDistance_avant_droite; //on sauvegarde notre distance au robot
Artiom 50:a5361ffeefc8 1783 if(distance<=distance_prev) {
Artiom 50:a5361ffeefc8 1784 distance_prev=distance;
Artiom 51:aa6e09f2cfec 1785 dist_robot_adversaire=distance+100;
Artiom 50:a5361ffeefc8 1786 theta_adversaire=theta_robot;
Artiom 50:a5361ffeefc8 1787 }
Artiom 50:a5361ffeefc8 1788 waitingId = RECEPTION_RECALAGE;
Artiom 50:a5361ffeefc8 1789 SendRawId(DATA_RECALAGE);
Artiom 50:a5361ffeefc8 1790 wait_us(150);
Artiom 44:badcbe8766e9 1791 }
Artiom 44:badcbe8766e9 1792 }
Artiom 51:aa6e09f2cfec 1793
Artiom 51:aa6e09f2cfec 1794 SendSpeed(300,5000,5000);//vitesse inintiale SendSpeed(600,5000,5000)
Artiom 51:aa6e09f2cfec 1795 waitingAckID = ASSERVISSEMENT_CONFIG;
Artiom 51:aa6e09f2cfec 1796 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 51:aa6e09f2cfec 1797 wait_us(150);
Artiom 51:aa6e09f2cfec 1798 while(waitingAckID !=0 && waitingAckFrom !=0)
Artiom 51:aa6e09f2cfec 1799 canProcessRx();
Artiom 51:aa6e09f2cfec 1800
Artiom 51:aa6e09f2cfec 1801 /* Rotate(theta_adversaire); //on tourne a gauche pour scanner
Artiom 51:aa6e09f2cfec 1802 waitingAckID = ASSERVISSEMENT_ROTATION;
Artiom 51:aa6e09f2cfec 1803 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 51:aa6e09f2cfec 1804 while(waitingAckID !=0 && waitingAckFrom !=0)
Artiom 51:aa6e09f2cfec 1805 canProcessRx();
Artiom 51:aa6e09f2cfec 1806
Artiom 51:aa6e09f2cfec 1807 waitingAckID_FIN = ASSERVISSEMENT_ROTATION;
Artiom 51:aa6e09f2cfec 1808 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Artiom 51:aa6e09f2cfec 1809 while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0)
Artiom 51:aa6e09f2cfec 1810 canProcessRx();*/
Artiom 51:aa6e09f2cfec 1811
Artiom 51:aa6e09f2cfec 1812 EvitEtat=1;
Artiom 50:a5361ffeefc8 1813
Artiom 50:a5361ffeefc8 1814 break;
Artiom 50:a5361ffeefc8 1815
Artiom 50:a5361ffeefc8 1816 case 1:
Artiom 44:badcbe8766e9 1817 short ang_target = (short)((atan2((float)(target_y_robot - y_robot), (float)(target_x_robot - x_robot)) * 1800 / M_PI) - theta_robot + 7200) % 3600;
Artiom 44:badcbe8766e9 1818 // On passe le résultat entre -1800 et 1800
Artiom 51:aa6e09f2cfec 1819 if (ang_target > 1800) ang_target = (ang_target - 3600);
Artiom 44:badcbe8766e9 1820
Artiom 44:badcbe8766e9 1821 // float dist_target = (short)sqrt((target_x_robot - x_robot)*(target_x_robot - x_robot)+(target_y_robot - y_robot)*(target_y_robot - y_robot));
Artiom 51:aa6e09f2cfec 1822 /*
Artiom 44:badcbe8766e9 1823 float x_robot_adversaire = x_robot + (dist_robot_adversaire)*cos((float)(theta_adversaire)* M_PI/1800);
Artiom 44:badcbe8766e9 1824 float y_robot_adversaire = y_robot + (dist_robot_adversaire)*sin((float)(theta_adversaire)*M_PI/1800);
Artiom 44:badcbe8766e9 1825
Artiom 44:badcbe8766e9 1826 x_cote_droit[0] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target+1300)*M_PI/1800);
Artiom 44:badcbe8766e9 1827 y_cote_droit[0] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target+1300)*M_PI/1800);
Artiom 44:badcbe8766e9 1828 x_cote_gauche[0] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-1300)*M_PI/1800);
Artiom 44:badcbe8766e9 1829 y_cote_gauche[0] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-1300)*M_PI/1800);
Artiom 44:badcbe8766e9 1830
Artiom 44:badcbe8766e9 1831 x_cote_droit[1] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target+900)*M_PI/1800);
Artiom 44:badcbe8766e9 1832 y_cote_droit[1] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target+900)*M_PI/1800);
Artiom 44:badcbe8766e9 1833 x_cote_gauche[1] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-900)*M_PI/1800);
Artiom 44:badcbe8766e9 1834 y_cote_gauche[1] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-900)*M_PI/1800);
Artiom 44:badcbe8766e9 1835
Artiom 44:badcbe8766e9 1836 x_cote_droit[2] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target+500)*M_PI/1800);
Artiom 44:badcbe8766e9 1837 y_cote_droit[2] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target+500)*M_PI/1800);
Artiom 44:badcbe8766e9 1838 x_cote_gauche[2] = x_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-500)*M_PI/1800);
Artiom 44:badcbe8766e9 1839 y_cote_gauche[2] = y_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-500)*M_PI/1800);
Artiom 51:aa6e09f2cfec 1840 */
Artiom 51:aa6e09f2cfec 1841 float x_robot_adversaire = x_robot + (dist_robot_adversaire)*sin((float)(theta_adversaire)* M_PI/1800);
Artiom 51:aa6e09f2cfec 1842 float y_robot_adversaire = y_robot + (dist_robot_adversaire)*cos((float)(theta_adversaire)*M_PI/1800);
Artiom 51:aa6e09f2cfec 1843
Artiom 51:aa6e09f2cfec 1844 x_cote_droit[0] = x_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target+1300)*M_PI/1800);
Artiom 51:aa6e09f2cfec 1845 y_cote_droit[0] = y_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target+1300)*M_PI/1800);
Artiom 51:aa6e09f2cfec 1846 x_cote_gauche[0] = x_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-1300)*M_PI/1800);
Artiom 51:aa6e09f2cfec 1847 y_cote_gauche[0] = y_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-1300)*M_PI/1800);
Artiom 51:aa6e09f2cfec 1848
Artiom 51:aa6e09f2cfec 1849 x_cote_droit[1] = x_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target+900)*M_PI/1800);
Artiom 51:aa6e09f2cfec 1850 y_cote_droit[1] = y_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target+900)*M_PI/1800);
Artiom 51:aa6e09f2cfec 1851 x_cote_gauche[1] = x_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-900)*M_PI/1800);
Artiom 51:aa6e09f2cfec 1852 y_cote_gauche[1] = y_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-900)*M_PI/1800);
Artiom 51:aa6e09f2cfec 1853
Artiom 51:aa6e09f2cfec 1854 x_cote_droit[2] = x_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target+500)*M_PI/1800);
Artiom 51:aa6e09f2cfec 1855 y_cote_droit[2] = y_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target+500)*M_PI/1800);
Artiom 51:aa6e09f2cfec 1856 x_cote_gauche[2] = x_robot_adversaire + (proxy)*sin((float)(theta_adversaire+ang_target-500)*M_PI/1800);
Artiom 51:aa6e09f2cfec 1857 y_cote_gauche[2] = y_robot_adversaire + (proxy)*cos((float)(theta_adversaire+ang_target-500)*M_PI/1800);
Artiom 44:badcbe8766e9 1858
Artiom 44:badcbe8766e9 1859 SendRawId(0x0D0);//calcul
Artiom 44:badcbe8766e9 1860
Artiom 44:badcbe8766e9 1861 //for(int i=0; i<3; i++) printf("point:%d | gauche(%.1f , %.1f) | droite(%.1f , %.1f)\n\r",i,x_cote_gauche[i],y_cote_gauche[i],x_cote_droit[i],y_cote_droit[i]) ;
Artiom 44:badcbe8766e9 1862 //printf("------------\n\r");
Artiom 44:badcbe8766e9 1863 //-------------------------process------------------------------------------------
Artiom 44:badcbe8766e9 1864
Artiom 44:badcbe8766e9 1865 bool cote_droit=false, cote_gauche=false;
Artiom 44:badcbe8766e9 1866
Artiom 44:badcbe8766e9 1867 for (int i=0; i<3; i++) {
Artiom 44:badcbe8766e9 1868 if (x_cote_droit[i]>taille_petit && x_cote_droit[i]<x_terrain-taille_petit && y_cote_droit[i] >taille_petit && y_cote_droit[i] < y_terrain-taille_petit) {
Artiom 44:badcbe8766e9 1869 cote_droit=true;
Artiom 44:badcbe8766e9 1870 cote=1;
Artiom 44:badcbe8766e9 1871 } else {
Artiom 44:badcbe8766e9 1872 cote_droit=false;
Artiom 44:badcbe8766e9 1873 break;
Artiom 44:badcbe8766e9 1874 }
Artiom 44:badcbe8766e9 1875
Artiom 44:badcbe8766e9 1876 if (x_cote_gauche[i]>taille_petit && x_cote_gauche[i]<x_terrain-taille_petit && y_cote_gauche[i] >taille_petit && y_cote_gauche[i] < y_terrain-taille_petit) {
Artiom 44:badcbe8766e9 1877 cote_gauche=true;
Artiom 44:badcbe8766e9 1878 cote=-1;
Artiom 44:badcbe8766e9 1879 } else {
Artiom 44:badcbe8766e9 1880 cote_gauche=false;
Artiom 44:badcbe8766e9 1881 break;
Artiom 44:badcbe8766e9 1882 }
Artiom 44:badcbe8766e9 1883 }
Artiom 44:badcbe8766e9 1884
Artiom 44:badcbe8766e9 1885 if(!cote_droit && !cote_gauche)cote=0;
Artiom 44:badcbe8766e9 1886
Artiom 44:badcbe8766e9 1887 if (cote_droit && cote_gauche) {
Artiom 44:badcbe8766e9 1888 if (ang_target<=0) {
Artiom 44:badcbe8766e9 1889 cote = -1;// cote gauche
Artiom 44:badcbe8766e9 1890 SendRawId(0x1D0);
Artiom 44:badcbe8766e9 1891 } else if (ang_target>0) {
Artiom 44:badcbe8766e9 1892 cote = 1; //cote droite
Artiom 44:badcbe8766e9 1893 SendRawId(0x2D0);
Artiom 44:badcbe8766e9 1894 }
Artiom 44:badcbe8766e9 1895 }
Artiom 44:badcbe8766e9 1896
Artiom 44:badcbe8766e9 1897 if ( ang_target>600 || ang_target<-600)cote=0;
Artiom 51:aa6e09f2cfec 1898 if (!cote_droit && !cote_gauche)cote=0;
Artiom 44:badcbe8766e9 1899 //--------------------test target --------------------------------------
Artiom 44:badcbe8766e9 1900 if ((x_robot_adversaire >= target_x_robot-proximity && x_robot_adversaire <= target_x_robot+proximity)&&(y_robot_adversaire >= target_y_robot-proximity && y_robot_adversaire <= target_y_robot+proximity)) cote=0;
Artiom 44:badcbe8766e9 1901
Artiom 50:a5361ffeefc8 1902 EvitEtat = 2;
Artiom 44:badcbe8766e9 1903
Artiom 44:badcbe8766e9 1904 break;
Artiom 44:badcbe8766e9 1905
Artiom 50:a5361ffeefc8 1906 case 2 ://on attend la fin de la première rotation pour activer la balise
Artiom 44:badcbe8766e9 1907 //ingnorBalise=1;
Artiom 44:badcbe8766e9 1908 SendRawId(0x0D1);//init evitement
Artiom 44:badcbe8766e9 1909 if(cote!=0) {
Artiom 44:badcbe8766e9 1910 for(int i=0; i<3; i++) {
Artiom 44:badcbe8766e9 1911 if(cote==-1) {
Artiom 51:aa6e09f2cfec 1912 GoToPosition(x_cote_droit[i],y_cote_droit[i],theta_robot,1);
Artiom 51:aa6e09f2cfec 1913 //GoToPosition(y_cote_droit[i],x_cote_droit[i],theta_robot,1);//
Artiom 44:badcbe8766e9 1914 SendRawId(0x1D1);//evitement a gauche
Artiom 44:badcbe8766e9 1915 } else if(cote==1) {
Artiom 51:aa6e09f2cfec 1916 GoToPosition(x_cote_gauche[i],y_cote_gauche[i],theta_robot,1);
Artiom 51:aa6e09f2cfec 1917 //GoToPosition(y_cote_gauche[i],x_cote_gauche[i],theta_robot,1);
Artiom 44:badcbe8766e9 1918 SendRawId(0x2D1);//evitement a droite
Artiom 44:badcbe8766e9 1919 }
Artiom 44:badcbe8766e9 1920 waitingAckID = ASSERVISSEMENT_XYT;
Artiom 44:badcbe8766e9 1921 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 44:badcbe8766e9 1922 while(waitingAckID !=0 && waitingAckFrom !=0)
Artiom 44:badcbe8766e9 1923 canProcessRx();
Artiom 44:badcbe8766e9 1924
Artiom 44:badcbe8766e9 1925 Fevitement=1;
Artiom 44:badcbe8766e9 1926 ingnorBalise=1;
Artiom 44:badcbe8766e9 1927 waitingAckID_FIN = ASSERVISSEMENT_XYT_ROTATE;
Artiom 44:badcbe8766e9 1928 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Artiom 44:badcbe8766e9 1929 while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0)
Artiom 44:badcbe8766e9 1930 canProcessRx();
Artiom 44:badcbe8766e9 1931
Artiom 44:badcbe8766e9 1932 ingnorBalise=0;
Artiom 44:badcbe8766e9 1933 waitingAckID_FIN = ASSERVISSEMENT_XYT_LINE;
Artiom 44:badcbe8766e9 1934 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Artiom 44:badcbe8766e9 1935 while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0)
Artiom 44:badcbe8766e9 1936 canProcessRx();
Artiom 44:badcbe8766e9 1937
Artiom 44:badcbe8766e9 1938 ingnorBalise=1;
Sitkah 38:76f886a1c8e6 1939 waitingAckID_FIN = ASSERVISSEMENT_XYT;
Sitkah 38:76f886a1c8e6 1940 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Artiom 44:badcbe8766e9 1941 while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0)
Artiom 44:badcbe8766e9 1942 canProcessRx();
Artiom 44:badcbe8766e9 1943
Sitkah 38:76f886a1c8e6 1944 }
Artiom 44:badcbe8766e9 1945 } else {
Artiom 44:badcbe8766e9 1946 SendRawId(0x3D1);//cote=0 evitement non possible
Artiom 44:badcbe8766e9 1947 EvitEtat=0;
Artiom 44:badcbe8766e9 1948 gameEtat=ETAT_WARNING_END_LAST_INSTRUCTION;
Artiom 44:badcbe8766e9 1949 ingnorBalise=0;
Artiom 44:badcbe8766e9 1950 Fevitement=0;
Artiom 44:badcbe8766e9 1951 }
Artiom 51:aa6e09f2cfec 1952 EvitEtat=3;
Artiom 44:badcbe8766e9 1953 break;
Artiom 44:badcbe8766e9 1954
Artiom 51:aa6e09f2cfec 1955 case 3://on va vers l'objectif initial
Artiom 44:badcbe8766e9 1956 SendRawId(0x0D2);
Artiom 44:badcbe8766e9 1957 GoToPosition(target_x_robot,target_y_robot,target_theta_robot,1);
Artiom 44:badcbe8766e9 1958 waitingAckID = ASSERVISSEMENT_XYT;
Artiom 44:badcbe8766e9 1959 waitingAckFrom = ACKNOWLEDGE_MOTEUR;
Artiom 44:badcbe8766e9 1960 while(waitingAckID !=0 && waitingAckFrom !=0)
Artiom 44:badcbe8766e9 1961 canProcessRx();
Artiom 44:badcbe8766e9 1962
Artiom 44:badcbe8766e9 1963 Fevitement=1;
Artiom 44:badcbe8766e9 1964 waitingAckID_FIN = ASSERVISSEMENT_XYT;
Artiom 44:badcbe8766e9 1965 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Artiom 44:badcbe8766e9 1966 while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0)
Artiom 44:badcbe8766e9 1967 canProcessRx();
Artiom 44:badcbe8766e9 1968
Artiom 51:aa6e09f2cfec 1969 EvitEtat=4;
Artiom 44:badcbe8766e9 1970 break;
Artiom 44:badcbe8766e9 1971
Artiom 50:a5361ffeefc8 1972 case 4: //on charge l'instruction suivante et sort de l'evitement
Artiom 44:badcbe8766e9 1973 actual_instruction++;//on charge l'instruction suivante
Artiom 44:badcbe8766e9 1974 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
Artiom 44:badcbe8766e9 1975 EvitEtat = 0;
Artiom 44:badcbe8766e9 1976 ingnorBalise=0;
Artiom 44:badcbe8766e9 1977 Fevitement=0;
Artiom 44:badcbe8766e9 1978 break;
Artiom 50:a5361ffeefc8 1979
Artiom 50:a5361ffeefc8 1980 case 10:
Artiom 50:a5361ffeefc8 1981 wait(0.2);
Artiom 50:a5361ffeefc8 1982
Artiom 50:a5361ffeefc8 1983 sprintf(message,"%d ",theta_adversaire);
Artiom 50:a5361ffeefc8 1984 lcd.SetBackColor(LCD_COLOR_WHITE);
Artiom 50:a5361ffeefc8 1985 lcd.DisplayStringAt(0, LINE(8),(unsigned char *)"Theta_adv : ",LEFT_MODE);
Artiom 50:a5361ffeefc8 1986 lcd.DisplayStringAt(200, LINE(8),(unsigned char *)message, LEFT_MODE);
Artiom 50:a5361ffeefc8 1987
Artiom 50:a5361ffeefc8 1988 sprintf(message1,"%04d mm",(short)dist_robot_adversaire);
Artiom 50:a5361ffeefc8 1989 lcd.SetBackColor(LCD_COLOR_WHITE);
Artiom 50:a5361ffeefc8 1990 lcd.DisplayStringAt(0, LINE(12),(unsigned char *)"Dist_adv : ",LEFT_MODE);
Artiom 50:a5361ffeefc8 1991 lcd.DisplayStringAt(200, LINE(12),(unsigned char *)message1, LEFT_MODE);
Artiom 50:a5361ffeefc8 1992 break;
Sitkah 37:fca332b64b42 1993 }
Artiom 44:badcbe8766e9 1994
Sitkah 37:fca332b64b42 1995 break;
Artiom 44:badcbe8766e9 1996
Artiom 44:badcbe8766e9 1997 case ETAT_END:
Artiom 44:badcbe8766e9 1998 if (ModeDemo) {
ClementBreteau 14:c8fc06c4887f 1999 gameEtat = ETAT_CHECK_CARTE_SCREEN;
ClementBreteau 14:c8fc06c4887f 2000 ModeDemo = 1;
ClementBreteau 14:c8fc06c4887f 2001 } else {
ClementBreteau 14:c8fc06c4887f 2002 gameEtat = ETAT_END_LOOP;
ClementBreteau 14:c8fc06c4887f 2003 }
Artiom 44:badcbe8766e9 2004 break;
antbig 0:ad97421fb1fb 2005 case ETAT_END_LOOP:
antbig 0:ad97421fb1fb 2006 //Rien, on tourne en rond
Artiom 44:badcbe8766e9 2007
Artiom 44:badcbe8766e9 2008 break;
antbig 0:ad97421fb1fb 2009 default:
Artiom 44:badcbe8766e9 2010
Artiom 44:badcbe8766e9 2011 break;
Artiom 44:badcbe8766e9 2012 }
Artiom 44:badcbe8766e9 2013 }
Sitkah 29:41e02746041d 2014
Sitkah 29:41e02746041d 2015
Sitkah 29:41e02746041d 2016
Sitkah 29:41e02746041d 2017
Sitkah 29:41e02746041d 2018
antbig 4:88431b537477 2019 /****************************************************************************************/
antbig 4:88431b537477 2020 /* FUNCTION NAME: canProcessRx */
Sitkah 29:41e02746041d 2021 /* DESCRIPTION : Fait évoluer l'automate de l'IHM en fonction des receptions sur le CAN*/
antbig 4:88431b537477 2022 /****************************************************************************************/
antbig 0:ad97421fb1fb 2023 void canProcessRx(void)
Sitkah 29:41e02746041d 2024 {
antbig 0:ad97421fb1fb 2025 static signed char FIFO_occupation=0,FIFO_max_occupation=0;
Sitkah 31:833fc481b002 2026 char message[10]="toto";
Sitkah 31:833fc481b002 2027 char message1[10]="toto";
Sitkah 31:833fc481b002 2028 char message2[10]="toto";
Sitkah 31:833fc481b002 2029 char message3[10]="toto";
Artiom 51:aa6e09f2cfec 2030 char message4[10]="toto";
antbig 0:ad97421fb1fb 2031 FIFO_occupation=FIFO_ecriture-FIFO_lecture;
Artiom 50:a5361ffeefc8 2032
antbig 0:ad97421fb1fb 2033 if(FIFO_occupation<0)
antbig 0:ad97421fb1fb 2034 FIFO_occupation=FIFO_occupation+SIZE_FIFO;
Artiom 50:a5361ffeefc8 2035
Artiom 50:a5361ffeefc8 2036 if(FIFO_max_occupation<FIFO_occupation) {
antbig 0:ad97421fb1fb 2037 FIFO_max_occupation=FIFO_occupation;
Artiom 50:a5361ffeefc8 2038 //SendRawId(
Artiom 50:a5361ffeefc8 2039 }
Artiom 50:a5361ffeefc8 2040
antbig 0:ad97421fb1fb 2041 if(FIFO_occupation!=0) {
Sitkah 42:657b6a573e11 2042 int identifiant=msgRxBuffer[FIFO_lecture].id;
Artiom 50:a5361ffeefc8 2043
Artiom 50:a5361ffeefc8 2044 if (waitingId == identifiant) waitingId = 0;
Sitkah 29:41e02746041d 2045 switch(identifiant) {
Artiom 44:badcbe8766e9 2046
Sitkah 29:41e02746041d 2047 case ALIVE_MOTEUR:
Artiom 44:badcbe8766e9 2048 if (etat == ATT) {
Artiom 44:badcbe8766e9 2049
Artiom 44:badcbe8766e9 2050 lcd.SetTextColor(LCD_COLOR_LIGHTGREEN);
Artiom 44:badcbe8766e9 2051 lcd.FillRect(0,400,400,150);
Artiom 44:badcbe8766e9 2052 lcd.SetTextColor(LCD_COLOR_BLACK);
Artiom 44:badcbe8766e9 2053 lcd.SetBackColor(LCD_COLOR_LIGHTGREEN);
Artiom 44:badcbe8766e9 2054 lcd.DisplayStringAt(80, 450, (uint8_t *)"Carte Moteur", LEFT_MODE);
Sitkah 29:41e02746041d 2055 }
Sitkah 29:41e02746041d 2056 break;
Artiom 44:badcbe8766e9 2057
Sitkah 29:41e02746041d 2058 case ALIVE_BALISE:
Artiom 44:badcbe8766e9 2059 if (etat == ATT) {
Artiom 44:badcbe8766e9 2060
Artiom 44:badcbe8766e9 2061 lcd.SetTextColor(LCD_COLOR_LIGHTGREEN);
Artiom 44:badcbe8766e9 2062 lcd.FillRect(0,600,400,150); //carte AX12
Artiom 44:badcbe8766e9 2063 lcd.SetTextColor(LCD_COLOR_BLACK);
Artiom 44:badcbe8766e9 2064 lcd.SetBackColor(LCD_COLOR_LIGHTGREEN);
Artiom 44:badcbe8766e9 2065 lcd.DisplayStringAt(110, 650, (uint8_t *)"Balise", LEFT_MODE);
Artiom 44:badcbe8766e9 2066 }
Sitkah 29:41e02746041d 2067 break;
Sitkah 29:41e02746041d 2068
Sitkah 29:41e02746041d 2069 case RESET_IHM:
Sitkah 29:41e02746041d 2070 etat = CHOIX;
Sitkah 29:41e02746041d 2071 break;
Sitkah 29:41e02746041d 2072
antbig 4:88431b537477 2073 case DEBUG_FAKE_JAKE://Permet de lancer le match à distance
ClementBreteau 23:ab87d308eaf9 2074 case GLOBAL_JACK:
antbig 1:116040d14164 2075 if(gameEtat == ETAT_GAME_WAIT_FOR_JACK) {
antbig 1:116040d14164 2076 gameEtat = ETAT_GAME_START;
ClementBreteau 23:ab87d308eaf9 2077 SendRawId(ACKNOWLEDGE_JACK);
antbig 1:116040d14164 2078 }
Artiom 44:badcbe8766e9 2079 break;
Artiom 44:badcbe8766e9 2080
Sitkah 30:a1e37af4bbde 2081 case ALIVE_ACTIONNEURS_AVANT: //pas de break donc passe directement dans ECRAN_ALL_CHECK mais conserve l'ident initial
ClementBreteau 23:ab87d308eaf9 2082 case ALIVE_ACTIONNEURS_ARRIERE:
Sitkah 30:a1e37af4bbde 2083 case ALIVE_HERKULEX:
antbig 0:ad97421fb1fb 2084 case ECRAN_ALL_CHECK:
antbig 0:ad97421fb1fb 2085 if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id) {
antbig 0:ad97421fb1fb 2086 waitingAckFrom = 0;//C'est la bonne carte qui indique qu'elle est en ligne
antbig 0:ad97421fb1fb 2087 }
Sitkah 38:76f886a1c8e6 2088 flag=1;
Artiom 44:badcbe8766e9 2089 break;
Artiom 44:badcbe8766e9 2090
Artiom 44:badcbe8766e9 2091
Artiom 44:badcbe8766e9 2092
Artiom 44:badcbe8766e9 2093
Artiom 44:badcbe8766e9 2094 /////////////////////////////////////Acknowledges de Reception de la demande d'action////////////////////////////////////////
Artiom 44:badcbe8766e9 2095 case ACKNOWLEDGE_HERKULEX:
Sitkah 42:657b6a573e11 2096 case ACKNOWLEDGE_BALISE: //pas de break donc passe directement dans ACK_FIN_ACTION mais conserve l'ident initial
Artiom 44:badcbe8766e9 2097
Artiom 44:badcbe8766e9 2098 case ACKNOWLEDGE_TELEMETRE:
Artiom 44:badcbe8766e9 2099 /////////////////////////////////////////////Acknowledges de la fin d'action/////////////////////////////////////////////////
Sitkah 34:6aa4b46b102e 2100 case ACKNOWLEDGE_MOTEUR:
antbig 0:ad97421fb1fb 2101 case INSTRUCTION_END_BALISE:
Artiom 44:badcbe8766e9 2102 case ACK_FIN_ACTION:
antbig 0:ad97421fb1fb 2103 case INSTRUCTION_END_MOTEUR:
Artiom 44:badcbe8766e9 2104 unsigned short recieveAckID;// = (unsigned short)msgRxBuffer[FIFO_lecture].data[0] | ( ((unsigned short)msgRxBuffer[FIFO_lecture].data[1]) <<8);
Artiom 44:badcbe8766e9 2105 memcpy(&recieveAckID, msgRxBuffer[FIFO_lecture].data, 2);
Artiom 44:badcbe8766e9 2106
Artiom 44:badcbe8766e9 2107 if( waitingAckFrom == msgRxBuffer[FIFO_lecture].id && recieveAckID == waitingAckID ) {
antbig 0:ad97421fb1fb 2108 waitingAckFrom = 0;
Artiom 44:badcbe8766e9 2109 waitingAckID = 0;
Artiom 44:badcbe8766e9 2110 }
Artiom 44:badcbe8766e9 2111 if( waitingAckFrom_FIN == msgRxBuffer[FIFO_lecture].id && recieveAckID == waitingAckID_FIN ) {
Artiom 44:badcbe8766e9 2112 //SendRawId(0x5D9);
Artiom 44:badcbe8766e9 2113 waitingAckFrom_FIN = 0;
Artiom 44:badcbe8766e9 2114 waitingAckID_FIN = 0;
antbig 0:ad97421fb1fb 2115 }
Artiom 44:badcbe8766e9 2116
Artiom 44:badcbe8766e9 2117 /*
Artiom 44:badcbe8766e9 2118 if((waitingAckFrom == msgRxBuffer[FIFO_lecture].id) &&
Artiom 44:badcbe8766e9 2119 ((unsigned short)msgRxBuffer[FIFO_lecture].data[0] | (((unsigned short)msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID) )
Artiom 44:badcbe8766e9 2120 {
Artiom 44:badcbe8766e9 2121 waitingAckFrom = 0;
Artiom 44:badcbe8766e9 2122 waitingAckID = 0;
Artiom 44:badcbe8766e9 2123 }
Artiom 44:badcbe8766e9 2124 if(waitingAckFrom_FIN == msgRxBuffer[FIFO_lecture].id && ((unsigned short)msgRxBuffer[FIFO_lecture].data[0]
Artiom 44:badcbe8766e9 2125 |(((unsigned short)msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID_FIN))
Artiom 44:badcbe8766e9 2126 {
Artiom 44:badcbe8766e9 2127 waitingAckFrom_FIN = 0;
Artiom 44:badcbe8766e9 2128 waitingAckID_FIN = 0;
Artiom 44:badcbe8766e9 2129 }
Artiom 44:badcbe8766e9 2130 */
Artiom 44:badcbe8766e9 2131 break;
Artiom 44:badcbe8766e9 2132
Artiom 44:badcbe8766e9 2133
Artiom 44:badcbe8766e9 2134
Artiom 44:badcbe8766e9 2135
Artiom 44:badcbe8766e9 2136
Artiom 44:badcbe8766e9 2137
Artiom 44:badcbe8766e9 2138
Artiom 44:badcbe8766e9 2139
Artiom 51:aa6e09f2cfec 2140 //#ifdef ROBOT_BIG
antbig 0:ad97421fb1fb 2141 case ODOMETRIE_BIG_POSITION:
Artiom 51:aa6e09f2cfec 2142 //#else
antbig 0:ad97421fb1fb 2143 case ODOMETRIE_SMALL_POSITION:
Artiom 51:aa6e09f2cfec 2144 //#endif
antbig 0:ad97421fb1fb 2145 x_robot=msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8);
antbig 0:ad97421fb1fb 2146 y_robot=msgRxBuffer[FIFO_lecture].data[2]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[3])<<8);
antbig 0:ad97421fb1fb 2147 theta_robot=msgRxBuffer[FIFO_lecture].data[4]|((signed short)(msgRxBuffer[FIFO_lecture].data[5])<<8);
Artiom 44:badcbe8766e9 2148 break;
Artiom 44:badcbe8766e9 2149
Sitkah 30:a1e37af4bbde 2150 case ACK_ACTION:
Sitkah 30:a1e37af4bbde 2151 if(waitingAckID == msgRxBuffer[FIFO_lecture].id) {
Sitkah 30:a1e37af4bbde 2152 waitingAckFrom = 0;
Sitkah 30:a1e37af4bbde 2153 waitingAckID = 0;
Sitkah 30:a1e37af4bbde 2154 }
Sitkah 30:a1e37af4bbde 2155 break;
Artiom 44:badcbe8766e9 2156
ClementBreteau 16:7321fb3bb396 2157 case BALISE_DANGER :
ClementBreteau 16:7321fb3bb396 2158 SendAck(ACKNOWLEDGE_BALISE, BALISE_END_DANGER);
Artiom 44:badcbe8766e9 2159 break;
Artiom 44:badcbe8766e9 2160
antbig 5:dcd817534b57 2161 case BALISE_STOP:
antbig 5:dcd817534b57 2162 SendAck(ACKNOWLEDGE_BALISE, BALISE_STOP);
Artiom 44:badcbe8766e9 2163
Sitkah 37:fca332b64b42 2164 signed char fin_angle_detection = msgRxBuffer[FIFO_lecture].data[0] & 0x0F;
Artiom 44:badcbe8766e9 2165 signed char debut_angle_detection = (msgRxBuffer[FIFO_lecture].data[0] & 0xF0) >> 4;
Artiom 44:badcbe8766e9 2166
Artiom 44:badcbe8766e9 2167
Artiom 44:badcbe8766e9 2168
Artiom 44:badcbe8766e9 2169
Artiom 44:badcbe8766e9 2170 if(debut_angle_detection > fin_angle_detection) {
Sitkah 38:76f886a1c8e6 2171 angle_moyen_balise_IR = (float)debut_angle_detection + ((15.0f-(float)debut_angle_detection)+(float)fin_angle_detection)/2.0f;
Sitkah 38:76f886a1c8e6 2172 if(angle_moyen_balise_IR > 15.0f)
Sitkah 38:76f886a1c8e6 2173 angle_moyen_balise_IR-=15.0f;
Artiom 44:badcbe8766e9 2174 } else
Sitkah 37:fca332b64b42 2175 angle_moyen_balise_IR = debut_angle_detection + (fin_angle_detection-debut_angle_detection)/2;
Artiom 44:badcbe8766e9 2176
Artiom 44:badcbe8766e9 2177 #ifdef ROBOT_BIG
Sitkah 38:76f886a1c8e6 2178 float seuil_bas_avant = 12.0; // >=
Sitkah 38:76f886a1c8e6 2179 float seuil_haut_avant = 0.0; // <=
Sitkah 38:76f886a1c8e6 2180 float seuil_bas_arriere = 4.0;
Sitkah 38:76f886a1c8e6 2181 float seuil_haut_arriere = 8.0;
Artiom 44:badcbe8766e9 2182 #else
Sitkah 38:76f886a1c8e6 2183 float seuil_bas_avant = 13.0;
Sitkah 38:76f886a1c8e6 2184 float seuil_haut_avant = 15.0;
Sitkah 38:76f886a1c8e6 2185 float seuil_bas_arriere = 5.0;
Sitkah 38:76f886a1c8e6 2186 float seuil_haut_arriere = 7.0;
Artiom 44:badcbe8766e9 2187 #endif
Artiom 44:badcbe8766e9 2188
Artiom 44:badcbe8766e9 2189
Artiom 44:badcbe8766e9 2190
Artiom 44:badcbe8766e9 2191 if (instruction.order == MV_LINE && instruction.direction == FORWARD && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
Artiom 44:badcbe8766e9 2192 || instruction.order == MV_LINE && instruction.direction == BACKWARD && angle_moyen_balise_IR >= seuil_bas_arriere && angle_moyen_balise_IR <= seuil_haut_arriere
Artiom 44:badcbe8766e9 2193 || instruction.order == MV_COURBURE && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
Artiom 44:badcbe8766e9 2194 || instruction.order == MV_COURBURE && angle_moyen_balise_IR >= seuil_bas_arriere && angle_moyen_balise_IR <= seuil_haut_arriere
Artiom 44:badcbe8766e9 2195 || instruction.order == MV_XYT && angle_moyen_balise_IR >= seuil_bas_avant && angle_moyen_balise_IR <= seuil_haut_avant
Artiom 44:badcbe8766e9 2196 || instruction.order == MV_XYT && angle_moyen_balise_IR >= seuil_bas_arriere && angle_moyen_balise_IR <= seuil_haut_arriere ) { //J'ai rajouté cette ligne mais il faut tester avec et sans pour voir le comportement du robot,
Artiom 44:badcbe8766e9 2197
Artiom 44:badcbe8766e9 2198
Artiom 44:badcbe8766e9 2199 if(needToStop() != 0 && ingnorBaliseOnce ==0 && ingnorBalise==0) {
Artiom 44:badcbe8766e9 2200 if(gameEtat > ETAT_GAME_START && gameEtat != ETAT_WARNING_TIMEOUT) {
ClementBreteau 16:7321fb3bb396 2201 SendRawId(ASSERVISSEMENT_STOP);
Artiom 50:a5361ffeefc8 2202
Artiom 50:a5361ffeefc8 2203 /*waitingAckID_FIN = ASSERVISSEMENT_STOP;
Artiom 50:a5361ffeefc8 2204 waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR;
Artiom 50:a5361ffeefc8 2205 while(waitingAckID_FIN !=0 && waitingAckFrom_FIN !=0)
Artiom 50:a5361ffeefc8 2206 canProcessRx();*/
Artiom 50:a5361ffeefc8 2207
ClementBreteau 26:2f4fcc2354f3 2208 //while(1); // ligne à décommenter si on est en homologation
ClementBreteau 16:7321fb3bb396 2209 if(gameEtat != ETAT_WARING_END_BALISE_WAIT) {
ClementBreteau 16:7321fb3bb396 2210 timeoutWarning.reset();
ClementBreteau 16:7321fb3bb396 2211 timeoutWarning.start();//Reset du timer utiliser par le timeout
ClementBreteau 16:7321fb3bb396 2212 }
Artiom 44:badcbe8766e9 2213 //stop_evitement=1;
antbig 27:76ead555a63d 2214 gameEtat = ETAT_WARNING_TIMEOUT;
antbig 12:14729d584500 2215 }
antbig 12:14729d584500 2216 }
antbig 28:acd18776ed2d 2217 }
antbig 12:14729d584500 2218 ingnorBaliseOnce = 0;
Artiom 44:badcbe8766e9 2219 break;
Artiom 44:badcbe8766e9 2220
antbig 5:dcd817534b57 2221 case BALISE_END_DANGER:
ClementBreteau 16:7321fb3bb396 2222 SendAck(ACKNOWLEDGE_BALISE, BALISE_END_DANGER);
antbig 5:dcd817534b57 2223 if(gameEtat == ETAT_WARNING_TIMEOUT) {
antbig 5:dcd817534b57 2224 timeoutWarningWaitEnd.reset();
antbig 5:dcd817534b57 2225 timeoutWarningWaitEnd.start();
antbig 5:dcd817534b57 2226 gameEtat = ETAT_WARING_END_BALISE_WAIT;
antbig 5:dcd817534b57 2227 }
Artiom 44:badcbe8766e9 2228 break;
Artiom 44:badcbe8766e9 2229
Sitkah 42:657b6a573e11 2230 /*case OBJET_SUR_TABLE:
ClementBreteau 15:c2fc239e85df 2231 if (msgRxBuffer[FIFO_lecture].data[1] == 0xff){
Artiom 44:badcbe8766e9 2232
ClementBreteau 15:c2fc239e85df 2233 gameEtat = ETAT_WARNING_END_LAST_INSTRUCTION;
ClementBreteau 15:c2fc239e85df 2234 }
ClementBreteau 15:c2fc239e85df 2235 else{
Artiom 44:badcbe8766e9 2236
ClementBreteau 15:c2fc239e85df 2237 waitingAckFrom = 0;
Artiom 44:badcbe8766e9 2238 waitingAckID = 0;
Artiom 44:badcbe8766e9 2239
ClementBreteau 15:c2fc239e85df 2240 strat_instructions[actual_instruction+1].arg1 = returnX(strat_instructions[actual_instruction].arg2);
ClementBreteau 15:c2fc239e85df 2241 strat_instructions[actual_instruction+1].arg2 = returnY(strat_instructions[actual_instruction].arg2);
ClementBreteau 15:c2fc239e85df 2242 }
ClementBreteau 15:c2fc239e85df 2243 modeTelemetre = 0;
Sitkah 42:657b6a573e11 2244 break;*/
Artiom 44:badcbe8766e9 2245
Sitkah 30:a1e37af4bbde 2246 case RECEPTION_DATA:
Sitkah 30:a1e37af4bbde 2247 telemetreDistance=char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[0], msgRxBuffer[FIFO_lecture].data[1]);
Sitkah 38:76f886a1c8e6 2248 telemetreDistance= (float)telemetreDistance*100.0f*35.5f+50.0f;
Sitkah 30:a1e37af4bbde 2249 waitingAckFrom = 0;
Sitkah 30:a1e37af4bbde 2250 waitingAckID = 0;
Sitkah 30:a1e37af4bbde 2251 break;
Artiom 44:badcbe8766e9 2252
Sitkah 31:833fc481b002 2253 case RECEPTION_RECALAGE:
Artiom 50:a5361ffeefc8 2254 wait_us(150);
Artiom 50:a5361ffeefc8 2255 // 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
Artiom 50:a5361ffeefc8 2256 memcpy(&telemetreDistance_avant_droite,msgRxBuffer[FIFO_lecture].data,2);
Artiom 49:d83a4851a257 2257 telemetreDistance_avant_gauche = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[2], msgRxBuffer[FIFO_lecture].data[3]);
Sitkah 34:6aa4b46b102e 2258 telemetreDistance_arriere_gauche = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[4], msgRxBuffer[FIFO_lecture].data[5]);
Artiom 49:d83a4851a257 2259 telemetreDistance_arriere_droite = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[6], msgRxBuffer[FIFO_lecture].data[7]);
Artiom 44:badcbe8766e9 2260
Artiom 44:badcbe8766e9 2261
Artiom 44:badcbe8766e9 2262 if(ModeDemo==1) {
Artiom 49:d83a4851a257 2263 sprintf(message,"%04d mm",telemetreDistance_avant_droite);
Sitkah 31:833fc481b002 2264 lcd.SetBackColor(LCD_COLOR_WHITE);
Artiom 50:a5361ffeefc8 2265 lcd.DisplayStringAt(0, LINE(8),(unsigned char *)"LASER AVD : ",LEFT_MODE);
Artiom 50:a5361ffeefc8 2266 lcd.DisplayStringAt(200, LINE(8),(unsigned char *)message, LEFT_MODE);
Artiom 44:badcbe8766e9 2267
Artiom 49:d83a4851a257 2268 sprintf(message1,"%04d mm",telemetreDistance_avant_gauche);
Sitkah 31:833fc481b002 2269 lcd.SetBackColor(LCD_COLOR_WHITE);
Artiom 50:a5361ffeefc8 2270 lcd.DisplayStringAt(0, LINE(10),(unsigned char *)"LASER AVG : ",LEFT_MODE);
Artiom 50:a5361ffeefc8 2271 lcd.DisplayStringAt(200, LINE(10),(unsigned char *)message1, LEFT_MODE);
Artiom 51:aa6e09f2cfec 2272
Artiom 51:aa6e09f2cfec 2273
Artiom 51:aa6e09f2cfec 2274 sprintf(message4,"%04d ",theta_robot);
Artiom 51:aa6e09f2cfec 2275 lcd.SetBackColor(LCD_COLOR_WHITE);
Artiom 51:aa6e09f2cfec 2276 lcd.DisplayStringAt(0, LINE(13),(unsigned char *)"THETA: ",LEFT_MODE);
Artiom 51:aa6e09f2cfec 2277 lcd.DisplayStringAt(200, LINE(13),(unsigned char *)message4, LEFT_MODE);
Artiom 51:aa6e09f2cfec 2278
Artiom 44:badcbe8766e9 2279
Sitkah 34:6aa4b46b102e 2280 sprintf(message2,"%04d mm",telemetreDistance_arriere_gauche);
Sitkah 31:833fc481b002 2281 lcd.SetBackColor(LCD_COLOR_WHITE);
Artiom 50:a5361ffeefc8 2282 lcd.DisplayStringAt(0, LINE(16),(unsigned char *)"LASER ARG : ",LEFT_MODE);
Artiom 50:a5361ffeefc8 2283 lcd.DisplayStringAt(200, LINE(16),(unsigned char *)message2, LEFT_MODE);
Artiom 44:badcbe8766e9 2284
Artiom 49:d83a4851a257 2285 sprintf(message3,"%04d mm",telemetreDistance_arriere_droite);
Sitkah 31:833fc481b002 2286 lcd.SetBackColor(LCD_COLOR_WHITE);
Artiom 50:a5361ffeefc8 2287 lcd.DisplayStringAt(0, LINE(18),(unsigned char *)"LASER ARD : ",LEFT_MODE);
Artiom 50:a5361ffeefc8 2288 lcd.DisplayStringAt(200, LINE(18),(unsigned char *)message3, LEFT_MODE);
Sitkah 31:833fc481b002 2289 }
Sitkah 30:a1e37af4bbde 2290 break;
Artiom 44:badcbe8766e9 2291
Sitkah 30:a1e37af4bbde 2292 case RECEPTION_COULEUR:
Artiom 44:badcbe8766e9 2293 if (blocage_balise==0) {
Artiom 44:badcbe8766e9 2294 couleur1=msgRxBuffer[FIFO_lecture].data[0];
Artiom 44:badcbe8766e9 2295 couleur2=msgRxBuffer[FIFO_lecture].data[1];
Artiom 44:badcbe8766e9 2296 couleur3=msgRxBuffer[FIFO_lecture].data[2];
Artiom 44:badcbe8766e9 2297
Artiom 44:badcbe8766e9 2298 /*lcd.DisplayStringAt(0,LINE(16),(unsigned char *)couleur1+'0',LEFT_MODE);
Artiom 44:badcbe8766e9 2299 lcd.DisplayStringAt(0,LINE(16+1),(unsigned char *)couleur2+'0',LEFT_MODE);
Artiom 44:badcbe8766e9 2300 lcd.DisplayStringAt(0,LINE(16+2),(unsigned char *)couleur3+'0',LEFT_MODE);*/
Sitkah 41:b029ddc4d60e 2301 }
Artiom 44:badcbe8766e9 2302
Artiom 44:badcbe8766e9 2303 break;
Artiom 44:badcbe8766e9 2304
Sitkah 35:742dc6b200b0 2305 case NO_BLOC: //il n'y a pas de bloc, on saute les étapes liées à l'attrape bloc
Sitkah 35:742dc6b200b0 2306 actual_instruction = instruction.nextLineError;
Artiom 44:badcbe8766e9 2307 gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
Sitkah 35:742dc6b200b0 2308 /*waitingAckID_FIN=0;
Sitkah 35:742dc6b200b0 2309 waitingAckFrom_FIN=0;*/
Sitkah 35:742dc6b200b0 2310 SendRawId(0x40);
Sitkah 35:742dc6b200b0 2311 break;
Sitkah 29:41e02746041d 2312 }
antbig 0:ad97421fb1fb 2313 FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
antbig 0:ad97421fb1fb 2314 }
antbig 0:ad97421fb1fb 2315 }
Sitkah 29:41e02746041d 2316
Sitkah 29:41e02746041d 2317
Sitkah 29:41e02746041d 2318
Sitkah 29:41e02746041d 2319
Sitkah 29:41e02746041d 2320
Sitkah 29:41e02746041d 2321 /****************************************************************************************/
Sitkah 29:41e02746041d 2322 /* FUNCTION NAME: Bouton_Strat */
Sitkah 29:41e02746041d 2323 /* DESCRIPTION : Sélection de la strat sur le lcd puis envoie sur CAN (à modifier!) */
Sitkah 29:41e02746041d 2324 /****************************************************************************************/
Sitkah 29:41e02746041d 2325 signed char Bouton_Strat (void)
Artiom 44:badcbe8766e9 2326 {
Sitkah 29:41e02746041d 2327 Button STRAT_1 (0, 30, 190, 110, strat_sd[0]);
Sitkah 29:41e02746041d 2328 Button STRAT_2 (210, 30, 190, 110, strat_sd[1]);
Sitkah 29:41e02746041d 2329 Button STRAT_3 (0, 150, 190, 110, strat_sd[2]);
Sitkah 29:41e02746041d 2330 Button STRAT_4 (210, 150, 190, 110, strat_sd[3]);
Sitkah 29:41e02746041d 2331 Button STRAT_5 (0, 270, 190, 110,strat_sd[4]);
Sitkah 29:41e02746041d 2332 Button STRAT_6 (210, 270, 190, 110, strat_sd[5]);
Sitkah 29:41e02746041d 2333 Button STRAT_7 (0, 390, 190, 110, strat_sd[6]);
Sitkah 29:41e02746041d 2334 Button STRAT_8 (210, 390, 190, 110, strat_sd[7]);
Sitkah 29:41e02746041d 2335 Button STRAT_9 (0, 510, 190, 110, strat_sd[8]);
Sitkah 29:41e02746041d 2336 Button STRAT_10 (210, 510, 190, 110, strat_sd[9]);
Sitkah 29:41e02746041d 2337 Button RETOUR (0, 680, 400, 110, "--Precedent--");
Sitkah 29:41e02746041d 2338 //Definition des boutons
Artiom 44:badcbe8766e9 2339
Sitkah 29:41e02746041d 2340 Ack_strat = 0;
Sitkah 29:41e02746041d 2341 Strat = 0;
Sitkah 29:41e02746041d 2342 STRAT_1.Draw(0xFFF0F0F0, 0);
Sitkah 29:41e02746041d 2343 STRAT_2.Draw(0xFFF0F0F0, 0);
Sitkah 29:41e02746041d 2344 STRAT_3.Draw(0xFFF0F0F0, 0);
Sitkah 29:41e02746041d 2345 STRAT_4.Draw(0xFFF0F0F0, 0);
Sitkah 29:41e02746041d 2346 STRAT_5.Draw(0xFFF0F0F0, 0);
Sitkah 29:41e02746041d 2347 STRAT_6.Draw(0xFFF0F0F0, 0);
Sitkah 29:41e02746041d 2348 STRAT_7.Draw(0xFFF0F0F0, 0);
Sitkah 29:41e02746041d 2349 STRAT_8.Draw(0xFFF0F0F0, 0);
Artiom 44:badcbe8766e9 2350 STRAT_9.Draw(0xFFF0F0F0, 0);
Artiom 44:badcbe8766e9 2351 STRAT_10.Draw(0xFFF0F0F0, 0);
Sitkah 29:41e02746041d 2352 RETOUR.Draw(0xFFFF0000, 0);
Artiom 44:badcbe8766e9 2353
Artiom 44:badcbe8766e9 2354 while(Ack_strat == 0) {
Sitkah 29:41e02746041d 2355 canProcessRx();
Sitkah 29:41e02746041d 2356 CANMessage msgTx=CANMessage();
Sitkah 30:a1e37af4bbde 2357 //msgTx.id=ECRAN_CHOICE_STRAT;
Sitkah 29:41e02746041d 2358 if (RETOUR.Touched())
Sitkah 29:41e02746041d 2359 return -1;
Sitkah 29:41e02746041d 2360 while(RETOUR.Touched());
Artiom 44:badcbe8766e9 2361 //////////////////////////////STRATEGIE N°1
Artiom 44:badcbe8766e9 2362 if (STRAT_1.Touched()) {
Artiom 44:badcbe8766e9 2363 Strat = 0;
Artiom 44:badcbe8766e9 2364 //msgTx.data[0] = 0x1;
Artiom 44:badcbe8766e9 2365 //can2.write(msgTx);
Artiom 44:badcbe8766e9 2366 while(STRAT_1.Touched());
Artiom 44:badcbe8766e9 2367 Ack_strat =1;
Artiom 44:badcbe8766e9 2368 }
Artiom 44:badcbe8766e9 2369 /////////////////////////////STRATEGIE N°2
Artiom 44:badcbe8766e9 2370 if (STRAT_2.Touched()) {
Artiom 44:badcbe8766e9 2371 Strat = 1;
Artiom 44:badcbe8766e9 2372 //msgTx.data[0] = 0x2;
Artiom 44:badcbe8766e9 2373 //can2.write(msgTx);
Artiom 44:badcbe8766e9 2374 while(STRAT_2.Touched());
Artiom 44:badcbe8766e9 2375 Ack_strat =1;
Artiom 44:badcbe8766e9 2376 }
Artiom 44:badcbe8766e9 2377 //////////////////////////////STRATEGIE N°3
Artiom 44:badcbe8766e9 2378 if (STRAT_3.Touched()) {
Artiom 44:badcbe8766e9 2379 Strat = 2;
Artiom 44:badcbe8766e9 2380 //msgTx.data[0] = 0x3;
Artiom 44:badcbe8766e9 2381 //can2.write(msgTx);
Artiom 44:badcbe8766e9 2382 while(STRAT_3.Touched());
Artiom 44:badcbe8766e9 2383 Ack_strat =1;
Artiom 44:badcbe8766e9 2384 }
Artiom 44:badcbe8766e9 2385 /////////////////////////////STRATEGIE N°4
Artiom 44:badcbe8766e9 2386 if (STRAT_4.Touched()) {
Artiom 44:badcbe8766e9 2387 Strat = 3;
Artiom 44:badcbe8766e9 2388 //msgTx.data[0] = 0x4;
Artiom 44:badcbe8766e9 2389 //can2.write(msgTx);
Artiom 44:badcbe8766e9 2390 while(STRAT_4.Touched());
Artiom 44:badcbe8766e9 2391 Ack_strat =1;
Artiom 44:badcbe8766e9 2392 }
Artiom 44:badcbe8766e9 2393 ///////////////////////////////STRATEGIE N°5
Artiom 44:badcbe8766e9 2394 if (STRAT_5.Touched()) {
Artiom 44:badcbe8766e9 2395 Strat = 4;
Artiom 44:badcbe8766e9 2396 //msgTx.data[0] = 0x5;
Artiom 44:badcbe8766e9 2397 //can2.write(msgTx);
Artiom 44:badcbe8766e9 2398 while(STRAT_5.Touched());
Artiom 44:badcbe8766e9 2399 Ack_strat =1;
Artiom 44:badcbe8766e9 2400 }
Artiom 44:badcbe8766e9 2401 ////////////////////////////////STRATEGIE N°6
Artiom 44:badcbe8766e9 2402 if (STRAT_6.Touched()) {
Artiom 44:badcbe8766e9 2403 Strat = 5;
Artiom 44:badcbe8766e9 2404 //msgTx.data[0] = 0x6;
Artiom 44:badcbe8766e9 2405 //can2.write(msgTx);
Artiom 44:badcbe8766e9 2406 while(STRAT_6.Touched());
Artiom 44:badcbe8766e9 2407 Ack_strat =1;
Artiom 44:badcbe8766e9 2408 }
Artiom 44:badcbe8766e9 2409 /////////////////////////////////STRATEGIE N°7
Artiom 44:badcbe8766e9 2410 if (STRAT_7.Touched()) {
Artiom 44:badcbe8766e9 2411 Strat = 6;
Artiom 44:badcbe8766e9 2412 //msgTx.data[0] = 0x7;
Artiom 44:badcbe8766e9 2413 //can2.write(msgTx);
Artiom 44:badcbe8766e9 2414 while(STRAT_7.Touched());
Artiom 44:badcbe8766e9 2415 Ack_strat =1;
Artiom 44:badcbe8766e9 2416 }
Artiom 44:badcbe8766e9 2417 /////////////////////////////////STRATEGIE N°8
Artiom 44:badcbe8766e9 2418 if (STRAT_8.Touched()) {
Artiom 44:badcbe8766e9 2419 Strat = 7;
Artiom 44:badcbe8766e9 2420 //msgTx.data[0] = 0x8;
Artiom 44:badcbe8766e9 2421 //can2.write(msgTx);
Artiom 44:badcbe8766e9 2422 while(STRAT_8.Touched());
Artiom 44:badcbe8766e9 2423 Ack_strat =1;
Artiom 44:badcbe8766e9 2424 }
Artiom 44:badcbe8766e9 2425 /////////////////////////////////STRATEGIE N°9
Artiom 44:badcbe8766e9 2426 if (STRAT_9.Touched()) {
Artiom 44:badcbe8766e9 2427 Strat = 8;
Artiom 44:badcbe8766e9 2428 //msgTx.data[0] = 0x9;
Artiom 44:badcbe8766e9 2429 //can2.write(msgTx);
Artiom 44:badcbe8766e9 2430 while(STRAT_9.Touched());
Artiom 44:badcbe8766e9 2431 Ack_strat =1;
Artiom 44:badcbe8766e9 2432 }
Artiom 44:badcbe8766e9 2433 ///////////////////////////////////STRATEGIE N°10
Artiom 44:badcbe8766e9 2434 if (STRAT_10.Touched()) {
Artiom 44:badcbe8766e9 2435 Strat = 9;
Artiom 44:badcbe8766e9 2436 //msgTx.data[0] = 0xA;
Artiom 44:badcbe8766e9 2437 //can2.write(msgTx);
Artiom 44:badcbe8766e9 2438 while(STRAT_10.Touched());
Artiom 44:badcbe8766e9 2439 Ack_strat =1;
Artiom 44:badcbe8766e9 2440 }
Artiom 44:badcbe8766e9 2441
Sitkah 29:41e02746041d 2442 }
Artiom 44:badcbe8766e9 2443 return Strat;
Artiom 44:badcbe8766e9 2444
Sitkah 29:41e02746041d 2445 }
Sitkah 29:41e02746041d 2446
Sitkah 29:41e02746041d 2447 void affichage_compteur (int nombre)
Sitkah 29:41e02746041d 2448 {
Sitkah 38:76f886a1c8e6 2449 int dizaine=0,unite=0,centaine=0;
Sitkah 38:76f886a1c8e6 2450 centaine=nombre/100;
Sitkah 29:41e02746041d 2451 dizaine = nombre/10;
Sitkah 29:41e02746041d 2452 unite = nombre-(10*dizaine);
Sitkah 38:76f886a1c8e6 2453 print_segment(unite,-50);
Sitkah 40:21bb685b553b 2454 print_segment(dizaine,100);
Artiom 44:badcbe8766e9 2455 if(centaine!=0) {
Sitkah 38:76f886a1c8e6 2456 print_segment(centaine,350);
Sitkah 38:76f886a1c8e6 2457 }
Artiom 44:badcbe8766e9 2458
Sitkah 29:41e02746041d 2459 }
Sitkah 29:41e02746041d 2460
Sitkah 29:41e02746041d 2461
Sitkah 29:41e02746041d 2462 //****print_segment***
Sitkah 29:41e02746041d 2463 //Dessine en 7 segment le nombre en parametre
Artiom 44:badcbe8766e9 2464 // A
Sitkah 29:41e02746041d 2465 // =====
Sitkah 29:41e02746041d 2466 // | |
Sitkah 29:41e02746041d 2467 // B | G | E
Sitkah 29:41e02746041d 2468 // |=====|
Sitkah 29:41e02746041d 2469 // C | | F
Sitkah 29:41e02746041d 2470 // | |
Sitkah 29:41e02746041d 2471 // =====
Sitkah 29:41e02746041d 2472 // D
Sitkah 29:41e02746041d 2473 /*
Sitkah 29:41e02746041d 2474 position pour le chiffre des unites
Sitkah 29:41e02746041d 2475 lcd.FillRect(460,75,120,25);// A
Artiom 44:badcbe8766e9 2476 lcd.FillRect(435,100,25,120);// B
Sitkah 29:41e02746041d 2477 lcd.FillRect(435,245,25,120);// C
Sitkah 29:41e02746041d 2478 lcd.FillRect(460,365,120,25);// D
Sitkah 29:41e02746041d 2479 lcd.FillRect(580,100,25,120);// E
Sitkah 29:41e02746041d 2480 lcd.FillRect(580,245,25,120);// F
Sitkah 29:41e02746041d 2481 lcd.FillRect(460,220,120,25);// G
Sitkah 29:41e02746041d 2482
Sitkah 29:41e02746041d 2483 position pour le chiffre des dizaines
Sitkah 29:41e02746041d 2484 lcd.FillRect(260,75,120,25);// A
Artiom 44:badcbe8766e9 2485 lcd.FillRect(235,100,25,120);// B
Sitkah 29:41e02746041d 2486 lcd.FillRect(235,245,25,120);// C
Sitkah 29:41e02746041d 2487 lcd.FillRect(260,365,120,25);// D
Sitkah 29:41e02746041d 2488 lcd.FillRect(380,100,25,120);// E
Sitkah 29:41e02746041d 2489 lcd.FillRect(380,245,25,120);// F
Sitkah 29:41e02746041d 2490 lcd.FillRect(260,220,120,25);// G
Sitkah 29:41e02746041d 2491 */
Artiom 44:badcbe8766e9 2492
Sitkah 29:41e02746041d 2493 void print_segment(int nombre, int decalage)
Artiom 44:badcbe8766e9 2494 {
Artiom 44:badcbe8766e9 2495
Artiom 44:badcbe8766e9 2496 switch(nombre) {
Sitkah 29:41e02746041d 2497 case 0:
Artiom 44:badcbe8766e9 2498 lcd.FillRect(240-decalage,75,120,25);
Artiom 44:badcbe8766e9 2499 lcd.FillRect(215-decalage,100,25,120);
Artiom 44:badcbe8766e9 2500 lcd.FillRect(215-decalage,245,25,120);
Artiom 44:badcbe8766e9 2501 lcd.FillRect(360-decalage,245,25,120);
Artiom 44:badcbe8766e9 2502 lcd.FillRect(360-decalage,100,25,120);
Artiom 44:badcbe8766e9 2503 lcd.FillRect(240-decalage,365,120,25);
Artiom 44:badcbe8766e9 2504 break;
Artiom 44:badcbe8766e9 2505
Sitkah 29:41e02746041d 2506 case 1:
Artiom 44:badcbe8766e9 2507 lcd.FillRect(360-decalage,100,25,120);// E
Artiom 44:badcbe8766e9 2508 lcd.FillRect(360-decalage,245,25,120);// F
Artiom 44:badcbe8766e9 2509 break;
Artiom 44:badcbe8766e9 2510
Sitkah 29:41e02746041d 2511 case 2:
Artiom 44:badcbe8766e9 2512 lcd.FillRect(240-decalage,75,120,25);// A
Artiom 44:badcbe8766e9 2513 lcd.FillRect(215-decalage,245,25,120);// C
Artiom 44:badcbe8766e9 2514 lcd.FillRect(240-decalage,365,120,25);// D
Artiom 44:badcbe8766e9 2515 lcd.FillRect(360-decalage,100,25,120);// E
Artiom 44:badcbe8766e9 2516 lcd.FillRect(240-decalage,220,120,25);// G
Artiom 44:badcbe8766e9 2517 break;
Artiom 44:badcbe8766e9 2518
Sitkah 29:41e02746041d 2519 case 3:
Artiom 44:badcbe8766e9 2520 lcd.FillRect(240-decalage,75,120,25);// A
Artiom 44:badcbe8766e9 2521 lcd.FillRect(360-decalage,100,25,120);// E
Artiom 44:badcbe8766e9 2522 lcd.FillRect(240-decalage,220,120,25);// G
Artiom 44:badcbe8766e9 2523 lcd.FillRect(240-decalage,365,120,25);// D
Artiom 44:badcbe8766e9 2524 lcd.FillRect(360-decalage,245,25,120);// F
Artiom 44:badcbe8766e9 2525 break;
Artiom 44:badcbe8766e9 2526
Sitkah 29:41e02746041d 2527 case 4:
Artiom 44:badcbe8766e9 2528 lcd.FillRect(215-decalage,100,25,120);// B
Artiom 44:badcbe8766e9 2529 lcd.FillRect(360-decalage,100,25,120);// E
Artiom 44:badcbe8766e9 2530 lcd.FillRect(360-decalage,245,25,120);// F
Artiom 44:badcbe8766e9 2531 lcd.FillRect(240-decalage,220,120,25);// G
Artiom 44:badcbe8766e9 2532 break;
Artiom 44:badcbe8766e9 2533
Sitkah 29:41e02746041d 2534 case 5:
Artiom 44:badcbe8766e9 2535 lcd.FillRect(240-decalage,75,120,25);// A
Artiom 44:badcbe8766e9 2536 lcd.FillRect(215-decalage,100,25,120);// B
Artiom 44:badcbe8766e9 2537 lcd.FillRect(240-decalage,220,120,25);// G
Artiom 44:badcbe8766e9 2538 lcd.FillRect(240-decalage,365,120,25);// D
Artiom 44:badcbe8766e9 2539 lcd.FillRect(360-decalage,245,25,120);// F
Artiom 44:badcbe8766e9 2540 break;
Artiom 44:badcbe8766e9 2541
Sitkah 29:41e02746041d 2542 case 6:
Artiom 44:badcbe8766e9 2543 lcd.FillRect(240-decalage,75,120,25);// A
Artiom 44:badcbe8766e9 2544 lcd.FillRect(215-decalage,100,25,120);// B
Artiom 44:badcbe8766e9 2545 lcd.FillRect(215-decalage,245,25,120);// C
Artiom 44:badcbe8766e9 2546 lcd.FillRect(240-decalage,365,120,25);// D
Artiom 44:badcbe8766e9 2547 lcd.FillRect(360-decalage,245,25,120);// F
Artiom 44:badcbe8766e9 2548 lcd.FillRect(240-decalage,220,120,25);// G
Artiom 44:badcbe8766e9 2549 break;
Artiom 44:badcbe8766e9 2550
Sitkah 29:41e02746041d 2551 case 7:
Artiom 44:badcbe8766e9 2552 lcd.FillRect(240-decalage,75,120,25);// A
Artiom 44:badcbe8766e9 2553 lcd.FillRect(360-decalage,100,25,120);// E
Artiom 44:badcbe8766e9 2554 lcd.FillRect(360-decalage,245,25,120);// F
Artiom 44:badcbe8766e9 2555 break;
Artiom 44:badcbe8766e9 2556
Sitkah 29:41e02746041d 2557 case 8:
Artiom 44:badcbe8766e9 2558 lcd.FillRect(240-decalage,75,120,25); // A
Artiom 44:badcbe8766e9 2559 lcd.FillRect(215-decalage,100,25,120);
Artiom 44:badcbe8766e9 2560 lcd.FillRect(215-decalage,245,25,120);
Artiom 44:badcbe8766e9 2561 lcd.FillRect(360-decalage,245,25,120);//...
Artiom 44:badcbe8766e9 2562 lcd.FillRect(360-decalage,100,25,120);
Artiom 44:badcbe8766e9 2563 lcd.FillRect(240-decalage,365,120,25);
Artiom 44:badcbe8766e9 2564 lcd.FillRect(240-decalage,220,120,25);// G
Artiom 44:badcbe8766e9 2565 break;
Artiom 44:badcbe8766e9 2566
Sitkah 29:41e02746041d 2567 case 9:
Artiom 44:badcbe8766e9 2568 lcd.FillRect(240-decalage,75,120,25);// A
Artiom 44:badcbe8766e9 2569 lcd.FillRect(215-decalage,100,25,120);// B
Artiom 44:badcbe8766e9 2570 lcd.FillRect(240-decalage,365,120,25);// D
Artiom 44:badcbe8766e9 2571 lcd.FillRect(360-decalage,100,25,120);// E
Artiom 44:badcbe8766e9 2572 lcd.FillRect(360-decalage,245,25,120);// F
Artiom 44:badcbe8766e9 2573 lcd.FillRect(240-decalage,220,120,25);// G
Artiom 44:badcbe8766e9 2574 break;
Sitkah 29:41e02746041d 2575 }
Sitkah 29:41e02746041d 2576 }
Sitkah 29:41e02746041d 2577
Sitkah 29:41e02746041d 2578 void effacer_segment(long couleur)
Sitkah 29:41e02746041d 2579 {
Sitkah 29:41e02746041d 2580 lcd.SetTextColor(couleur);
Sitkah 29:41e02746041d 2581 lcd.FillRect(240-200,75,120,25); // A
Sitkah 29:41e02746041d 2582 lcd.FillRect(215-200,100,25,120);
Sitkah 29:41e02746041d 2583 lcd.FillRect(215-200,245,25,120);
Sitkah 29:41e02746041d 2584 lcd.FillRect(360-200,245,25,120);//...
Sitkah 29:41e02746041d 2585 lcd.FillRect(360-200,100,25,120);
Sitkah 29:41e02746041d 2586 lcd.FillRect(240-200,365,120,25);
Sitkah 29:41e02746041d 2587 lcd.FillRect(240-200,220,120,25);// G
Artiom 44:badcbe8766e9 2588
Sitkah 29:41e02746041d 2589 lcd.FillRect(240,75,120,25); // A
Sitkah 29:41e02746041d 2590 lcd.FillRect(215,100,25,120);
Sitkah 29:41e02746041d 2591 lcd.FillRect(215,245,25,120);
Sitkah 29:41e02746041d 2592 lcd.FillRect(360,245,25,120);//...
Sitkah 29:41e02746041d 2593 lcd.FillRect(360,100,25,120);
Sitkah 29:41e02746041d 2594 lcd.FillRect(240,365,120,25);
Sitkah 29:41e02746041d 2595 lcd.FillRect(240,220,120,25);// G
Sitkah 29:41e02746041d 2596 }
Sitkah 36:6dd30780bd8e 2597
Sitkah 36:6dd30780bd8e 2598 short recalageAngulaireCapteur(void)
Sitkah 36:6dd30780bd8e 2599 {
Sitkah 36:6dd30780bd8e 2600 unsigned char nombresDeMesuresAuxTelemetresQuiSontCoherentes = 0;
Sitkah 36:6dd30780bd8e 2601 unsigned int moyennageTelemetre = 0;
Sitkah 36:6dd30780bd8e 2602 unsigned short angleAvant = 0;
Sitkah 36:6dd30780bd8e 2603 unsigned short angleArriere = 0;
Sitkah 36:6dd30780bd8e 2604 unsigned short orientationArrondie = 0;
Artiom 44:badcbe8766e9 2605
Sitkah 36:6dd30780bd8e 2606 unsigned short position_avant_gauche=0;
Sitkah 36:6dd30780bd8e 2607 unsigned short position_avant_droite=0;
Sitkah 36:6dd30780bd8e 2608 unsigned short position_arriere_gauche=0;
Sitkah 36:6dd30780bd8e 2609 unsigned short position_arriere_droite=0;
Artiom 44:badcbe8766e9 2610
Sitkah 36:6dd30780bd8e 2611 if(theta_robot >= 450 && theta_robot <= 1350)
Artiom 44:badcbe8766e9 2612 orientationArrondie = 90;
Sitkah 36:6dd30780bd8e 2613 else if(theta_robot <= -450 && theta_robot >= -1350)
Sitkah 36:6dd30780bd8e 2614 orientationArrondie = 270;
Sitkah 36:6dd30780bd8e 2615 else if(theta_robot <= 450 && theta_robot >= -450)
Sitkah 36:6dd30780bd8e 2616 orientationArrondie = 0;
Sitkah 36:6dd30780bd8e 2617 else if(theta_robot >= 1350 && theta_robot <= -1350)
Sitkah 36:6dd30780bd8e 2618 orientationArrondie = 180;
Artiom 44:badcbe8766e9 2619
Sitkah 36:6dd30780bd8e 2620 // Calcul de position pour faire la vérification de cohérence
Artiom 44:badcbe8766e9 2621 if(orientationArrondie == 90 || orientationArrondie == 270) {
Sitkah 36:6dd30780bd8e 2622 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;
Sitkah 36:6dd30780bd8e 2623 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;
Sitkah 36:6dd30780bd8e 2624 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;
Sitkah 36:6dd30780bd8e 2625 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;
Artiom 44:badcbe8766e9 2626
Artiom 44:badcbe8766e9 2627 } else if(orientationArrondie == 0 || orientationArrondie == 180) {
Sitkah 36:6dd30780bd8e 2628 position_avant_gauche = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_gauche;
Sitkah 36:6dd30780bd8e 2629 position_avant_droite = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_droite;
Sitkah 36:6dd30780bd8e 2630 position_arriere_gauche = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_gauche;
Sitkah 36:6dd30780bd8e 2631 position_arriere_droite = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_droite;
Sitkah 36:6dd30780bd8e 2632 }
Artiom 44:badcbe8766e9 2633
Artiom 44:badcbe8766e9 2634
Artiom 44:badcbe8766e9 2635 if(orientationArrondie == 90 || orientationArrondie == 270) { // Si il est en axe Y
Artiom 44:badcbe8766e9 2636 if(position_arriere_droite >= y_robot-instruction.arg1 && position_arriere_droite <= y_robot+instruction.arg1) { // Et que les mesures sont cohérentes
Artiom 44:badcbe8766e9 2637 if(position_arriere_gauche >= y_robot-instruction.arg1 && position_arriere_gauche <= y_robot+instruction.arg1) {
Sitkah 36:6dd30780bd8e 2638 if(telemetreDistance_arriere_droite > telemetreDistance_arriere_gauche)
Artiom 44:badcbe8766e9 2639 angleArriere =900+(1800 * atan2((double)(telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche), (double)ESPACE_INTER_TELEMETRE ))/M_PI;
Artiom 44:badcbe8766e9 2640 else
Sitkah 36:6dd30780bd8e 2641 angleArriere =(1800 * atan2( (double) ESPACE_INTER_TELEMETRE,(double) (telemetreDistance_arriere_gauche-telemetreDistance_arriere_droite) ))/M_PI;
Artiom 44:badcbe8766e9 2642
Sitkah 36:6dd30780bd8e 2643 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
Sitkah 36:6dd30780bd8e 2644 moyennageTelemetre += angleArriere;
Sitkah 36:6dd30780bd8e 2645 }
Sitkah 36:6dd30780bd8e 2646 }
Artiom 44:badcbe8766e9 2647 } else if(orientationArrondie == 0 || orientationArrondie == 180) { // Si il est en axe X
Artiom 44:badcbe8766e9 2648 if(position_arriere_droite >= x_robot-instruction.arg1 && position_arriere_droite <= x_robot+instruction.arg1) { // Et que les mesures sont cohérentes
Artiom 44:badcbe8766e9 2649 if(position_arriere_gauche >= x_robot-instruction.arg1 && position_arriere_gauche <= x_robot+instruction.arg1) {
Sitkah 36:6dd30780bd8e 2650 if(telemetreDistance_arriere_droite > telemetreDistance_arriere_gauche)
Artiom 44:badcbe8766e9 2651 angleArriere =900+(1800 * atan2( (double) (telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche), (double) ESPACE_INTER_TELEMETRE ))/M_PI;
Artiom 44:badcbe8766e9 2652 else
Sitkah 36:6dd30780bd8e 2653 angleArriere =(1800 * atan2( (double) ESPACE_INTER_TELEMETRE,(double) (telemetreDistance_arriere_gauche-telemetreDistance_arriere_droite) ))/M_PI;
Artiom 44:badcbe8766e9 2654
Sitkah 36:6dd30780bd8e 2655 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
Sitkah 36:6dd30780bd8e 2656 moyennageTelemetre += angleArriere;
Sitkah 36:6dd30780bd8e 2657 }
Sitkah 36:6dd30780bd8e 2658 }
Sitkah 36:6dd30780bd8e 2659 }
Artiom 44:badcbe8766e9 2660
Artiom 44:badcbe8766e9 2661 if(orientationArrondie == 90 || orientationArrondie == 270) { // Si il est en axe Y
Artiom 44:badcbe8766e9 2662 if(position_avant_droite >= y_robot-instruction.arg1 && position_avant_droite <= y_robot+instruction.arg1) { // Et que les mesures sont cohérentes
Artiom 44:badcbe8766e9 2663 if(position_avant_gauche >= y_robot-instruction.arg1 && position_avant_gauche <= y_robot+instruction.arg1) {
Sitkah 36:6dd30780bd8e 2664 if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche)
Artiom 44:badcbe8766e9 2665 angleAvant = (1800 * atan2( (double) ESPACE_INTER_TELEMETRE,(double) (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) ))/M_PI;
Artiom 44:badcbe8766e9 2666 else
Artiom 44:badcbe8766e9 2667 angleAvant = 900 + (1800 * atan2( (double)( telemetreDistance_avant_gauche-telemetreDistance_avant_droite),(double) ESPACE_INTER_TELEMETRE ))/M_PI;
Artiom 44:badcbe8766e9 2668
Artiom 44:badcbe8766e9 2669 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
Artiom 44:badcbe8766e9 2670 moyennageTelemetre += angleAvant;
Artiom 44:badcbe8766e9 2671 }
Artiom 44:badcbe8766e9 2672 }
Artiom 44:badcbe8766e9 2673 } else if(orientationArrondie == 0 || orientationArrondie == 180) { // Si il est en axe X
Artiom 44:badcbe8766e9 2674 if(position_avant_droite >= x_robot-instruction.arg1 && position_avant_droite <= x_robot+instruction.arg1) { // Et que les mesures sont cohérentes
Artiom 44:badcbe8766e9 2675 if(position_avant_gauche >= x_robot-instruction.arg1 && position_avant_gauche <= x_robot+instruction.arg1) {
Artiom 44:badcbe8766e9 2676 if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche)
Artiom 44:badcbe8766e9 2677 angleAvant = (1800 * atan2((double) ESPACE_INTER_TELEMETRE, (double) (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) ))/M_PI;
Artiom 44:badcbe8766e9 2678 else
Artiom 44:badcbe8766e9 2679 angleAvant = 900 + (1800 * atan2( (double) (telemetreDistance_avant_gauche-telemetreDistance_avant_droite),(double) ESPACE_INTER_TELEMETRE ))/M_PI;
Artiom 44:badcbe8766e9 2680
Sitkah 36:6dd30780bd8e 2681 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
Sitkah 36:6dd30780bd8e 2682 moyennageTelemetre += angleAvant;
Sitkah 36:6dd30780bd8e 2683 }
Sitkah 36:6dd30780bd8e 2684 }
Sitkah 36:6dd30780bd8e 2685 }
Artiom 44:badcbe8766e9 2686
Sitkah 36:6dd30780bd8e 2687 angleRecalage = moyennageTelemetre/nombresDeMesuresAuxTelemetresQuiSontCoherentes;
Artiom 44:badcbe8766e9 2688
Artiom 44:badcbe8766e9 2689
Artiom 44:badcbe8766e9 2690
Artiom 44:badcbe8766e9 2691 if(nombresDeMesuresAuxTelemetresQuiSontCoherentes) {
Artiom 44:badcbe8766e9 2692 if(orientationArrondie == 0) {
Sitkah 36:6dd30780bd8e 2693 angleRecalage -= 900;
Artiom 44:badcbe8766e9 2694
Sitkah 36:6dd30780bd8e 2695 /*if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche)
Artiom 44:badcbe8766e9 2696 distanceRecalage = *);
Artiom 44:badcbe8766e9 2697 else
Artiom 44:badcbe8766e9 2698 distanceRecalage = 900 + (1800 * atan( (double)( (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;*/
Artiom 44:badcbe8766e9 2699 } else if(orientationArrondie == 90) {
Artiom 44:badcbe8766e9 2700 angleRecalage += 0;
Artiom 44:badcbe8766e9 2701 } else if(orientationArrondie == 180) {
Artiom 44:badcbe8766e9 2702 angleRecalage += 900;
Artiom 44:badcbe8766e9 2703 } else if(orientationArrondie == 270) {
Sitkah 36:6dd30780bd8e 2704 angleRecalage += 1800;
Sitkah 36:6dd30780bd8e 2705 }
Sitkah 36:6dd30780bd8e 2706 }
Artiom 44:badcbe8766e9 2707
Sitkah 36:6dd30780bd8e 2708 return (nombresDeMesuresAuxTelemetresQuiSontCoherentes && (angleAvant-angleArriere<80 && angleAvant-angleArriere>-80)) ? angleRecalage : theta_robot;
Sitkah 36:6dd30780bd8e 2709 }
Sitkah 36:6dd30780bd8e 2710
Sitkah 36:6dd30780bd8e 2711 short recalageDistanceX(void)
Sitkah 36:6dd30780bd8e 2712 {
Sitkah 36:6dd30780bd8e 2713 unsigned char nombresDeMesuresAuxTelemetresQuiSontCoherentes = 0;
Sitkah 36:6dd30780bd8e 2714 unsigned int moyennageTelemetre = 0;
Artiom 44:badcbe8766e9 2715
Sitkah 36:6dd30780bd8e 2716 telemetreDistance_avant_gauche = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_gauche;
Sitkah 36:6dd30780bd8e 2717 telemetreDistance_avant_droite = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_droite;
Sitkah 36:6dd30780bd8e 2718 telemetreDistance_arriere_gauche = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_gauche;
Sitkah 36:6dd30780bd8e 2719 telemetreDistance_arriere_droite = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_droite;
Artiom 44:badcbe8766e9 2720
Artiom 44:badcbe8766e9 2721 if(telemetreDistance_avant_gauche >= x_robot-instruction.arg1 && telemetreDistance_avant_gauche <= x_robot+instruction.arg1) {
Sitkah 36:6dd30780bd8e 2722 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
Sitkah 36:6dd30780bd8e 2723 moyennageTelemetre += telemetreDistance_avant_gauche;
Sitkah 36:6dd30780bd8e 2724 }
Artiom 44:badcbe8766e9 2725 if(telemetreDistance_avant_droite >= x_robot-instruction.arg1 && telemetreDistance_avant_droite <= x_robot+instruction.arg1) {
Sitkah 36:6dd30780bd8e 2726 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
Sitkah 36:6dd30780bd8e 2727 moyennageTelemetre += telemetreDistance_avant_droite;
Sitkah 36:6dd30780bd8e 2728 }
Artiom 44:badcbe8766e9 2729 if(telemetreDistance_arriere_gauche >= x_robot-instruction.arg1 && telemetreDistance_arriere_gauche <= x_robot+instruction.arg1) {
Sitkah 36:6dd30780bd8e 2730 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
Sitkah 36:6dd30780bd8e 2731 moyennageTelemetre += telemetreDistance_arriere_gauche;
Sitkah 36:6dd30780bd8e 2732 }
Artiom 44:badcbe8766e9 2733 if(telemetreDistance_arriere_droite >= x_robot-instruction.arg1 && telemetreDistance_arriere_droite <= x_robot+instruction.arg1) {
Sitkah 36:6dd30780bd8e 2734 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
Sitkah 36:6dd30780bd8e 2735 moyennageTelemetre += telemetreDistance_arriere_droite;
Sitkah 36:6dd30780bd8e 2736 }
Artiom 44:badcbe8766e9 2737
Sitkah 36:6dd30780bd8e 2738 moyennageTelemetre /= nombresDeMesuresAuxTelemetresQuiSontCoherentes;
Artiom 44:badcbe8766e9 2739
Sitkah 36:6dd30780bd8e 2740 return (nombresDeMesuresAuxTelemetresQuiSontCoherentes)? moyennageTelemetre : x_robot; //SetOdometrie(ODOMETRIE_SMALL_POSITION, moyennageTelemetre, y_robot, theta_robot);
Sitkah 36:6dd30780bd8e 2741 }
Sitkah 36:6dd30780bd8e 2742
Sitkah 36:6dd30780bd8e 2743 short recalageDistanceY(void)
Sitkah 36:6dd30780bd8e 2744 {
Sitkah 36:6dd30780bd8e 2745 unsigned char nombresDeMesuresAuxTelemetresQuiSontCoherentes = 0;
Sitkah 36:6dd30780bd8e 2746 unsigned int moyennageTelemetre = 0;
Artiom 44:badcbe8766e9 2747
Sitkah 36:6dd30780bd8e 2748 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;
Sitkah 36:6dd30780bd8e 2749 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;
Sitkah 36:6dd30780bd8e 2750 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;
Sitkah 36:6dd30780bd8e 2751 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;
Artiom 44:badcbe8766e9 2752
Artiom 44:badcbe8766e9 2753 if(telemetreDistance_avant_gauche >= y_robot-instruction.arg1 && telemetreDistance_avant_gauche <= y_robot+instruction.arg1) {
Sitkah 36:6dd30780bd8e 2754 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
Sitkah 36:6dd30780bd8e 2755 moyennageTelemetre += telemetreDistance_avant_gauche;
Sitkah 36:6dd30780bd8e 2756 }
Artiom 44:badcbe8766e9 2757 if(telemetreDistance_avant_droite >= y_robot-instruction.arg1 && telemetreDistance_avant_droite <= y_robot+instruction.arg1) {
Sitkah 36:6dd30780bd8e 2758 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
Sitkah 36:6dd30780bd8e 2759 moyennageTelemetre += telemetreDistance_avant_droite;
Sitkah 36:6dd30780bd8e 2760 }
Artiom 44:badcbe8766e9 2761 if(telemetreDistance_arriere_gauche >= y_robot-instruction.arg1 && telemetreDistance_arriere_gauche <= y_robot+instruction.arg1) {
Sitkah 36:6dd30780bd8e 2762 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
Sitkah 36:6dd30780bd8e 2763 moyennageTelemetre += telemetreDistance_arriere_gauche;
Sitkah 36:6dd30780bd8e 2764 }
Artiom 44:badcbe8766e9 2765 if(telemetreDistance_arriere_droite >= y_robot-instruction.arg1 && telemetreDistance_arriere_droite <= y_robot+instruction.arg1) {
Sitkah 36:6dd30780bd8e 2766 nombresDeMesuresAuxTelemetresQuiSontCoherentes++;
Sitkah 36:6dd30780bd8e 2767 moyennageTelemetre += telemetreDistance_arriere_droite;
Sitkah 36:6dd30780bd8e 2768 }
Artiom 44:badcbe8766e9 2769
Sitkah 36:6dd30780bd8e 2770 moyennageTelemetre /= nombresDeMesuresAuxTelemetresQuiSontCoherentes;
Artiom 44:badcbe8766e9 2771
Artiom 44:badcbe8766e9 2772 return (nombresDeMesuresAuxTelemetresQuiSontCoherentes)? moyennageTelemetre : y_robot ; // SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, moyennageTelemetre, theta_robot);
Sitkah 36:6dd30780bd8e 2773 }
Sitkah 36:6dd30780bd8e 2774