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:
Thu Mar 12 09:25:48 2020 +0000
Revision:
16:c6c765e8b982
Parent:
15:5ece9c3ba389
Child:
17:1cf2043c5c01
demo;

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