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@37:fca332b64b42, 2018-05-03 (annotated)

Committer:
Sitkah
Date:
Thu May 03 22:35:27 2018 +0000
Revision:
37:fca332b64b42
Parent:
36:6dd30780bd8e
Child:
38:76f886a1c8e6
rep?rage rob adv;

Who changed what in which revision?

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