![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
code avec la sortie d'évitement en plus géré par un ticker, ce code et le code "avec_modifs" buggent en match avec seulement la stratégie agressive.
Dependencies: mbed SerialHalfDuplex SDFileSystem liaison_Bluetooth ident_crac DISCO-F469NI_portrait
Diff: Strategie/Strategie.cpp
- Revision:
- 1:7e925468f9d9
- Child:
- 5:81aac085516f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Strategie/Strategie.cpp Thu Jan 30 16:48:59 2020 +0000 @@ -0,0 +1,2389 @@ +#include "global.h" +#include <string.h> +#include <sstream> +#include <math.h> +#include <vector> +//#include "StrategieManager.h" + + + +#define M_PI 3.14159265358979323846 +#define VERT 0xFF00FF00 +#define ROUGE 0xFFFF0000 +#define BLEU 0xFF0000FF +#define JAUNE 0xFFFDD835//FEFE00 +#define BLANC 0xFF000000 +#define ORANGE 0xFFFFA500 +#define NOIR 0xFF000000 +#define DIY_GREY 0xFFDFDFDF +#define VIOLET 0xFF4527A0 + +char tableau_aff[10][50]; +char tableau_etat[22][50]= { + "Check_carte_screen", + "Check_carte_screen_wait_ack", + "Check_cartes", + "Check_cartes_wait_ack", + "Wait_force", + "Config", + "Game_init", + "Game_wait_for_jack", + "Game_start", + "Game_next_instruction", + "Game_instruction", + "Game_wait_ack", + "Game_jump_time", + "Game_jump_config", + "Game_jump_position", + "Game_wait_end_instruction", + "Warning_timeout", + "Waring_end_balise_wait", + "Warning_end_last_instruction", + "Warning_switch_strategie", + "End", + "End_loop", +}; + +int waitingAckID_FIN; +int waitingAckFrom_FIN; + +Ticker ticker; +TS_DISCO_F469NI ts; +LCD_DISCO_F469NI lcd; + +TS_StateTypeDef TS_State; + +Ticker chrono; +Timeout AffTime; +Timer timer; +Timer cartesCheker;//Le timer pour le timeout de la vérification des cartes +Timer gameTimer; +Timer debugetatTimer; +Timer timeoutWarning; +Timer timeoutWarningWaitEnd; +Timeout chronoEnd;//permet d'envoyer la trame CAN pour la fin + +unsigned char screenChecktry = 0; +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}; + +char counter = 0; +char check; +char Jack = 1; +short SCORE_GLOBAL=0; +short SCORE_GR=0; +short SCORE_PR=0; + +int flag = 0, flag_strat = 0, flag_timer; +int flagReceptionTelemetres = 0, flagNonRepriseErrorMot = 0; +char Ack_strat = 0; +signed char Strat = 0; +signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN + +signed short x_robot,y_robot,theta_robot;//La position du robot +signed short target_x_robot, target_y_robot, target_theta_robot; +E_InstructionType actionPrecedente; +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 +//unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN +int flagSendCan=1; +unsigned char Cote = 0; //0 -> JAUNE | 1 -> VIOLET +unsigned short angleRecalage = 0; +unsigned char checkCurrent = 0; +unsigned char countAliveCard = 0; +unsigned char ligne=0; + + +int Fevitement=0; +int EvitEtat= 0; +int stop_evitement=0; + + + + + +signed char Strategie = 0; //N° de la strategie (1-10) + +unsigned char ModeDemo = 0; // Si à 1, indique que l'on est dans le mode demo + +unsigned char countRobotNear = 0;//Le nombre de robot à proximité + +unsigned char ingnorBaliseOnce = 0;//une fois détecté réinitialise +unsigned char ingnorBalise = 0;//0:balise ignore 1:on ecoute la balise +short direction; + +unsigned char ingnorInversionOnce = 0;//Pour ignorer l'inversion des instruction une fois + +struct S_Instruction instruction; +struct S_Dodge_queue dodgeq; + +char couleur1, couleur2, couleur3; +float cptf; +int cpt,cpt1; + +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; +T_etat etat = INIT; +E_stratGameEtat gameEtat = ETAT_CHECK_CARTES; +E_stratGameEtat memGameEtat= gameEtat; +E_stratGameEtat lastEtat = ETAT_CHECK_CARTES; +E_Stratposdebut etat_pos=RECALAGE_1; + +/////////////////DEFINITION DES BOUTONS//////////////////// +Button COTE_VERT(0, 25, 400, 300, "JAUNE"); +Button COTE_ORANGE(0, 350, 400, 300, "VIOLET"); +Button COTE_JAUNE(0, 25, 400, 300, "JAUNE"); +Button COTE_VIOLET(0, 350, 400, 300, "VIOLET"); +Button RETOUR (0, 680, 400, 110, "--Precedent--"); +Button LANCER (0, 200, 400, 200, "--LANCER--"); +Button CHECK (0, 420, 400, 200, "Valider"); +Button MATCH (0, 50, 400, 320, "Match"); +Button DEMONSTRATION (0, 400, 400, 320, "Demo"); +Button TEST_HERKULEX(0, 25, 400, 100, "Test servos"); +Button TEST_LASER(0, 135, 400, 100, "Test telemetre"); +Button TEST_COULEURS(0,245,400,100,"Test capteurs"); +Button TEST_TIR_BALLE(0,355,400,100,"Test Lanceur"); +Button TEST_IMMEUBLE(0,465,400,100,"Test immeuble"); +Button TEST_TRIEUR(0,575,400,100,"Test aiguilleur"); +Button TIR_CHATEAU(0, 25, 400, 100, "Tir chateau"); +Button EPURATION(0, 150, 400, 100, "epuration"); +Button LANCEUR_ON(0,275,400,100,"allumer le lanceur"); +Button LANCEUR_OFF(0,400,400,100,"eteindre le lanceur"); +Button ABAISSE_BLOC(0, 25, 400, 100, "Ramasser blocs"); +Button RELEVE_BLOC(0, 135, 400, 100, "lacher blocs"); +Button BRAS_ABEILLE_ON(0,245,400,100,"bras abeille"); +Button BRAS_ABEILLE_OFF(0,355,400,100,"baisser bras abeille"); +Button INTERRUPTEUR_ON(0,465,400,100,"baisser bras interrupt"); +Button INTERRUPTEUR_OFF(0,575,400,100,"baisser bras interrupt"); +Button FORCE_LAUNCH(0, 50, 400, 320, "Force Launch"); +Button TRI(0, 25, 400, 100, "Test tri"); +Button AIGUILLEUR_D(0, 150, 400, 100, "aiguilleur droite"); +Button AIGUILLEUR_G(0,275,400,100,"aiguilleur gauche"); +Button AIGUILLEUR_CTRE(0,400,400,100,"aiguilleur centre"); +Button SUIVANT(0,380,200,100,"Suivant"); +Button COLOR_ORANGE (0, 230, 190, 110,""); +Button COLOR_JAUNE (210, 230, 190, 110,""); +Button COLOR_VERT (0, 350, 190, 110,""); +Button COLOR_BLEU (210, 350, 190, 110,""); +Button COLOR_NOIR (105, 470, 190, 110,""); +//////////////////////////////////////////////////////////// + +void SendRawId (unsigned short id); +void SelectionStrat (unsigned char numeroStrat); +void Setflag(void); +void can2Rx_ISR(void); +signed char Bouton_Strat (void); +signed char blocage_balise; +void print_segment(int nombre, int decalage); +void affichage_compteur (int nombre); +void effacer_segment(long couleur); + +unsigned short telemetreDistance=0; +unsigned short telemetreDistance_avant_gauche=0; +unsigned short telemetreDistance_avant_droite=0; +unsigned short telemetreDistance_arriere_gauche=0; +unsigned short telemetreDistance_arriere_droite=0; + +unsigned char DT_AVD_interrupt=0; +unsigned char DT_AVG_interrupt=0; +unsigned char DT_ARD_interrupt=0; +unsigned char DT_ARG_interrupt=0; + + +#ifdef ROBOT_BIG + + +unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR,CHECK_BALISE}; +unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR,ALIVE_BALISE}; + +InterruptIn jack(PG_11); // entrée numerique en interruption pour le jack +#else + + +unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR,CHECK_BALISE}; +unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR,ALIVE_BALISE}; +InterruptIn jack(PG_11); // entrée numerique en interruption pour le jack + + +#endif + + + + + + +/****************************************************************************************/ +/* FUNCTION NAME: chronometre_ISR */ +/* DESCRIPTION : Interruption à la fin des 90s du match */ +/****************************************************************************************/ +void chronometre_ISR (void) +{ + SendRawId(ASSERVISSEMENT_STOP);//On stope les moteurs + SendRawId(GLOBAL_GAME_END);//Indication fin de match + etat=FIN; + gameTimer.stop();//Arret du timer + + while(1);//On bloque la programme dans l'interruption +} + + + +/****************************************************************************************/ +/* FUNCTION NAME: jack_ISR */ +/* DESCRIPTION : Interruption en changement d'état sur le Jack */ +/****************************************************************************************/ +void jack_ISR (void) +{ + if(gameEtat == ETAT_GAME_WAIT_FOR_JACK) { + gameEtat = ETAT_GAME_START;//On débute le match + //etat=COMPTEUR; + blocage_balise=1; + } +} + +/****************************************************************************************/ +/* FUNCTION NAME: SelectionStrat */ +/* DESCRIPTION : Affiche la Stratégie sélectionnée sur l'ihm */ +/****************************************************************************************/ + + +void SelectionStrat (unsigned char Strategie) +{ + lcd.SetBackColor(LCD_COLOR_WHITE); + lcd.SetTextColor(LCD_COLOR_BLACK); + + switch (Strategie+1) { + case 0x1 : + //description de Strategie n°1 + lcd.DisplayStringAt(150, 0, (uint8_t *)strat_sd[Strategie], LEFT_MODE); + break; + + case 0x2 : + //description de Strategie n°2 + lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE); + break; + + case 0x3 : + //description de Strategie n°3 + lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE); + break; + + case 0x4 : + //description de Strategie n°4 + lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE); + break; + + case 0x5 : + //description de Strategie n°5 + lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE); + break; + + case 0x6 : + //description de Strategie n°5 + lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE); + break; + + case 0x7 : + //description de Strategie n°5 + lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE); + break; + + case 0x8 : + //description de Strategie n°5 + lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE); + break; + + case 0x9 : + //description de Strategie n°5 + lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE); + break; + + case 0xA : + //description de Strategie n°5 + lcd.DisplayStringAt(150,0, (uint8_t *)strat_sd[Strategie], LEFT_MODE); + break; + } +} + +void Setflag(void) +{ + flagSendCan = 1; +} + + +//Affiche une variable sur l'écran tactile// +void affichage_var(double Var) +{ + if(ligne==7) + ligne=0; + char aff[10]="toto"; + sprintf(aff,"%lf ",Var); + lcd.DisplayStringAt(120, LINE(20+(ligne)), (uint8_t *)aff, LEFT_MODE); + //ligne++; + +} + + +/****************************************************************************************/ +/* FUNCTION NAME: affichage_debug */ +/* DESCRIPTION : Affiche l'état de gameEtat sur l'écran lcd */ +/****************************************************************************************/ +void affichage_debug(int Var) +{ + int i; + int conv=(int)Var; + SUIVANT.Draw(ROUGE, 0); + for(i=0; i<9; i++) { + strcpy(tableau_aff[i],""); + strcpy(tableau_aff[i],tableau_aff[i+1]); + } + strcpy(tableau_aff[9],tableau_etat[conv]); + + for(i=0; i<10; i++) { + lcd.SetBackColor(VERT); + lcd.DisplayStringAt(0, LINE(20+i), (uint8_t *)tableau_aff[i], LEFT_MODE); + } + /*while(!ack_bluetooth){ // mode pas à pas en bluetooth ou via écran + //liaison_bluetooth(); + } + ack_bluetooth=0;*/ + /*while(SUIVANT.Touched()==0); + while(SUIVANT.Touched());*/ +} + +/****************************************************************************************/ +/* FUNCTION NAME: automate_etat_ihm */ +/* DESCRIPTION : Automate de gestion de l'affichage */ +/****************************************************************************************/ +void automate_etat_ihm(void) +{ + int j; + if (j==0) { + ts.Init(lcd.GetXSize(), lcd.GetYSize()); + j++; + } + ts.GetState(&TS_State); + switch (etat) { + case INIT : //intialise l'écran et passe à l'attente d'initialisation des cartes + ts.GetState(&TS_State); + canProcessRx(); + lcd.SetBackColor(LCD_COLOR_WHITE); + lcd.SetTextColor(LCD_COLOR_BLACK); + lcd.Clear (LCD_COLOR_WHITE); + wait(0.15); + lcd.DisplayStringAt(0, 10, (uint8_t *)"Verification des cartes", LEFT_MODE); + //cartes non verifiées//////////////// + lcd.SetTextColor(DIY_GREY); + lcd.FillRect(0,400,400,150); //carte moteur + lcd.FillRect(0,600,400,150); //Balise + lcd.SetTextColor(LCD_COLOR_BLACK); + lcd.SetBackColor(DIY_GREY); + lcd.DisplayStringAt(80, 450, (uint8_t *)"Carte Moteur", LEFT_MODE); + lcd.DisplayStringAt(110,650, (uint8_t *)"Balise", LEFT_MODE); + //////////////////////////////////////// + FORCE_LAUNCH.Draw(0xFFFF0000, 0); + + etat=ATT; + break; + + 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...) + if (flag==1) { + etat = CHOIX; + gameEtat = ETAT_CONFIG; + } else if (FORCE_LAUNCH.Touched()) { + etat = CHOIX; + gameEtat = ETAT_CONFIG; + while(FORCE_LAUNCH.Touched()); + } + + break; + + + case CHOIX : //Match ou DEMO + lcd.SetBackColor(LCD_COLOR_WHITE); + lcd.SetTextColor(LCD_COLOR_BLACK); + lcd.Clear (LCD_COLOR_WHITE); + lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Match ou demonstration ?", LEFT_MODE); + DEMONSTRATION.Draw(LCD_COLOR_LIGHTGREEN, 0); + MATCH.Draw(0xFFF01010, 0); + while(etat == CHOIX) { + canProcessRx(); + if(DEMONSTRATION.Touched()) { + etat = DEMO; + while(DEMONSTRATION.Touched()); + } + + if(MATCH.Touched()) { + etat = SELECT_SIDE; + while(MATCH.Touched()); + } + + } + break; + + case DEMO : + lcd.Clear(LCD_COLOR_WHITE); + RETOUR.Draw(0xFFFF0000, 0); + TEST_HERKULEX.Draw(VERT, 0); + TEST_LASER.Draw(VERT, 0); + TEST_COULEURS.Draw(VERT, 0); + TEST_TIR_BALLE.Draw(VERT, 0); + TEST_IMMEUBLE.Draw(VERT,0); + TEST_TRIEUR.Draw(VERT,0); + if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config// + InversStrat = 0;//Pas d'inversion de la couleur + } + while (etat == DEMO) { ////////////////////////////LISTE DES DIFFERENTES DEMOS POSSIBLES/////////////////////////////////////////// + canProcessRx(); + if(TEST_HERKULEX.Touched()) { + //Strat = 0x10; + while(TEST_HERKULEX.Touched()); + CANMessage trame_Tx = CANMessage(); + trame_Tx.len = 1; + trame_Tx.format = CANStandard; + trame_Tx.type = CANData; + trame_Tx.id=CHOICE_COLOR; + trame_Tx.data[0]=0x2; + can2.write(trame_Tx); + TEST_HERKULEX.Draw(0xFFF0F0F0, 0); + etat = TEST_SERVO; + lcd.Clear(LCD_COLOR_WHITE); + ModeDemo=1; + } else if(TEST_LASER.Touched()) { + //Strat = 0x11; + while(TEST_LASER.Touched()); + TEST_LASER.Draw(0xFFF0F0F0, 0); + etat = TEST_TELEMETRE; + } else if (TEST_COULEURS.Touched()) { + while(TEST_COULEURS.Touched()); + TEST_LASER.Draw(0xFFF0F0F0, 0); + etat =TEST_CAPTEURS ; + } + + else if (TEST_TIR_BALLE.Touched()) { + while(TEST_TIR_BALLE.Touched()); + while(1) { + TEST_TIR_BALLE.Draw(0xFFF0F0F0, 0); + // rn42_Tx.printf("A");//experience +#ifdef ROBOT_SMALL + liaison_Tx.envoyer_short(0x30,666); + pc.printf("data\r"); +#else + + // lire(); + /* if (bluetooth.readable()) + pc.putc(bluetooth.getc()); + + if (pc.readable()) { + char c = pc.getc(); + if (c == 'A') { + liaison.envoyer_short(PAQUET_IDENTIFIANT_RAFRAICHIRSCORE, 20); + pc.printf("rafraichir\n"); + } + }*/ +#endif + } + //ModeDemo=1; + } else if(TEST_IMMEUBLE.Touched()) { + while(TEST_IMMEUBLE.Touched()); + TEST_IMMEUBLE.Draw(0xFFF0F0F0, 0); + etat =DEMO_IMMEUBLE; + lcd.Clear(LCD_COLOR_WHITE); + } else if(TEST_TRIEUR.Touched()) { + while(TEST_TRIEUR.Touched()); + etat=DEMO_TRIEUR; + lcd.Clear(LCD_COLOR_WHITE); + } + if(RETOUR.Touched()) { + etat = CHOIX; + while(RETOUR.Touched()); + + } + if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config + Ack_strat = 1; + wait_ms(10); + } + } + break; + + + case TEST_SERVO: //TEST DU RESTE DES SERVOS DISPO HORS TIR + lcd.SetBackColor(LCD_COLOR_WHITE); + lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"DEMONSTRATION COURS", LEFT_MODE); + ABAISSE_BLOC.Draw(VERT, 0); + RELEVE_BLOC.Draw(VERT, 0); + BRAS_ABEILLE_ON.Draw(VERT, 0); + BRAS_ABEILLE_OFF.Draw(VERT, 0); + INTERRUPTEUR_ON.Draw(VERT, 0); + INTERRUPTEUR_OFF.Draw(VERT, 0); + RETOUR.Draw(0xFFFF0000,0); + while(etat==TEST_SERVO) { + if(RETOUR.Touched()) { + while (RETOUR.Touched()); + etat=DEMO; + } else if(ABAISSE_BLOC.Touched()) { + while (ABAISSE_BLOC.Touched()); + SendRawId(GABARIT_PETIT_ROBOT); + break; + } else if(RELEVE_BLOC.Touched()) { + while (RELEVE_BLOC.Touched()); + SendRawId(PRESENTOIR_AVANT); + break; + } else if(BRAS_ABEILLE_ON.Touched()) { + while (BRAS_ABEILLE_ON.Touched()); + //SendRawId(BRAS_ABEILLE_UP); + break; + + } else if(BRAS_ABEILLE_OFF.Touched()) { + while (BRAS_ABEILLE_OFF.Touched()); + //SendRawId(BRAS_ABEILLE_DOWN); + break; + } else if(INTERRUPTEUR_ON.Touched()) { + while (INTERRUPTEUR_ON.Touched()); + //SendRawId(ALLUMER_PANNEAU_UP); + break; + } else if(INTERRUPTEUR_OFF.Touched()) { + while (INTERRUPTEUR_OFF.Touched()); + //SendRawId(ALLUMER_PANNEAU_DOWN); + break; + } + } + break; + + case TEST_TIR: // TEST DES FONCTIONS LIEES AUX TIRS + lcd.SetBackColor(LCD_COLOR_WHITE); + lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"DEMONSTRATION COURS", LEFT_MODE); + TIR_CHATEAU.Draw(VERT, 0); + EPURATION.Draw(VERT, 0); + LANCEUR_ON.Draw(VERT, 0); + LANCEUR_OFF.Draw(VERT, 0); + RETOUR.Draw(ROUGE, 0); + + etat=DEMO; + + break; + + + + case TEST_TELEMETRE: //AFFICHAGE DE LA VALEUR LUE PAR LES 4 TELEMETRES + ModeDemo=1; + lcd.Clear(LCD_COLOR_WHITE); + lcd.SetBackColor(LCD_COLOR_WHITE); + lcd.DisplayStringAt(20, LINE(2), (uint8_t *)"DEMONSTRATION COURS", LEFT_MODE); + RETOUR.Draw(0xFFFF0000, 0); + while(etat==TEST_TELEMETRE) { + SendRawId(DATA_RECALAGE); + SendRawId(DATA_TELEMETRE_LOGIQUE); + wait_ms(100); + canProcessRx(); + if(RETOUR.Touched()) { + while( RETOUR.Touched()); + etat=DEMO; + lcd.Clear(LCD_COLOR_WHITE); + } + } + break; ///////////////////////////////////////////FIN DES DEMOS///////////////////////////////////////////////// + + + case SELECT_SIDE : // CHOIX DU COTE DU TERRAIN + INVERSION DE LA STRAT SI COTE ORANGE+ ENVOI DU COTE A LA CARTE CAPTEUR/ACTIONNEURS + lcd.Clear(LCD_COLOR_WHITE); + lcd.SetBackColor(LCD_COLOR_WHITE); + lcd.SetTextColor(LCD_COLOR_BLACK); + + lcd.DisplayStringAt(70, LINE(0), (uint8_t *)"Choisir le cote", LEFT_MODE); + COTE_JAUNE.Draw(JAUNE, 0); + COTE_VIOLET.Draw(VIOLET, 0); + RETOUR.Draw(LCD_COLOR_RED, 0); + + + while (etat == SELECT_SIDE) { + canProcessRx(); + if(COTE_JAUNE.Touched()) { + Cote = 0x0; + InversStrat = Cote; + etat = TACTIQUE; + CANMessage trame_Tx = CANMessage(); + trame_Tx.len = 1; + trame_Tx.format = CANStandard; + trame_Tx.type = CANData; + trame_Tx.id=CHOICE_COLOR; + trame_Tx.data[0]=Cote; + can2.write(trame_Tx); + while(COTE_JAUNE.Touched()); + + } + + if(COTE_VIOLET.Touched()) { + Cote = 0x1; + InversStrat= Cote; + etat = TACTIQUE; + CANMessage trame_Tx = CANMessage(); + trame_Tx.len = 1; + trame_Tx.format = CANStandard; + trame_Tx.type = CANData; + trame_Tx.id=CHOICE_COLOR; + trame_Tx.data[0]=Cote; + can2.write(trame_Tx); + while(COTE_VIOLET.Touched()); + } + + if(RETOUR.Touched()) { + etat = CHOIX; + while(RETOUR.Touched()); + } + } + + break; + + case TACTIQUE : //AFFICHE LA LISTE DES STRATS AFIN DE SELECTIONNER CELLE VOULUE + if (Cote == 0) { + lcd.Clear(JAUNE); + lcd.SetBackColor(JAUNE); + } else if (Cote == 1) { + lcd.Clear(VIOLET); + lcd.SetBackColor(VIOLET); + } else { + lcd.Clear(BLEU); + lcd.SetBackColor(BLEU); + } + + lcd.SetTextColor(LCD_COLOR_BLACK); + + lcd.DisplayStringAt(20, LINE(0), (uint8_t *)"Choisir une strategie", LEFT_MODE); + + Strategie = Bouton_Strat(); // retourne valeur de Strategie si bouton strat renvoi -1 on reviens en arriere + if (Strategie == -1) { + etat = SELECT_SIDE; + } else { + etat = DETAILS; + } + wait(0.1); + break; + + case DETAILS : //SECONDE VALIDATION DE LA STRAT + lcd.Clear(LCD_COLOR_WHITE); + lcd.SetBackColor(LCD_COLOR_WHITE); + lcd.SetTextColor(LCD_COLOR_BLACK); + CHECK.Draw(VERT); + RETOUR.Draw(LCD_COLOR_RED); + + SelectionStrat(Strategie); //affiche la stratégie selectionnée + + while (etat == DETAILS) { + canProcessRx(); + if (CHECK.Touched()) { + if(gameEtat == ETAT_CONFIG) { + gameEtat = ETAT_GAME_INIT; + etat=LECTURE; + + } + while(CHECK.Touched()); + } + + if(RETOUR.Touched()) { + etat = TACTIQUE; + while(RETOUR.Touched()); + } + } + break; + + + case LECTURE : + break; + case AFF_WAIT_JACK : //FONCTIONS D'AFFICHAGE DE L'ATTENTE DU JACK + lcd.Clear(BLANC); + lcd.SetBackColor(LCD_COLOR_WHITE); + lcd.SetTextColor(LCD_COLOR_BLACK); + + if (Cote == 0) { + lcd.Clear(VERT); + lcd.SetBackColor(VERT); + } else if (Cote == 1) { + lcd.Clear(ORANGE); + lcd.SetBackColor(ORANGE); + } else { + lcd.Clear(VERT); + lcd.SetBackColor(VERT); + } + canProcessRx(); + lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"En attente du Jack", CENTER_MODE); + etat=WAIT_JACK; + break; + + case WAIT_JACK: //VERITABLE ATTENTE DU JACK + break; + + 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 + cptf=gameTimer.read(); + lcd.SetTextColor(LCD_COLOR_BLACK); + cpt=(int)cptf; + if(cpt != cpt1) { + lcd.Clear(VERT); + // affichage_compteur(100-cpt); + //affichage_compteur(SCORE_PR); +#ifdef ROBOT_BIG + affichage_var(SCORE_GR); +#else + affichage_var(SCORE_PR); +#endif + if(liaison_pr.paquet_en_attente()) { + PaquetDomotique *paquet=liaison_pr.lire(); + if(paquet->identifiant==PAQUET_IDENTIFIANT_AJOUTERSCORE) { + SCORE_PR+=convertir_score(paquet); + } + delete paquet; + } + } + cpt1=cpt; + flag_timer=0; + + //affichage_debug(gameEtat); + lcd.SetBackColor(LCD_COLOR_WHITE); + + break; + + case FIN : //AFFICHAGE DE FIN AVEC LE SCORE FINAL + lcd.Clear (LCD_COLOR_WHITE); + lcd.SetBackColor(LCD_COLOR_WHITE); +#ifdef ROBOT_BIG + // affichage_compteur(SCORE_GR); + affichage_var(SCORE_GR); + //liaison_Tx.envoyer_short(PAQUET_IDENTIFIANT_FINMATCH,SCORE_GLOBAL); +#else + //affichage_compteur(SCORE_PR); + affichage_var(SCORE_PR); +#endif + while(1); // force le redemarage du robot + //break; + + } +} + + + +/****************************************************************************************/ +/* FUNCTION NAME: automate_process */ +/* DESCRIPTION : Automate de gestion de la stratégie du robot */ +/****************************************************************************************/ +void automate_process(void) +{ + static unsigned char AX12_enchainement = 0; + static unsigned char MV_enchainement = 0; + signed char localData1 = 0; + signed short localData2 = 0; + unsigned short localData3 = 0; + //signed short localData4 = 0; + unsigned char localData5 = 0; + + + if(gameTimer.read_ms() >= 99000) {//Fin du match (On autorise 2s pour déposer des éléments + gameTimer.stop(); + gameTimer.reset(); + gameEtat = ETAT_END;//Fin du temps + etat=FIN; + } + + if(lastEtat != gameEtat || debugetatTimer.read_ms() >= 1000) { + lastEtat = gameEtat; + debugetatTimer.reset(); + sendStratEtat((unsigned char)gameEtat, (unsigned char)actual_instruction); + } + + + + switch(gameEtat) { + + case ETAT_CHECK_CARTES: + /* + Il faut faire une boucle pour verifier toutes les cartes les une apres les autres + */ + waitingAckFrom = id_alive[checkCurrent];//On indique que l'on attend un ack de la carte IHM + SendRawId(id_check[checkCurrent]);//On demande à la carte d'indiquer ça présence + + screenChecktry++;//On incrèment le conteur de tentative de 1 + cartesCheker.reset();//On reset le timeOut + cartesCheker.start();//On lance le timer pour le timeout + gameEtat = ETAT_CHECK_CARTES_WAIT_ACK; + break; + + case ETAT_CHECK_CARTES_WAIT_ACK: + /* + On attend l'ack de la carte en cours de vérification + */ + //printf("cartesCheker = %d waitingAckFrom = %d\n",cartesCheker.read_ms(), waitingAckFrom); + if(waitingAckFrom == 0) {//C'est bon la carte est en ligne + cartesCheker.stop(); + screenChecktry = 0; + countAliveCard++; + checkCurrent++; + if(checkCurrent >= NOMBRE_CARTES) { + printf("all card check, missing %d cards\n",(NOMBRE_CARTES-countAliveCard)); + if(countAliveCard >= NOMBRE_CARTES) { + gameEtat = ETAT_CONFIG; + SendRawId(ECRAN_ALL_CHECK); + flag=1; + + //tactile_printf("Selection couleur et strategie"); + } else { + gameEtat = ETAT_WAIT_FORCE;//Passage en attente de forçage du lancement + waitingAckFrom = ECRAN_ALL_CHECK; + } + } else + gameEtat = ETAT_CHECK_CARTES; + } else if(cartesCheker.read_ms () > 100) { + cartesCheker.stop(); + if(screenChecktry >=3) { + //printf("missing card %d\n",id_check[checkCurrent]); + screenChecktry = 0; + checkCurrent++; + + if(checkCurrent >= NOMBRE_CARTES) { + if(countAliveCard == NOMBRE_CARTES) { + gameEtat = ETAT_CONFIG; + flag=1; + } else { + gameEtat = ETAT_WAIT_FORCE; + waitingAckFrom = ECRAN_ALL_CHECK; + } + } else + gameEtat = ETAT_CHECK_CARTES; + + } else + gameEtat = ETAT_CHECK_CARTES; + + } + break; + case ETAT_WAIT_FORCE: + /* + Attente du forçage de la part de la carte IHM + */ + if(waitingAckFrom == 0) { + gameEtat = ETAT_CONFIG; + } + break; + case ETAT_CONFIG: + /* + Attente de l'odre de choix de mode, + Il est possible de modifier la couleur et l'id de la stratégie + Il est aussi possible d'envoyer les ordres de debug + */ + modeTelemetre = 0; + break; + case ETAT_GAME_INIT: + //On charge la liste des instructions + + loadAllInstruction(Strategie);//Mise en cache de toute les instructions + led3=1; + + SendRawId(GLOBAL_START); + + gameEtat = ETAT_GAME_WAIT_FOR_JACK; + if (etat == TEST_TELEMETRE|| etat ==TEST_CAPTEURS || etat == TEST_SERVO || etat ==TEST_TIR || etat == DEMO_IMMEUBLE) { + SendRawId(DEBUG_FAKE_JAKE); + } else { + etat = AFF_WAIT_JACK; + } + //tactile_printf("Attente du JACK."); + setAsservissementEtat(1);//On réactive l'asservissement + jack.mode(PullDown); // désactivation de la résistance interne du jack + jack.fall(&jack_ISR); // création de l'interrupt attachée au changement d'état (front descendant) sur le jack + + localData2 = POSITION_DEBUT_T; + localData3 = POSITION_DEBUT_Y; + if(InversStrat == 1) { + localData2 = -localData2;//Inversion theta + localData3 = 3000 - POSITION_DEBUT_Y;//Inversion du Y + } + SetOdometrie(ODOMETRIE_SMALL_POSITION, POSITION_DEBUT_X,1800,localData2); + + instruction = strat_instructions[actual_instruction]; + //On effectue le traitement de l'instruction + + break; + case ETAT_GAME_WAIT_FOR_JACK: + if(instruction.order==POSITION_DEBUT) { + 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 !!!) + case RECALAGE_1: + SendRawId(RECALAGE_START); + waitingAckID = ASSERVISSEMENT_RECALAGE; + waitingAckFrom = ACKNOWLEDGE_MOTEUR; +#ifdef ROBOT_SMALL + 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) +#else + GoStraight(-3000, 1,MOITIEE_ROBOT, 0); +#endif + while(waitingAckID !=0 && waitingAckFrom !=0) + canProcessRx(); + waitingAckID_FIN=ASSERVISSEMENT_RECALAGE; + waitingAckFrom_FIN= INSTRUCTION_END_MOTEUR; + while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0) + canProcessRx(); + etat_pos=RECULER_1; + break; + + case RECULER_1: + waitingAckID = ASSERVISSEMENT_RECALAGE; + waitingAckFrom = ACKNOWLEDGE_MOTEUR; +#ifdef ROBOT_SMALL + GoStraight(-100, 0, 0, 0);//-450 +#else + GoStraight(250, 0, 0, 0); +#endif + while(waitingAckID !=0 && waitingAckFrom !=0) + canProcessRx(); + waitingAckID_FIN=ASSERVISSEMENT_RECALAGE; + waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR; + while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0) + canProcessRx(); + etat_pos=TOURNER; + break; + + case TOURNER: + waitingAckID = ASSERVISSEMENT_ROTATION; + waitingAckFrom = ACKNOWLEDGE_MOTEUR; + if(Cote==0) { + localData2 = 900; + } else { + localData2=-900; + } + Rotate(localData2); + while(waitingAckID !=0 && waitingAckFrom !=0) + canProcessRx(); + waitingAckID_FIN=ASSERVISSEMENT_ROTATION; + waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR; + while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0) + canProcessRx(); + etat_pos=RECALAGE_2; + break; + + case RECALAGE_2: + waitingAckID = ASSERVISSEMENT_RECALAGE; + waitingAckFrom = ACKNOWLEDGE_MOTEUR; + if(Cote==1) { + localData3=3000-(MOITIEE_ROBOT); + } else { + localData3=MOITIEE_ROBOT; + } +#ifdef ROBOT_SMALL + GoStraight(3000, 2,localData3, 0); //on se recale contre le mur donc il faut donner la valeur du centre du robot +#else + GoStraight(-3000, 2,localData3, 0); +#endif + while(waitingAckID !=0 && waitingAckFrom !=0) + canProcessRx(); + waitingAckID_FIN=ASSERVISSEMENT_RECALAGE; + waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR; + while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0) + canProcessRx(); + etat_pos=RECULER_2; + break; + + case RECULER_2: + waitingAckID = ASSERVISSEMENT_RECALAGE; + waitingAckFrom = ACKNOWLEDGE_MOTEUR; +#ifdef ROBOT_SMALL + GoStraight(-100, 0, 0, 0); +#else + GoStraight(250, 0, 0, 0); +#endif + while(waitingAckID !=0 && waitingAckFrom !=0) + canProcessRx(); + waitingAckID_FIN=ASSERVISSEMENT_RECALAGE; + waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR; + while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0) + canProcessRx(); + etat_pos=GOTOPOS; + break; + + case GOTOPOS: + localData1 = -1; + + if(InversStrat == 1 && ingnorInversionOnce == 0) { + localData2 = -instruction.arg3; + localData3 = 3000 - instruction.arg2;//Inversion du Y + } else { + localData3 = instruction.arg2; + localData2 = instruction.arg3; + } + + GoToPosition(instruction.arg1,localData3,localData2,localData1); + waitingAckID = ASSERVISSEMENT_XYT; + waitingAckFrom = ACKNOWLEDGE_MOTEUR; + + while(waitingAckID !=0 && waitingAckFrom !=0) + canProcessRx(); + waitingAckID_FIN=ASSERVISSEMENT_XYT; + waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR; + while(waitingAckID_FIN!=0 && waitingAckFrom_FIN !=0) + canProcessRx(); + etat_pos=FIN_POS; + break; + case FIN_POS: + actual_instruction = instruction.nextLineOK; + break; + } + } + + + break; + case ETAT_GAME_START: + + gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; + + if (ModeDemo == 0) { + chronoEnd.attach(&chronometre_ISR,100);//On lance le chrono de 90s + gameTimer.start(); + } + gameTimer.reset(); + jack.fall(NULL);//On désactive l'interruption du jack + //SendRawId(GLOBAL_START); + Jack=0; //à envoyer sur le CAN et en direct pour l'automate de l'ihm ou sur CANV + //tactile_printf("Start");//Pas vraiment utile mais bon + break; + case ETAT_GAME_LOAD_NEXT_INSTRUCTION: + flagNonRepriseErrorMot = 0; + /* + Chargement de l'instruction suivante ou arret du robot si il n'y a plus d'instruction + */ + //printf("load next instruction\n"); + if(dodgeq.nb > 0){//dodge q + instruction.order=dodgeq.inst[dodgeq.nb-1].order; + instruction.arg1=dodgeq.inst[dodgeq.nb-1].arg1; + instruction.arg2=dodgeq.inst[dodgeq.nb-1].arg2; + instruction.arg3=dodgeq.inst[dodgeq.nb-1].arg3; + gameEtat=ETAT_GAME_PROCESS_INSTRUCTION; + dodgeq.nb--; + }//end dodge q + else{// no dodge q + if(actual_instruction >= nb_instructions || actual_instruction == 255) { + gameEtat = ETAT_END; + //Il n'y a plus d'instruction, fin du jeu + } else { + instruction = strat_instructions[actual_instruction]; + //On effectue le traitement de l'instruction + gameEtat = ETAT_GAME_PROCESS_INSTRUCTION; + } + }//end no dodge q + screenChecktry = 0; + ingnorInversionOnce = 0; + break; + case ETAT_GAME_PROCESS_INSTRUCTION: + /* + Traitement de l'instruction, envoie de la trame CAN + */ + //debug_Instruction(instruction); + //affichage_debug(gameEtat); + //rn42_Tx.printf("A");//lance l'electron + actionPrecedente = instruction.order; + switch(instruction.order) { + case MV_BEZIER: { + static vector< vector<short> >P1; + static vector< vector<short> >C1; + static vector< vector<short> >C2; + static int i = 0; + + //Ajoute une ligne aux tableaux pour chaques courbes de la trajectoire + P1.push_back(vector<short>()); //Nouvelle ligne + C1.push_back(vector<short>()); //Nouvelle ligne + C2.push_back(vector<short>()); //Nouvelle ligne + + P1[i].push_back(instruction.arg1); //Nouvelle colonne X + C1[i].push_back(instruction.arg3); //Nouvelle colonne X + C2[i].push_back(instruction.arg5); //Nouvelle colonne X + + if(InversStrat == 1 && ingnorInversionOnce == 0) { + P1[i].push_back(3000-instruction.arg2); //Nouvelle colonne Y + C1[i].push_back(3000-instruction.arg4); //Nouvelle colonne Y + C2[i].push_back(3000-instruction.arg6); //Nouvelle colonne Y + } else { + P1[i].push_back(instruction.arg2); //Nouvelle colonne Y + C1[i].push_back(instruction.arg4); //Nouvelle colonne Y + C2[i].push_back(instruction.arg6); //Nouvelle colonne Y + } + + i++; + + if(instruction.nextActionType == WAIT) { //Si il n'y a qu'une seule courbe ou que c'est la dernière courbe de la trajectoire + //Passage des points dans des variables temporaires pour pouvoir clear les vector avant d'être bloqué dans l'attente de l'ack + //Empeche les vector de ne pas être reset si l'ack n'est pas reçu avant la fin du match + int nbCourbes = P1.size(); + short P1_temp[nbCourbes][2]; + short C1_temp[nbCourbes][2]; + short C2_temp[nbCourbes][2]; + + for(int j=0; j<nbCourbes; j++) { + for(int i=0; i<2; i++) { + P1_temp[j][i] = P1[j][i]; + C1_temp[j][i] = C1[j][i]; + C2_temp[j][i] = C2[j][i]; + } + } + + //Clear des tableaux de points pour la prochaine trajectoire + P1.clear(); + C1.clear(); + C2.clear(); + i = 0; + + //Calcul de la courbe et envoi des valeurs + if(instruction.direction == FORWARD) { + courbeBezier(nbCourbes, P1_temp, C1_temp, C2_temp, 0); + } else if(instruction.direction == BACKWARD) { + courbeBezier(nbCourbes, P1_temp, C1_temp, C2_temp, 1); + } + + waitingAckID = ASSERVISSEMENT_BEZIER; + waitingAckFrom = ACKNOWLEDGE_MOTEUR; + } + break; + } + case MV_COURBURE://C'est un rayon de courbure + float alpha=0, theta=0; + unsigned short alph=0; + actionPrecedente = MV_COURBURE; + waitingAckID = ASSERVISSEMENT_COURBURE; + waitingAckFrom = ACKNOWLEDGE_MOTEUR; + if(instruction.nextActionType == ENCHAINEMENT) { + MV_enchainement++; + localData5 = 1; + } else { + if(MV_enchainement > 0) { + localData5 = 2; + MV_enchainement = 0; + } else { + localData5 = 0; + } + } + if(InversStrat == 1 && ingnorInversionOnce == 0) { + if(instruction.direction == LEFT) instruction.direction = RIGHT; + else instruction.direction = LEFT; + } + + localData1 = ((instruction.direction == LEFT)?1:-1); + localData2 = instruction.arg3; + /*if(InversStrat == 1 && ingnorInversionOnce == 0) { + localData1 = -localData1;//Inversion de la direction + }*/ + + BendRadius(instruction.arg1, localData2, localData1, localData5); +#ifdef ROBOT_SMALL + if(localData2>0) { + direction=1; + } else { + direction=-1; + } +#else + if(localData2>0) { + direction=-1; + } else { + direction=1; + } +#endif + if(localData2>0)alph=localData2; + else alph=-localData2; + alpha = localData2*M_PI/1800.0f; + theta = theta_robot*M_PI/1800.0f; + + if(instruction.direction == LEFT) { //-------------LEFT + if(alph<450){ // 1 XYT + dodgeq.inst[0].order = MV_XYT; + dodgeq.inst[0].arg1 = x_robot + instruction.arg1*(sin(alpha+theta)-sin(theta));// X + dodgeq.inst[0].arg2 = y_robot + instruction.arg1*(cos(theta)-cos(alpha+theta));// Y + dodgeq.inst[0].arg3 = theta_robot + alph;// T + } + else if(alph<900){ + for(int c=0;c<2;c++){ // 2 points de passages + dodgeq.inst[c].order = MV_XYT; + dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha+theta)-sin(theta));// X + dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(theta)-cos(alpha+theta));// Y + dodgeq.inst[c].arg3 = theta_robot + alph;// T + alpha-=alpha/2.0f; + alph-=alph/2; + } + } + else if(alph<1350){ + for(int c=0;c<3;c++){ // 3 points de passages + dodgeq.inst[c].order = MV_XYT; + dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha+theta)-sin(theta));// X + dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(theta)-cos(alpha+theta));// Y + dodgeq.inst[c].arg3 = theta_robot + alph;// T + alpha-=alpha/3.0f; + alph-=alph/3; + } + } + else if(alph<1800){ + for(int c=0;c<4;c++){ // 4 points de passages + dodgeq.inst[c].order = MV_XYT; + dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha+theta)-sin(theta));// X + dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(theta)-cos(alpha+theta));// Y + dodgeq.inst[c].arg3 = theta_robot + alph;// T + alpha-=alpha/4.0f; + alph-=alph/4; + } + } + else if(alph<2250){ + for(int c=0;c<5;c++){ // 5 points de passages + dodgeq.inst[c].order = MV_XYT; + dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha+theta)-sin(theta));// X + dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(theta)-cos(alpha+theta));// Y + dodgeq.inst[c].arg3 = theta_robot + alph;// T + alpha-=alpha/5.0f; + alph-=alph/5; + } + } + else { + for(int c=0;c<6;c++){ // 6 points de passages + dodgeq.inst[c].order = MV_XYT; + dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha+theta)-sin(theta));// X + dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(theta)-cos(alpha+theta));// Y + dodgeq.inst[c].arg3 = theta_robot + alph;// T + alpha-=alpha/6.0f; + alph-=alph/6; + } + } + } else { //-----------------------------------------RIGHT + if(alph<450){ // 1 XYT + dodgeq.inst[0].order = MV_XYT; + dodgeq.inst[0].arg1 = x_robot + instruction.arg1*(sin(alpha-theta)+sin(theta));// X + dodgeq.inst[0].arg2 = y_robot + instruction.arg1*(cos(alpha-theta)-cos(theta));// Y + dodgeq.inst[0].arg3 = theta_robot - alph;// T + } + else if(alph<900){ + for(int c=0;c<2;c++){ // 2 points de passages + dodgeq.inst[c].order = MV_XYT; + dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha-theta)+sin(theta));// X + dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(alpha-theta)-cos(theta));// Y + dodgeq.inst[c].arg3 = theta_robot - alph;// T + alpha-=alpha/2.0f; + } + } + else if(alph<1350){ + for(int c=0;c<3;c++){ // 3 points de passages + dodgeq.inst[c].order = MV_XYT; + dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha-theta)+sin(theta));// X + dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(alpha-theta)-cos(theta));// Y + dodgeq.inst[c].arg3 = theta_robot - alph;// T + alpha-=alpha/3.0f; + } + } + else if(alph<1800){ + for(int c=0;c<4;c++){ // 4 points de passages + dodgeq.inst[c].order = MV_XYT; + dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha-theta)+sin(theta));// X + dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(alpha-theta)-cos(theta));// Y + dodgeq.inst[c].arg3 = theta_robot - alph;// T + alpha-=alpha/4.0f; + } + } + else if(alph<2250){ + for(int c=0;c<5;c++){ // 5 points de passages + dodgeq.inst[c].order = MV_XYT; + dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha-theta)+sin(theta));// X + dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(alpha-theta)-cos(theta));// Y + dodgeq.inst[c].arg3 = theta_robot - alph;// T + alpha-=alpha/5.0f; + } + } + else { + for(int c=0;c<6;c++){ // 6 points de passages + dodgeq.inst[c].order = MV_XYT; + dodgeq.inst[c].arg1 = x_robot + instruction.arg1*(sin(alpha-theta)+sin(theta));// X + dodgeq.inst[c].arg2 = y_robot + instruction.arg1*(cos(alpha-theta)-cos(theta));// Y + dodgeq.inst[c].arg3 = theta_robot - alph;// T + alpha-=alpha/6.0f; + } + } + } + break; + + + + case MV_LINE://Ligne droite + waitingAckID = ASSERVISSEMENT_RECALAGE; + waitingAckFrom = ACKNOWLEDGE_MOTEUR; + if(instruction.nextActionType == ENCHAINEMENT) { + MV_enchainement++; + localData5 = 1; + } else { + if(MV_enchainement > 0) {//Utilisé en cas d'enchainement, + localData5 = 2; + MV_enchainement = 0; + } else { + localData5 = 0; + } + } + localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1); + GoStraight(localData2, 0, 0, localData5); + + target_x_robot = x_robot + localData2*cos((double)theta_robot*M_PI/1800); + target_y_robot = y_robot + localData2*sin((double)theta_robot*M_PI/1800); + target_theta_robot = theta_robot; + + break; + case MV_TURN: //Rotation sur place + target_x_robot = x_robot; + target_y_robot = y_robot; + target_theta_robot = theta_robot + localData2; + localData2 = instruction.arg3; + + if(InversStrat == 1 && ingnorInversionOnce == 0) { + localData2 = -localData2; + } + + + if(instruction.direction == ABSOLUTE) { + //C'est un rotation absolu, il faut la convertir en relative + + localData2 = (localData2 - theta_robot)%3600; + if(localData2 > 1800) localData2 = localData2-3600; + + else if(localData2 <-1800) localData2 = localData2+3600; + } + + + waitingAckID = ASSERVISSEMENT_ROTATION; + waitingAckFrom = ACKNOWLEDGE_MOTEUR; + Rotate(localData2); + + break; + case MV_XYT: + if(instruction.direction == BACKWARD) { + localData1 = -1; + } else { + localData1 = 1; + } + + if(InversStrat == 1 && ingnorInversionOnce == 0) { + localData2 = -instruction.arg3; + localData3 = 3000 - instruction.arg2;//Inversion du Y + } else { + localData3 = instruction.arg2; + localData2 = instruction.arg3; + } + + GoToPosition(instruction.arg1,localData3,localData2,localData1); + waitingAckID = ASSERVISSEMENT_XYT; + waitingAckFrom = ACKNOWLEDGE_MOTEUR; + + target_x_robot = instruction.arg1; + target_y_robot = localData3; + target_theta_robot = localData2; + + break; + case MV_RECALAGE: + if(instruction.nextActionType == MECANIQUE) { + instruction.nextActionType = WAIT; + + waitingAckID = ASSERVISSEMENT_RECALAGE; + waitingAckFrom = ACKNOWLEDGE_MOTEUR; + + localData2 = (((instruction.direction == FORWARD)?1:-1)*3000);//On indique une distance de 3000 pour etre sur que le robot va ce recaler + + if(instruction.precision == RECALAGE_Y) { + localData5 = 2; + if(InversStrat == 1 && ingnorInversionOnce == 0) { + localData3 = 3000 - instruction.arg1;//Inversion du Y + } else { + localData3 = instruction.arg1; + } + } else { + localData5 = 1; + localData3 = instruction.arg1; + } + GoStraight(localData2, localData5, localData3, 0); + } else { //CAPTEUR + SendRawId(DATA_RECALAGE); + waitingAckID = RECEPTION_RECALAGE; + waitingAckFrom = ACKNOWLEDGE_TELEMETRE; + + // On attend que les variables soient actualisé + while(!(waitingAckID == 0 && waitingAckFrom == 0)) + canProcessRx(); + while(!(waitingAckID_FIN==0 && waitingAckFrom_FIN==0)) + canProcessRx(); + + if(instruction.precision == RECALAGE_Y) { // ((theta_robot < 1800 && theta_robot > 0) || (theta_robot < -1800 && theta_robot > -3600)) (theta_robot < 900 && theta_robot > -900) + SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, recalageDistanceY(), theta_robot); + } else if(instruction.precision == RECALAGE_X) { + SetOdometrie(ODOMETRIE_SMALL_POSITION, recalageDistanceX(), y_robot, theta_robot); + } else if(instruction.precision == RECALAGE_T) { + SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, y_robot, recalageAngulaireCapteur() ); + } + } + break; + + case ACTION: + + waitingAckID_FIN = 0; + waitingAckFrom_FIN = 0; + + int tempo = 0; + waitingAckID= ACK_ACTION; //On veut un ack de type action + waitingAckFrom = ACKNOWLEDGE_HERKULEX; //de la part des herkulex + tempo = doAction(instruction.arg1,instruction.arg2,instruction.arg3); + // unsigned char test=(unsigned char) tempo; + // SendMsgCan(0x5BD, &test,1); + if(tempo == 1) { + //L'action est spécifique + if((waitingAckFrom == 0 && waitingAckID == 0) && instruction.nextActionType == ENCHAINEMENT) { + actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante + gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; + } else { + gameEtat = ETAT_GAME_WAIT_ACK; + } +#ifdef ROBOT_SMALL + /*actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante + gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;*/ +#endif + return; +#ifdef ROBOT_SMALL + } else if (tempo == 2) { + // on est dans le cas de l'avance selon le telemetre + waitingAckID = ASSERVISSEMENT_RECALAGE; + waitingAckFrom = ACKNOWLEDGE_MOTEUR; + + localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1); + GoStraight(telemetreDistance, 0, 0, 0); + // on reset la distance du telemetre à 0 + telemetreDistance = 5000; +#endif + } else { + //C'est un AX12 qu'il faut bouger + //AX12_setGoal(instruction.arg1,instruction.arg3/10,instruction.arg2); + //AX12_enchainement++; + + } + break; + default: + //Instruction inconnue, on l'ignore + break; + } + + if(instruction.nextActionType == JUMP || instruction.nextActionType == WAIT) { + gameEtat = ETAT_GAME_WAIT_ACK;//Il faut attendre que la carte est bien reçu l'acknowledge + screenChecktry++;//On incrèment le conteur de tentative de 1 + cartesCheker.reset();//On reset le timeOut + cartesCheker.start(); + if(AX12_enchainement > 0) { + //AX12_processChange();//Il faut lancer le déplacement des AX12 + //AX12_enchainement = 0; + } + } else { //C'est un enchainement + if(instruction.order == MV_LINE) { + gameEtat = ETAT_GAME_WAIT_ACK; + } else { + actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante + gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//C'est un enchainement, on charge directement l'instruction suivante + } + } + + break; + case ETAT_GAME_WAIT_ACK: + canProcessRx(); + //SendSpeed(200);//--------------------------------------------------MODE RALENTI !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + if(waitingAckID == 0 && waitingAckFrom == 0) {//Les ack ont été reset, c'est bon on continue + //if(true) { + cartesCheker.stop(); + if(instruction.nextActionType == JUMP) { + if(instruction.jumpAction == JUMP_POSITION) { + gameEtat = ETAT_GAME_JUMP_POSITION; + } else { //Pour eviter les erreurs, on dit que c'est par défaut un jump time + gameEtat = ETAT_GAME_JUMP_TIME; + cartesCheker.reset();//On reset le timeOut + cartesCheker.start(); + } + } else if(instruction.nextActionType == WAIT) { ///Actualisation des waiting ack afin d'attendre la fin des actions + /*wait_ms(200); + #ifdef ROBOT_BIG + SetOdometrie(ODOMETRIE_BIG_POSITION, x_robot, y_robot, theta_robot); + #else + SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, y_robot, theta_robot); + #endif + wait_ms(200);*/ + + gameEtat = ETAT_GAME_WAIT_END_INSTRUCTION; + switch(instruction.order) { + case MV_BEZIER: + waitingAckID_FIN = ASSERVISSEMENT_BEZIER; + waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR; + break; + case MV_COURBURE: + waitingAckID_FIN = ASSERVISSEMENT_COURBURE; + waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR; + break; + case MV_LINE: + waitingAckID_FIN = ASSERVISSEMENT_RECALAGE; + waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR; + break; + case MV_TURN: + waitingAckID_FIN = ASSERVISSEMENT_ROTATION; + waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR; + break; + case MV_XYT: + waitingAckID_FIN = ASSERVISSEMENT_XYT; + waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR; + break; + case MV_RECALAGE: + waitingAckID_FIN = ASSERVISSEMENT_RECALAGE; + waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR; + break; + case ACTION: + + if (modeTelemetre == 0) { + if (telemetreDistance == 0) { + waitingAckID_FIN = ACK_FIN_ACTION;// ack de type action + waitingAckFrom_FIN = ACKNOWLEDGE_HERKULEX; //de la part des herkulex/actionneurs + } else if(telemetreDistance == 5000) { + // on est dans le cas ou l'on fait une ligne suivant la distance du telemetre + //waitingAckID_FIN = ASSERVISSEMENT_RECALAGE; + //waitingAckFrom_FIN = INSTRUCTION_END_MOTEUR; + telemetreDistance = 0; + } + } else { // si on attend la reponse du telemetre + //modeTelemetre = 1; + waitingAckID_FIN = OBJET_SUR_TABLE; + waitingAckFrom_FIN = 0; + } + break; + default: + break; + } + } else { + gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; + actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante + } + } else if(cartesCheker.read_ms () > 1000) { + cartesCheker.stop(); + if(screenChecktry >=2) {//La carte n'a pas reçus l'information, on passe à l'instruction d'erreur + actual_instruction = instruction.nextLineError; + gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; + } else { + gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;//On retourne dans l'etat d'envois de l'instruction + } + } + break; + + case ETAT_GAME_JUMP_TIME: + if(cartesCheker.read_ms () >= instruction.JumpTimeOrX) { + cartesCheker.stop();//On arrete le timer + actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante + gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante + } + break; + + case ETAT_GAME_JUMP_CONFIG: + signed int depasX = 1, depasY = 1; // servent à indiquer le sens de dépassement des coordonnées + // 1 si l'instruction est plus grande que la position du robot + // -1 si l'instruction est plus petite que la position du robot + // 0 si l'instruction et position du robot sont proche de moins de 1cm + if (abs(x_robot-instruction.JumpTimeOrX)<10) { + depasX = 0; + } else if(x_robot > instruction.JumpTimeOrX) { + depasX = -1; + } + + if(abs(y_robot-instruction.JumpY)<10) { + depasY = 0; + } else if(y_robot > instruction.JumpY) { + depasY = -1; + } + + gameEtat = ETAT_GAME_JUMP_POSITION; + break; + case ETAT_GAME_JUMP_POSITION: + bool Xok = false, Yok = false; + + if (depasX == 0) { + Xok = true; + } else if ((instruction.JumpTimeOrX - x_robot)*depasX < -5) { + Xok = true; + } + + if (depasY == 0) { + Yok = true; + } else if ((instruction.JumpY - y_robot)*depasY < -5) { + Yok = true; + } + + // on teste si les deux coordonnées ont été dépassées, si oui on lance l'instruction suivante + if (Xok && Yok) { + actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante + gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante + } + + break; + case ETAT_GAME_WAIT_END_INSTRUCTION: + canProcessRx(); + if(waitingAckID_FIN == 0 && waitingAckFrom_FIN ==0) {//On attend que la carte nous indique que l'instruction est terminée + actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante + gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante + } + + break; + + case ETAT_END: + if (ModeDemo) { + gameEtat = ETAT_CHECK_CARTE_SCREEN; + ModeDemo = 1; + } else { + gameEtat = ETAT_END_LOOP; + } + break; + case ETAT_END_LOOP: + //Rien, on tourne en rond + + break; + default: + + break; + } +} + + + + + +/****************************************************************************************/ +/* FUNCTION NAME: canProcessRx */ +/* DESCRIPTION : Fait évoluer l'automate de l'IHM en fonction des receptions sur le CAN*/ +/****************************************************************************************/ +void canProcessRx(void) +{ + static signed char FIFO_occupation=0,FIFO_max_occupation=0; + char message[10]="toto"; + char message1[10]="toto"; + char message2[10]="toto"; + char message3[10]="toto"; + char message4[10]="toto"; + FIFO_occupation=FIFO_ecriture-FIFO_lecture; + + if(FIFO_occupation<0) + FIFO_occupation=FIFO_occupation+SIZE_FIFO; + + if(FIFO_max_occupation<FIFO_occupation) { + FIFO_max_occupation=FIFO_occupation; + //SendRawId( + } + + if(FIFO_occupation!=0) { + int identifiant=msgRxBuffer[FIFO_lecture].id; + + if (waitingId == identifiant) waitingId = 0; + switch(identifiant) { + + case ALIVE_MOTEUR: + if (etat == ATT) { + + lcd.SetTextColor(LCD_COLOR_LIGHTGREEN); + lcd.FillRect(0,400,400,150); + lcd.SetTextColor(LCD_COLOR_BLACK); + lcd.SetBackColor(LCD_COLOR_LIGHTGREEN); + lcd.DisplayStringAt(80, 450, (uint8_t *)"Carte Moteur", LEFT_MODE); + } + break; + + case ALIVE_BALISE: + if (etat == ATT) { + + lcd.SetTextColor(LCD_COLOR_LIGHTGREEN); + lcd.FillRect(0,600,400,150); //carte AX12 + lcd.SetTextColor(LCD_COLOR_BLACK); + lcd.SetBackColor(LCD_COLOR_LIGHTGREEN); + lcd.DisplayStringAt(110, 650, (uint8_t *)"Balise", LEFT_MODE); + } + break; + + case RESET_IHM: + etat = CHOIX; + break; + + case DEBUG_FAKE_JAKE://Permet de lancer le match à distance + case GLOBAL_JACK: + if(gameEtat == ETAT_GAME_WAIT_FOR_JACK) { + gameEtat = ETAT_GAME_START; + SendRawId(ACKNOWLEDGE_JACK); + } + break; + + case ALIVE_ACTIONNEURS_AVANT: //pas de break donc passe directement dans ECRAN_ALL_CHECK mais conserve l'ident initial + case ALIVE_ACTIONNEURS_ARRIERE: + case ALIVE_HERKULEX: + case ECRAN_ALL_CHECK: + if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id) { + waitingAckFrom = 0;//C'est la bonne carte qui indique qu'elle est en ligne + } + flag=1; + break; + + case ASSERVISSEMENT_ERROR_MOTEUR://erreur asservissement + { + + unsigned short recieveAckID;// = (unsigned short)msgRxBuffer[FIFO_lecture].data[0] | ( ((unsigned short)msgRxBuffer[FIFO_lecture].data[1]) <<8); + memcpy(&recieveAckID, msgRxBuffer[FIFO_lecture].data, 2); + if(recieveAckID == waitingAckID_FIN && waitingAckFrom_FIN == INSTRUCTION_END_MOTEUR) + { + if(flagNonRepriseErrorMot) { + actual_instruction = instruction.nextLineError; + gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; + flagNonRepriseErrorMot = 0; + } else { + flagNonRepriseErrorMot = 1; + timeoutWarningWaitEnd.reset(); + timeoutWarningWaitEnd.start(); + //gameEtat = ETAT_WARING_END_BALISE_WAIT; + } + } + /* + if(flagNonRepriseErrorMot) { + actual_instruction = instruction.nextLineError; + gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; + flagNonRepriseErrorMot = 0; + } else { + flagNonRepriseErrorMot = 1; + gameEtat = ETAT_WARNING_END_LAST_INSTRUCTION; + }*/ + } break; + + /////////////////////////////////////Acknowledges de Reception de la demande d'action//////////////////////////////////////// + case ACKNOWLEDGE_HERKULEX: + case ACKNOWLEDGE_BALISE: //pas de break donc passe directement dans ACK_FIN_ACTION mais conserve l'ident initial + + case ACKNOWLEDGE_TELEMETRE: + /////////////////////////////////////////////Acknowledges de la fin d'action///////////////////////////////////////////////// + case ACKNOWLEDGE_MOTEUR: + case INSTRUCTION_END_BALISE: + case ACK_FIN_ACTION: + case INSTRUCTION_END_MOTEUR: + unsigned short recieveAckID;// = (unsigned short)msgRxBuffer[FIFO_lecture].data[0] | ( ((unsigned short)msgRxBuffer[FIFO_lecture].data[1]) <<8); + memcpy(&recieveAckID, msgRxBuffer[FIFO_lecture].data, 2); + /* +//on desactive la balise dans les rotations XYT + if(msgRxBuffer[FIFO_lecture].id==ACKNOWLEDGE_MOTEUR && ASSERVISSEMENT_XYT==recieveAckID)ingnorBalise=1; + if(msgRxBuffer[FIFO_lecture].id==INSTRUCTION_END_MOTEUR && ASSERVISSEMENT_XYT_ROTATE==recieveAckID)ingnorBalise=0; + +//on desactive la balise dans les rotations + if(msgRxBuffer[FIFO_lecture].id==ACKNOWLEDGE_MOTEUR && ASSERVISSEMENT_ROTATION==recieveAckID)ingnorBalise=1; + if(msgRxBuffer[FIFO_lecture].id==INSTRUCTION_END_MOTEUR && ASSERVISSEMENT_ROTATION==recieveAckID)ingnorBalise=0; + */ + + // SendMsgCan(0x666,&ingnorBalise,1); + + if( waitingAckFrom == msgRxBuffer[FIFO_lecture].id && recieveAckID == waitingAckID ) { + waitingAckFrom = 0; + waitingAckID = 0; + } + if( waitingAckFrom_FIN == msgRxBuffer[FIFO_lecture].id && recieveAckID == waitingAckID_FIN ) { + waitingAckFrom_FIN = 0; + waitingAckID_FIN = 0; + } + + /* + if((waitingAckFrom == msgRxBuffer[FIFO_lecture].id) && + ((unsigned short)msgRxBuffer[FIFO_lecture].data[0] | (((unsigned short)msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID) ) + { + waitingAckFrom = 0; + waitingAckID = 0; + } + if(waitingAckFrom_FIN == msgRxBuffer[FIFO_lecture].id && ((unsigned short)msgRxBuffer[FIFO_lecture].data[0] + |(((unsigned short)msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID_FIN)) + { + waitingAckFrom_FIN = 0; + waitingAckID_FIN = 0; + } + */ + break; + + +#ifdef ROBOT_BIG +#define point_balance 12 +#define point_accelerateur 10 + case NB_PALETS_BLEU: //nb de palets bleu mis dans l'accelerateur + unsigned short palets_bleu=(unsigned short)msgRxBuffer[FIFO_lecture].data[0]; + SCORE_GR+=palets_bleu*point_balance; + //SCORE_GLOBAL=SCORE_GR+SCORE_PR; + //liaison_Tx.envoyer_short(0x30,SCORE_GLOBAL); + break; + + case NB_PALETS_VERTS://nb de palets vert/rouge mis dans l'accelerateur + unsigned short palets_verts=(unsigned short)msgRxBuffer[FIFO_lecture].data[0]; + SCORE_GR+=palets_verts*point_accelerateur; + //SCORE_GLOBAL=SCORE_GR+SCORE_PR; + //liaison_Tx.envoyer_short(0x30,SCORE_GLOBAL); + break; +#endif + case ODOMETRIE_BIG_POSITION: + case ODOMETRIE_SMALL_POSITION: + + x_robot=msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8); + y_robot=msgRxBuffer[FIFO_lecture].data[2]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[3])<<8); + theta_robot=msgRxBuffer[FIFO_lecture].data[4]|((signed short)(msgRxBuffer[FIFO_lecture].data[5])<<8); + break; + + case ACK_ACTION: + if(waitingAckID == msgRxBuffer[FIFO_lecture].id) { + waitingAckFrom = 0; + waitingAckID = 0; + } + break; + + case BALISE_DANGER : + SendAck(ACKNOWLEDGE_BALISE, BALISE_END_DANGER); + balise_danger(); + break; + + case BALISE_STOP: + SendAck(ACKNOWLEDGE_BALISE, BALISE_STOP); + balise_stop(FIFO_lecture); + break; + + case BALISE_END_DANGER: + SendAck(ACKNOWLEDGE_BALISE, BALISE_END_DANGER); + balise_end_danger(&instruction,&dodgeq,&gameEtat,target_x_robot,target_y_robot,target_theta_robot, theta_robot,x_robot,y_robot); + break; + + case RECEPTION_DATA: + telemetreDistance=char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[0], msgRxBuffer[FIFO_lecture].data[1]); + telemetreDistance= (float)telemetreDistance*100.0f*35.5f+50.0f; + waitingAckFrom = 0; + waitingAckID = 0; + break; + + case RECEPTION_RECALAGE: + wait_us(150); + flagReceptionTelemetres = 1; + // 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 + memcpy(&telemetreDistance_avant_droite,msgRxBuffer[FIFO_lecture].data,2); + telemetreDistance_avant_gauche = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[2], msgRxBuffer[FIFO_lecture].data[3]); + telemetreDistance_arriere_gauche = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[4], msgRxBuffer[FIFO_lecture].data[5]); + telemetreDistance_arriere_droite = char_to_short_transformation(msgRxBuffer[FIFO_lecture].data[6], msgRxBuffer[FIFO_lecture].data[7]); + + + if(ModeDemo==1) { + sprintf(message,"%04dmm L:%d",telemetreDistance_avant_droite,DT_AVD_interrupt); + lcd.SetBackColor(LCD_COLOR_WHITE); + lcd.DisplayStringAt(0, LINE(8),(unsigned char *)"LASER AVD : ",LEFT_MODE); + lcd.DisplayStringAt(200, LINE(8),(unsigned char *)message, LEFT_MODE); + + sprintf(message1,"%04dmm L:%d",telemetreDistance_avant_gauche,DT_AVG_interrupt); + lcd.SetBackColor(LCD_COLOR_WHITE); + lcd.DisplayStringAt(0, LINE(10),(unsigned char *)"LASER AVG : ",LEFT_MODE); + lcd.DisplayStringAt(200, LINE(10),(unsigned char *)message1, LEFT_MODE); + + + sprintf(message4,"%04d",theta_robot); + lcd.SetBackColor(LCD_COLOR_WHITE); + lcd.DisplayStringAt(0, LINE(13),(unsigned char *)"THETA: ",LEFT_MODE); + lcd.DisplayStringAt(200, LINE(13),(unsigned char *)message4, LEFT_MODE); + + + sprintf(message2,"%04dmm L:%d",telemetreDistance_arriere_gauche,DT_ARG_interrupt); + lcd.SetBackColor(LCD_COLOR_WHITE); + lcd.DisplayStringAt(0, LINE(16),(unsigned char *)"LASER ARG : ",LEFT_MODE); + lcd.DisplayStringAt(200, LINE(16),(unsigned char *)message2, LEFT_MODE); + + sprintf(message3,"%04dmm L:%d",telemetreDistance_arriere_droite,DT_ARD_interrupt); + lcd.SetBackColor(LCD_COLOR_WHITE); + lcd.DisplayStringAt(0, LINE(18),(unsigned char *)"LASER ARD : ",LEFT_MODE); + lcd.DisplayStringAt(200, LINE(18),(unsigned char *)message3, LEFT_MODE); + } + break; + case RECEPTION_TELEMETRE_LOGIQUE: + + DT_AVD_interrupt=msgRxBuffer[FIFO_lecture].data[0]; + DT_AVG_interrupt=msgRxBuffer[FIFO_lecture].data[1]; + DT_ARG_interrupt=msgRxBuffer[FIFO_lecture].data[2]; + DT_ARD_interrupt=msgRxBuffer[FIFO_lecture].data[3]; + + break; + case RECEPTION_COULEUR: + if (blocage_balise==0) { + couleur1=msgRxBuffer[FIFO_lecture].data[0]; + couleur2=msgRxBuffer[FIFO_lecture].data[1]; + couleur3=msgRxBuffer[FIFO_lecture].data[2]; + + /*lcd.DisplayStringAt(0,LINE(16),(unsigned char *)couleur1+'0',LEFT_MODE); + lcd.DisplayStringAt(0,LINE(16+1),(unsigned char *)couleur2+'0',LEFT_MODE); + lcd.DisplayStringAt(0,LINE(16+2),(unsigned char *)couleur3+'0',LEFT_MODE);*/ + } + + break; + /* + case NO_BLOC: //il n'y a pas de bloc, on saute les étapes liées à l'attrape bloc + actual_instruction = instruction.nextLineError; + gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; + // waitingAckID_FIN=0; + // waitingAckFrom_FIN=0; + SendRawId(0x40); + break;*/ + } + FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO; + } +} + + + + + +/****************************************************************************************/ +/* FUNCTION NAME: Bouton_Strat */ +/* DESCRIPTION : Sélection de la strat sur le lcd puis envoie sur CAN (à modifier!) */ +/****************************************************************************************/ +signed char Bouton_Strat (void) +{ + Button STRAT_1 (0, 30, 190, 110, strat_sd[0]); + Button STRAT_2 (210, 30, 190, 110, strat_sd[1]); + Button STRAT_3 (0, 150, 190, 110, strat_sd[2]); + Button STRAT_4 (210, 150, 190, 110, strat_sd[3]); + Button STRAT_5 (0, 270, 190, 110,strat_sd[4]); + Button STRAT_6 (210, 270, 190, 110, strat_sd[5]); + Button STRAT_7 (0, 390, 190, 110, strat_sd[6]); + Button STRAT_8 (210, 390, 190, 110, strat_sd[7]); + Button STRAT_9 (0, 510, 190, 110, strat_sd[8]); + Button STRAT_10 (210, 510, 190, 110, strat_sd[9]); + Button RETOUR (0, 680, 400, 110, "--Precedent--"); + //Definition des boutons + + Ack_strat = 0; + Strat = 0; + STRAT_1.Draw(0xFFF0F0F0, 0); + STRAT_2.Draw(0xFFF0F0F0, 0); + STRAT_3.Draw(0xFFF0F0F0, 0); + STRAT_4.Draw(0xFFF0F0F0, 0); + STRAT_5.Draw(0xFFF0F0F0, 0); + STRAT_6.Draw(0xFFF0F0F0, 0); + STRAT_7.Draw(0xFFF0F0F0, 0); + STRAT_8.Draw(0xFFF0F0F0, 0); + STRAT_9.Draw(0xFFF0F0F0, 0); + STRAT_10.Draw(0xFFF0F0F0, 0); + RETOUR.Draw(0xFFFF0000, 0); + + while(Ack_strat == 0) { + canProcessRx(); + CANMessage msgTx=CANMessage(); + //msgTx.id=ECRAN_CHOICE_STRAT; + if (RETOUR.Touched()) + return -1; + while(RETOUR.Touched()); + //////////////////////////////STRATEGIE N°1 + if (STRAT_1.Touched()) { + Strat = 0; + //msgTx.data[0] = 0x1; + //can2.write(msgTx); + while(STRAT_1.Touched()); + Ack_strat =1; + } + /////////////////////////////STRATEGIE N°2 + if (STRAT_2.Touched()) { + Strat = 1; + //msgTx.data[0] = 0x2; + //can2.write(msgTx); + while(STRAT_2.Touched()); + Ack_strat =1; + } + //////////////////////////////STRATEGIE N°3 + if (STRAT_3.Touched()) { + Strat = 2; + //msgTx.data[0] = 0x3; + //can2.write(msgTx); + while(STRAT_3.Touched()); + Ack_strat =1; + } + /////////////////////////////STRATEGIE N°4 + if (STRAT_4.Touched()) { + Strat = 3; + //msgTx.data[0] = 0x4; + //can2.write(msgTx); + while(STRAT_4.Touched()); + Ack_strat =1; + } + ///////////////////////////////STRATEGIE N°5 + if (STRAT_5.Touched()) { + Strat = 4; + //msgTx.data[0] = 0x5; + //can2.write(msgTx); + while(STRAT_5.Touched()); + Ack_strat =1; + } + ////////////////////////////////STRATEGIE N°6 + if (STRAT_6.Touched()) { + Strat = 5; + //msgTx.data[0] = 0x6; + //can2.write(msgTx); + while(STRAT_6.Touched()); + Ack_strat =1; + } + /////////////////////////////////STRATEGIE N°7 + if (STRAT_7.Touched()) { + Strat = 6; + //msgTx.data[0] = 0x7; + //can2.write(msgTx); + while(STRAT_7.Touched()); + Ack_strat =1; + } + /////////////////////////////////STRATEGIE N°8 + if (STRAT_8.Touched()) { + Strat = 7; + //msgTx.data[0] = 0x8; + //can2.write(msgTx); + while(STRAT_8.Touched()); + Ack_strat =1; + } + /////////////////////////////////STRATEGIE N°9 + if (STRAT_9.Touched()) { + Strat = 8; + //msgTx.data[0] = 0x9; + //can2.write(msgTx); + while(STRAT_9.Touched()); + Ack_strat =1; + } + ///////////////////////////////////STRATEGIE N°10 + if (STRAT_10.Touched()) { + Strat = 9; + //msgTx.data[0] = 0xA; + //can2.write(msgTx); + while(STRAT_10.Touched()); + Ack_strat =1; + } + + } + return Strat; + +} + +void affichage_compteur (int nombre) +{ + int dizaine=0,unite=0,centaine=0; + centaine = nombre/100; + dizaine = nombre/10; + unite = nombre-(10*dizaine); + print_segment(unite,-50); + print_segment(dizaine,100); + if(centaine!=0) { + print_segment(centaine,350); + } + +} + + +//****print_segment*** +//Dessine en 7 segment le nombre en parametre +// A +// ===== +// | | +// B | G | E +// |=====| +// C | | F +// | | +// ===== +// D +/* +position pour le chiffre des unites +lcd.FillRect(460,75,120,25);// A +lcd.FillRect(435,100,25,120);// B +lcd.FillRect(435,245,25,120);// C +lcd.FillRect(460,365,120,25);// D +lcd.FillRect(580,100,25,120);// E +lcd.FillRect(580,245,25,120);// F +lcd.FillRect(460,220,120,25);// G + +position pour le chiffre des dizaines +lcd.FillRect(260,75,120,25);// A +lcd.FillRect(235,100,25,120);// B +lcd.FillRect(235,245,25,120);// C +lcd.FillRect(260,365,120,25);// D +lcd.FillRect(380,100,25,120);// E +lcd.FillRect(380,245,25,120);// F +lcd.FillRect(260,220,120,25);// G +*/ + +void print_segment(int nombre, int decalage) +{ + + switch(nombre) { + case 0: + lcd.FillRect(240-decalage,75,120,25); + lcd.FillRect(215-decalage,100,25,120); + lcd.FillRect(215-decalage,245,25,120); + lcd.FillRect(360-decalage,245,25,120); + lcd.FillRect(360-decalage,100,25,120); + lcd.FillRect(240-decalage,365,120,25); + break; + + case 1: + lcd.FillRect(360-decalage,100,25,120);// E + lcd.FillRect(360-decalage,245,25,120);// F + break; + + case 2: + lcd.FillRect(240-decalage,75,120,25);// A + lcd.FillRect(215-decalage,245,25,120);// C + lcd.FillRect(240-decalage,365,120,25);// D + lcd.FillRect(360-decalage,100,25,120);// E + lcd.FillRect(240-decalage,220,120,25);// G + break; + + case 3: + lcd.FillRect(240-decalage,75,120,25);// A + lcd.FillRect(360-decalage,100,25,120);// E + lcd.FillRect(240-decalage,220,120,25);// G + lcd.FillRect(240-decalage,365,120,25);// D + lcd.FillRect(360-decalage,245,25,120);// F + break; + + case 4: + lcd.FillRect(215-decalage,100,25,120);// B + lcd.FillRect(360-decalage,100,25,120);// E + lcd.FillRect(360-decalage,245,25,120);// F + lcd.FillRect(240-decalage,220,120,25);// G + break; + + case 5: + lcd.FillRect(240-decalage,75,120,25);// A + lcd.FillRect(215-decalage,100,25,120);// B + lcd.FillRect(240-decalage,220,120,25);// G + lcd.FillRect(240-decalage,365,120,25);// D + lcd.FillRect(360-decalage,245,25,120);// F + break; + + case 6: + lcd.FillRect(240-decalage,75,120,25);// A + lcd.FillRect(215-decalage,100,25,120);// B + lcd.FillRect(215-decalage,245,25,120);// C + lcd.FillRect(240-decalage,365,120,25);// D + lcd.FillRect(360-decalage,245,25,120);// F + lcd.FillRect(240-decalage,220,120,25);// G + break; + + case 7: + lcd.FillRect(240-decalage,75,120,25);// A + lcd.FillRect(360-decalage,100,25,120);// E + lcd.FillRect(360-decalage,245,25,120);// F + break; + + case 8: + lcd.FillRect(240-decalage,75,120,25); // A + lcd.FillRect(215-decalage,100,25,120); + lcd.FillRect(215-decalage,245,25,120); + lcd.FillRect(360-decalage,245,25,120);//... + lcd.FillRect(360-decalage,100,25,120); + lcd.FillRect(240-decalage,365,120,25); + lcd.FillRect(240-decalage,220,120,25);// G + break; + + case 9: + lcd.FillRect(240-decalage,75,120,25);// A + lcd.FillRect(215-decalage,100,25,120);// B + lcd.FillRect(240-decalage,365,120,25);// D + lcd.FillRect(360-decalage,100,25,120);// E + lcd.FillRect(360-decalage,245,25,120);// F + lcd.FillRect(240-decalage,220,120,25);// G + break; + } +} + +void effacer_segment(long couleur) +{ + lcd.SetTextColor(couleur); + lcd.FillRect(240-200,75,120,25); // A + lcd.FillRect(215-200,100,25,120); + lcd.FillRect(215-200,245,25,120); + lcd.FillRect(360-200,245,25,120);//... + lcd.FillRect(360-200,100,25,120); + lcd.FillRect(240-200,365,120,25); + lcd.FillRect(240-200,220,120,25);// G + + lcd.FillRect(240,75,120,25); // A + lcd.FillRect(215,100,25,120); + lcd.FillRect(215,245,25,120); + lcd.FillRect(360,245,25,120);//... + lcd.FillRect(360,100,25,120); + lcd.FillRect(240,365,120,25); + lcd.FillRect(240,220,120,25);// G +} + +short recalageAngulaireCapteur(void) +{ + unsigned char nombresDeMesuresAuxTelemetresQuiSontCoherentes = 0; + unsigned int moyennageTelemetre = 0; + unsigned short angleAvant = 0; + unsigned short angleArriere = 0; + unsigned short orientationArrondie = 0; + + unsigned short position_avant_gauche=0; + unsigned short position_avant_droite=0; + unsigned short position_arriere_gauche=0; + unsigned short position_arriere_droite=0; + + unsigned short tempo= telemetreDistance_arriere_gauche; + telemetreDistance_arriere_gauche=telemetreDistance_arriere_droite; + telemetreDistance_arriere_droite=tempo; + + + + if(theta_robot >= 450 && theta_robot <= 1350) + orientationArrondie = 90; + else if(theta_robot <= -450 && theta_robot >= -1350) + orientationArrondie = 270; + else if(theta_robot <= 450 && theta_robot >= -450) + orientationArrondie = 0; + else if(theta_robot >= 1350 && theta_robot <= -1350) + orientationArrondie = 180; + + // Calcul de position pour faire la vérification de cohérence + if(orientationArrondie == 90 || orientationArrondie == 270) { + 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; + 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; + 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; + 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; + + } else if(orientationArrondie == 0 || orientationArrondie == 180) { + position_avant_gauche = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_gauche; + position_avant_droite = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_droite; + position_arriere_gauche = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_gauche; + position_arriere_droite = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_droite; + } + + + if(orientationArrondie == 90 || orientationArrondie == 270) { // Si il est en axe Y + if(position_arriere_droite >= y_robot-instruction.arg1 && position_arriere_droite <= y_robot+instruction.arg1) { // Et que les mesures sont cohérentes + if(position_arriere_gauche >= y_robot-instruction.arg1 && position_arriere_gauche <= y_robot+instruction.arg1) { + if(telemetreDistance_arriere_droite > telemetreDistance_arriere_gauche) + angleArriere =900+(1800 * atan2((double)(telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche), (double)ESPACE_INTER_TELEMETRE ))/M_PI; + else + angleArriere =(1800 * atan2( (double) ESPACE_INTER_TELEMETRE,(double) (telemetreDistance_arriere_gauche-telemetreDistance_arriere_droite) ))/M_PI; + + nombresDeMesuresAuxTelemetresQuiSontCoherentes++; + moyennageTelemetre += angleArriere; + } + } + } else if(orientationArrondie == 0 || orientationArrondie == 180) { // Si il est en axe X + if(position_arriere_droite >= x_robot-instruction.arg1 && position_arriere_droite <= x_robot+instruction.arg1) { // Et que les mesures sont cohérentes + if(position_arriere_gauche >= x_robot-instruction.arg1 && position_arriere_gauche <= x_robot+instruction.arg1) { + if(telemetreDistance_arriere_droite > telemetreDistance_arriere_gauche) + angleArriere =900+(1800 * atan2( (double) (telemetreDistance_arriere_droite-telemetreDistance_arriere_gauche), (double) ESPACE_INTER_TELEMETRE ))/M_PI; + else + angleArriere =(1800 * atan2( (double) ESPACE_INTER_TELEMETRE,(double) (telemetreDistance_arriere_gauche-telemetreDistance_arriere_droite) ))/M_PI; + + nombresDeMesuresAuxTelemetresQuiSontCoherentes++; + moyennageTelemetre += angleArriere; + } + } + } + + if(orientationArrondie == 90 || orientationArrondie == 270) { // Si il est en axe Y + if(position_avant_droite >= y_robot-instruction.arg1 && position_avant_droite <= y_robot+instruction.arg1) { // Et que les mesures sont cohérentes + if(position_avant_gauche >= y_robot-instruction.arg1 && position_avant_gauche <= y_robot+instruction.arg1) { + if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche) + angleAvant = (1800 * atan2( (double) ESPACE_INTER_TELEMETRE,(double) (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) ))/M_PI; + else + angleAvant = 900 + (1800 * atan2( (double)( telemetreDistance_avant_gauche-telemetreDistance_avant_droite),(double) ESPACE_INTER_TELEMETRE ))/M_PI; + + nombresDeMesuresAuxTelemetresQuiSontCoherentes++; + moyennageTelemetre += angleAvant; + } + } + } else if(orientationArrondie == 0 || orientationArrondie == 180) { // Si il est en axe X + if(position_avant_droite >= x_robot-instruction.arg1 && position_avant_droite <= x_robot+instruction.arg1) { // Et que les mesures sont cohérentes + if(position_avant_gauche >= x_robot-instruction.arg1 && position_avant_gauche <= x_robot+instruction.arg1) { + if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche) + angleAvant = (1800 * atan2((double) ESPACE_INTER_TELEMETRE, (double) (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) ))/M_PI; + else + angleAvant = 900 + (1800 * atan2( (double) (telemetreDistance_avant_gauche-telemetreDistance_avant_droite),(double) ESPACE_INTER_TELEMETRE ))/M_PI; + + nombresDeMesuresAuxTelemetresQuiSontCoherentes++; + moyennageTelemetre += angleAvant; + } + } + } + + angleRecalage = moyennageTelemetre/nombresDeMesuresAuxTelemetresQuiSontCoherentes; + + if(nombresDeMesuresAuxTelemetresQuiSontCoherentes) { + if(orientationArrondie == 0) { + angleRecalage -= 900; + + /*if(telemetreDistance_avant_droite > telemetreDistance_avant_gauche) + distanceRecalage = *); + else + distanceRecalage = 900 + (1800 * atan( (double)( (telemetreDistance_avant_droite-telemetreDistance_avant_gauche) / ESPACE_INTER_TELEMETRE )))/M_PI;*/ + } else if(orientationArrondie == 90) { + angleRecalage += 0; + } else if(orientationArrondie == 180) { + angleRecalage += 900; + } else if(orientationArrondie == 270) { + angleRecalage += 1800; + } + } + + return (nombresDeMesuresAuxTelemetresQuiSontCoherentes && (angleAvant-angleArriere<80 && angleAvant-angleArriere>-80)) ? angleRecalage : theta_robot; +} + +short recalageDistanceX(void) +{ + unsigned char nombresDeMesuresAuxTelemetresQuiSontCoherentes = 0; + unsigned int moyennageTelemetre = 0; + + unsigned short tempo= telemetreDistance_arriere_gauche; + telemetreDistance_arriere_gauche=telemetreDistance_arriere_droite; + telemetreDistance_arriere_droite=tempo; + + telemetreDistance_avant_gauche = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_gauche; + telemetreDistance_avant_droite = ((theta_robot < 900 && theta_robot > -900)?2000:0) + ((theta_robot < 900 && theta_robot > -900)?-1:1)*telemetreDistance_avant_droite; + telemetreDistance_arriere_gauche = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_gauche; + telemetreDistance_arriere_droite = ((theta_robot < 900 && theta_robot > -900)?0:2000) + ((theta_robot < 900 && theta_robot > -900)?1:-1)*telemetreDistance_arriere_droite; + + if(telemetreDistance_avant_gauche >= x_robot-instruction.arg1 && telemetreDistance_avant_gauche <= x_robot+instruction.arg1) { + nombresDeMesuresAuxTelemetresQuiSontCoherentes++; + moyennageTelemetre += telemetreDistance_avant_gauche; + } + if(telemetreDistance_avant_droite >= x_robot-instruction.arg1 && telemetreDistance_avant_droite <= x_robot+instruction.arg1) { + nombresDeMesuresAuxTelemetresQuiSontCoherentes++; + moyennageTelemetre += telemetreDistance_avant_droite; + } + if(telemetreDistance_arriere_gauche >= x_robot-instruction.arg1 && telemetreDistance_arriere_gauche <= x_robot+instruction.arg1) { + nombresDeMesuresAuxTelemetresQuiSontCoherentes++; + moyennageTelemetre += telemetreDistance_arriere_gauche; + } + if(telemetreDistance_arriere_droite >= x_robot-instruction.arg1 && telemetreDistance_arriere_droite <= x_robot+instruction.arg1) { + nombresDeMesuresAuxTelemetresQuiSontCoherentes++; + moyennageTelemetre += telemetreDistance_arriere_droite; + } + + moyennageTelemetre /= nombresDeMesuresAuxTelemetresQuiSontCoherentes; + + return (nombresDeMesuresAuxTelemetresQuiSontCoherentes)? moyennageTelemetre : x_robot; //SetOdometrie(ODOMETRIE_SMALL_POSITION, moyennageTelemetre, y_robot, theta_robot); +} + +short recalageDistanceY(void) +{ + unsigned char nombresDeMesuresAuxTelemetresQuiSontCoherentes = 0; + unsigned int moyennageTelemetre = 0; + + unsigned short tempo= telemetreDistance_arriere_gauche; + telemetreDistance_arriere_gauche=telemetreDistance_arriere_droite; + telemetreDistance_arriere_droite=tempo; + + 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; + 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; + 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; + 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; + + if(telemetreDistance_avant_gauche >= y_robot-instruction.arg1 && telemetreDistance_avant_gauche <= y_robot+instruction.arg1) { + nombresDeMesuresAuxTelemetresQuiSontCoherentes++; + moyennageTelemetre += telemetreDistance_avant_gauche; + } + if(telemetreDistance_avant_droite >= y_robot-instruction.arg1 && telemetreDistance_avant_droite <= y_robot+instruction.arg1) { + nombresDeMesuresAuxTelemetresQuiSontCoherentes++; + moyennageTelemetre += telemetreDistance_avant_droite; + } + if(telemetreDistance_arriere_gauche >= y_robot-instruction.arg1 && telemetreDistance_arriere_gauche <= y_robot+instruction.arg1) { + nombresDeMesuresAuxTelemetresQuiSontCoherentes++; + moyennageTelemetre += telemetreDistance_arriere_gauche; + } + if(telemetreDistance_arriere_droite >= y_robot-instruction.arg1 && telemetreDistance_arriere_droite <= y_robot+instruction.arg1) { + nombresDeMesuresAuxTelemetresQuiSontCoherentes++; + moyennageTelemetre += telemetreDistance_arriere_droite; + } + + moyennageTelemetre /= nombresDeMesuresAuxTelemetresQuiSontCoherentes; + + return (nombresDeMesuresAuxTelemetresQuiSontCoherentes)? moyennageTelemetre : y_robot ; // SetOdometrie(ODOMETRIE_SMALL_POSITION, x_robot, moyennageTelemetre, theta_robot); +} +