CRAC Team / Mbed 2 deprecated carte_strategie_2019

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac

Strategie/Strategie.cpp@85:c46c39d879a9, 2019-05-31 (annotated)

Committer:
kyxstark
Date:
Fri May 31 18:04:13 2019 +0000
Revision:
85:c46c39d879a9
Parent:
81:ef50ec0ef328 
tests_bt;

Who changed what in which revision?

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