CRAC Team / Mbed 2 deprecated carte_strategie_2019

carte_strategie_2019

Dependencies:   mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac

Strategie/Strategie.cpp@42:657b6a573e11, 2018-05-17 (annotated)

Committer:
Sitkah
Date:
Thu May 17 13:08:31 2018 +0000
Revision:
42:657b6a573e11
Parent:
41:b029ddc4d60e
Child:
44:badcbe8766e9
useless

Who changed what in which revision?

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