carte_strategie_2019 - carte_strategie_2019

CRAC Team » Code » carte_strategie_2019

CRAC Team / Mbed 2 deprecated carte_strategie_2019

carte_strategie_2019

Dependencies: mbed SerialHalfDuplex SDFileSystem DISCO-F469NI_portrait liaison_Bluetooth ident_crac

Strategie/Strategie.cpp@91:42ae63e5daf5, 2019-06-01 (annotated)

Committer:: kyxstark
Date:: Sat Jun 01 02:50:52 2019 +0000
Revision:: 91:42ae63e5daf5
Parent:: 90:2a3e2dca09a0

reprise erreur asserv correction bug;

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

Repository toolbox

Export to desktop IDE

Build repository

Repository details

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

Strategie/Strategie.cpp@91:42ae63e5daf5, 2019-06-01 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning