CRAC Team
code avec modifs, programme mit dans les robots pour les derniers matchs
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){