CRAC Team / Mbed 2 deprecated carte_straegie_2019

Programme carte strategie (disco)

Dependencies:   mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac

Strategie/Strategie.cpp@35:742dc6b200b0, 2018-05-01 (annotated)

Committer:
Sitkah
Date:
Tue May 01 13:25:42 2018 +0000
Revision:
35:742dc6b200b0
Parent:
34:6aa4b46b102e
Child:
36:6dd30780bd8e
bug turn

Who changed what in which revision?

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