File content as of revision 37:9d6a3ccc0582:

#include "global.h"

#define M_PI 3.14159265358979323846f
#define MARGE_VENT 72.0f
#define MARGE_BRAS 54.0f
#define ECART 100.0f

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, port_lu = 0;
int score_phare=0, phare_active=0;
int score_pavillon=0, pavillon=0;
int offset_score=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 old_gobelet_vert=0;
int old_gobelet_rouge=0;
int old_gobelet_port=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;
    
    if(identifiant==0x220)
    {
        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
                {
                    VentEtat[num_vent] = ETAT_COMPTER;
                }
                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);
    
    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;
    
    //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_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)
{   
    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(((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(((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(((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(((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(((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(((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(((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(((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(((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(((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++;

}

/********************************************************************************************************************/
/* 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;
}