CRAC-Strat_2019

CRAC Team » Code » CRAC-Strat_2019

CRAC Team / CRAC-Strat_2019

Dependencies: CRAC-Strat_2019 SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac

Dependents: Codeprincipal_2019 CRAC-Strat_2019

Strategie/Strategie.cpp@61:4046a91e1b0e, 2019-05-24 (annotated)

Committer:: Artiom
Date:: Fri May 24 21:18:26 2019 +0000
Revision:: 61:4046a91e1b0e
Parent:: 58:faef8d1b8ed8
Child:: 62:c4863b4b2543

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

Repository toolbox

Repository details

Type:	Library
Created:	09 May 2019
Imports:	4
Forks:	0
Commits:	66
Dependents:	2
Dependencies:	6
Followers:	96

Important changes to repositories hosted on mbed.com

Strategie/Strategie.cpp@61:4046a91e1b0e, 2019-05-24 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Important changes to repositories hosted on mbed.com

Strategie/Strategie.cpp@61:4046a91e1b0e, 2019-05-24 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning