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