![](/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:
- 14:6aa8aa1699ad
- Parent:
- 11:848ea39838bf
- Child:
- 15:5ece9c3ba389
--- a/Strategie/Strategie.cpp Wed Mar 04 12:38:22 2020 +0000 +++ b/Strategie/Strategie.cpp Wed Mar 11 14:21:02 2020 +0000 @@ -3,8 +3,6 @@ #include <sstream> #include <math.h> #include <vector> -//#include "StrategieManager.h" - #define M_PI 3.14159265358979323846 @@ -72,7 +70,12 @@ 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; @@ -118,7 +121,7 @@ float cptf; int cpt,cpt1; -typedef enum {INIT, ATT, CHOIX, DEMO, TEST_TELEMETRE, TEST_COULEUR, TEST_SERVO_BRAS, TEST_VENTOUSE, TEST_ELECTROV, SELECT_SIDE, TACTIQUE, DETAILS,LECTURE, LAUNCH, AFF_WAIT_JACK, WAIT_JACK, COMPTEUR, FIN} T_etat; +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; @@ -135,11 +138,11 @@ Button DEMONSTRATION (0, 400, 400, 320, "Demo"); //menu demo Button TEST_VENT(0, 25, 400, 100, "Test ventouses"); -Button TEST_TELE(0, 135, 400, 100, "Test telemetre"); +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 Electro Vannes"); +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"); @@ -163,7 +166,7 @@ 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; @@ -210,7 +213,7 @@ { if(gameEtat == ETAT_GAME_WAIT_FOR_JACK) { gameEtat = ETAT_GAME_START;//On débute le match - //etat=COMPTEUR; + etat=COMPTEUR; blocage_balise=1; } } @@ -404,19 +407,20 @@ lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"DEMO", LEFT_MODE); RETOUR.Draw(0xFFFF0000, 0); TEST_VENT.Draw(VERT, 0); - TEST_TELE.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_TELE.Touched()) { - while(TEST_TELE.Touched()); - etat = TEST_TELEMETRE; + } else if(TEST_MOT.Touched()) { + while(TEST_MOT.Touched()); + etat = TEST_MOTEUR; } else if(TEST_ELECTRV.Touched()) { while(TEST_ELECTRV.Touched()); etat = TEST_ELECTROV; @@ -428,8 +432,7 @@ etat =TEST_SERVO_BRAS ; } else if(TEST_AUDIO.Touched()) { while(TEST_AUDIO.Touched()); - TEST_AUDIO.Draw(VERT, 0); - Debug_Audio(3,2); + etat=TEST_AUD; } else if(RETOUR.Touched()) { while(RETOUR.Touched()); etat = CHOIX; @@ -457,29 +460,29 @@ etat=DEMO; } else if(TEST_A.Touched()) { while (TEST_A.Touched()); - SendRawId(TEST_VENT_A); + //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); + //SendRawId(TEST_VENT_1); break; case 2: - SendRawId(TEST_VENT_2); + //SendRawId(TEST_VENT_2); break; case 3: - SendRawId(TEST_VENT_3); + //SendRawId(TEST_VENT_3); break; case 4: - SendRawId(TEST_VENT_4); + //SendRawId(TEST_VENT_4); break; case 5: - SendRawId(TEST_VENT_5); + //SendRawId(TEST_VENT_5); break; case 6: - SendRawId(TEST_VENT_6); + //SendRawId(TEST_VENT_6); break; } } else if(TEST_PLUS.Touched()) { @@ -507,49 +510,51 @@ lcd.SetTextColor(LCD_COLOR_BLACK); lcd.Clear (LCD_COLOR_WHITE); lcd.Clear (LCD_COLOR_WHITE); - lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Test Electro Vannes", LEFT_MODE); - sprintf(toto,"%hd",maximilien); - lcd.DisplayStringAt(50, LINE(20), (unsigned char *)toto, LEFT_MODE); + lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"A : Manches B : Pavillon", 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); + 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); + //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); - switch(maximilien){ - case 1: - SendRawId(TEST_ELECTROV_1); - break; - case 2: - SendRawId(TEST_ELECTROV_2); - break; - case 3: - SendRawId(TEST_ELECTROV_3); - break; - case 4: - SendRawId(TEST_ELECTROV_4); - break; - case 5: - SendRawId(TEST_ELECTROV_5); - break; - case 6: - SendRawId(TEST_ELECTROV_6); - break; - } + Debug_Audio(3,maximilien); } else if(TEST_PLUS.Touched()) { while (TEST_PLUS.Touched()); TEST_PLUS.Draw(BLEU, BLANC); - if(maximilien!=6)maximilien++; + if(maximilien!=10)maximilien++; sprintf(toto,"%hd",maximilien); lcd.SetBackColor(LCD_COLOR_WHITE); lcd.SetTextColor(LCD_COLOR_BLACK); @@ -565,7 +570,7 @@ } } break; - + case TEST_SERVO_BRAS: lcd.SetBackColor(LCD_COLOR_WHITE); @@ -659,21 +664,28 @@ } break; - case TEST_TELEMETRE: //AFFICHAGE DE LA VALEUR LUE PAR LES 4 TELEMETRES - ModeDemo=1; + 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 Telemetre", LEFT_MODE); - RETOUR.Draw(0xFFFF0000, 0); - while(etat==TEST_TELEMETRE) { - SendRawId(DATA_RECALAGE); - SendRawId(DATA_TELEMETRE_LOGIQUE); - wait_ms(100); - canProcessRx(); + 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()); + while (RETOUR.Touched()); etat=DEMO; - ModeDemo=0; - } + } 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///////////////////////////////////////////////// @@ -964,7 +976,7 @@ gameEtat = ETAT_GAME_WAIT_FOR_JACK; Debug_Audio(3,7); - if (etat == TEST_TELEMETRE|| etat ==TEST_VENTOUSE || etat == TEST_COULEUR || etat ==TEST_SERVO_BRAS) { + if (etat == TEST_MOTEUR|| etat ==TEST_VENTOUSE || etat == TEST_COULEUR || etat ==TEST_SERVO_BRAS) { SendRawId(DEBUG_FAKE_JAKE); } else { etat = AFF_WAIT_JACK; @@ -1485,7 +1497,7 @@ 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); + tempo = doAction(instruction.arg1,instruction.arg2,instruction.arg3); // unsigned char test=(unsigned char) tempo; // SendMsgCan(0x5BD, &test,1); if(tempo == 1) { @@ -1948,8 +1960,6 @@ } break; - case SOMO: - break; default: break; /* @@ -2435,3 +2445,339 @@ 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 101: //bras avant droit + SendRawId(TEST_BRAS_1_AT); + break; + case 102: //bras avant millieu + SendRawId(TEST_BRAS_2_AT); + break; + case 103: //attraper presentoir arriere + //SendRawId(PRESENTOIR_ARRIERE); + break; + case 104: //balance avant + //SendRawId(BALANCE_AVANT); + break; + case 105: //balance arriere + //SendRawId(BALANCE_ARRIERE); + break; + case 106: //accelerateur avant + //SendRawId(ACCELERATEUR_AVANT); + //distance_recalage=arg1; + //distance_revenir=arg2; + //Send2Short(ACCELERATEUR_AVANT, distance_recalage,distance_revenir); + break; + case 107: //accelerateur arriere + //distance_recalage=arg1; + //distance_revenir=arg2; + //Send2Short(ACCELERATEUR_ARRIERE, distance_recalage,distance_revenir); + break; + case 108: //goldenium avant + //SendRawId(GOLDENIUM_AVANT); + x = arg1; + if(InversStrat == 1) { + y = 3000 - arg2; + } else { + y = arg2; + } + //Send2Short(GOLDENIUM_AVANT, x, y); + break; + case 109: //goldenium arriere + x=arg1; + y=arg2; + //Send2Short(GOLDENIUM_ARRIERE, x,y); + break; + case 110: //sol avant + //SendRawId(SOL_AVANT); + case 130: //sol avant + SendRawId(TEST_BRAS_1_RE); + break; + case 131: //sol avant + SendRawId(TEST_BRAS_2_RE); + break; + case 132: //sol avant + SendRawId(TEST_BRAS_3_RE); + break; + case 133: //sol avant + SendRawId(TEST_BRAS_4_RE); + break; + case 134: //sol avant + SendRawId(TEST_BRAS_5_RE); + break; + case 135: //sol avant + SendRawId(TEST_BRAS_6_RE); + break; + case 111: //sol arriere + // SendRawId(SOL_ARRIERE); + break; + case 112: //sol avant relache + //SendRawId(SOL_AVANT_RELACHE); + break; + case 113: //sol arriere relache + //SendRawId(SOL_ARRIERE_RELACHE); + break; + case 114://relache les 3 ventouses avants + // SendRawId(AVANT_RELACHE); + break; + + case 115://bras avant gauche attraper + SendRawId(TEST_BRAS_3_AT); + break; + + case 116://bras arreiere droit + SendRawId(TEST_BRAS_6_AT); + break; + /* case 116://relache une pompe + unsigned char arg_tempo; + if(InversStrat == 1) { + switch(arg1) { + case AV_DROIT: + arg_tempo = AV_GAUCHE; + break; + case AV_GAUCHE: + arg_tempo = AV_DROIT; + break; + case AR_DROIT: + arg_tempo = AR_GAUCHE; + break; + case AR_GAUCHE: + arg_tempo = AR_DROIT; + break; + default : + arg_tempo =(unsigned char)arg1; + break; + } + + } else arg_tempo =(unsigned char)arg1; + SendMsgCan(HACHEUR_RELEASE_ATOM, &arg_tempo,1); + waitingAckFrom = 0; + waitingAckID =0; + break;*/ + case 117://bras arriere millieu + SendRawId(TEST_BRAS_5_AT); + break; + case 118://bras arriere gauche + SendRawId(TEST_BRAS_4_AT); + break; + case 119: + //SendRawId(VENTOUSE_AR_CENTRE_BALANCE); + break; + case 120: + //SendRawId(ACCELERATEUR_INSERTION_AVANT_GAUCHE); + break; + + case 121: + //SendRawId(ACCELERATEUR_INSERTION_ARRIERE_GAUCHE); + break; + + case 150: + SCORE_PR+=arg1; + liaison_Tx.envoyer_short(0x30,SCORE_PR); + 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 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 19: // CHANGER LA VITESSE + DECELERATION + //SendSpeedDecel(arg1,(unsigned short) arg2); + wait_us(200); + waitingAckFrom = 0; + waitingAckID =0; + break; + + case 30://Action tempo + wait_ms(arg1); + waitingAckFrom = 0; + waitingAckID = 0; + break; + //pompe 1 à 6 ON + case 202 : + //SendRawId(TEST_VENT_1_ON); + waitingAckFrom = 0; + waitingAckID =0; + break; + case 203 : + //SendRawId(TEST_VENT_2_ON); + waitingAckFrom = 0; + waitingAckID =0; + break; + case 204 : + //SendRawId(TEST_VENT_3_ON); + waitingAckFrom = 0; + waitingAckID =0; + break; + case 205 : + // SendRawId(TEST_VENT_4_ON); + waitingAckFrom = 0; + waitingAckID =0; + break; + case 206 : + // SendRawId(TEST_VENT_5_ON); + waitingAckFrom = 0; + waitingAckID =0; + break; + case 207 : + // SendRawId(TEST_VENT_6_ON); + waitingAckFrom = 0; + waitingAckID =0; + break; + case 208 : + // SendRawId(TEST_VENT_1_OFF); + waitingAckFrom = 0; + waitingAckID =0; + break; + case 209 : + //SendRawId(TEST_VENT_2_OFF); + waitingAckFrom = 0; + waitingAckID =0; + break; + case 210 : + // SendRawId(TEST_VENT_3_OFF); + waitingAckFrom = 0; + waitingAckID =0; + break; + case 211 : + //SendRawId(TEST_VENT_4_OFF); + waitingAckFrom = 0; + waitingAckID =0; + break; + case 212 : + // SendRawId(TEST_VENT_5_OFF); + waitingAckFrom = 0; + waitingAckID =0; + break; + case 213 : + //SendRawId(TEST_VENT_6_OFF); + waitingAckFrom = 0; + waitingAckID =0; + break; + case 214 : + SendRawId(BRAS_AT); + break; + + case 215://test bras competition ( à tester) + unsigned char arg_tempo; + switch(arg1) + { + case AV_DROIT: + arg_tempo = 1; + break; + case AV_CENTRE: + arg_tempo = 2; + break; + case AV_GAUCHE: + arg_tempo = 3; + break; + case AR_DROIT: + arg_tempo = 4; + break; + case AR_CENTRE: + arg_tempo = 5; + break; + case AR_GAUCHE: + arg_tempo = 6; + break; + default : + arg_tempo =(unsigned char)arg1; + break; + } + SendMsgCan(BRAS_AT, 0,arg_tempo); + waitingAckFrom = 0; + waitingAckID =0; + break; + + case 216://test bras competition ( à tester) + unsigned char arg_tempo1; + switch(arg1) + { + case AV_DROIT: + arg_tempo1 = 1; + break; + case AV_CENTRE: + arg_tempo1 = 2; + break; + case AV_GAUCHE: + arg_tempo1 = 3; + break; + case AR_DROIT: + arg_tempo1 = 4; + break; + case AR_CENTRE: + arg_tempo1 = 5; + break; + case AR_GAUCHE: + arg_tempo1 = 6; + break; + default : + arg_tempo1 =(unsigned char)arg1; + break; + } + //SendMsgCan(BRAS_RE, 0,arg_temp1); + waitingAckFrom = 0; + waitingAckID =0; + break; + + default: + retour = 0;//L'action n'existe pas, il faut utiliser le CAN + + } + return retour;//L'action est spécifique. + +}