![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
code de la carte IHM avant les bugs et avant le travail effectué avec Melchior
Dependencies: mbed SerialHalfDuplex SDFileSystem liaison_Bluetooth ident_crac DISCO-F469NI_portrait
Revision 38:9d6a3ccc0582, committed 2021-07-17
- Comitter:
- goldmas
- Date:
- Sat Jul 17 11:07:17 2021 +0000
- Parent:
- 36:c37dbe2be916
- Commit message:
- Dernier code de la coupe de fracne 2021
Changed in this revision
diff -r c37dbe2be916 -r 9d6a3ccc0582 Arriver_bon_port/bon_port.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Arriver_bon_port/bon_port.cpp Sat Jul 17 11:07:17 2021 +0000 @@ -0,0 +1,137 @@ +#include "global.h" + +// {X,Y,T,Sens} +int Nord_haut[4]={175,500,-90,1}; +int Sud_haut[4]={1500,500,-90,1}; + +int Nord_bas[4]={175,500,-90,1}; +int Sud_bas[4]={1500,500,-90,1}; + +int X_rob; +int Y_rob; +int T_rob; +int Sens; + +int tourner; + + +void arriver_bon_port(void) +{ +} +/* +//Variable de comptage des points +port_lu = val_girou + 1; + +////////////////////////////////Girouette Sud//////////////////////////////// + if(val_girou == 0) //Sud + { + if(Hauteur == 0) //Bas + { + if(InversStrat == 1) + { + X_rob = Sud_bas[0]; + Y_rob = 3000 - Sud_bas[1];//Inversion du Y + T_rob = -Sud_bas[2]; + Sens = Sud_bas[3]; + } + else + { + X_rob = Sud_bas[0]; + Y_rob = Sud_bas[1]; + T_rob = Sud_bas[2]; + Sens = Sud_bas[3]; + } + } + else if(Hauteur == 1) //Haut + { + if(InversStrat == 1) + { + X_rob = Sud_haut[0]; + Y_rob = 3000 - Sud_haut[1];//Inversion du Y + T_rob = -Sud_haut[2]; + Sens = Sud_haut[3]; + } + else + { + X_rob = Sud_haut[0]; + Y_rob = Sud_haut[1]; + T_rob = Sud_haut[2]; + Sens = Sud_haut[3]; + } + } + + GoToPosition(X_rob, Y_rob, T_rob, Sens); + waitingAckID = ASSERVISSEMENT_XYT; + waitingAckFrom = ACKNOWLEDGE_MOTEUR; + + //target_x_robot = X_rob; + //target_y_robot = Y_rob; + //target_theta_robot = T_rob; + + //GoStraight(Sens*350,0,0,0); + } + +////////////////////////////////Girouette Nord//////////////////////////////// + else if(val_girou == 1) //Nord + { + if(Hauteur == 0) //Bas + { + if(InversStrat == 1) + { + X_rob = Nord_bas[0]; + Y_rob = 3000 - Nord_bas[1];//Inversion du Y + T_rob = -Nord_bas[2]; + Sens = Nord_bas[3]; + } + else + { + X_rob = Nord_bas[0]; + Y_rob = Nord_bas[1]; + T_rob = Nord_bas[2]; + Sens = Nord_bas[3]; + } + } + else if(Hauteur == 1) //Haut + { + if(InversStrat == 1) + { + X_rob = Nord_haut[0]; + Y_rob = 3000 - Nord_haut[1];//Inversion du Y + T_rob = -Nord_haut[2]; + Sens = Nord_haut[3]; + } + else + { + X_rob = Nord_haut[0]; + Y_rob = Nord_haut[1]; + T_rob = Nord_haut[2]; + Sens = Nord_haut[3]; + } + } + GoToPosition(X_rob, Y_rob, T_rob, Sens); + waitingAckID = ASSERVISSEMENT_XYT; + waitingAckFrom = ACKNOWLEDGE_MOTEUR; + + //target_x_robot = X_rob; + //target_y_robot = Y_rob; + //target_theta_robot = T_rob; + + //GoStraight(Sens*350,0,0,0); + } + +////////////////////////////////Girouette Inconnu//////////////////////////////// + else if(val_girou == 2) //Jsp + { + tourner = 3600; + + if(InversStrat == 1) + { + tourner = -tourner; + } + + waitingAckID = ASSERVISSEMENT_ROTATION; + waitingAckFrom = ACKNOWLEDGE_MOTEUR; + Rotate(tourner); + } +} +*/ \ No newline at end of file
diff -r c37dbe2be916 -r 9d6a3ccc0582 Arriver_bon_port/bon_port.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Arriver_bon_port/bon_port.h Sat Jul 17 11:07:17 2021 +0000 @@ -0,0 +1,6 @@ +#ifndef CRAC_BON_PORT +#define CRAC_BON_PORT + +void arriver_bon_port(void); + +#endif \ No newline at end of file
diff -r c37dbe2be916 -r 9d6a3ccc0582 Asservissement/Asservissement.cpp --- a/Asservissement/Asservissement.cpp Mon May 31 13:36:03 2021 +0000 +++ b/Asservissement/Asservissement.cpp Sat Jul 17 11:07:17 2021 +0000 @@ -22,7 +22,7 @@ CANMessage msgTx=CANMessage(); msgTx.id=id; msgTx.len=2; - msgTx.format=CANStandard; +// msgTx.format=CANStandard; msgTx.type=CANData; // from sur 2 octets msgTx.data[0]=(unsigned char)from; @@ -37,7 +37,7 @@ CANMessage msgTx=CANMessage(); msgTx.id=id; msgTx.len=4; - msgTx.format=CANStandard; +// msgTx.format=CANStandard; msgTx.type=CANData; // from sur 2 octets msgTx.data[0]=(unsigned char)d1; @@ -53,7 +53,7 @@ CANMessage msgTx=CANMessage(); msgTx.id=id; msgTx.len=len; - msgTx.format=CANStandard; +// msgTx.format=CANStandard; msgTx.type=CANData; // from sur 2 octets for(int i = 0; i<len; i++) @@ -68,7 +68,7 @@ CANMessage msgTx=CANMessage(); msgTx.id=id; msgTx.len=1; - msgTx.format=CANStandard; +// msgTx.format=CANStandard; msgTx.type=CANData; msgTx.data[0]=data;
diff -r c37dbe2be916 -r 9d6a3ccc0582 Compteur_points/Compteur.cpp --- a/Compteur_points/Compteur.cpp Mon May 31 13:36:03 2021 +0000 +++ b/Compteur_points/Compteur.cpp Sat Jul 17 11:07:17 2021 +0000 @@ -1,24 +1,31 @@ #include "global.h" -#define M_PI 3.14159265358979323846 -#define DEMI_GOBELET 30 +#define M_PI 3.14159265358979323846f +#define MARGE_VENT 72.0f +#define MARGE_BRAS 54.0f +#define ECART 100.0f -int Ventouse_coord[6][2]={{145,-75},{145,0},{145,75},{-145,75},{-145,0},{-145,-75}}; -int Bras_coord[6][2]; -int Manche_air_coord[6][2]; +float Ventouse_coord[6][2]={{150,-80},{150,0},{150,80},{-150,80},{-150,0},{-150,-80}}; +float Bras_coord[6][2]={{232,-75},{232,0},{232,75},{-232,75},{-232,0},{-232,-75}}; + +float Position_de_pose[6][2]; int score_final=0; int score_ventouse=0,gobelet_vert=0,gobelet_rouge=0,gobelet_port=0; int score_manche=0,manche_releve=0; -int score_bon_port=0; -int score_phare=0; -int score_pavillon=0; +int score_bon_port=0, port_lu = 0; +int score_phare=0, phare_active=0; +int score_pavillon=0, pavillon=0; +int offset_score=0; -int etat_groupe, gobelet_en_place=0, old_gobelet=0; +int etat_groupe[6], gobelet_en_place=0, old_gobelet=0; unsigned char num_groupe; CompteurGameEtat VentEtat[6] = {ETAT_ATTENDRE, ETAT_ATTENDRE, ETAT_ATTENDRE, ETAT_ATTENDRE, ETAT_ATTENDRE, ETAT_ATTENDRE}; -int deja_compter[6] = {0, 0, 0, 0, 0, 0}; + +int old_gobelet_vert=0; +int old_gobelet_rouge=0; +int old_gobelet_port=0; /********************************************************************************************************************/ /* FUNCTION NAME: gestion_Message_CAN */ @@ -28,133 +35,255 @@ { int identifiant = msgRxBuffer[FIFO_ecriture].id; - switch(identifiant) + if(identifiant==0x220) { - case 0x220: - for(int num_vent=0;num_vent<6;num_vent++) - { - switch(VentEtat[num_vent]) + for(int num_vent=0;num_vent<6;num_vent++) + { + switch(VentEtat[num_vent]) + { + case ETAT_ATTENDRE: + etat_groupe[num_vent] = msgRxBuffer[FIFO_ecriture].data[num_vent]; + if(etat_groupe[num_vent] == 5) //Pompe et capteur + { + VentEtat[num_vent] = ETAT_RECUP; + } + break; + + case ETAT_RECUP: + etat_groupe[num_vent] = msgRxBuffer[FIFO_ecriture].data[num_vent]; + if(etat_groupe[num_vent] == 2) //Electrovane { - case ETAT_ATTENDRE: - etat_groupe = msgRxBuffer[FIFO_ecriture].data[num_vent]; - if(etat_groupe == 5) //Pompe et capteur - { - VentEtat[num_vent] = ETAT_RECUP; - } - break; - - case ETAT_RECUP: - etat_groupe = msgRxBuffer[FIFO_ecriture].data[num_vent]; - if(etat_groupe == 2) //Electrovane - { - VentEtat[num_vent] = ETAT_COMPTER; - } - break; - - case ETAT_COMPTER: - verif_position_ventouse(num_vent); - compteur_de_points(); - VentEtat[num_vent] = ETAT_ATTENDRE; - break; + VentEtat[num_vent] = ETAT_COMPTER; } - } - break; + break; - case BRAS_RE: - num_groupe = msgRxBuffer[FIFO_ecriture].data[0]; - verif_position_bras(num_groupe); - compteur_de_points(); - break; - - case AUTOMATE_MANCHE_BAS: - num_groupe = msgRxBuffer[FIFO_ecriture].data[0]; - verif_position_manche(num_groupe); - compteur_de_points(); - break; + case ETAT_COMPTER: + verif_position_ventouse(num_vent); + compteur_de_points(); + VentEtat[num_vent] = ETAT_ATTENDRE; + break; + } + } + } + + if(Flag_Bras_Re == 1) + { + num_groupe = Flag_num_bras; + if(num_groupe <6) verif_position_bras(num_groupe); + else if(num_groupe == 10) {verif_position_bras(1); verif_position_bras(0);} + else if(num_groupe == 20) {verif_position_bras(2); verif_position_bras(0);} + else if(num_groupe == 21) {verif_position_bras(2); verif_position_bras(1);} + else if(num_groupe == 210){verif_position_bras(2); verif_position_bras(1); verif_position_bras(0);} + else if(num_groupe == 43) {verif_position_bras(4); verif_position_bras(3);} + else if(num_groupe == 53) {verif_position_bras(5); verif_position_bras(3);} + else if(num_groupe == 54) {verif_position_bras(5); verif_position_bras(4);} + else if(num_groupe == 66) {verif_position_bras(5); verif_position_bras(4); verif_position_bras(3);} + compteur_de_points(); + Flag_Bras_Re = 0; + } + + if(Flag_Manche_Bas == 1) + { + verif_position_manche(); + compteur_de_points(); + Flag_Manche_Bas = 0; } + + if(Flag_Manche_Moy == 1) + { + verif_position_phare(); + compteur_de_points(); + Flag_Manche_Moy = 0; + } + + if(Flag_pavillon == 1) + { + pavillon = 1; + compteur_de_points(); + Flag_pavillon = 0; + } + } void compteur_de_points(void) { + if(Hauteur == 1) score_phare = 2; + else if(Hauteur == 0) offset_score = 10; + + //Points gobelets int paire_ventouse = abs(gobelet_vert - gobelet_rouge); - //Points gobelets - score_ventouse = (gobelet_vert*2 + gobelet_rouge*2)*2 - (paire_ventouse)*2 + gobelet_port; + score_ventouse = (gobelet_vert + gobelet_rouge - paire_ventouse)*3 + paire_ventouse*2 + gobelet_port; //Points manche à air if(manche_releve == 1) score_manche = 5; - else if(manche_releve == 2) score_manche = 15; + else if(manche_releve >= 2) score_manche = 15; //Points phare + if(phare_active == 1) score_phare = 15; //Points pavillons + if(pavillon == 1) score_pavillon = 5; //Points d'arrivée à bon port + if(port_lu == 1 || port_lu == 2) score_bon_port = 10; + else score_bon_port = 3; //Points totaux - score_final = score_ventouse; //+ score_manche + score_phare + score_pavillon + score_bon_port; + score_final = score_ventouse + score_manche + score_pavillon + score_phare + score_bon_port;//+ offset_score; } - - /********************************************************************************************************************/ /* FUNCTION NAME: verif_position_ventouse */ /* DESCRIPTION : Vérifie si les gobelets posés par les ventouses sont dans un chenal/un port et augmente le score */ /********************************************************************************************************************/ void verif_position_ventouse(int num_groupe) { - int x_offset = Ventouse_coord[num_groupe][0]*cos(theta_robot*M_PI/1800) - Ventouse_coord[num_groupe][1]*sin(theta_robot*M_PI*1800); - int y_offset = Ventouse_coord[num_groupe][0]*sin(theta_robot*M_PI/1800) + Ventouse_coord[num_groupe][1]*cos(theta_robot*M_PI*1800); + float x_offset = Ventouse_coord[num_groupe][0]*cos((float)theta_robot*M_PI/1800.0f) - Ventouse_coord[num_groupe][1]*sin((float)theta_robot*M_PI/1800.0f); + float y_offset = Ventouse_coord[num_groupe][0]*sin((float)theta_robot*M_PI/1800.0f) + Ventouse_coord[num_groupe][1]*cos((float)theta_robot*M_PI/1800.0f); //chenal vert port départ bleu - if((x_robot+x_offset <= 530+DEMI_GOBELET)&&(x_robot+x_offset >= 500-DEMI_GOBELET) - && (y_robot+y_offset <= 400+DEMI_GOBELET)&&(y_robot+y_offset >= 0)) gobelet_vert++; + if(((float)x_robot+x_offset <= 530.0f+MARGE_VENT+ECART)&&((float)x_robot+x_offset >= 500.0f-MARGE_VENT-ECART) + && ((float)y_robot+y_offset <= 400.0f+MARGE_VENT)&&((float)y_robot+y_offset >= 0)) + + {gobelet_vert++; Position_de_pose[num_groupe][0] = (float)x_robot+x_offset; Position_de_pose[num_groupe][1] = (float)y_robot+y_offset;} + //chenal rouge port départ bleu - else if((x_robot+x_offset <= 1100+DEMI_GOBELET)&&(x_robot+x_offset >= 1070-DEMI_GOBELET) - && (y_robot+y_offset <= 400+DEMI_GOBELET)&&(y_robot+y_offset >= 0)) gobelet_rouge++; + else if(((float)x_robot+x_offset <= 1100.0f+MARGE_VENT+ECART)&&((float)x_robot+x_offset >= 1070.0f-MARGE_VENT-ECART) + && ((float)y_robot+y_offset <= 400.0f+MARGE_VENT)&&((float)y_robot+y_offset >= 0)) + + {gobelet_rouge++; Position_de_pose[num_groupe][0] = (float)x_robot+x_offset; Position_de_pose[num_groupe][1] = (float)y_robot+y_offset;} + //port départ bleu - else if((x_robot+x_offset > 530+DEMI_GOBELET)&&(x_robot+x_offset < 1070-DEMI_GOBELET) - && (y_robot+y_offset <= 400+DEMI_GOBELET)&&(y_robot+y_offset >= 0)) gobelet_port++; + else if(((float)x_robot+x_offset > 530.0f+MARGE_VENT+ECART)&&((float)x_robot+x_offset < 1070.0f-MARGE_VENT-ECART) + && ((float)y_robot+y_offset <= 400.0f+MARGE_VENT)&&((float)y_robot+y_offset >= 0)) + + {gobelet_port++; Position_de_pose[num_groupe][0] = (float)x_robot+x_offset; Position_de_pose[num_groupe][1] = (float)y_robot+y_offset;} //chenal vert port ext bleu - else if((x_robot+x_offset >= 1700-DEMI_GOBELET)&&(x_robot+x_offset <= 2000+DEMI_GOBELET) - && (y_robot+y_offset >= 1650-DEMI_GOBELET)&&(y_robot+y_offset <= 1750+DEMI_GOBELET)) gobelet_vert++; + else if(((float)x_robot+x_offset >= 1700.0f-MARGE_VENT)&&((float)x_robot+x_offset <= 2000.0f+MARGE_VENT) + && ((float)y_robot+y_offset >= 1650.0f-MARGE_VENT)&&((float)y_robot+y_offset < 1800.0f)) + + {gobelet_vert++; Position_de_pose[num_groupe][0] = (float)x_robot+x_offset; Position_de_pose[num_groupe][1] = (float)y_robot+y_offset;} + //chenal rouge port ext bleu - else if((x_robot+x_offset >= 1700-DEMI_GOBELET)&&(x_robot+x_offset <= 2000+DEMI_GOBELET) - && (y_robot+y_offset >= 1850-DEMI_GOBELET)&&(y_robot+y_offset <= 1950+DEMI_GOBELET)) gobelet_rouge++; - //port ext bleu - else if((x_robot+x_offset >= 1700-DEMI_GOBELET)&&(x_robot+x_offset <= 2000+DEMI_GOBELET) - && (y_robot+y_offset > 1750+DEMI_GOBELET)&&(y_robot+y_offset < 1850-DEMI_GOBELET)) gobelet_port++; + else if(((float)x_robot+x_offset >= 1700.0f-MARGE_VENT)&&((float)x_robot+x_offset <= 2000.0f+MARGE_VENT) + && ((float)y_robot+y_offset >= 1800.0f)&&((float)y_robot+y_offset <= 1950.0f+MARGE_VENT)) + + {gobelet_rouge++; Position_de_pose[num_groupe][0] = (float)x_robot+x_offset; Position_de_pose[num_groupe][1] = (float)y_robot+y_offset;} //chenal vert port départ jaune - else if((x_robot+x_offset <= 530+DEMI_GOBELET)&&(x_robot+x_offset >= 500-DEMI_GOBELET) - && (y_robot+y_offset <= 3000+DEMI_GOBELET)&&(y_robot+y_offset >= 2600-DEMI_GOBELET)) gobelet_vert++; + else if(((float)x_robot+x_offset <= 530.0f+MARGE_VENT+ECART)&&((float)x_robot+x_offset >= 500.0f-MARGE_VENT-ECART) + && ((float)y_robot+y_offset <= 3000.0f+MARGE_VENT)&&((float)y_robot+y_offset >= 2600.0f-MARGE_VENT)) + + {gobelet_rouge++; Position_de_pose[num_groupe][0] = (float)x_robot+x_offset; Position_de_pose[num_groupe][1] = (float)y_robot+y_offset;} + //chenal rouge port départ jaune - else if((x_robot+x_offset <= 1100+DEMI_GOBELET)&&(x_robot+x_offset >= 1070-DEMI_GOBELET) - && (y_robot+y_offset <= 3000+DEMI_GOBELET)&&(y_robot+y_offset >= 2600-DEMI_GOBELET)) gobelet_rouge++; + else if(((float)x_robot+x_offset <= 1100.0f+MARGE_VENT+ECART)&&((float)x_robot+x_offset >= 1070.0f-MARGE_VENT-ECART) + && ((float)y_robot+y_offset <= 3000.0f+MARGE_VENT)&&((float)y_robot+y_offset >= 2600.0f-MARGE_VENT)) + + {gobelet_vert++; Position_de_pose[num_groupe][0] = (float)x_robot+x_offset; Position_de_pose[num_groupe][1] = (float)y_robot+y_offset;} + //port départ jaune - else if((x_robot+x_offset > 530+DEMI_GOBELET)&&(x_robot+x_offset < 1070-DEMI_GOBELET) - && (y_robot+y_offset <= 3000+DEMI_GOBELET)&&(y_robot+y_offset >= 2600-DEMI_GOBELET)) gobelet_port++; + else if(((float)x_robot+x_offset > 530.0f+MARGE_VENT+ECART)&&((float)x_robot+x_offset < 1070.0f-MARGE_VENT-ECART) + && ((float)y_robot+y_offset <= 3000.0f+MARGE_VENT)&&((float)y_robot+y_offset >= 2600.0f-MARGE_VENT)) + + {gobelet_port++; Position_de_pose[num_groupe][0] = (float)x_robot+x_offset; Position_de_pose[num_groupe][1] = (float)y_robot+y_offset;} //chenal vert port ext jaune - else if((x_robot+x_offset >= 1700-DEMI_GOBELET)&&(x_robot+x_offset <= 2000+DEMI_GOBELET) - && (y_robot+y_offset >= 1050-DEMI_GOBELET)&&(y_robot+y_offset <= 1150+DEMI_GOBELET)) gobelet_vert++; + else if(((float)x_robot+x_offset >= 1700.0f-MARGE_VENT)&&((float)x_robot+x_offset <= 2000.0f+MARGE_VENT) + && ((float)y_robot+y_offset >= 1050.0f-MARGE_VENT)&&((float)y_robot+y_offset < 1200.0f)) + + {gobelet_vert++; Position_de_pose[num_groupe][0] = (float)x_robot+x_offset; Position_de_pose[num_groupe][1] = (float)y_robot+y_offset;} + //chenal rouge port ext jaune - else if((x_robot+x_offset >= 1700-DEMI_GOBELET)&&(x_robot+x_offset <= 2000+DEMI_GOBELET) - && (y_robot+y_offset >= 1250-DEMI_GOBELET)&&(y_robot+y_offset <= 1350+DEMI_GOBELET)) gobelet_rouge++; - //port ext jaune - else if((x_robot+x_offset >= 1700-DEMI_GOBELET)&&(x_robot+x_offset <= 2000+DEMI_GOBELET) - && (y_robot+y_offset > 1150+DEMI_GOBELET)&&(y_robot+y_offset < 1250-DEMI_GOBELET)) gobelet_port++; + else if(((float)x_robot+x_offset >= 1700.0f-MARGE_VENT)&&((float)x_robot+x_offset <= 2000.0f+MARGE_VENT) + && ((float)y_robot+y_offset >= 1200.0f)&&((float)y_robot+y_offset <= 1350.0f+MARGE_VENT)) + + {gobelet_rouge++; Position_de_pose[num_groupe][0] = (float)x_robot+x_offset; Position_de_pose[num_groupe][1] = (float)y_robot+y_offset;} } +/********************************************************************************************************************/ +/* FUNCTION NAME: verif_position_bras */ +/* DESCRIPTION : Vérifie si les gobelets posés par les bras sont dans un chenal/un port et augmente le score */ +/********************************************************************************************************************/ void verif_position_bras(int num_groupe) -{ +{ + float x_offset = Bras_coord[num_groupe][0]*cos((float)theta_robot*M_PI/1800.0f) - Bras_coord[num_groupe][1]*sin((float)theta_robot*M_PI/1800.0f); + float y_offset = Bras_coord[num_groupe][0]*sin((float)theta_robot*M_PI/1800.0f) + Bras_coord[num_groupe][1]*cos((float)theta_robot*M_PI/1800.0f); + + + //chenal vert port départ bleu + if(((float)x_robot+x_offset <= 530.0f+MARGE_BRAS+ECART)&&((float)x_robot+x_offset >= 500.0f-MARGE_BRAS-ECART) + && ((float)y_robot+y_offset <= 400.0f+MARGE_BRAS)&&((float)y_robot+y_offset >= 0)) gobelet_vert++; + //chenal rouge port départ bleu + else if(((float)x_robot+x_offset <= 1100.0f+MARGE_BRAS+ECART)&&((float)x_robot+x_offset >= 1070.0f-MARGE_BRAS-ECART) + && ((float)y_robot+y_offset <= 400.0f+MARGE_BRAS)&&((float)y_robot+y_offset >= 0)) gobelet_rouge++; + //port départ bleu + else if(((float)x_robot+x_offset > 530.0f+MARGE_BRAS+ECART)&&((float)x_robot+x_offset < 1070.0f-MARGE_BRAS-ECART) + && ((float)y_robot+y_offset <= 400.0f+MARGE_BRAS)&&((float)y_robot+y_offset >= 0)) gobelet_port++; + + //chenal vert port ext bleu + else if(((float)x_robot+x_offset >= 1700.0f-MARGE_BRAS)&&((float)x_robot+x_offset <= 2000.0f+MARGE_BRAS) + && ((float)y_robot+y_offset >= 1650.0f-MARGE_BRAS)&&((float)y_robot+y_offset <= 1780.0f)) gobelet_vert++; + //chenal rouge port ext bleu + else if(((float)x_robot+x_offset >= 1700.0f-MARGE_BRAS)&&((float)x_robot+x_offset <= 2000.0f+MARGE_BRAS) + && ((float)y_robot+y_offset >= 1820.0f)&&((float)y_robot+y_offset <= 1950.0f+MARGE_BRAS)) gobelet_rouge++; + //Port ext bleu + else if((x_robot+x_offset >= 1700.0f-MARGE_BRAS)&&(x_robot+x_offset <= 2000.0f+MARGE_BRAS) + && (y_robot+y_offset > 1780.0f)&&(y_robot+y_offset < 1820.0f)) gobelet_port++; + + + //chenal vert port départ jaune + else if(((float)x_robot+x_offset <= 530.0f+MARGE_BRAS+ECART)&&((float)x_robot+x_offset >= 500.0f-MARGE_BRAS-ECART) + && ((float)y_robot+y_offset <= 3000.0f+MARGE_BRAS)&&((float)y_robot+y_offset >= 2600.0f-MARGE_BRAS)) gobelet_rouge++; + //chenal rouge port départ jaune + else if(((float)x_robot+x_offset <= 1100.0f+MARGE_BRAS+ECART)&&((float)x_robot+x_offset >= 1070.0f-MARGE_BRAS-ECART) + && ((float)y_robot+y_offset <= 3000.0f+MARGE_BRAS)&&((float)y_robot+y_offset >= 2600.0f-MARGE_BRAS)) gobelet_vert++; + //port départ jaune + else if(((float)x_robot+x_offset > 530.0f+MARGE_BRAS+ECART)&&((float)x_robot+x_offset < 1070.0f-MARGE_BRAS-ECART) + && ((float)y_robot+y_offset <= 3000.0f+MARGE_BRAS)&&((float)y_robot+y_offset >= 2600.0f-MARGE_BRAS)) gobelet_port++; + + //chenal vert port ext jaune + else if(((float)x_robot+x_offset >= 1700.0f-MARGE_BRAS)&&((float)x_robot+x_offset <= 2000.0f+MARGE_BRAS) + && ((float)y_robot+y_offset >= 1050.0f-MARGE_BRAS)&&((float)y_robot+y_offset <= 1180.0f)) gobelet_vert++; + //chenal rouge port ext jaune + else if(((float)x_robot+x_offset >= 1700.0f-MARGE_BRAS)&&((float)x_robot+x_offset <= 2000.0f+MARGE_BRAS) + && ((float)y_robot+y_offset >= 1220.0f)&&((float)y_robot+y_offset <= 1350.0f+MARGE_BRAS)) gobelet_rouge++; + //Port ext jaune + else if((float)(x_robot+x_offset >= 1700.0f-MARGE_BRAS)&&((float)x_robot+x_offset <= 2000.0f+MARGE_BRAS) + && ((float)y_robot+y_offset > 1180.0f)&&((float)y_robot+y_offset < 1220.0f)) gobelet_port++; } -void verif_position_manche(int num_groupe) -{ +/********************************************************************************************************************/ +/* FUNCTION NAME: verif_position_manche */ +/* DESCRIPTION : Vérifie si les bras des manches à air sont déployés au bon enroit et augmente le score */ +/********************************************************************************************************************/ +void verif_position_manche(void) +{ + //Manches Bleus + if((x_robot <= 2000+MARGE_VENT)&&(x_robot >= 1700-MARGE_VENT) + && (y_robot <= 890+MARGE_VENT)&&(y_robot >= 0)) manche_releve++; + + //Manches Jaunes + else if((x_robot <= 2000+MARGE_VENT)&&(x_robot >= 1700-MARGE_VENT) + && (y_robot <= 3000)&&(y_robot >= 2090-MARGE_VENT)) manche_releve++; +} +/********************************************************************************************************************/ +/* FUNCTION NAME: verif_position_phare */ +/* DESCRIPTION : Vérifie si les bras des manches à air sont déployés au bon enroit et augmente le score */ +/********************************************************************************************************************/ +void verif_position_phare(void) +{ + //Phare Bleu + if((x_robot <= 300+MARGE_VENT)&&(x_robot >= 0-MARGE_VENT) + && (y_robot <= 890+MARGE_VENT)&&(y_robot >= 0)) phare_active=1; + + //Phare Jaune + else if((x_robot <= 300+MARGE_VENT)&&(x_robot >= 0-MARGE_VENT) + && (y_robot <= 3000)&&(y_robot >= 2090-MARGE_VENT)) phare_active=1; } \ No newline at end of file
diff -r c37dbe2be916 -r 9d6a3ccc0582 Compteur_points/Compteur.h --- a/Compteur_points/Compteur.h Mon May 31 13:36:03 2021 +0000 +++ b/Compteur_points/Compteur.h Sat Jul 17 11:07:17 2021 +0000 @@ -11,13 +11,28 @@ void verif_position_bras(int num_groupe); -void verif_position_manche(int num_groupe); +void verif_position_manche(void); +void verif_position_phare(void); + +extern int etat_groupe[6]; extern int score_final; extern int gobelet_vert; extern int gobelet_rouge; extern int gobelet_port; +extern int manche_releve; +extern int phare_active; +extern int pavillon; +extern int port_lu; + +extern int old_gobelet_vert, old_gobelet_rouge, old_gobelet_port; + +extern float Position_de_pose[6][2]; + + +extern unsigned char num_groupe; + typedef enum { ETAT_ATTENDRE,
diff -r c37dbe2be916 -r 9d6a3ccc0582 Evitement/Evitement.cpp --- a/Evitement/Evitement.cpp Mon May 31 13:36:03 2021 +0000 +++ b/Evitement/Evitement.cpp Sat Jul 17 11:07:17 2021 +0000 @@ -25,10 +25,56 @@ /* FUNCTION NAME: Balise Danger */ /* DESCRIPTION : FIFO -> BALISE_DANGER */ /****************************************************************************************/ -unsigned short balise_danger(void){ - SendSpeed(150); - return(0); + +int Flag_stoped_danger = 0; + +Ticker ticker_timeout_evitement; + +unsigned short balise_danger(signed char FIFO_lecture) +{ + /*signed char fin_angle_detection; + signed char debut_angle_detection; + float angle_moyen_balise_IR = 0.0; +// Debug_Audio(3,2); + //on recupere l'info d'angle de detection-------------------------------------- + if(msgRxBuffer[FIFO_lecture].data[0]!=0) { //data balise Petit Robot Detecte + fin_angle_detection = msgRxBuffer[FIFO_lecture].data[0] & 0x0F; + debut_angle_detection = (msgRxBuffer[FIFO_lecture].data[0] & 0xF0) >> 4; + } else { //data balise Gros Robot Detecte + fin_angle_detection = msgRxBuffer[FIFO_lecture].data[2] & 0x0F; + debut_angle_detection = (msgRxBuffer[FIFO_lecture].data[2] & 0xF0) >> 4; + } + //on moyenne l'angle------------------------------------------------------------ + if(debut_angle_detection > fin_angle_detection) { + angle_moyen_balise_IR = (float)debut_angle_detection + ((15.0f-(float)debut_angle_detection)+(float)fin_angle_detection)/2.0f; + if(angle_moyen_balise_IR > 15.0f) + angle_moyen_balise_IR-=15.0f; + } else + angle_moyen_balise_IR = debut_angle_detection + (fin_angle_detection-debut_angle_detection)/2; + char seuil_bas_arriere = 5; + char seuil_haut_arriere = 7; + char seuil_bas_avant = 0xF; + char seuil_haut_avant = 0xD; + // #endif + // LARNAUDIE 3/7/2021 if((angle_moyen_balise_IR>=seuil_bas_avant && angle_moyen_balise_IR<=seuil_haut_avant)) // || (angle_moyen_balise_IR>=seuil_bas_arriere && angle_moyen_balise_IR<=seuil_haut_arriere)) + if((actionPrecedente==MV_COURBURE)||(actionPrecedente==MV_LINE)||(actionPrecedente==MV_XYT)) + { + if(asser_stop_direction==1) + { + if( ((debut_angle_detection>=seuil_haut_avant) && (debut_angle_detection<=seuil_bas_avant)) + ||((fin_angle_detection>=seuil_haut_avant) && (fin_angle_detection<=seuil_bas_avant)) ) + SendRawId(ASSERVISSEMENT_STOP); + } + else + { + if( ((debut_angle_detection>=seuil_bas_arriere) && (debut_angle_detection<=seuil_haut_arriere)) + ||((fin_angle_detection>=seuil_bas_arriere) && (fin_angle_detection<=seuil_haut_arriere)) ) + SendRawId(ASSERVISSEMENT_STOP); + } + } + return(0);*/ } + /****************************************************************************************/ /* FUNCTION NAME: Balise Stop */ /* DESCRIPTION : FIFO -> BALISE_STOP */ @@ -38,9 +84,9 @@ signed char fin_angle_detection; signed char debut_angle_detection; float angle_moyen_balise_IR = 0.0; - Debug_Audio(3,2); +// Debug_Audio(3,2); //on recupere l'info d'angle de detection-------------------------------------- - if(msgRxBuffer[FIFO_lecture].data[0]!=0) { //data balise Petit Robot Detecte + if(msgRxBuffer[FIFO_lecture].data[0]!=0) { //data balise Petit Robot Detecte fin_angle_detection = msgRxBuffer[FIFO_lecture].data[0] & 0x0F; debut_angle_detection = (msgRxBuffer[FIFO_lecture].data[0] & 0xF0) >> 4; } else { //data balise Gros Robot Detecte @@ -54,84 +100,155 @@ angle_moyen_balise_IR-=15.0f; } else angle_moyen_balise_IR = debut_angle_detection + (fin_angle_detection-debut_angle_detection)/2; - #ifdef ROBOT_BIG - float seuil_bas_avant = 12.0; - float seuil_haut_avant = 15.0; - float seuil_bas_arriere = 5.0; - float seuil_haut_arriere = 7.0; - #else - float seuil_bas_arriere = 12.0; - float seuil_haut_arriere = 15.0; - float seuil_bas_avant = 4.0; - float seuil_haut_avant = 7.0; - #endif - if((angle_moyen_balise_IR>=seuil_bas_avant && angle_moyen_balise_IR<=seuil_haut_avant)) // || (angle_moyen_balise_IR>=seuil_bas_arriere && angle_moyen_balise_IR<=seuil_haut_arriere)) - SendRawId(ASSERVISSEMENT_STOP); + char seuil_bas_arriere = 5; + char seuil_haut_arriere = 7; + char seuil_bas_avant = 0xF; + char seuil_haut_avant = 0xD; + // #endif + // LARNAUDIE 3/7/2021 if((angle_moyen_balise_IR>=seuil_bas_avant && angle_moyen_balise_IR<=seuil_haut_avant)) // || (angle_moyen_balise_IR>=seuil_bas_arriere && angle_moyen_balise_IR<=seuil_haut_arriere)) + if((actionPrecedente==MV_COURBURE)||(actionPrecedente==MV_LINE)||(actionPrecedente==MV_XYT)) + { + if(Cote == 1)//Jaune + { + if((x_robot >= 1600 && y_robot >= 2200) || (y_robot >= 2500) || (x_robot >= 1750 && y_robot >= 1000 && y_robot <= 1400)) {} + else + { + if(asser_stop_direction==1) + { + if( ((debut_angle_detection>=seuil_haut_avant) && (debut_angle_detection<=seuil_bas_avant)) + ||((fin_angle_detection>=seuil_haut_avant) && (fin_angle_detection<=seuil_bas_avant)) ) + SendRawId(ASSERVISSEMENT_STOP); + Flag_stoped_danger = 1; + } + else + { + if( ((debut_angle_detection>=seuil_bas_arriere) && (debut_angle_detection<=seuil_haut_arriere)) + ||((fin_angle_detection>=seuil_bas_arriere) && (fin_angle_detection<=seuil_haut_arriere)) ) + SendRawId(ASSERVISSEMENT_STOP); + Flag_stoped_danger = 1; + } + } + } + + else if(Cote == 0)//Bleu + { + if((x_robot >= 1600 && y_robot <= 800) || (y_robot <= 500) || (x_robot >= 1750 && y_robot >= 1600 && y_robot <= 2000)) {} + else + { + if(asser_stop_direction==1) + { + if( ((debut_angle_detection>=seuil_haut_avant) && (debut_angle_detection<=seuil_bas_avant)) + ||((fin_angle_detection>=seuil_haut_avant) && (fin_angle_detection<=seuil_bas_avant)) ) + SendRawId(ASSERVISSEMENT_STOP); + Flag_stoped_danger = 1; + } + else + { + if( ((debut_angle_detection>=seuil_bas_arriere) && (debut_angle_detection<=seuil_haut_arriere)) + ||((fin_angle_detection>=seuil_bas_arriere) && (fin_angle_detection<=seuil_haut_arriere)) ) + SendRawId(ASSERVISSEMENT_STOP); + Flag_stoped_danger = 1; + } + } + } + if(Flag_stoped_danger) + { + ticker_timeout_evitement.detach(); + ticker_timeout_evitement.attach(&isr_end_danger, 2); + } + } return(0); } /****************************************************************************************/ /* FUNCTION NAME: Balise end Danger */ /* DESCRIPTION : FIFO -> BALISE_END_DANGER */ /****************************************************************************************/ -unsigned short balise_end_danger(S_Instruction* instruction,S_Dodge_queue* dodgeq, signed short target_x_robot, signed short target_y_robot, signed short target_theta_robot, signed short theta_robot,signed short x_robot,signed short y_robot){ - Debug_Audio(3,1); - switch(instruction->order){ - case MV_LINE: - gameEtat=ETAT_GAME_PROCESS_INSTRUCTION; - instruction->order = MV_XYT; - instruction->arg1 = target_x_robot;// X - instruction->arg2 = target_y_robot;// Y - instruction->arg3 = target_theta_robot;// T - break; - case MV_TURN: - gameEtat=ETAT_GAME_PROCESS_INSTRUCTION; - instruction->order = MV_XYT; - instruction->arg1 = target_x_robot;// X - instruction->arg2 = target_y_robot;// Y - instruction->arg3 = target_theta_robot;// T - break; - case MV_XYT: - gameEtat=ETAT_GAME_PROCESS_INSTRUCTION; - break; - case MV_COURBURE: - unsigned short alpha; - gameEtat=ETAT_GAME_PROCESS_INSTRUCTION; - instruction->order=MV_XYT; - if(instruction->direction==LEFT) alpha=(dodgeq->inst[0].arg3-theta_robot); - else alpha=(theta_robot-dodgeq->inst[0].arg3); - if(alpha<450){ - dodgeq->nb=0; - instruction->arg1=dodgeq->inst[0].arg1;//x - instruction->arg2=dodgeq->inst[0].arg2;//y - instruction->arg3=dodgeq->inst[0].arg3;//t - } else if(alpha<900){ - dodgeq->nb=1; - instruction->arg1=dodgeq->inst[1].arg1;//x - instruction->arg2=dodgeq->inst[1].arg2;//y - instruction->arg3=dodgeq->inst[1].arg3;//t - } else if(alpha<1350){ - dodgeq->nb=2; - instruction->arg1=dodgeq->inst[2].arg1;//x - instruction->arg2=dodgeq->inst[2].arg2;//y - instruction->arg3=dodgeq->inst[2].arg3;//t - } else if(alpha<1800){ - dodgeq->nb=3; - instruction->arg1=dodgeq->inst[3].arg1;//x - instruction->arg2=dodgeq->inst[3].arg2;//y - instruction->arg3=dodgeq->inst[3].arg3;//t - } else if(alpha<2250){ - dodgeq->nb=4; - instruction->arg1=dodgeq->inst[4].arg1;//x - instruction->arg2=dodgeq->inst[4].arg2;//y - instruction->arg3=dodgeq->inst[4].arg3;//t - } else { - dodgeq->nb=5; - instruction->arg1=dodgeq->inst[5].arg1;//x - instruction->arg2=dodgeq->inst[5].arg2;//y - instruction->arg3=dodgeq->inst[5].arg3;//t - } - break; +unsigned short balise_end_danger(S_Instruction* instruction,S_Dodge_queue* dodgeq, signed short local_target_x_robot, signed short local_target_y_robot, signed short local_target_theta_robot, signed short local_theta_robot,signed short x_robot,signed short y_robot){ + + ticker_timeout_evitement.detach(); + if(Flag_stoped_danger) + { + + ingnorInversionOnce=1; + wait_ms(1000); + switch(instruction->order){ + case MV_RECALAGE: + gameEtat=ETAT_GAME_PROCESS_INSTRUCTION; + Flag_stoped_danger = 0; + break; + + case MV_LINE: + gameEtat=ETAT_GAME_PROCESS_INSTRUCTION; + instruction->order = MV_XYT; + instruction->arg1 = local_target_x_robot;// X + instruction->arg2 = local_target_y_robot;// Y + instruction->arg3 = local_target_theta_robot;// T + Flag_stoped_danger = 0; + break; + case MV_TURN: + gameEtat=ETAT_GAME_PROCESS_INSTRUCTION; + instruction->order = MV_XYT; + instruction->arg1 = local_target_x_robot;// X + instruction->arg2 = local_target_y_robot;// Y + instruction->arg3 = local_target_theta_robot;// T + Flag_stoped_danger = 0; + break; + case MV_XYT: + gameEtat=ETAT_GAME_PROCESS_INSTRUCTION; + Flag_stoped_danger = 0; + break; + case MV_COURBURE: + short alpha; + gameEtat=ETAT_GAME_PROCESS_INSTRUCTION; + instruction->order=MV_XYT; + if(instruction->direction==LEFT) + alpha=(dodgeq->inst[0].arg3-theta_robot); + else + alpha=(theta_robot-dodgeq->inst[0].arg3); + if(alpha>3600) + alpha=alpha-3600; + if(alpha<-3600) + alpha=alpha+3600; + if(alpha<0) + alpha=-alpha; + if(alpha<450) + { + dodgeq->nb=0; + instruction->arg1=dodgeq->inst[0].arg1;//x + instruction->arg2=dodgeq->inst[0].arg2;//y + instruction->arg3=dodgeq->inst[0].arg3;//t + } + else if(alpha<900) + { + dodgeq->nb=1; + instruction->arg1=dodgeq->inst[1].arg1;//x + instruction->arg2=dodgeq->inst[1].arg2;//y + instruction->arg3=dodgeq->inst[1].arg3;//t + } else if(alpha<1350){ + dodgeq->nb=2; + instruction->arg1=dodgeq->inst[2].arg1;//x + instruction->arg2=dodgeq->inst[2].arg2;//y + instruction->arg3=dodgeq->inst[2].arg3;//t + } else if(alpha<1800){ + dodgeq->nb=3; + instruction->arg1=dodgeq->inst[3].arg1;//x + instruction->arg2=dodgeq->inst[3].arg2;//y + instruction->arg3=dodgeq->inst[3].arg3;//t + } else if(alpha<2250){ + dodgeq->nb=4; + instruction->arg1=dodgeq->inst[4].arg1;//x + instruction->arg2=dodgeq->inst[4].arg2;//y + instruction->arg3=dodgeq->inst[4].arg3;//t + } else { + dodgeq->nb=5; + instruction->arg1=dodgeq->inst[5].arg1;//x + instruction->arg2=dodgeq->inst[5].arg2;//y + instruction->arg3=dodgeq->inst[5].arg3;//t + } + Flag_stoped_danger = 0; + break; } - SendSpeed(300); +// SendSpeed(300); + } return(0); }
diff -r c37dbe2be916 -r 9d6a3ccc0582 Evitement/Evitement.h --- a/Evitement/Evitement.h Mon May 31 13:36:03 2021 +0000 +++ b/Evitement/Evitement.h Sat Jul 17 11:07:17 2021 +0000 @@ -9,7 +9,7 @@ #define T_X 2000 #define T_Y 3000 -unsigned short balise_danger(void); +unsigned short balise_danger(signed char FIFO_lecture); unsigned short balise_stop(signed char FIFO_lecture); unsigned short balise_end_danger(S_Instruction* instruction,S_Dodge_queue* dodgeq, signed short target_x_robot, signed short target_y_robot, signed short target_theta_robot, signed short theta_robot,signed short x_robot,signed short y_robot);
diff -r c37dbe2be916 -r 9d6a3ccc0582 Globals/global.h --- a/Globals/global.h Mon May 31 13:36:03 2021 +0000 +++ b/Globals/global.h Sat Jul 17 11:07:17 2021 +0000 @@ -24,8 +24,10 @@ #include "Evitement.h" #include "ihm.h" #include "Compteur.h" - - +#include "bon_port.h" +extern unsigned char ingnorInversionOnce; +extern signed char asser_stop_direction; +extern E_InstructionType actionPrecedente; extern Serial pc; //extern E_stratGameEtat gameEtat = ETAT_CHECK_CARTES; //extern T_etat strat_etat_s = INIT;
diff -r c37dbe2be916 -r 9d6a3ccc0582 Globals/ident_crac.lib --- a/Globals/ident_crac.lib Mon May 31 13:36:03 2021 +0000 +++ b/Globals/ident_crac.lib Sat Jul 17 11:07:17 2021 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/teams/CRAC-Team/code/ident_crac/#3cdea2100f98 +https://os.mbed.com/teams/CRAC-Team/code/ident_crac/#8efe164137c7
diff -r c37dbe2be916 -r 9d6a3ccc0582 IHM/ihm.cpp --- a/IHM/ihm.cpp Mon May 31 13:36:03 2021 +0000 +++ b/IHM/ihm.cpp Sat Jul 17 11:07:17 2021 +0000 @@ -40,6 +40,10 @@ Button TEST_NEXT(0,575,400,100,"------>"); Button TEST_DIV(0,25,400,100,"Test Divers"); Button TEST_ASSERV(0, 135, 400, 100, "Test asserv"); +Button TEST_MANCHE(0,245,400,100,"Test manches a air"); +Button TEST_GIROU(0,355,400,100,"Test capteur girouette"); + + //menu demo/demo/ Button TEST_A(0,25,195,100,"A"); Button TEST_B(205,25,195,100,"B"); @@ -51,19 +55,19 @@ Button TEST_PLUS(205,355,195,100,"+"); Button TEST_MOINS(0,355,195,100,"-"); -Button TEST_SERVO_0(280,50,120,100,"Bras 0"); -Button TEST_SERVO_1(140,50,120,100,"Bras 1"); -Button TEST_SERVO_2(0,50,120,100,"Bras 2"); -Button TEST_SERVO_3(0,160,120,100,"Bras 3"); -Button TEST_SERVO_4(140,160,120,100,"Bras 4"); -Button TEST_SERVO_5(280,160,120,100,"Bras 5"); +Button TEST_SERVO_0(0,160,120,100,"Bras 0"); +Button TEST_SERVO_1(140,160,120,100,"Bras 1"); +Button TEST_SERVO_2(280,160,120,100,"Bras 2"); +Button TEST_SERVO_3(280,50,120,100,"Bras 3"); +Button TEST_SERVO_4(140,50,120,100,"Bras 4"); +Button TEST_SERVO_5(0,50,120,100,"Bras 5"); -Button TEST_VENT_0(280,50,120,100,"Vent 0"); -Button TEST_VENT_1(140,50,120,100,"Vent 1"); -Button TEST_VENT_2(0,50,120,100,"Vent 2"); -Button TEST_VENT_3(0,160,120,100,"Vent 3"); -Button TEST_VENT_4(140,160,120,100,"Vent 4"); -Button TEST_VENT_5(280,160,120,100,"Vent 5"); +Button TEST_VENT_0(0,160,120,100,"Vent 0"); +Button TEST_VENT_1(140,160,120,100,"Vent 1"); +Button TEST_VENT_2(280,160,120,100,"Vent 2"); +Button TEST_VENT_3(280,50,120,100,"Vent 3"); +Button TEST_VENT_4(140,50,120,100,"Vent 4"); +Button TEST_VENT_5(0,50,120,100,"Vent 5"); Button FORCE_LAUNCH(0, 600, 400, 100, "Force Launch"); Button SUIVANT(0,380,200,100,"Suivant"); @@ -81,7 +85,7 @@ /* DESCRIPTION : Automate de gestion de l'affichage */ /****************************************************************************************/ void automate_etat_ihm(void) -{ +{ char toto[12]; int j, old_time=0, actual_time=0, old_score=0, actual_score=0, unique=0; unsigned char maximilien=1, choix_groupe; @@ -167,25 +171,38 @@ maximilien=0; while (strat_etat_s == DEMO) { canProcessRx(); - if(TEST_VENT.Touched()) { + if(TEST_VENT.Touched()) + { while(TEST_VENT.Touched()); strat_etat_s = TEST_VENTOUSE; - } else if(TEST_MOT.Touched()) { + } + else if(TEST_MOT.Touched()) + { while(TEST_MOT.Touched()); strat_etat_s = TEST_MOTEUR; - } else if(TEST_NEXT.Touched()) { + } + else if(TEST_NEXT.Touched()) + { while(TEST_NEXT.Touched()); strat_etat_s = DEMO2; - } else if (TEST_COUL.Touched()) { + } + else if (TEST_COUL.Touched()) + { while(TEST_COUL.Touched()); strat_etat_s =TEST_COULEUR ; - } else if (TEST_BRAS.Touched()) { + } + else if (TEST_BRAS.Touched()) + { while(TEST_BRAS.Touched()); strat_etat_s =TEST_SERVO_BRAS ; - } else if(TEST_AUDIO.Touched()) { + } + else if(TEST_AUDIO.Touched()) + { while(TEST_AUDIO.Touched()); strat_etat_s=TEST_AUD; - } else if(RETOUR.Touched()) { + } + else if(RETOUR.Touched()) + { while(RETOUR.Touched()); strat_etat_s = CHOIX; } @@ -200,26 +217,119 @@ RETOUR.Draw(0xFFFF0000, 0); TEST_DIV.Draw(VERT, 0); TEST_ASSERV.Draw(VERT, 0); + TEST_MANCHE.Draw(VERT, 0); + TEST_GIROU.Draw(VERT, 0); TEST_NEXT.Draw(BLEU, 0); maximilien=0; while (strat_etat_s == DEMO2) { canProcessRx(); - if(TEST_NEXT.Touched()) { + if(TEST_NEXT.Touched()) + { while(TEST_NEXT.Touched()); strat_etat_s = DEMO; - } else if(TEST_DIV.Touched()) { + } + else if(TEST_DIV.Touched()) + { while(TEST_DIV.Touched()); strat_etat_s = TEST_DIVE; - } else if(TEST_ASSERV.Touched()) { + } + else if(TEST_ASSERV.Touched()) + { while(TEST_ASSERV.Touched()); strat_etat_s = TEST_ASSERVE; - } else if(RETOUR.Touched()) { + } + else if(TEST_MANCHE.Touched()) + { + while(TEST_MANCHE.Touched()); + strat_etat_s = TEST_MANCHES; + } + else if(TEST_GIROU.Touched()) + { + while(TEST_GIROU.Touched()); + strat_etat_s = TEST_GIROUS; + } + + else if(RETOUR.Touched()) + { while(RETOUR.Touched()); strat_etat_s = CHOIX; } } break; + case TEST_MANCHES: + lcd.SetBackColor(LCD_COLOR_WHITE); + lcd.SetTextColor(LCD_COLOR_BLACK); + lcd.Clear (LCD_COLOR_WHITE); + lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Manches a air", 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) ; + + while(strat_etat_s==TEST_MANCHES) + { + if(RETOUR.Touched()) + { + while (RETOUR.Touched()); + strat_etat_s=DEMO2; + } + else if(TEST_A.Touched()) + { + while (TEST_A.Touched()); + TEST_A.Draw(BLEU, BLANC); + SendRawId(TEST_BRAS_1); + } + else if(TEST_B.Touched()) + { + while (TEST_B.Touched()); + TEST_B.Draw(BLEU, BLANC); + choix_groupe = 0; + SendMsgCan(AUTOMATE_MANCHE_MOY, &choix_groupe, sizeof(choix_groupe)) ; + wait_ms(500); + SendMsgCan(AUTOMATE_MANCHE_BAS, &choix_groupe, sizeof(choix_groupe)) ; + wait_ms(500); + SendMsgCan(AUTOMATE_MANCHE_HAUT, &choix_groupe, sizeof(choix_groupe)) ; + wait_ms(500); + choix_groupe = 1; + SendMsgCan(AUTOMATE_MANCHE_MOY, &choix_groupe, sizeof(choix_groupe)) ; + wait_ms(500); + SendMsgCan(AUTOMATE_MANCHE_BAS, &choix_groupe, sizeof(choix_groupe)) ; + wait_ms(500); + SendMsgCan(AUTOMATE_MANCHE_HAUT, &choix_groupe, sizeof(choix_groupe)) ; + wait_ms(500); + } + else if(TEST_C.Touched()) + { + while (TEST_A.Touched()); + TEST_C.Draw(BLEU, BLANC); + SendRawId(PAVILLON_DEPLOYE); + } + + } + break; + + case TEST_GIROUS : + SendRawId(LECTURE_GIROUETTE) ; + wait_ms(5); + canProcessRx(); + lcd.SetBackColor(LCD_COLOR_WHITE); + lcd.SetTextColor(LCD_COLOR_BLACK); + lcd.Clear (LCD_COLOR_WHITE); + lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Test Girouette", LEFT_MODE); + sprintf(toto,"%hd",val_girou); + lcd.DisplayStringAt(0, LINE(10), (unsigned char *)"Valeur Girou :", LEFT_MODE); + lcd.DisplayStringAt(0, LINE(11), (unsigned char *)toto, LEFT_MODE); + RETOUR.Draw(0xFFFF0000,0); + if(RETOUR.Touched()) { + while (RETOUR.Touched()); + strat_etat_s=DEMO2; + } + wait_ms(200); + break ; + case TEST_ASSERVE: lcd.SetBackColor(LCD_COLOR_WHITE); lcd.SetTextColor(LCD_COLOR_BLACK); @@ -231,20 +341,26 @@ TEST_B.Draw(BLEU, BLANC); TEST_C.Draw(BLEU, BLANC) ; - while(strat_etat_s==TEST_ASSERVE) { - if(RETOUR.Touched()) { + while(strat_etat_s==TEST_ASSERVE) + { + if(RETOUR.Touched()) + { while (RETOUR.Touched()); strat_etat_s=DEMO2; - } else if(TEST_A.Touched()) { + } + else if(TEST_A.Touched()) + { while (TEST_A.Touched()); - GoToPosition(2000,0,0,0); + GoStraight(2000,0,0,0); TEST_A.Draw(BLEU, BLANC); - } else if(TEST_B.Touched()) { + } + else if(TEST_B.Touched()) + { while (TEST_B.Touched()); - Rotate(3599); + Rotate(3600*5); TEST_B.Draw(BLEU, BLANC); } - else if(TEST_C.Touched()) + else if(TEST_C.Touched()) { while (TEST_C.Touched()); BendRadius(1000,900,1,0); @@ -868,18 +984,21 @@ break ; case 217 : choix_bras = 21 ; + GoStraight(-40, 0, 0, 0); SendMsgCan(BRAS_AT, &choix_bras,sizeof(choix_bras)); waitingAckFrom = 0; waitingAckID =0; break ; case 200 : choix_bras = 20 ; + GoStraight(-50, 0, 0, 0); SendMsgCan(BRAS_AT, &choix_bras,sizeof(choix_bras)); waitingAckFrom = 0; waitingAckID =0; break ; case 10 : choix_bras = 10 ; + GoStraight(-45, 0, 0, 0); SendMsgCan(BRAS_AT, &choix_bras,sizeof(choix_bras)); waitingAckFrom = 0; waitingAckID =0; @@ -1249,49 +1368,47 @@ lcd.DisplayStringAt(100, LINE(8), (unsigned char *)"Score :", LEFT_MODE); lcd.SetTextColor(LCD_COLOR_RED); lcd.DrawRect(0,400,400,320); - lcd.DisplayStringAt(100, LINE(22), (unsigned char *)"Timer :", LEFT_MODE); + lcd.DisplayStringAt(100, LINE(28), (unsigned char *)"Timer :", LEFT_MODE); unique = 1; } - int x_offsetc = -145*cos(theta_robot*M_PI/1800) - 0*sin(theta_robot*M_PI*1800); - int y_offsetc = -145*sin(theta_robot*M_PI/1800) + 0*cos(theta_robot*M_PI*1800); + lcd.SetTextColor(LCD_COLOR_BLACK); - lcd.DisplayStringAt(100, LINE(14), (unsigned char *)"x_robot :", LEFT_MODE); - sprintf(toto,"%04d",(short)(x_robot+x_offsetc)); - lcd.DisplayStringAt(250, LINE(14), (unsigned char *)toto, LEFT_MODE); - lcd.DisplayStringAt(100, LINE(15), (unsigned char *)"y_robot :", LEFT_MODE); - sprintf(toto,"%04d",(short)(y_robot+y_offsetc)); + lcd.DisplayStringAt(100, LINE(15), (unsigned char *)"girou :", LEFT_MODE); + sprintf(toto,"%02d",val_girou); lcd.DisplayStringAt(250, LINE(15), (unsigned char *)toto, LEFT_MODE); - lcd.DisplayStringAt(100, LINE(16), (unsigned char *)"t_robot :", LEFT_MODE); - sprintf(toto,"%04d",theta_robot); + lcd.DisplayStringAt(100, LINE(16), (unsigned char *)"debug :", LEFT_MODE); + sprintf(toto,"%02d",debug_bon_port); lcd.DisplayStringAt(250, LINE(16), (unsigned char *)toto, LEFT_MODE); - lcd.DisplayStringAt(100, LINE(17), (unsigned char *)"g_vert :", LEFT_MODE); - sprintf(toto,"%04d",gobelet_vert); - lcd.DisplayStringAt(250, LINE(17), (unsigned char *)toto, LEFT_MODE); - lcd.DisplayStringAt(100, LINE(18), (unsigned char *)"g_rouge :", LEFT_MODE); + lcd.DisplayStringAt(100, LINE(10), (unsigned char *)"g_vert :", LEFT_MODE); + sprintf(toto,"%04d",gobelet_vert); + lcd.DisplayStringAt(250, LINE(10), (unsigned char *)toto, LEFT_MODE); + + lcd.DisplayStringAt(100, LINE(11), (unsigned char *)"g_rouge :", LEFT_MODE); sprintf(toto,"%04d",gobelet_rouge); - lcd.DisplayStringAt(250, LINE(18), (unsigned char *)toto, LEFT_MODE); + lcd.DisplayStringAt(250, LINE(11), (unsigned char *)toto, LEFT_MODE); - lcd.DisplayStringAt(100, LINE(19), (unsigned char *)"g_port :", LEFT_MODE); + lcd.DisplayStringAt(100, LINE(12), (unsigned char *)"g_port :", LEFT_MODE); sprintf(toto,"%04d",gobelet_port); - lcd.DisplayStringAt(250, LINE(19), (unsigned char *)toto, LEFT_MODE); + lcd.DisplayStringAt(250, LINE(12), (unsigned char *)toto, LEFT_MODE); + actual_time = gameTimer.read(); - if(actual_time != old_time) - { +// if(actual_time != old_time) +// { lcd.SetTextColor(LCD_COLOR_BLUE); sprintf(toto,"%hd",score_final); lcd.DisplayStringAt(250, LINE(8), (unsigned char *)toto, LEFT_MODE); lcd.SetTextColor(LCD_COLOR_RED); sprintf(toto,"%hd",actual_time); - lcd.DisplayStringAt(250, LINE(22), (unsigned char *)toto, LEFT_MODE); - old_time = actual_time; - } + lcd.DisplayStringAt(250, LINE(28), (unsigned char *)toto, LEFT_MODE); +// old_time = actual_time; +// } break;
diff -r c37dbe2be916 -r 9d6a3ccc0582 Strategie/Strategie.cpp --- a/Strategie/Strategie.cpp Mon May 31 13:36:03 2021 +0000 +++ b/Strategie/Strategie.cpp Sat Jul 17 11:07:17 2021 +0000 @@ -14,7 +14,7 @@ Ticker chrono; Timeout AffTime; -Timer timer; +Ticker timer; Timer cartesCheker;//Le timer pour le timeout de la vérification des cartes Timer gameTimer; Timer debugetatTimer; @@ -25,6 +25,7 @@ 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}; +signed char asser_stop_direction=0; char counter = 0; char check; char Jack = 1; @@ -40,6 +41,8 @@ unsigned short flag_check_carte = 0, flag_strat = 0, flag_timer; int flagReceptionTelemetres = 0, flagNonRepriseErrorMot = 0; +int Flag_Bras_Re = 0, Flag_Manche_Bas = 0, Flag_Manche_Moy = 0, Flag_pavillon = 0, Flag_bon_port = 0; //Flag utilisé pour compter le score des bras / manches / pavillon +unsigned short Flag_num_bras; signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN @@ -49,7 +52,7 @@ //unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN int flagSendCan=1; unsigned char Cote = 0; //0 -> JAUNE | 1 -> VIOLET -unsigned char Hauteur = 0; +unsigned char Hauteur = 0; //Robot du haut -> 1, du bas -> 0 unsigned short angleRecalage = 0; unsigned char checkCurrent = 0; unsigned char countAliveCard = 0; @@ -63,6 +66,9 @@ unsigned char ingnorBaliseOnce = 0;//une fois détecté réinitialise unsigned char ingnorBalise = 0;//0:balise ignore 1:on ecoute la balise short direction; +char val_girou ; + +unsigned char debug_bon_port = 0; unsigned char ingnorInversionOnce = 0;//Pour ignorer l'inversion des instruction une fois @@ -138,7 +144,23 @@ } } +void wait_ISR(void) +{ + actual_instruction = instruction.nextLineOK; + gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; + timer.detach(); +} +void girouette_ISR(void) +{ + actual_instruction = instruction.nextLineOK; + gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION; +} + +void isr_end_danger(void) +{ + balise_end_danger(&instruction,&dodgeq,target_x_robot,target_y_robot,target_theta_robot, theta_robot,x_robot,y_robot); +} /****************************************************************************************/ /* FUNCTION NAME: Strategie */ @@ -151,10 +173,21 @@ signed char localData1 = 0; signed short localData2 = 0; unsigned short localData3 = 0; - //signed short localData4 = 0; + unsigned short localData4 = 0; unsigned char localData5 = 0; + int unique = 0 ; - + if(gameTimer.read_ms() >= 95000) + { + int unique = 0 ; + if (unique == 0) + { + SendRawId(PAVILLON_DEPLOYE) ; + Flag_pavillon=1; + unique = 1; + } + } + if(gameTimer.read_ms() >= 99000) {//Fin du match (On autorise 2s pour déposer des éléments gameTimer.stop(); gameTimer.reset(); @@ -366,30 +399,82 @@ 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; + if(Hauteur == 0) + { + 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; } - - 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; + else if(Hauteur == 1) + { + localData1 = 1; + + if(InversStrat == 1 && ingnorInversionOnce == 0) { + localData2 = -instruction.arg3; + localData3 = 3000 - 600;//Inversion du Y + } else { + localData3 = 600; + 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(); + + 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; + target_x_robot=x_robot; + target_y_robot=y_robot; + target_theta_robot=theta_robot; break; } } @@ -443,13 +528,13 @@ Traitement de l'instruction, envoie de la trame CAN */ //debug_Instruction(instruction); - actionPrecedente = instruction.order; - switch(instruction.order) { + switch(instruction.order) + { case MV_COURBURE://C'est un rayon de courbure - Debug_Audio(3,6); +// Debug_Audio(3,6); float alpha=0, theta=0; - unsigned short alph=0; + short alph=0; actionPrecedente = MV_COURBURE; waitingAckID = ASSERVISSEMENT_COURBURE; waitingAckFrom = ACKNOWLEDGE_MOTEUR; @@ -474,134 +559,58 @@ /*if(InversStrat == 1 && ingnorInversionOnce == 0) { localData1 = -localData1;//Inversion de la direction }*/ - + enum E_InstructionDirection directionxyt; BendRadius(instruction.arg1, localData2, localData1, localData5); + alph=localData2; if(localData2>0) { direction=1; } else { direction=-1; } - if(localData2>0)alph=localData2; - else alph=-localData2; + if(localData2>0) + { + directionxyt=FORWARD; + asser_stop_direction=1; + } + else + { + directionxyt=BACKWARD; + asser_stop_direction=-1; + } 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; - } + int nbre; + nbre=abs(alph)/450; + for(int c=0;c<nbre+1;c++) + { + dodgeq.inst[c].order = MV_XYT; + dodgeq.inst[c].direction=directionxyt; + if(instruction.direction == LEFT) //-------------LEFT + { + target_x_robot = x_robot + instruction.arg1*(sin(theta+alpha)-sin(theta));// X + target_y_robot = y_robot + instruction.arg1*(cos(theta)-cos(theta+alpha));// Y + target_theta_robot = theta_robot + alph;// T + dodgeq.inst[c].arg1 = target_x_robot;// X + dodgeq.inst[c].arg2 = target_y_robot;// Y + dodgeq.inst[c].arg3 = target_theta_robot;// T } - 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 + { + target_x_robot = x_robot + instruction.arg1*(sin(theta)-sin(theta-alpha));// X + target_y_robot = y_robot + instruction.arg1*(cos(theta-alpha)-cos(theta));// Y + target_theta_robot = theta_robot - alph;// T + dodgeq.inst[c].arg1 = target_x_robot;// X + dodgeq.inst[c].arg2 = target_y_robot;// Y + dodgeq.inst[c].arg3 = target_theta_robot;// T } - 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; - } - } + alpha-=alpha/((float)nbre); + alph-=alph/(nbre+1); } break; case MV_LINE://Ligne droite - Debug_Audio(3,8); waitingAckID = ASSERVISSEMENT_RECALAGE; waitingAckFrom = ACKNOWLEDGE_MOTEUR; if(instruction.nextActionType == ENCHAINEMENT) { @@ -616,6 +625,10 @@ } } localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1); + if(instruction.direction == FORWARD) + asser_stop_direction=1; + else + asser_stop_direction=-1; GoStraight(localData2, 0, 0, localData5); target_x_robot = x_robot + localData2*cos((double)theta_robot*M_PI/1800); @@ -624,7 +637,6 @@ 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; @@ -650,12 +662,61 @@ Rotate(localData2); break; + case MV_XYT: + + if(Flag_bon_port ==1) //XYT pour la rentrée dans le port + { Debug_Audio(3,10); if(instruction.direction == BACKWARD) { localData1 = -1; + asser_stop_direction=-1; } else { localData1 = 1; + asser_stop_direction=1; + } + + if(val_girou == 0) //Si c'est Sud on déplace le X + { + localData4 = 1300; + } + else + { + localData4 = instruction.arg1; + } + + if(InversStrat == 1 && ingnorInversionOnce == 0) + { + localData2 = -instruction.arg3; + localData3 = 3000 - instruction.arg2;//Inversion du Y + } + else + { + localData3 = instruction.arg2; + localData2 = instruction.arg3; + } + if((instruction.arg1<=0)||(localData3<=0)) + GoToPosition(target_x_robot,target_y_robot,target_theta_robot,localData1); + else + GoToPosition(localData4,localData3,localData2,localData1); + waitingAckID = ASSERVISSEMENT_XYT; + waitingAckFrom = ACKNOWLEDGE_MOTEUR; + + target_x_robot = instruction.arg1; + target_y_robot = localData3; + target_theta_robot = localData2; + + Flag_bon_port = 0; + } + else //Sinon on effectue XYT normalement selon les instructions + { + Debug_Audio(3,10); + if(instruction.direction == BACKWARD) { + localData1 = -1; + asser_stop_direction=-1; + } else { + localData1 = 1; + asser_stop_direction=1; } if(InversStrat == 1 && ingnorInversionOnce == 0) { @@ -665,65 +726,134 @@ localData3 = instruction.arg2; localData2 = instruction.arg3; } - - GoToPosition(instruction.arg1,localData3,localData2,localData1); + if((instruction.arg1<=0)||(localData3<=0)) + GoToPosition(target_x_robot,target_y_robot,target_theta_robot,localData1); + else + 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; + } + break; + case MV_RECALAGE: - if(instruction.nextActionType == MECANIQUE) { - instruction.nextActionType = WAIT; - - waitingAckID = ASSERVISSEMENT_RECALAGE; - waitingAckFrom = ACKNOWLEDGE_MOTEUR; - - localData2 = (((instruction.direction == FORWARD)?1:-1)*1000);//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 { + //Effectuer un recalage si on a aucun gobelet ou au moins 2 + /* + if( ((instruction.direction == FORWARD)&&(etat_groupe[0]==5&&etat_groupe[1]==5) || (etat_groupe[0]==5&&etat_groupe[2]==5) || (etat_groupe[1]==5&&etat_groupe[2]==5)) || + ((instruction.direction == BACKWARD)&&(etat_groupe[3]==5&&etat_groupe[4]==5) || (etat_groupe[3]==5&&etat_groupe[5]==5) || (etat_groupe[4]==5&&etat_groupe[5]==5)) || + (etat_groupe[0]!=5 && etat_groupe[1]!=5 && etat_groupe[2]!=5 && etat_groupe[3]!=5 && etat_groupe[4]!=5 && etat_groupe[5]!=5)) + { + */ + if(instruction.nextActionType == MECANIQUE) + { + instruction.nextActionType = WAIT; + waitingAckID = ASSERVISSEMENT_RECALAGE; + waitingAckFrom = ACKNOWLEDGE_MOTEUR; + localData2 = (((instruction.direction == FORWARD)?1:-1)*1000);//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; } - } 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() ); + } } - 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() ); + /*} + //Ou effectuer une ligne droite jusqu'à la nouvelle valeur de recalage + + else + { + Debug_Audio(3,8); + instruction.nextActionType = WAIT; + waitingAckID = ASSERVISSEMENT_RECALAGE; + waitingAckFrom = ACKNOWLEDGE_MOTEUR; + + //Calcul de la distance à parcourir pour se retrouver au même point que si on avait fait un recalage + localData2 = instruction.arg1; + if(instruction.precision == RECALAGE_Y) + { + if(localData2 == 2832) localData2 = 2832 - y_robot; //Gob+ en y + else if(localData2 == 168) localData2 = y_robot - 168; //Gob- en y } + else if(instruction.precision == RECALAGE_X) + { + if(localData2 == 1832) localData2 = 1832 - x_robot; //Gob+ en x + else if(localData2 == 168) localData2 = x_robot - 168; //Gob- en x + } + localData2 = ((instruction.direction == FORWARD)?1:-1)*localData2; + GoStraight(localData2, 0, 0, 0);//x,x,x,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 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 + if(InversStrat==1 && ingnorInversionOnce == 0) + { + if((instruction.arg1 >= 151 && instruction.arg1 <= 154) || (instruction.arg1 >= 163 && instruction.arg1 <= 164)) + { + if(instruction.arg2 == 0) instruction.arg2 = 2; + else if(instruction.arg2 == 2) instruction.arg2 = 0; + else if(instruction.arg2 == 10) instruction.arg2 = 21; + else if(instruction.arg2 == 21) instruction.arg2 = 10; + else if(instruction.arg2 == 3) instruction.arg2 = 5; + else if(instruction.arg2 == 5) instruction.arg2 = 3; + else if(instruction.arg2 == 43) instruction.arg2 = 54; + else if(instruction.arg2 == 54) instruction.arg2 = 43; + } + else if(instruction.arg1 >= 157 && instruction.arg1 <= 159) + { + if(instruction.arg2 == 0) instruction.arg2 = 1; + else if(instruction.arg2 == 1) instruction.arg2 = 0; + } + } tempo = doAction(instruction.arg1,instruction.arg2,instruction.arg3); // unsigned char test=(unsigned char) tempo; // SendMsgCan(0x5BD, &test,1); @@ -751,7 +881,10 @@ //AX12_enchainement++; } - break; + /*target_x_robot=x_robot; + target_y_robot=y_robot; + target_theta_robot=theta_robot; + */break; default: //Instruction inconnue, on l'ignore break; @@ -779,18 +912,25 @@ 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(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) { + 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 + } + 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 + } + 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); @@ -827,17 +967,23 @@ break; case ACTION: - if (modeTelemetre == 0) { - if (telemetreDistance == 0) { + 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) { + } + 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 + } + else // si on attend la reponse du telemetre + { //modeTelemetre = 1; waitingAckID_FIN = OBJET_SUR_TABLE; waitingAckFrom_FIN = 0; @@ -846,16 +992,23 @@ default: break; } - } else { + } + 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) { + } + 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 + 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 { + } + else + { gameEtat = ETAT_GAME_PROCESS_INSTRUCTION;//On retourne dans l'strat_etat_s d'envois de l'instruction } } @@ -1102,7 +1255,7 @@ case BALISE_DANGER : SendAck(ACKNOWLEDGE_BALISE, BALISE_END_DANGER); - balise_danger(); + balise_danger(FIFO_lecture); break; case BALISE_STOP: @@ -1181,6 +1334,13 @@ } break; + + case VALEUR_GIROUETTE : + val_girou = msgRxBuffer[FIFO_lecture].data[0] ; + girouette_ISR(); + + break ; + default: break; /* @@ -1191,6 +1351,11 @@ // waitingAckFrom_FIN=0; SendRawId(0x40); break;*/ + + + + + } FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO; } @@ -1619,7 +1784,7 @@ waitingAckID = 0; break; - case 151: + case 151: //Bras attraper unsigned char argu_at_bras = arg1; if(arg1 == 543) argu_at_bras = 66; SendMsgCan(BRAS_AT, &argu_at_bras,sizeof(arg1)); @@ -1627,43 +1792,18 @@ waitingAckID =0; break; - case 152: + case 152: //Bras relacher unsigned char argu_re_bras = arg1; if(arg1 == 543) argu_re_bras = 66; SendMsgCan(BRAS_RE, &argu_re_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)); + SendRawId(0x050); + Flag_Bras_Re = 1; + Flag_num_bras = argu_re_bras; 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 153: + case 153: //Ventouse attraper unsigned char argu_at_vent = arg1; if(arg1 == 543) argu_at_vent = 66; SendMsgCan(VENT_AT, &argu_at_vent,sizeof(arg1)); @@ -1671,7 +1811,7 @@ waitingAckID =0; break; - case 154: + case 154: //Ventouse Relacher unsigned char argu_re_vent = arg1; if(arg1 == 543) argu_re_vent = 66; SendMsgCan(VENT_RE, &argu_re_vent,sizeof(arg1)); @@ -1679,20 +1819,66 @@ waitingAckID =0; break; - case 157: + case 157: //Manche à air bas unsigned char argu_manche_bas = arg1; SendMsgCan(AUTOMATE_MANCHE_BAS, &argu_manche_bas,sizeof(arg1)); + Flag_Manche_Bas = 1; + Flag_Manche_Moy = 1; waitingAckFrom = 0; waitingAckID =0; break; - case 158: + case 158: //Manche à air moyen + unsigned char argu_manche_moy = arg1; + SendMsgCan(AUTOMATE_MANCHE_MOY,&argu_manche_moy,sizeof(arg1)); + Flag_Manche_Bas = 1; + Flag_Manche_Moy = 1; + waitingAckFrom = 0; + waitingAckID =0; + break; + + case 159: //Manche à air haut unsigned char argu_manche_haut = arg1; SendMsgCan(AUTOMATE_MANCHE_HAUT, &argu_manche_haut,sizeof(arg1)); waitingAckFrom = 0; waitingAckID =0; break; + case 160 : //lecture de la girouette + SendRawId(LECTURE_GIROUETTE) ; + debug_bon_port = 1; + wait_ms(100); + break; + + case 161 : //aller dans le bon port de fin de match + Flag_bon_port = 1; + waitingAckFrom = 0; + waitingAckID =0; + break ; + + case 162 : //attendre le temps indiqué en seconde + debug_bon_port = 2; + timer.attach(&wait_ISR, arg1); + break ; + + case 163 : //bras prépare à poser + unsigned char argu_prepa_bras = arg1; + if(arg1 == 543) argu_prepa_bras = 66; + SendMsgCan(BRAS_PREPA, &argu_prepa_bras,sizeof(arg1)); + waitingAckFrom = 0; + waitingAckID =0; + break ; + + case 164 : //bras pose les gobelets en préparation + unsigned char argu_pose_bras = arg1; + if(arg1 == 543) argu_pose_bras = 66; + SendMsgCan(BRAS_POSE, &argu_pose_bras,sizeof(arg1)); + Flag_Bras_Re = 1; + Flag_num_bras = argu_re_bras; + waitingAckFrom = 0; + waitingAckID =0; + break ; + default: retour = 0;//L'action n'existe pas, il faut utiliser le CAN break;
diff -r c37dbe2be916 -r 9d6a3ccc0582 Strategie/Strategie.h --- a/Strategie/Strategie.h Mon May 31 13:36:03 2021 +0000 +++ b/Strategie/Strategie.h Sat Jul 17 11:07:17 2021 +0000 @@ -26,6 +26,8 @@ TEST_AUD, TEST_DIVE, TEST_ASSERVE, + TEST_MANCHES, + TEST_GIROUS, SELECT_SIDE, SELECT_ROB, TACTIQUE, @@ -46,7 +48,7 @@ ETAT_CONFIG, //attente reception du choix du mode( debug ou game) ETAT_GAME_INIT,//Mise en mémoire du fichier de stratégie ETAT_GAME_WAIT_FOR_JACK,//Attente du retrait du jack - ETAT_GAME_START,//Lancement du timer 90s + ETAT_GAME_START,//Lancement du timer 100s ETAT_GAME_LOAD_NEXT_INSTRUCTION, ETAT_GAME_PROCESS_INSTRUCTION, ETAT_GAME_WAIT_ACK, @@ -60,9 +62,21 @@ extern E_stratGameEtat gameEtat; extern T_etat strat_etat_s; extern Timer gameTimer; + +extern char val_girou ; + +extern int Flag_Bras_Re; +extern int Flag_Manche_Bas; +extern int Flag_pavillon; +extern int Flag_Manche_Moy; +extern int Flag_bon_port; +extern unsigned short Flag_num_bras; +extern unsigned char debug_bon_port; + void canProcessRx(void); void Strategie(void); short recalageAngulaireCapteur(void); short recalageDistanceX(void); short recalageDistanceY(void); +void isr_end_danger(void); #endif
diff -r c37dbe2be916 -r 9d6a3ccc0582 main.cpp --- a/main.cpp Mon May 31 13:36:03 2021 +0000 +++ b/main.cpp Sat Jul 17 11:07:17 2021 +0000 @@ -65,8 +65,8 @@ while(true) { automate_etat_ihm(); Strategie();//Boucle dans l'automate principal - gestion_Message_CAN(); - canProcessRx(); + gestion_Message_CAN(); + canProcessRx(); } } void bluetooth_init(void){