CRAC Team / Mbed 2 deprecated code_carte_IHM_avec_modifs

code avec modifs, programme mit dans les robots pour les derniers matchs

Dependencies:   mbed SerialHalfDuplex SDFileSystem liaison_Bluetooth ident_crac DISCO-F469NI_portrait

Compteur_points/Compteur.cpp@37:ee2c72e76d9c, 2021-07-17 (annotated)

Committer:
goldmas
Date:
Sat Jul 17 16:19:46 2021 +0000
Revision:
37:ee2c72e76d9c
Parent:
36:c37dbe2be916 
ouais

Who changed what in which revision?

UserRevisionLine numberNew contents of line
goldmas 35:2a745eeb7922 1 #include "global.h"
goldmas 35:2a745eeb7922 2
goldmas 37:ee2c72e76d9c 3 #define M_PI 3.14159265358979323846f
goldmas 37:ee2c72e76d9c 4 #define MARGE_VENT 72.0f
goldmas 37:ee2c72e76d9c 5 #define MARGE_BRAS 54.0f
goldmas 37:ee2c72e76d9c 6 #define ECART 100.0f
goldmas 35:2a745eeb7922 7
goldmas 37:ee2c72e76d9c 8 float Ventouse_coord[6][2]={{150,-80},{150,0},{150,80},{-150,80},{-150,0},{-150,-80}};
goldmas 37:ee2c72e76d9c 9 float Bras_coord[6][2]={{232,-75},{232,0},{232,75},{-232,75},{-232,0},{-232,-75}};
goldmas 37:ee2c72e76d9c 10
goldmas 37:ee2c72e76d9c 11 float Position_de_pose[6][2];
goldmas 35:2a745eeb7922 12
goldmas 35:2a745eeb7922 13 int score_final=0;
goldmas 35:2a745eeb7922 14 int score_ventouse=0,gobelet_vert=0,gobelet_rouge=0,gobelet_port=0;
goldmas 35:2a745eeb7922 15 int score_manche=0,manche_releve=0;
goldmas 37:ee2c72e76d9c 16 int score_bon_port=0, port_lu = 0;
goldmas 37:ee2c72e76d9c 17 int score_phare=0, phare_active=0;
goldmas 37:ee2c72e76d9c 18 int score_pavillon=0, pavillon=0;
goldmas 37:ee2c72e76d9c 19 int offset_score=0;
goldmas 35:2a745eeb7922 20
goldmas 37:ee2c72e76d9c 21 int etat_groupe[6], gobelet_en_place=0, old_gobelet=0;
goldmas 35:2a745eeb7922 22 unsigned char num_groupe;
goldmas 35:2a745eeb7922 23
goldmas 35:2a745eeb7922 24 CompteurGameEtat VentEtat[6] = {ETAT_ATTENDRE, ETAT_ATTENDRE, ETAT_ATTENDRE, ETAT_ATTENDRE, ETAT_ATTENDRE, ETAT_ATTENDRE};
goldmas 37:ee2c72e76d9c 25
goldmas 37:ee2c72e76d9c 26 int old_gobelet_vert=0;
goldmas 37:ee2c72e76d9c 27 int old_gobelet_rouge=0;
goldmas 37:ee2c72e76d9c 28 int old_gobelet_port=0;
goldmas 35:2a745eeb7922 29
goldmas 35:2a745eeb7922 30 /********************************************************************************************************************/
goldmas 35:2a745eeb7922 31 /* FUNCTION NAME: gestion_Message_CAN */
goldmas 35:2a745eeb7922 32 /* DESCRIPTION : Récupère message CAN et appelle la fonction correspondante à l'action rapportant des points */
goldmas 35:2a745eeb7922 33 /********************************************************************************************************************/
goldmas 35:2a745eeb7922 34 void gestion_Message_CAN(void)
goldmas 35:2a745eeb7922 35 {
goldmas 35:2a745eeb7922 36 int identifiant = msgRxBuffer[FIFO_ecriture].id;
goldmas 35:2a745eeb7922 37
goldmas 37:ee2c72e76d9c 38 if(identifiant==0x220)
goldmas 35:2a745eeb7922 39 {
goldmas 37:ee2c72e76d9c 40 for(int num_vent=0;num_vent<6;num_vent++)
goldmas 37:ee2c72e76d9c 41 {
goldmas 37:ee2c72e76d9c 42 switch(VentEtat[num_vent])
goldmas 37:ee2c72e76d9c 43 {
goldmas 37:ee2c72e76d9c 44 case ETAT_ATTENDRE:
goldmas 37:ee2c72e76d9c 45 etat_groupe[num_vent] = msgRxBuffer[FIFO_ecriture].data[num_vent];
goldmas 37:ee2c72e76d9c 46 if(etat_groupe[num_vent] == 5) //Pompe et capteur
goldmas 37:ee2c72e76d9c 47 {
goldmas 37:ee2c72e76d9c 48 VentEtat[num_vent] = ETAT_RECUP;
goldmas 37:ee2c72e76d9c 49 }
goldmas 37:ee2c72e76d9c 50 break;
goldmas 37:ee2c72e76d9c 51
goldmas 37:ee2c72e76d9c 52 case ETAT_RECUP:
goldmas 37:ee2c72e76d9c 53 etat_groupe[num_vent] = msgRxBuffer[FIFO_ecriture].data[num_vent];
goldmas 37:ee2c72e76d9c 54 if(etat_groupe[num_vent] == 2) //Electrovane
goldmas 35:2a745eeb7922 55 {
goldmas 37:ee2c72e76d9c 56 VentEtat[num_vent] = ETAT_COMPTER;
goldmas 35:2a745eeb7922 57 }
goldmas 37:ee2c72e76d9c 58 break;
goldmas 35:2a745eeb7922 59
goldmas 37:ee2c72e76d9c 60 case ETAT_COMPTER:
goldmas 37:ee2c72e76d9c 61 verif_position_ventouse(num_vent);
goldmas 37:ee2c72e76d9c 62 compteur_de_points();
goldmas 37:ee2c72e76d9c 63 VentEtat[num_vent] = ETAT_ATTENDRE;
goldmas 37:ee2c72e76d9c 64 break;
goldmas 37:ee2c72e76d9c 65 }
goldmas 37:ee2c72e76d9c 66 }
goldmas 37:ee2c72e76d9c 67 }
goldmas 37:ee2c72e76d9c 68
goldmas 37:ee2c72e76d9c 69 if(Flag_Bras_Re == 1)
goldmas 37:ee2c72e76d9c 70 {
goldmas 37:ee2c72e76d9c 71 num_groupe = Flag_num_bras;
goldmas 37:ee2c72e76d9c 72 if(num_groupe <6) verif_position_bras(num_groupe);
goldmas 37:ee2c72e76d9c 73 else if(num_groupe == 10) {verif_position_bras(1); verif_position_bras(0);}
goldmas 37:ee2c72e76d9c 74 else if(num_groupe == 20) {verif_position_bras(2); verif_position_bras(0);}
goldmas 37:ee2c72e76d9c 75 else if(num_groupe == 21) {verif_position_bras(2); verif_position_bras(1);}
goldmas 37:ee2c72e76d9c 76 else if(num_groupe == 210){verif_position_bras(2); verif_position_bras(1); verif_position_bras(0);}
goldmas 37:ee2c72e76d9c 77 else if(num_groupe == 43) {verif_position_bras(4); verif_position_bras(3);}
goldmas 37:ee2c72e76d9c 78 else if(num_groupe == 53) {verif_position_bras(5); verif_position_bras(3);}
goldmas 37:ee2c72e76d9c 79 else if(num_groupe == 54) {verif_position_bras(5); verif_position_bras(4);}
goldmas 37:ee2c72e76d9c 80 else if(num_groupe == 66) {verif_position_bras(5); verif_position_bras(4); verif_position_bras(3);}
goldmas 37:ee2c72e76d9c 81 compteur_de_points();
goldmas 37:ee2c72e76d9c 82 Flag_Bras_Re = 0;
goldmas 37:ee2c72e76d9c 83 }
goldmas 37:ee2c72e76d9c 84
goldmas 37:ee2c72e76d9c 85 if(Flag_Manche_Bas == 1)
goldmas 37:ee2c72e76d9c 86 {
goldmas 37:ee2c72e76d9c 87 verif_position_manche();
goldmas 37:ee2c72e76d9c 88 compteur_de_points();
goldmas 37:ee2c72e76d9c 89 Flag_Manche_Bas = 0;
goldmas 35:2a745eeb7922 90 }
goldmas 37:ee2c72e76d9c 91
goldmas 37:ee2c72e76d9c 92 if(Flag_Manche_Moy == 1)
goldmas 37:ee2c72e76d9c 93 {
goldmas 37:ee2c72e76d9c 94 verif_position_phare();
goldmas 37:ee2c72e76d9c 95 compteur_de_points();
goldmas 37:ee2c72e76d9c 96 Flag_Manche_Moy = 0;
goldmas 37:ee2c72e76d9c 97 }
goldmas 37:ee2c72e76d9c 98
goldmas 37:ee2c72e76d9c 99 if(Flag_pavillon == 1)
goldmas 37:ee2c72e76d9c 100 {
goldmas 37:ee2c72e76d9c 101 pavillon = 1;
goldmas 37:ee2c72e76d9c 102 compteur_de_points();
goldmas 37:ee2c72e76d9c 103 Flag_pavillon = 0;
goldmas 37:ee2c72e76d9c 104 }
goldmas 37:ee2c72e76d9c 105
goldmas 35:2a745eeb7922 106 }
goldmas 35:2a745eeb7922 107
goldmas 35:2a745eeb7922 108 void compteur_de_points(void)
goldmas 35:2a745eeb7922 109 {
goldmas 37:ee2c72e76d9c 110 if(Hauteur == 1) score_phare = 2;
goldmas 37:ee2c72e76d9c 111 else if(Hauteur == 0) offset_score = 10;
goldmas 37:ee2c72e76d9c 112
goldmas 37:ee2c72e76d9c 113 //Points gobelets
goldmas 35:2a745eeb7922 114 int paire_ventouse = abs(gobelet_vert - gobelet_rouge);
goldmas 35:2a745eeb7922 115
goldmas 37:ee2c72e76d9c 116 score_ventouse = (gobelet_vert + gobelet_rouge - paire_ventouse)*3 + paire_ventouse*2 + gobelet_port;
goldmas 35:2a745eeb7922 117
goldmas 35:2a745eeb7922 118 //Points manche à air
goldmas 35:2a745eeb7922 119 if(manche_releve == 1) score_manche = 5;
goldmas 37:ee2c72e76d9c 120 else if(manche_releve >= 2) score_manche = 15;
goldmas 35:2a745eeb7922 121
goldmas 35:2a745eeb7922 122 //Points phare
goldmas 37:ee2c72e76d9c 123 if(phare_active == 1) score_phare = 15;
goldmas 35:2a745eeb7922 124
goldmas 35:2a745eeb7922 125 //Points pavillons
goldmas 37:ee2c72e76d9c 126 if(pavillon == 1) score_pavillon = 5;
goldmas 35:2a745eeb7922 127
goldmas 35:2a745eeb7922 128 //Points d'arrivée à bon port
goldmas 37:ee2c72e76d9c 129 score_bon_port = 10;
goldmas 35:2a745eeb7922 130
goldmas 35:2a745eeb7922 131 //Points totaux
goldmas 37:ee2c72e76d9c 132 score_final = score_ventouse + score_manche + score_pavillon + score_phare + score_bon_port;//+ offset_score;
goldmas 35:2a745eeb7922 133 }
goldmas 35:2a745eeb7922 134
goldmas 35:2a745eeb7922 135
goldmas 35:2a745eeb7922 136 /********************************************************************************************************************/
goldmas 35:2a745eeb7922 137 /* FUNCTION NAME: verif_position_ventouse */
goldmas 35:2a745eeb7922 138 /* DESCRIPTION : Vérifie si les gobelets posés par les ventouses sont dans un chenal/un port et augmente le score */
goldmas 35:2a745eeb7922 139 /********************************************************************************************************************/
goldmas 35:2a745eeb7922 140 void verif_position_ventouse(int num_groupe)
goldmas 35:2a745eeb7922 141 {
goldmas 37:ee2c72e76d9c 142 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);
goldmas 37:ee2c72e76d9c 143 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);
goldmas 35:2a745eeb7922 144
goldmas 35:2a745eeb7922 145 //chenal vert port départ bleu
goldmas 37:ee2c72e76d9c 146 if(((float)x_robot+x_offset <= 530.0f+MARGE_VENT+ECART)&&((float)x_robot+x_offset >= 500.0f-MARGE_VENT-ECART)
goldmas 37:ee2c72e76d9c 147 && ((float)y_robot+y_offset <= 400.0f+MARGE_VENT)&&((float)y_robot+y_offset >= 0))
goldmas 37:ee2c72e76d9c 148
goldmas 37:ee2c72e76d9c 149 {gobelet_vert++; Position_de_pose[num_groupe][0] = (float)x_robot+x_offset; Position_de_pose[num_groupe][1] = (float)y_robot+y_offset;}
goldmas 37:ee2c72e76d9c 150
goldmas 35:2a745eeb7922 151 //chenal rouge port départ bleu
goldmas 37:ee2c72e76d9c 152 else if(((float)x_robot+x_offset <= 1100.0f+MARGE_VENT+ECART)&&((float)x_robot+x_offset >= 1070.0f-MARGE_VENT-ECART)
goldmas 37:ee2c72e76d9c 153 && ((float)y_robot+y_offset <= 400.0f+MARGE_VENT)&&((float)y_robot+y_offset >= 0))
goldmas 37:ee2c72e76d9c 154
goldmas 37:ee2c72e76d9c 155 {gobelet_rouge++; Position_de_pose[num_groupe][0] = (float)x_robot+x_offset; Position_de_pose[num_groupe][1] = (float)y_robot+y_offset;}
goldmas 37:ee2c72e76d9c 156
goldmas 35:2a745eeb7922 157 //port départ bleu
goldmas 37:ee2c72e76d9c 158 else if(((float)x_robot+x_offset > 530.0f+MARGE_VENT+ECART)&&((float)x_robot+x_offset < 1070.0f-MARGE_VENT-ECART)
goldmas 37:ee2c72e76d9c 159 && ((float)y_robot+y_offset <= 400.0f+MARGE_VENT)&&((float)y_robot+y_offset >= 0))
goldmas 37:ee2c72e76d9c 160
goldmas 37:ee2c72e76d9c 161 {gobelet_port++; Position_de_pose[num_groupe][0] = (float)x_robot+x_offset; Position_de_pose[num_groupe][1] = (float)y_robot+y_offset;}
goldmas 35:2a745eeb7922 162
goldmas 35:2a745eeb7922 163 //chenal vert port ext bleu
goldmas 37:ee2c72e76d9c 164 else if(((float)x_robot+x_offset >= 1700.0f-MARGE_VENT)&&((float)x_robot+x_offset <= 2000.0f+MARGE_VENT)
goldmas 37:ee2c72e76d9c 165 && ((float)y_robot+y_offset >= 1650.0f-MARGE_VENT)&&((float)y_robot+y_offset < 1800.0f))
goldmas 37:ee2c72e76d9c 166
goldmas 37:ee2c72e76d9c 167 {gobelet_vert++; Position_de_pose[num_groupe][0] = (float)x_robot+x_offset; Position_de_pose[num_groupe][1] = (float)y_robot+y_offset;}
goldmas 37:ee2c72e76d9c 168
goldmas 35:2a745eeb7922 169 //chenal rouge port ext bleu
goldmas 37:ee2c72e76d9c 170 else if(((float)x_robot+x_offset >= 1700.0f-MARGE_VENT)&&((float)x_robot+x_offset <= 2000.0f+MARGE_VENT)
goldmas 37:ee2c72e76d9c 171 && ((float)y_robot+y_offset >= 1800.0f)&&((float)y_robot+y_offset <= 1950.0f+MARGE_VENT))
goldmas 37:ee2c72e76d9c 172
goldmas 37:ee2c72e76d9c 173 {gobelet_rouge++; Position_de_pose[num_groupe][0] = (float)x_robot+x_offset; Position_de_pose[num_groupe][1] = (float)y_robot+y_offset;}
goldmas 35:2a745eeb7922 174
goldmas 35:2a745eeb7922 175
goldmas 35:2a745eeb7922 176 //chenal vert port départ jaune
goldmas 37:ee2c72e76d9c 177 else if(((float)x_robot+x_offset <= 530.0f+MARGE_VENT+ECART)&&((float)x_robot+x_offset >= 500.0f-MARGE_VENT-ECART)
goldmas 37:ee2c72e76d9c 178 && ((float)y_robot+y_offset <= 3000.0f+MARGE_VENT)&&((float)y_robot+y_offset >= 2600.0f-MARGE_VENT))
goldmas 37:ee2c72e76d9c 179
goldmas 37:ee2c72e76d9c 180 {gobelet_rouge++; Position_de_pose[num_groupe][0] = (float)x_robot+x_offset; Position_de_pose[num_groupe][1] = (float)y_robot+y_offset;}
goldmas 37:ee2c72e76d9c 181
goldmas 35:2a745eeb7922 182 //chenal rouge port départ jaune
goldmas 37:ee2c72e76d9c 183 else if(((float)x_robot+x_offset <= 1100.0f+MARGE_VENT+ECART)&&((float)x_robot+x_offset >= 1070.0f-MARGE_VENT-ECART)
goldmas 37:ee2c72e76d9c 184 && ((float)y_robot+y_offset <= 3000.0f+MARGE_VENT)&&((float)y_robot+y_offset >= 2600.0f-MARGE_VENT))
goldmas 37:ee2c72e76d9c 185
goldmas 37:ee2c72e76d9c 186 {gobelet_vert++; Position_de_pose[num_groupe][0] = (float)x_robot+x_offset; Position_de_pose[num_groupe][1] = (float)y_robot+y_offset;}
goldmas 37:ee2c72e76d9c 187
goldmas 35:2a745eeb7922 188 //port départ jaune
goldmas 37:ee2c72e76d9c 189 else if(((float)x_robot+x_offset > 530.0f+MARGE_VENT+ECART)&&((float)x_robot+x_offset < 1070.0f-MARGE_VENT-ECART)
goldmas 37:ee2c72e76d9c 190 && ((float)y_robot+y_offset <= 3000.0f+MARGE_VENT)&&((float)y_robot+y_offset >= 2600.0f-MARGE_VENT))
goldmas 37:ee2c72e76d9c 191
goldmas 37:ee2c72e76d9c 192 {gobelet_port++; Position_de_pose[num_groupe][0] = (float)x_robot+x_offset; Position_de_pose[num_groupe][1] = (float)y_robot+y_offset;}
goldmas 35:2a745eeb7922 193
goldmas 35:2a745eeb7922 194 //chenal vert port ext jaune
goldmas 37:ee2c72e76d9c 195 else if(((float)x_robot+x_offset >= 1700.0f-MARGE_VENT)&&((float)x_robot+x_offset <= 2000.0f+MARGE_VENT)
goldmas 37:ee2c72e76d9c 196 && ((float)y_robot+y_offset >= 1050.0f-MARGE_VENT)&&((float)y_robot+y_offset < 1200.0f))
goldmas 37:ee2c72e76d9c 197
goldmas 37:ee2c72e76d9c 198 {gobelet_vert++; Position_de_pose[num_groupe][0] = (float)x_robot+x_offset; Position_de_pose[num_groupe][1] = (float)y_robot+y_offset;}
goldmas 37:ee2c72e76d9c 199
goldmas 35:2a745eeb7922 200 //chenal rouge port ext jaune
goldmas 37:ee2c72e76d9c 201 else if(((float)x_robot+x_offset >= 1700.0f-MARGE_VENT)&&((float)x_robot+x_offset <= 2000.0f+MARGE_VENT)
goldmas 37:ee2c72e76d9c 202 && ((float)y_robot+y_offset >= 1200.0f)&&((float)y_robot+y_offset <= 1350.0f+MARGE_VENT))
goldmas 37:ee2c72e76d9c 203
goldmas 37:ee2c72e76d9c 204 {gobelet_rouge++; Position_de_pose[num_groupe][0] = (float)x_robot+x_offset; Position_de_pose[num_groupe][1] = (float)y_robot+y_offset;}
goldmas 35:2a745eeb7922 205 }
goldmas 35:2a745eeb7922 206
goldmas 37:ee2c72e76d9c 207 /********************************************************************************************************************/
goldmas 37:ee2c72e76d9c 208 /* FUNCTION NAME: verif_position_bras */
goldmas 37:ee2c72e76d9c 209 /* DESCRIPTION : Vérifie si les gobelets posés par les bras sont dans un chenal/un port et augmente le score */
goldmas 37:ee2c72e76d9c 210 /********************************************************************************************************************/
goldmas 35:2a745eeb7922 211 void verif_position_bras(int num_groupe)
goldmas 37:ee2c72e76d9c 212 {
goldmas 37:ee2c72e76d9c 213 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);
goldmas 37:ee2c72e76d9c 214 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);
goldmas 37:ee2c72e76d9c 215
goldmas 37:ee2c72e76d9c 216
goldmas 37:ee2c72e76d9c 217 //chenal vert port départ bleu
goldmas 37:ee2c72e76d9c 218 if(((float)x_robot+x_offset <= 530.0f+MARGE_BRAS+ECART)&&((float)x_robot+x_offset >= 500.0f-MARGE_BRAS-ECART)
goldmas 37:ee2c72e76d9c 219 && ((float)y_robot+y_offset <= 400.0f+MARGE_BRAS)&&((float)y_robot+y_offset >= 0)) gobelet_vert++;
goldmas 37:ee2c72e76d9c 220 //chenal rouge port départ bleu
goldmas 37:ee2c72e76d9c 221 else if(((float)x_robot+x_offset <= 1100.0f+MARGE_BRAS+ECART)&&((float)x_robot+x_offset >= 1070.0f-MARGE_BRAS-ECART)
goldmas 37:ee2c72e76d9c 222 && ((float)y_robot+y_offset <= 400.0f+MARGE_BRAS)&&((float)y_robot+y_offset >= 0)) gobelet_rouge++;
goldmas 37:ee2c72e76d9c 223 //port départ bleu
goldmas 37:ee2c72e76d9c 224 else if(((float)x_robot+x_offset > 530.0f+MARGE_BRAS+ECART)&&((float)x_robot+x_offset < 1070.0f-MARGE_BRAS-ECART)
goldmas 37:ee2c72e76d9c 225 && ((float)y_robot+y_offset <= 400.0f+MARGE_BRAS)&&((float)y_robot+y_offset >= 0)) gobelet_port++;
goldmas 37:ee2c72e76d9c 226
goldmas 37:ee2c72e76d9c 227 //chenal vert port ext bleu
goldmas 37:ee2c72e76d9c 228 else if(((float)x_robot+x_offset >= 1700.0f-MARGE_BRAS)&&((float)x_robot+x_offset <= 2000.0f+MARGE_BRAS)
goldmas 37:ee2c72e76d9c 229 && ((float)y_robot+y_offset >= 1650.0f-MARGE_BRAS)&&((float)y_robot+y_offset <= 1780.0f)) gobelet_vert++;
goldmas 37:ee2c72e76d9c 230 //chenal rouge port ext bleu
goldmas 37:ee2c72e76d9c 231 else if(((float)x_robot+x_offset >= 1700.0f-MARGE_BRAS)&&((float)x_robot+x_offset <= 2000.0f+MARGE_BRAS)
goldmas 37:ee2c72e76d9c 232 && ((float)y_robot+y_offset >= 1820.0f)&&((float)y_robot+y_offset <= 1950.0f+MARGE_BRAS)) gobelet_rouge++;
goldmas 37:ee2c72e76d9c 233 //Port ext bleu
goldmas 37:ee2c72e76d9c 234 else if((x_robot+x_offset >= 1700.0f-MARGE_BRAS)&&(x_robot+x_offset <= 2000.0f+MARGE_BRAS)
goldmas 37:ee2c72e76d9c 235 && (y_robot+y_offset > 1780.0f)&&(y_robot+y_offset < 1820.0f)) gobelet_port++;
goldmas 37:ee2c72e76d9c 236
goldmas 37:ee2c72e76d9c 237
goldmas 37:ee2c72e76d9c 238 //chenal vert port départ jaune
goldmas 37:ee2c72e76d9c 239 else if(((float)x_robot+x_offset <= 530.0f+MARGE_BRAS+ECART)&&((float)x_robot+x_offset >= 500.0f-MARGE_BRAS-ECART)
goldmas 37:ee2c72e76d9c 240 && ((float)y_robot+y_offset <= 3000.0f+MARGE_BRAS)&&((float)y_robot+y_offset >= 2600.0f-MARGE_BRAS)) gobelet_rouge++;
goldmas 37:ee2c72e76d9c 241 //chenal rouge port départ jaune
goldmas 37:ee2c72e76d9c 242 else if(((float)x_robot+x_offset <= 1100.0f+MARGE_BRAS+ECART)&&((float)x_robot+x_offset >= 1070.0f-MARGE_BRAS-ECART)
goldmas 37:ee2c72e76d9c 243 && ((float)y_robot+y_offset <= 3000.0f+MARGE_BRAS)&&((float)y_robot+y_offset >= 2600.0f-MARGE_BRAS)) gobelet_vert++;
goldmas 37:ee2c72e76d9c 244 //port départ jaune
goldmas 37:ee2c72e76d9c 245 else if(((float)x_robot+x_offset > 530.0f+MARGE_BRAS+ECART)&&((float)x_robot+x_offset < 1070.0f-MARGE_BRAS-ECART)
goldmas 37:ee2c72e76d9c 246 && ((float)y_robot+y_offset <= 3000.0f+MARGE_BRAS)&&((float)y_robot+y_offset >= 2600.0f-MARGE_BRAS)) gobelet_port++;
goldmas 37:ee2c72e76d9c 247
goldmas 37:ee2c72e76d9c 248 //chenal vert port ext jaune
goldmas 37:ee2c72e76d9c 249 else if(((float)x_robot+x_offset >= 1700.0f-MARGE_BRAS)&&((float)x_robot+x_offset <= 2000.0f+MARGE_BRAS)
goldmas 37:ee2c72e76d9c 250 && ((float)y_robot+y_offset >= 1050.0f-MARGE_BRAS)&&((float)y_robot+y_offset <= 1180.0f)) gobelet_vert++;
goldmas 37:ee2c72e76d9c 251 //chenal rouge port ext jaune
goldmas 37:ee2c72e76d9c 252 else if(((float)x_robot+x_offset >= 1700.0f-MARGE_BRAS)&&((float)x_robot+x_offset <= 2000.0f+MARGE_BRAS)
goldmas 37:ee2c72e76d9c 253 && ((float)y_robot+y_offset >= 1220.0f)&&((float)y_robot+y_offset <= 1350.0f+MARGE_BRAS)) gobelet_rouge++;
goldmas 37:ee2c72e76d9c 254 //Port ext jaune
goldmas 37:ee2c72e76d9c 255 else if((float)(x_robot+x_offset >= 1700.0f-MARGE_BRAS)&&((float)x_robot+x_offset <= 2000.0f+MARGE_BRAS)
goldmas 37:ee2c72e76d9c 256 && ((float)y_robot+y_offset > 1180.0f)&&((float)y_robot+y_offset < 1220.0f)) gobelet_port++;
goldmas 35:2a745eeb7922 257
goldmas 35:2a745eeb7922 258 }
goldmas 35:2a745eeb7922 259
goldmas 37:ee2c72e76d9c 260 /********************************************************************************************************************/
goldmas 37:ee2c72e76d9c 261 /* FUNCTION NAME: verif_position_manche */
goldmas 37:ee2c72e76d9c 262 /* DESCRIPTION : Vérifie si les bras des manches à air sont déployés au bon enroit et augmente le score */
goldmas 37:ee2c72e76d9c 263 /********************************************************************************************************************/
goldmas 37:ee2c72e76d9c 264 void verif_position_manche(void)
goldmas 37:ee2c72e76d9c 265 {
goldmas 37:ee2c72e76d9c 266 //Manches Bleus
goldmas 37:ee2c72e76d9c 267 if((x_robot <= 2000+MARGE_VENT)&&(x_robot >= 1700-MARGE_VENT)
goldmas 37:ee2c72e76d9c 268 && (y_robot <= 890+MARGE_VENT)&&(y_robot >= 0)) manche_releve++;
goldmas 37:ee2c72e76d9c 269
goldmas 37:ee2c72e76d9c 270 //Manches Jaunes
goldmas 37:ee2c72e76d9c 271 else if((x_robot <= 2000+MARGE_VENT)&&(x_robot >= 1700-MARGE_VENT)
goldmas 37:ee2c72e76d9c 272 && (y_robot <= 3000)&&(y_robot >= 2090-MARGE_VENT)) manche_releve++;
goldmas 37:ee2c72e76d9c 273 }
goldmas 35:2a745eeb7922 274
goldmas 37:ee2c72e76d9c 275 /********************************************************************************************************************/
goldmas 37:ee2c72e76d9c 276 /* FUNCTION NAME: verif_position_phare */
goldmas 37:ee2c72e76d9c 277 /* DESCRIPTION : Vérifie si les bras des manches à air sont déployés au bon enroit et augmente le score */
goldmas 37:ee2c72e76d9c 278 /********************************************************************************************************************/
goldmas 37:ee2c72e76d9c 279 void verif_position_phare(void)
goldmas 37:ee2c72e76d9c 280 {
goldmas 37:ee2c72e76d9c 281 //Phare Bleu
goldmas 37:ee2c72e76d9c 282 if((x_robot <= 300+MARGE_VENT)&&(x_robot >= 0-MARGE_VENT)
goldmas 37:ee2c72e76d9c 283 && (y_robot <= 890+MARGE_VENT)&&(y_robot >= 0)) phare_active=1;
goldmas 37:ee2c72e76d9c 284
goldmas 37:ee2c72e76d9c 285 //Phare Jaune
goldmas 37:ee2c72e76d9c 286 else if((x_robot <= 300+MARGE_VENT)&&(x_robot >= 0-MARGE_VENT)
goldmas 37:ee2c72e76d9c 287 && (y_robot <= 3000)&&(y_robot >= 2090-MARGE_VENT)) phare_active=1;
goldmas 35:2a745eeb7922 288 }