CRAC Team / Mbed 2 deprecated code_carte_IHM_sans_bug

code de la carte IHM avant les bugs et avant le travail effectué avec Melchior

Dependencies:   mbed SerialHalfDuplex SDFileSystem liaison_Bluetooth ident_crac DISCO-F469NI_portrait

Compteur_points/Compteur.cpp@38:9d6a3ccc0582, 2021-07-17 (annotated)

Committer:
goldmas
Date:
Sat Jul 17 11:07:17 2021 +0000
Revision:
38:9d6a3ccc0582
Parent:
36:c37dbe2be916 
Dernier code de la coupe de fracne 2021

Who changed what in which revision?

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