CRAC Team / Mbed 2 deprecated carte_strategie_2019

carte_strategie_2019

Dependencies:   mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac

Strategie/Strategie.cpp@49:d83a4851a257, 2019-05-16 (annotated)

Committer:
Artiom
Date:
Thu May 16 12:08:07 2019 +0000
Revision:
49:d83a4851a257
Parent:
48:43e72239fb02
Child:
50:a5361ffeefc8
mise a jour variable telemetres

Who changed what in which revision?

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