CRAC Team / Mbed 2 deprecated carte_straegie_2019

Programme carte strategie (disco)

Dependencies:   mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac

Strategie/Strategie.cpp@51:aa6e09f2cfec, 2019-05-18 (annotated)

Committer:
Artiom
Date:
Sat May 18 16:54:06 2019 +0000
Revision:
51:aa6e09f2cfec
Parent:
50:a5361ffeefc8
Child:
52:a47350923b5e
maj evitement

Who changed what in which revision?

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