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@37:f1a8734c193d, 2021-07-17 (annotated)

Committer:
goldmas
Date:
Sat Jul 17 16:07:29 2021 +0000
Revision:
37:f1a8734c193d
Parent:
36:c37dbe2be916 
code de l'IHM avant bug

Who changed what in which revision?

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