![](/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
Strategie/Strategie.cpp
- Committer:
- maximilienlv
- Date:
- 2020-06-23
- Revision:
- 20:2f0b1c48a35f
- Parent:
- 19:e70b9d4a319c
- Child:
- 21:d137ec53c3a9
File content as of revision 20:2f0b1c48a35f:
#include "global.h" #include <string.h> #include <sstream> #include <math.h> #include <vector> #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; unsigned short distance_recalage; unsigned short distance_revenir; unsigned short x; unsigned short y; unsigned char isStopEnable = 1;//Permet de savoir si il faut autoriser le stop via les balises 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; //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 nbStrat = 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_MOTEUR, TEST_COULEUR, TEST_SERVO_BRAS, TEST_VENTOUSE, TEST_ELECTROV,TEST_AUD, 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_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"); //menu demo Button TEST_VENT(0, 25, 400, 100, "Test ventouses"); Button TEST_MOT(0, 135, 400, 100, "Test moteur"); Button TEST_COUL(0,245,400,100,"Test capteurs couleur"); Button TEST_BRAS(0,355,400,100,"Test Bras"); Button TEST_AUDIO(0,465,400,100,"Test Audio"); Button TEST_ELECTRV(0,575,400,100,"Test Divers"); //menu demo/demo/ Button TEST_A(0,25,195,100,"A"); Button TEST_B(205,25,195,100,"B"); Button TEST_C(0,135,195,100,"C"); Button TEST_D(205,135,195,100,"D"); Button TEST_NUM(0,245,400,100,"Bras num"); Button TEST_PLUS(205,355,195,100,"+"); Button TEST_MOINS(0,355,195,100,"-"); Button FORCE_LAUNCH(0, 50, 400, 320, "Force Launch"); Button SUIVANT(0,380,200,100,"Suivant"); //////////////////////////////////////////////////////////// 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 char doAction(unsigned char id, unsigned short arg1, short arg2); 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; 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 /****************************************************************************************/ /* 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;//gab le zgeg } //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: automate_etat_ihm */ /* DESCRIPTION : Automate de gestion de l'affichage */ /****************************************************************************************/ void automate_etat_ihm(void) { char toto[2]; int j; unsigned char maximilien=1; 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.SetBackColor(LCD_COLOR_WHITE); lcd.SetTextColor(LCD_COLOR_BLACK); lcd.Clear (LCD_COLOR_WHITE); lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"DEMO", LEFT_MODE); RETOUR.Draw(0xFFFF0000, 0); TEST_VENT.Draw(VERT, 0); TEST_MOT.Draw(VERT, 0); TEST_COUL.Draw(VERT, 0); TEST_BRAS.Draw(VERT, 0); TEST_AUDIO.Draw(VERT, 0); TEST_ELECTRV.Draw(VERT, 0); maximilien=0; while (etat == DEMO) { canProcessRx(); if(TEST_VENT.Touched()) { while(TEST_VENT.Touched()); etat = TEST_VENTOUSE; } else if(TEST_MOT.Touched()) { while(TEST_MOT.Touched()); etat = TEST_MOTEUR; } else if(TEST_ELECTRV.Touched()) { while(TEST_ELECTRV.Touched()); etat = TEST_ELECTROV; } else if (TEST_COUL.Touched()) { while(TEST_COUL.Touched()); etat =TEST_COULEUR ; } else if (TEST_BRAS.Touched()) { while(TEST_BRAS.Touched()); etat =TEST_SERVO_BRAS ; } else if(TEST_AUDIO.Touched()) { while(TEST_AUDIO.Touched()); etat=TEST_AUD; } else if(RETOUR.Touched()) { while(RETOUR.Touched()); etat = CHOIX; } } break; case TEST_VENTOUSE: lcd.SetBackColor(LCD_COLOR_WHITE); lcd.SetTextColor(LCD_COLOR_BLACK); lcd.Clear (LCD_COLOR_WHITE); lcd.Clear (LCD_COLOR_WHITE); lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Test Ventouse", LEFT_MODE); sprintf(toto,"%hd",maximilien); lcd.DisplayStringAt(50, LINE(20), (unsigned char *)toto, LEFT_MODE); RETOUR.Draw(0xFFFF0000,0); TEST_A.Draw(BLEU, BLANC); TEST_NUM.Draw(BLEU, BLANC); TEST_PLUS.Draw(BLEU, BLANC); TEST_MOINS.Draw(BLEU, BLANC); while(etat==TEST_VENTOUSE) { if(RETOUR.Touched()) { while (RETOUR.Touched()); etat=DEMO; } else if(TEST_A.Touched()) { while (TEST_A.Touched()); //SendRawId(TEST_VENT_A); TEST_A.Draw(BLEU, BLANC); } else if(TEST_NUM.Touched()) { while (TEST_NUM.Touched()); TEST_NUM.Draw(BLEU, BLANC); switch(maximilien){ case 1: SendRawId(TEST_VENT_1_ON); break; case 2: SendRawId(TEST_VENT_1_OFF); break; case 3: //SendRawId(TEST_VENT_3); break; case 4: //SendRawId(TEST_VENT_4); break; case 5: //SendRawId(TEST_VENT_5); break; case 6: //SendRawId(TEST_VENT_6); break; } } else if(TEST_PLUS.Touched()) { while (TEST_PLUS.Touched()); TEST_PLUS.Draw(BLEU, BLANC); if(maximilien!=6)maximilien++; sprintf(toto,"%hd",maximilien); lcd.SetBackColor(LCD_COLOR_WHITE); lcd.SetTextColor(LCD_COLOR_BLACK); lcd.DisplayStringAt(50, LINE(20), (unsigned char *)toto, LEFT_MODE); } else if(TEST_MOINS.Touched()) { while (TEST_MOINS.Touched()); TEST_MOINS.Draw(BLEU, BLANC); if(maximilien!=1)maximilien--; sprintf(toto,"%hd",maximilien); lcd.SetBackColor(LCD_COLOR_WHITE); lcd.SetTextColor(LCD_COLOR_BLACK); lcd.DisplayStringAt(50, LINE(20), (unsigned char *)toto, LEFT_MODE); } } break; case TEST_ELECTROV: lcd.SetBackColor(LCD_COLOR_WHITE); lcd.SetTextColor(LCD_COLOR_BLACK); lcd.Clear (LCD_COLOR_WHITE); lcd.Clear (LCD_COLOR_WHITE); lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"A : Manches B : Pavillon", LEFT_MODE); RETOUR.Draw(0xFFFF0000,0); TEST_A.Draw(BLEU, BLANC); TEST_B.Draw(BLEU, BLANC); while(etat==TEST_ELECTROV) { if(RETOUR.Touched()) { while (RETOUR.Touched()); etat=DEMO; } else if(TEST_A.Touched()) { while (TEST_A.Touched()); //SendRawId(TEST_ELECTROV_A); TEST_A.Draw(BLEU, BLANC); } else if(TEST_B.Touched()) { while (TEST_B.Touched()); //SendRawId(TEST_ELECTROV_A); TEST_B.Draw(BLEU, BLANC); } } break; case TEST_AUD: lcd.SetBackColor(LCD_COLOR_WHITE); lcd.SetTextColor(LCD_COLOR_BLACK); lcd.Clear (LCD_COLOR_WHITE); lcd.Clear (LCD_COLOR_WHITE); lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Test Audio", LEFT_MODE); sprintf(toto,"%hd",maximilien); lcd.DisplayStringAt(50, LINE(20), (unsigned char *)toto, LEFT_MODE); RETOUR.Draw(0xFFFF0000,0); TEST_NUM.Draw(BLEU, BLANC); TEST_PLUS.Draw(BLEU, BLANC); TEST_MOINS.Draw(BLEU, BLANC); while(etat==TEST_AUD) { if(RETOUR.Touched()) { while (RETOUR.Touched()); etat=DEMO; } else if(TEST_NUM.Touched()) { while (TEST_NUM.Touched()); TEST_NUM.Draw(BLEU, BLANC); Debug_Audio(3,maximilien); } else if(TEST_PLUS.Touched()) { while (TEST_PLUS.Touched()); TEST_PLUS.Draw(BLEU, BLANC); if(maximilien!=20)maximilien++; sprintf(toto,"%hd",maximilien); lcd.SetBackColor(LCD_COLOR_WHITE); lcd.SetTextColor(LCD_COLOR_BLACK); lcd.DisplayStringAt(50, LINE(20), (unsigned char *)toto, LEFT_MODE); } else if(TEST_MOINS.Touched()) { while (TEST_MOINS.Touched()); TEST_MOINS.Draw(BLEU, BLANC); if(maximilien!=1)maximilien--; sprintf(toto,"%hd",maximilien); lcd.SetBackColor(LCD_COLOR_WHITE); lcd.SetTextColor(LCD_COLOR_BLACK); lcd.DisplayStringAt(50, LINE(20), (unsigned char *)toto, LEFT_MODE); } } break; case TEST_SERVO_BRAS: lcd.SetBackColor(LCD_COLOR_WHITE); lcd.SetTextColor(LCD_COLOR_BLACK); lcd.Clear (LCD_COLOR_WHITE); lcd.Clear (LCD_COLOR_WHITE); lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Test Bras", LEFT_MODE); sprintf(toto,"%hd",maximilien); lcd.DisplayStringAt(50, LINE(20), (unsigned char *)toto, LEFT_MODE); RETOUR.Draw(0xFFFF0000,0); TEST_A.Draw(BLEU, BLANC); TEST_B.Draw(BLEU, BLANC); TEST_C.Draw(BLEU, BLANC); TEST_D.Draw(BLEU, BLANC); TEST_NUM.Draw(BLEU, BLANC); TEST_PLUS.Draw(BLEU, BLANC); TEST_MOINS.Draw(BLEU, BLANC); while(etat==TEST_SERVO_BRAS) { if(RETOUR.Touched()) { while (RETOUR.Touched()); etat=DEMO; } else if(TEST_A.Touched()) { while (TEST_A.Touched()); SendRawId(TEST_BRAS_A); TEST_A.Draw(BLEU, BLANC); } else if(TEST_B.Touched()) { while (TEST_B.Touched()); SendRawId(TEST_BRAS_B); TEST_B.Draw(BLEU, BLANC); } else if(TEST_C.Touched()) { while (TEST_C.Touched()); SendRawId(TEST_BRAS_C); TEST_C.Draw(BLEU, BLANC); } else if(TEST_D.Touched()) { while (TEST_D.Touched()); SendRawId(TEST_BRAS_D); TEST_D.Draw(BLEU, BLANC); } else if(TEST_NUM.Touched()) { while (TEST_NUM.Touched()); TEST_NUM.Draw(BLEU, BLANC); switch(maximilien){ case 1: SendRawId(TEST_BRAS_1); break; case 2: SendRawId(TEST_BRAS_2); break; case 3: SendRawId(TEST_BRAS_3); break; case 4: SendRawId(TEST_BRAS_4); break; case 5: SendRawId(TEST_BRAS_5); break; case 6: SendRawId(TEST_BRAS_6); break; } } else if(TEST_PLUS.Touched()) { while (TEST_PLUS.Touched()); TEST_PLUS.Draw(BLEU, BLANC); if(maximilien!=6)maximilien++; sprintf(toto,"%hd",maximilien); lcd.SetBackColor(LCD_COLOR_WHITE); lcd.SetTextColor(LCD_COLOR_BLACK); lcd.DisplayStringAt(50, LINE(20), (unsigned char *)toto, LEFT_MODE); } else if(TEST_MOINS.Touched()) { while (TEST_MOINS.Touched()); TEST_MOINS.Draw(BLEU, BLANC); if(maximilien!=1)maximilien--; sprintf(toto,"%hd",maximilien); lcd.SetBackColor(LCD_COLOR_WHITE); lcd.SetTextColor(LCD_COLOR_BLACK); lcd.DisplayStringAt(50, LINE(20), (unsigned char *)toto, LEFT_MODE); } } break; case TEST_COULEUR: lcd.Clear (LCD_COLOR_WHITE); lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Test Couleur", LEFT_MODE); RETOUR.Draw(0xFFFF0000,0); while(etat==TEST_COULEUR) { if(RETOUR.Touched()) { while (RETOUR.Touched()); etat=DEMO; lcd.Clear (LCD_COLOR_WHITE); } } break; case TEST_MOTEUR: lcd.SetBackColor(LCD_COLOR_WHITE); lcd.SetTextColor(LCD_COLOR_BLACK); lcd.Clear (LCD_COLOR_WHITE); lcd.Clear (LCD_COLOR_WHITE); lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Test Moteur", LEFT_MODE); RETOUR.Draw(0xFFFF0000,0); TEST_A.Draw(BLEU, BLANC); TEST_B.Draw(BLEU, BLANC); while(etat==TEST_MOTEUR) { if(RETOUR.Touched()) { while (RETOUR.Touched()); etat=DEMO; } else if(TEST_A.Touched()) { while (TEST_A.Touched()); BendRadius(150,1800,1,0); TEST_A.Draw(BLEU, BLANC); } else if(TEST_B.Touched()) { while (TEST_B.Touched()); BendRadius(150,1800,0,0); TEST_B.Draw(BLEU, BLANC); } } 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); nbStrat = Bouton_Strat(); // retourne valeur de Strategie si bouton strat renvoi -1 on reviens en arriere if (nbStrat == -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(nbStrat); //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); affichage_var(SCORE_PR); } cpt1=cpt; flag_timer=0; 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); //affichage_compteur(SCORE_PR); affichage_var(SCORE_PR); while(1); // force le redemarage du robot //break; } } /****************************************************************************************/ /* FUNCTION NAME: Strategie */ /* DESCRIPTION : Automate de gestion de la stratégie du robot */ /****************************************************************************************/ void Strategie(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(nbStrat);//Mise en cache de toute les instructions led3=1; SendRawId(GLOBAL_START); gameEtat = ETAT_GAME_WAIT_FOR_JACK; Debug_Audio(3,7); if (etat == TEST_MOTEUR|| etat ==TEST_VENTOUSE || etat == TEST_COULEUR || etat ==TEST_SERVO_BRAS) { 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; 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) 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; GoStraight(-100, 0, 0, 0);//-450 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; } GoStraight(3000, 2,localData3, 0); //on se recale contre le mur donc il faut donner la valeur du centre du robot 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; GoStraight(-100, 0, 0, 0); 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: Debug_Audio(3,3); 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); 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 Debug_Audio(3,6); 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); if(localData2>0) { direction=1; } else { direction=-1; } 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 Debug_Audio(3,8); 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 Debug_Audio(3,9); 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: Debug_Audio(3,10); 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; } return; } 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; } 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: Debug_Audio(3,4); 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; 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; default: 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); } /*************************************************************************************************/ /* FUNCTION NAME: doAction */ /* DESCRIPTION : Effectuer une action specifique correspondant au numéro dans le fichier strat */ /*************************************************************************************************/ unsigned char doAction(unsigned char id, unsigned short arg1, short arg2) { int retour = 1; CANMessage msgTx=CANMessage(); msgTx.format=CANStandard; msgTx.type=CANData; switch(id) { case 120: //SendRawId(ACCELERATEUR_INSERTION_AVANT_GAUCHE); break; case 121: //SendRawId(ACCELERATEUR_INSERTION_ARRIERE_GAUCHE); break; case 150: SCORE_PR+=arg1; waitingAckFrom = 0; waitingAckID = 0; break; case 200 : SendRawId(DATA_TELEMETRE); /*telemetreDistance = dataTelemetre(); wait_ms(1); telemetreDistance = dataTelemetre(); telemetreDistance = telemetreDistance - 170;*/ break; case 201 : SendRawId(0x99);// retour = 2; break; case 11://0 Désactiver le stop,1 Activer le stop saut de strat,2 Activer le stop avec evitement isStopEnable =(unsigned char) arg1; // SendMsgCan(0x5BC, &isStopEnable,1); waitingAckFrom = 0; waitingAckID =0; break; case 19: // CHANGER LA VITESSE + DECELERATION //SendSpeedDecel(arg1,(unsigned short) arg2); wait_us(200); waitingAckFrom = 0; waitingAckID =0; break; case 20://Désactiver l'asservissement setAsservissementEtat(0); break; case 21://Activer l'asservissement setAsservissementEtat(1); break; case 22://Changer la vitesse du robot SendSpeed(arg1);//,(unsigned short)arg2, (unsigned short)arg2); wait_us(200); waitingAckFrom = 0; waitingAckID = 0; break; case 23: SendAccel(arg1,(unsigned short)arg2);//,(unsigned short)arg2, (unsigned short)arg2); wait_us(200); waitingAckFrom = 0; waitingAckID = 0; break; case 151: unsigned char argu_at_1_bras = arg1; SendMsgCan(BRAS_AT_1, &argu_at_1_bras,sizeof(arg1)); waitingAckFrom = 0; waitingAckID =0; break; case 152: unsigned char argu_re_1_bras = arg1; SendMsgCan(BRAS_RE_1, &argu_re_1_bras,sizeof(arg1)); waitingAckFrom = 0; waitingAckID =0; break; case 153: unsigned char argu_at_2_bras = arg1; SendMsgCan(BRAS_AT_2, &argu_at_2_bras,sizeof(arg1)); waitingAckFrom = 0; waitingAckID =0; break; case 154: unsigned char argu_re_2_bras = arg1; SendMsgCan(BRAS_RE_2, &argu_re_2_bras,sizeof(arg1)); waitingAckFrom = 0; waitingAckID =0; break; case 155: unsigned char argu_at_3_bras = arg1; SendMsgCan(BRAS_AT_3, &argu_at_3_bras,sizeof(arg1)); waitingAckFrom = 0; waitingAckID =0; break; case 156: unsigned char argu_re_3_bras = arg1; SendMsgCan(BRAS_RE_3, &argu_re_3_bras,sizeof(arg1)); waitingAckFrom = 0; waitingAckID =0; break; case 157: unsigned char argu_at_vent = arg1; SendMsgCan(VENT_AT, &argu_at_vent,sizeof(arg1)); waitingAckFrom = 0; waitingAckID =0; break; case 158: unsigned char argu_re_vent = arg1; SendMsgCan(VENT_RE, &argu_re_vent,sizeof(arg1)); waitingAckFrom = 0; waitingAckID =0; break; default: retour = 0;//L'action n'existe pas, il faut utiliser le CAN break; } return retour;//L'action est spécifique. }