CRAC-Strat_2019 - v2019

CRAC Team » Code » CRAC-Strat_2019

CRAC Team / CRAC-Strat_2019

v2019

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

Dependents: Codeprincipal_2019 CRAC-Strat_2019

Strategie/Strategie.cpp@65:1789068cf5ab, 2019-05-25 (annotated)

Committer:: Artiom
Date:: Sat May 25 16:08:22 2019 +0000
Revision:: 65:1789068cf5ab
Parent:: 63:bcfe62e3f1d2

Who changed what in which revision?

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

Repository toolbox

Export to desktop IDE

Repository details

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

Strategie/Strategie.cpp@65:1789068cf5ab, 2019-05-25 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning