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@34:6aa4b46b102e, 2018-04-26 (annotated)

Committer:
Sitkah
Date:
Thu Apr 26 20:14:18 2018 +0000
Revision:
34:6aa4b46b102e
Parent:
33:388aa0bf6af4
Child:
35:742dc6b200b0
Ne pas tenir compte si besoin du code 2017

Who changed what in which revision?

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