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 35:2a745eeb7922, committed 2021-05-21
- Comitter:
- goldmas
- Date:
- Fri May 21 16:19:18 2021 +0000
- Parent:
- 34:c314feaa0d3e
- Child:
- 36:c37dbe2be916
- Commit message:
- comptage point marche avant ok mais pas marche arriere
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Compteur_points/Compteur.cpp Fri May 21 16:19:18 2021 +0000
@@ -0,0 +1,160 @@
+#include "global.h"
+
+#define M_PI 3.14159265358979323846
+#define DEMI_GOBELET 30
+
+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];
+
+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 etat_groupe, 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};
+
+/********************************************************************************************************************/
+/* FUNCTION NAME: gestion_Message_CAN */
+/* DESCRIPTION : Récupère message CAN et appelle la fonction correspondante à l'action rapportant des points */
+/********************************************************************************************************************/
+void gestion_Message_CAN(void)
+{
+ int identifiant = msgRxBuffer[FIFO_ecriture].id;
+
+ switch(identifiant)
+ {
+ case 0x220:
+ for(int num_vent=0;num_vent<6;num_vent++)
+ {
+ switch(VentEtat[num_vent])
+ {
+ 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;
+ }
+ }
+ 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;
+ }
+}
+
+void compteur_de_points(void)
+{
+ int paire_ventouse = abs(gobelet_vert - gobelet_rouge);
+
+ //Points gobelets
+ score_ventouse = (gobelet_vert*2 + gobelet_rouge*2)*2 - (paire_ventouse)*2 + gobelet_port;
+
+ //Points manche à air
+ if(manche_releve == 1) score_manche = 5;
+ else if(manche_releve == 2) score_manche = 15;
+
+ //Points phare
+
+ //Points pavillons
+
+ //Points d'arrivée à bon port
+
+ //Points totaux
+ score_final = score_ventouse; //+ score_manche + score_phare + score_pavillon + score_bon_port;
+}
+
+
+
+
+/********************************************************************************************************************/
+/* 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);
+
+ //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++;
+ //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++;
+ //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++;
+
+ //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++;
+ //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++;
+
+
+ //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++;
+ //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++;
+ //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++;
+
+ //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++;
+ //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++;
+}
+
+void verif_position_bras(int num_groupe)
+{
+
+}
+
+void verif_position_manche(int num_groupe)
+{
+
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Compteur_points/Compteur.h Fri May 21 16:19:18 2021 +0000
@@ -0,0 +1,31 @@
+#ifndef CRAC_COMPTEUR
+#define CRAC_COMPTEUR
+
+void gestion_Message_CAN(void);
+
+void recup_position(void);
+
+void compteur_de_points(void);
+
+void verif_position_ventouse(int num_groupe);
+
+void verif_position_bras(int num_groupe);
+
+void verif_position_manche(int num_groupe);
+
+extern int score_final;
+extern int gobelet_vert;
+extern int gobelet_rouge;
+extern int gobelet_port;
+
+typedef enum
+{
+ ETAT_ATTENDRE,
+ ETAT_RECUP,
+ ETAT_COMPTER
+} CompteurGameEtat;
+
+extern CompteurGameEtat VentEtat[6];
+
+
+#endif
\ No newline at end of file
--- a/Globals/global.h Mon May 17 07:19:15 2021 +0000 +++ b/Globals/global.h Fri May 21 16:19:18 2021 +0000 @@ -23,6 +23,7 @@ #include "F469_GUI.hpp" #include "Evitement.h" #include "ihm.h" +#include "Compteur.h" extern Serial pc;
--- a/IHM/DISCO-F469NI_portrait.lib Mon May 17 07:19:15 2021 +0000 +++ b/IHM/DISCO-F469NI_portrait.lib Fri May 21 16:19:18 2021 +0000 @@ -1,1 +1,1 @@ -http://os.mbed.com/teams/CRAC-Team/code/DISCO-F469NI_portrait/#d84fc295915e +http://os.mbed.com/teams/CRAC-Team/code/DISCO-F469NI_portrait/#74d204a4f5cd
--- a/IHM/ihm.cpp Mon May 17 07:19:15 2021 +0000
+++ b/IHM/ihm.cpp Fri May 21 16:19:18 2021 +0000
@@ -1,5 +1,7 @@
#include "global.h"
+#define M_PI 3.14159265358979323846
+
#define VERT 0xFF00FF00
#define ROUGE 0xFFFF0000
#define BLEU 0xFF0000FF
@@ -10,6 +12,7 @@
#define DIY_GREY 0xFFDFDFDF
#define VIOLET 0xFF4527A0
#define VERT_F 0xFF00C400
+#define VIOLET_F 0x788000FF
TS_DISCO_F469NI ts;
LCD_DISCO_F469NI lcd;
@@ -78,7 +81,7 @@
void automate_etat_ihm(void)
{
char toto[12];
- int j;
+ int j, old_time=0, actual_time=0, old_score=0, actual_score=0, unique=0;
unsigned char maximilien=1, choix_groupe;
if (j==0) {
ts.Init(lcd.GetXSize(), lcd.GetYSize());
@@ -220,9 +223,12 @@
lcd.SetTextColor(LCD_COLOR_BLACK);
lcd.Clear (LCD_COLOR_WHITE);
lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Asservissements", LEFT_MODE);
+
RETOUR.Draw(0xFFFF0000,0);
TEST_A.Draw(BLEU, BLANC);
- TEST_B.Draw(BLEU, BLANC);
+ TEST_B.Draw(BLEU, BLANC);
+ TEST_C.Draw(BLEU, BLANC) ;
+
while(strat_etat_s==TEST_ASSERVE) {
if(RETOUR.Touched()) {
while (RETOUR.Touched());
@@ -236,6 +242,12 @@
Rotate(3599);
TEST_B.Draw(BLEU, BLANC);
}
+ else if(TEST_C.Touched())
+ {
+ while (TEST_C.Touched());
+ BendRadius(1000,900,1,0);
+ TEST_B.Draw(BLEU, BLANC);
+ }
}
break;
@@ -247,7 +259,6 @@
lcd.SetTextColor(LCD_COLOR_BLACK);
lcd.Clear (LCD_COLOR_WHITE);
lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"Coordonnees", LEFT_MODE);
- RETOUR.Draw(0xFFFF0000,0);
sprintf(toto,"%hd",x_robot);
lcd.DisplayStringAt(0, LINE(10), (unsigned char *)"X :", LEFT_MODE);
lcd.DisplayStringAt(50, LINE(10), (unsigned char *)toto, LEFT_MODE);
@@ -257,6 +268,7 @@
sprintf(toto,"%hd",theta_robot);
lcd.DisplayStringAt(0, LINE(12), (unsigned char *)"T :", LEFT_MODE);
lcd.DisplayStringAt(50, LINE(12), (unsigned char *) toto, LEFT_MODE);
+ RETOUR.Draw(0xFFFF0000,0);
if(RETOUR.Touched()) {
while (RETOUR.Touched());
strat_etat_s=DEMO2;
@@ -1171,11 +1183,11 @@
lcd.SetTextColor(LCD_COLOR_BLACK);
if (Cote == 0) {
- lcd.Clear(VERT);
- lcd.SetBackColor(VERT);
+ lcd.Clear(LCD_COLOR_WHITE);
+ lcd.SetBackColor(LCD_COLOR_WHITE);
} else if (Cote == 1) {
- lcd.Clear(ORANGE);
- lcd.SetBackColor(ORANGE);
+ lcd.Clear(LCD_COLOR_WHITE);
+ lcd.SetBackColor(LCD_COLOR_WHITE);
} else {
lcd.Clear(VERT);
lcd.SetBackColor(VERT);
@@ -1183,20 +1195,71 @@
canProcessRx();
lcd.DisplayStringAt(0, LINE(0), (uint8_t *)"En attente du Jack", CENTER_MODE);
strat_etat_s=WAIT_JACK;
+
break;
- case WAIT_JACK: //VERITABLE ATTENTE DU JACK
+ case WAIT_JACK :
+ if(unique == 0)
+ {
+ lcd.SetTextColor(LCD_COLOR_BLUE);
+ lcd.DrawRect(0,50,400,320);
+ 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);
+ 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(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(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);
+ sprintf(toto,"%04d",gobelet_rouge);
+ lcd.DisplayStringAt(250, LINE(18), (unsigned char *)toto, LEFT_MODE);
+
+ lcd.DisplayStringAt(100, LINE(19), (unsigned char *)"g_port :", LEFT_MODE);
+ sprintf(toto,"%04d",gobelet_port);
+ lcd.DisplayStringAt(250, LINE(19), (unsigned char *)toto, LEFT_MODE);
+
+ actual_time = gameTimer.read();
+ 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;
+ }
break;
-
+
+
case FIN : //AFFICHAGE DE FIN AVEC LE SCORE FINAL
- lcd.Clear (LCD_COLOR_WHITE);
- lcd.SetBackColor(LCD_COLOR_WHITE);
+// lcd.Clear(LCD_COLOR_WHITE);
+// lcd.SetBackColor(LCD_COLOR_WHITE);
while(1); // force le redemarage du robot
//break;
-
- }
+ }
}
/****************************************************************************************/
/* FUNCTION NAME: Bouton_Strat */
--- a/IHM/ihm.h Mon May 17 07:19:15 2021 +0000 +++ b/IHM/ihm.h Fri May 21 16:19:18 2021 +0000 @@ -3,4 +3,5 @@ void automate_etat_ihm(void); void DrawCheck (int l,int h,int x, int y, int cochage); + #endif \ No newline at end of file
--- a/Strategie/Strategie.cpp Mon May 17 07:19:15 2021 +0000
+++ b/Strategie/Strategie.cpp Fri May 21 16:19:18 2021 +0000
@@ -101,7 +101,7 @@
unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR,CHECK_BALISE};
unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR,ALIVE_BALISE};
-InterruptIn jack(PG_11); // entrée numerique en interruption pour le jack
+InterruptIn jack(PG_11); // entrée numerique en interruption pour le jack (JackB1 sur la carte esclave)
@@ -1613,7 +1613,7 @@
case 151:
unsigned char argu_at_bras = arg1;
- if(argu_at_bras == 543) argu_at_bras = 66;
+ if(arg1 == 543) argu_at_bras = 66;
SendMsgCan(BRAS_AT, &argu_at_bras,sizeof(arg1));
waitingAckFrom = 0;
waitingAckID =0;
@@ -1621,14 +1621,43 @@
case 152:
unsigned char argu_re_bras = arg1;
- if(argu_re_bras == 543) argu_re_bras = 66;
+ 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));
+ 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:
unsigned char argu_at_vent = arg1;
+ if(arg1 == 543) argu_at_vent = 66;
SendMsgCan(VENT_AT, &argu_at_vent,sizeof(arg1));
waitingAckFrom = 0;
waitingAckID =0;
@@ -1636,23 +1665,25 @@
case 154:
unsigned char argu_re_vent = arg1;
+ if(arg1 == 543) argu_re_vent = 66;
SendMsgCan(VENT_RE, &argu_re_vent,sizeof(arg1));
- wait_ms(500);
waitingAckFrom = 0;
waitingAckID =0;
break;
- case 157 :
+ case 157:
+ unsigned char argu_manche_bas = arg1;
+ SendMsgCan(AUTOMATE_MANCHE_BAS, &argu_manche_bas,sizeof(arg1));
+ waitingAckFrom = 0;
+ waitingAckID =0;
+ break;
+
+ case 158:
unsigned char argu_manche_haut = arg1;
SendMsgCan(AUTOMATE_MANCHE_HAUT, &argu_manche_haut,sizeof(arg1));
waitingAckFrom = 0;
waitingAckID =0;
-
- case 158 :
- unsigned char argu_manche_bas = arg1;
- SendMsgCan(AUTOMATE_MANCHE_BAS, &argu_manche_bas,sizeof(arg1));
- waitingAckFrom = 0;
- waitingAckID =0;
+ break;
default:
retour = 0;//L'action n'existe pas, il faut utiliser le CAN
--- a/Strategie/Strategie.h Mon May 17 07:19:15 2021 +0000 +++ b/Strategie/Strategie.h Fri May 21 16:19:18 2021 +0000 @@ -58,6 +58,7 @@ } E_stratGameEtat; extern E_stratGameEtat gameEtat; extern T_etat strat_etat_s; +extern Timer gameTimer; void canProcessRx(void); void Strategie(void); short recalageAngulaireCapteur(void);
--- a/main.cpp Mon May 17 07:19:15 2021 +0000
+++ b/main.cpp Fri May 21 16:19:18 2021 +0000
@@ -56,6 +56,7 @@
can2.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
can2.frequency(1000000);
bluetooth_init();
+ SendRawId(ALIVE_IHM);
lcd.DisplayStringAt(0, 0,(uint8_t *)"Initialisation", LEFT_MODE);
led1 = 1;
lecture_fichier(); //bloquant si pas de carte SD
@@ -64,6 +65,7 @@
while(true) {
automate_etat_ihm();
Strategie();//Boucle dans l'automate principal
+ gestion_Message_CAN();
canProcessRx();
}
}