code de la carte IHM avant les bugs et avant le travail effectué avec Melchior

Dependencies:   mbed SerialHalfDuplex SDFileSystem liaison_Bluetooth ident_crac DISCO-F469NI_portrait

Committer:
maximilienlv
Date:
Wed Mar 11 12:37:35 2020 +0000
Revision:
13:6b94eee81751
Parent:
12:227c15c7ebe6
modification strategie small;

Who changed what in which revision?

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