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@55:1e3dab1f90f4, 2019-05-23 (annotated)

Committer:
kyxstark
Date:
Thu May 23 10:01:25 2019 +0000
Revision:
55:1e3dab1f90f4
Parent:
54:8996a5b18d9b
Child:
58:faef8d1b8ed8
inclusion ident_crac.h en mod lib

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