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@60:c7632af62d21, 2019-05-23 (annotated)

Committer:
kyxstark
Date:
Thu May 23 15:53:48 2019 +0000
Revision:
60:c7632af62d21
Parent:
58:faef8d1b8ed8 
Carte strat plus lib , mais programme

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