carte_straegie_2019 - Programme carte strategie (disco)

CRAC Team » Code » carte_straegie_2019

CRAC Team / Mbed 2 deprecated carte_straegie_2019

Programme carte strategie (disco)

Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac

Strategie/Strategie.cpp@52:a47350923b5e, 2019-05-21 (annotated)

Committer:: Artiom
Date:: Tue May 21 14:38:09 2019 +0000
Revision:: 52:a47350923b5e
Parent:: 51:aa6e09f2cfec
Child:: 53:e96acb11a51f

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

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	23 May 2019
Imports:	2
Forks:	0
Commits:	61
Dependents:	0
Dependencies:	6
Followers:	96

Strategie/Strategie.cpp@52:a47350923b5e, 2019-05-21 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning