RPiPico-Holonome2023/Strategie.c

#include "hardware/gpio.h"
#include "i2c_annexe.h"
#include "Asser_Position.h"
#include "Commande_vitesse.h"
#include "Geometrie_robot.h"
#include "Localisation.h"
#include "Moteurs.h"
#include "Score.h"
#include "Strategie_prise_cerises.h"
#include "Strategie.h"
#include "Trajet.h"
#include "math.h"

#define SEUIL_RECAL_DIST_MM 75
#define SEUIL_RECAL_ANGLE_RADIAN (5 * DEGRE_EN_RADIAN)
#define DISTANCE_OBSTACLE_CM 50
#define DISTANCE_PAS_OBSTACLE_MM 2000

#define NB_OBJECTIFS 4

struct objectif_t{
    int priorite;
    enum {A_FAIRE, EN_COURS, BLOQUE, FAIT} etat;
    enum {CERISE_BAS, CERISE_HAUT, CERISE_GAUCHE, CERISE_DROITE} cible;
} objectifs[NB_OBJECTIFS];
struct objectif_t * objectif_courant;

uint32_t Score_cerises_dans_robot=0;

void Strategie_set_cerise_dans_robot(uint32_t nb_cerises){
    Score_cerises_dans_robot = nb_cerises;
}

uint32_t Strategie_get_cerise_dans_robot(void){
    return Score_cerises_dans_robot;
}



// TODO: Peut-être à remetttre en variable locale après
float distance_obstacle;

enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms, uint32_t nb_cerises);
enum etat_action_t lance_balles(uint32_t step_ms, uint32_t nb_cerises);
enum etat_action_t calage_angle(enum longer_direction_t longer_direction, float x_mm, float y_mm, float angle_radian);


int Robot_est_dans_quart_haut_gauche();
int Robot_est_dans_quart_haut_droit();
int Robot_est_dans_zone_cerise_haut(enum couleur_t couleur);
int Robot_est_dans_zone_cerise_gauche();

enum etat_homologation_t etat_homologation=STRATEGIE_INIT;


void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
    
    float angle_fin;
    float recal_pos_x, recal_pos_y;
    float point_x, point_y;
    enum longer_direction_t longer_direction;
    enum etat_action_t etat_action;
    enum etat_trajet_t etat_trajet;

    struct trajectoire_t trajectoire;

    static enum etat_strategie_t {
        STRATEGIE_INIT,
        ALLER_CERISE_BAS,
        ATTRAPER_CERISE_BAS,
        ALLER_CERISE_HAUT,
        ATTRAPER_CERISE_HAUT,
        ALLER_CERISE_GAUCHE,
        ATTRAPER_CERISE_GAUCHE,
        ALLER_CERISE_DROITE,
        ATTRAPER_CERISE_DROITE,
        ALLER_PANIER,
        LANCER_PANIER,
        DECISION_STRATEGIE,
    }etat_strategie;

    switch(etat_strategie){
        case STRATEGIE_INIT:
            if(couleur == COULEUR_BLEU){
                Localisation_set(775., 109., (-60.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN);
                struct objectif_t objectif_1 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_BAS};
                struct objectif_t objectif_2 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_HAUT};
                struct objectif_t objectif_3 = { .priorite = 3, .etat = A_FAIRE, .cible = CERISE_GAUCHE};
                struct objectif_t objectif_4 = { .priorite = 4, .etat = FAIT, .cible = CERISE_DROITE};
                objectifs[0]= objectif_1;
                objectifs[1]= objectif_2;
                objectifs[2]= objectif_3;
                objectifs[3]= objectif_4;
            }else{
                Localisation_set(2000 - 775., 109., (-60.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN);
                struct objectif_t objectif_1 = { .priorite = 1, .etat = A_FAIRE, .cible = CERISE_BAS};
                struct objectif_t objectif_2 = { .priorite = 2, .etat = A_FAIRE, .cible = CERISE_HAUT};
                struct objectif_t objectif_3 = { .priorite = 3, .etat = A_FAIRE, .cible = CERISE_DROITE};
                struct objectif_t objectif_4 = { .priorite = 4, .etat = FAIT, .cible = CERISE_GAUCHE};
                objectifs[0]= objectif_1;
                objectifs[1]= objectif_2;
                objectifs[2]= objectif_3;
                objectifs[3]= objectif_4;
            }
            objectif_courant = &objectifs[0];
            etat_strategie = ALLER_CERISE_BAS;
            break;

        case ALLER_CERISE_BAS:
            
            if(couleur == COULEUR_BLEU){
                angle_fin = 30. * DEGRE_EN_RADIAN;
                point_x = 857;
            }else{
                angle_fin = -150. * DEGRE_EN_RADIAN;
                point_x = 2000 - 857;
            }

            Trajet_config(250, 500);            
            Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 156,
                     Localisation_get().angle_radian, angle_fin);

            if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){
                etat_strategie = ATTRAPER_CERISE_BAS;
            }
            break;

        case ATTRAPER_CERISE_BAS:
            recal_pos_y = RAYON_ROBOT;
            if(couleur == COULEUR_BLEU){
                longer_direction = LONGER_VERS_C;
                recal_pos_x = 1000 - 15 - PETIT_RAYON_ROBOT_MM;
            }else{
                longer_direction = LONGER_VERS_A;
                recal_pos_x = 1000 + 15 + PETIT_RAYON_ROBOT_MM;
            }

            etat_action = cerise_attraper_bordure(longer_direction, step_ms, recal_pos_x, recal_pos_y);
            if(etat_action == ACTION_TERMINEE){
                Strategie_set_cerise_dans_robot(6);
                etat_strategie = ALLER_PANIER;
            }
            break;
        
        case ALLER_CERISE_HAUT:
            
            if(couleur == COULEUR_BLEU){
                angle_fin = 30. * DEGRE_EN_RADIAN;
                point_x = 857;
            }else{
                angle_fin = -150. * DEGRE_EN_RADIAN;
                point_x = 2000 - 857;
            }

            Trajet_config(250, 500);            
            Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 175,
                     Localisation_get().angle_radian, angle_fin);

            if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){
                etat_strategie = ATTRAPER_CERISE_HAUT;
            }
            break;

        case ATTRAPER_CERISE_HAUT:
            recal_pos_y = 3000 - RAYON_ROBOT;
            if(couleur == COULEUR_BLEU){
                longer_direction = LONGER_VERS_A;
                recal_pos_x = 1000 - 15 - PETIT_RAYON_ROBOT_MM;
            }else{
                longer_direction = LONGER_VERS_C;
                recal_pos_x = 1000 + 15 + PETIT_RAYON_ROBOT_MM;
            }

            etat_action = cerise_attraper_bordure(longer_direction, step_ms, recal_pos_x, recal_pos_y);
            if(etat_action == ACTION_TERMINEE){
                Strategie_set_cerise_dans_robot(6);
                etat_strategie = ALLER_PANIER;
            }
            break;

        case ALLER_CERISE_GAUCHE:
            Trajet_config(300, 250);
            angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN);
            if(couleur == COULEUR_BLEU){
                Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
                        740, 3000 - 550,
                        510, 3000 - 1580,
                        180, 1500,
                        Localisation_get().angle_radian, angle_fin);
            }else{
                Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
                        1225, 3000 - 540,
                        440, 3000 - 775,
                        225, 3000 - (1500 - 45),
                        Localisation_get().angle_radian, angle_fin);

            }

            if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
                etat_strategie = ATTRAPER_CERISE_GAUCHE;
                Trajet_config(500, 500);
            }
            break;

        case ATTRAPER_CERISE_GAUCHE:
            if(cerises_attraper_cerises_gauches(step_ms) == ACTION_TERMINEE){
                Strategie_set_cerise_dans_robot(10);
                etat_strategie = ALLER_PANIER;
            }
            break;
            
        case ALLER_PANIER:
            if(Strategie_aller_panier(couleur, step_ms) == ACTION_TERMINEE){
                etat_strategie = LANCER_PANIER;
            }
            break;

        case LANCER_PANIER:
            if(lance_balles_dans_panier(couleur, step_ms, Strategie_get_cerise_dans_robot())== ACTION_TERMINEE){
                Score_ajout_cerise(Strategie_get_cerise_dans_robot());
                Strategie_set_cerise_dans_robot(0);
                objectif_courant->etat = FAIT;
                etat_strategie = DECISION_STRATEGIE;
            }
            break;

        case DECISION_STRATEGIE:
            // Obtenir l'objectif suivant
            for(uint m_objectif=0; m_objectif < NB_OBJECTIFS; m_objectif++){
                if(objectif_courant->etat == FAIT && objectifs[m_objectif].etat == A_FAIRE){
                    objectif_courant = &(objectifs[m_objectif]);
                }else if(objectif_courant->priorite > objectifs[m_objectif].priorite){
                    objectif_courant = &(objectifs[m_objectif]);
                }
            }

            if(objectif_courant->etat == FAIT){
                etat_strategie = STRATEGIE_FIN;
            }else{
                switch(objectif_courant->cible){
                    case CERISE_BAS:
                        etat_strategie = ALLER_CERISE_BAS;
                        break;
                    case CERISE_HAUT:
                        etat_strategie = ALLER_CERISE_HAUT;
                        break;
                    case CERISE_GAUCHE:
                        etat_strategie = ALLER_CERISE_GAUCHE;
                        break;
                    case CERISE_DROITE:
                        etat_strategie = ALLER_CERISE_DROITE;
                        break;
                }
            }

            break;

        case STRATEGIE_FIN:
            i2c_annexe_desactive_propulseur();
            commande_vitesse_stop();
            i2c_annexe_couleur_balise(0xE0, 0x0FFF);
            break;
    }
}

enum etat_action_t  Strategie_aller_panier(enum couleur_t couleur, uint32_t step_ms){
    enum etat_action_t etat_action = ACTION_EN_COURS;
    struct trajectoire_t trajectoire;
    Trajet_config(500, 500);

    // Definition des trajectoires
    if(couleur == COULEUR_BLEU){
        // Si le robot est déjà dans la zone cerise haut
        // On va en ligne droite
        if(Robot_est_dans_zone_cerise_haut(couleur)){
            Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
                180,2800,
                Localisation_get().angle_radian, 
                Geometrie_get_angle_optimal(Localisation_get().angle_radian, +120. * DEGRE_EN_RADIAN));
        }else if (Robot_est_dans_zone_cerise_gauche()){
            Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
                Localisation_get().x_mm + 330, Localisation_get().y_mm - 100,
                745, 3000 - 475,
                180, 3000 - 200,
                Localisation_get().angle_radian, 
                Geometrie_get_angle_optimal(Localisation_get().angle_radian, +120. * DEGRE_EN_RADIAN));
        }else{
        // Sinon, on a une courbe de bézier
            Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
                485, Localisation_get().y_mm,
                465, 857,
                465,2830,
                Localisation_get().angle_radian, 
                Geometrie_get_angle_optimal(Localisation_get().angle_radian, +120. * DEGRE_EN_RADIAN));
        }
    }else{ // COULEUR_VERT
        // Si le robot est déjà dans le quart haut droit, 
        // On va en ligne droite
        if(Robot_est_dans_quart_haut_droit()){
            Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
                2000-465, 2830,
                -150. * DEGRE_EN_RADIAN, -240. * DEGRE_EN_RADIAN);
        }else{
        // Sinon, on a une courbe de bézier
            Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
                            2000-485, Localisation_get().y_mm,
                            2000-465, 857,
                            2000-465, 2830,
                            -150. * DEGRE_EN_RADIAN, -240. * DEGRE_EN_RADIAN);
        }
        
    }

    // parcours du trajet
    if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
        etat_action = ACTION_TERMINEE;
    }
    return etat_action;

}

/// Fonction de localisation approximatives pour la stratégie.

/// @brief Renvoi 1 si le robot est dans le quart supérieur gauche du terrain
/// @return 1 si oui, 0 si non.
int Robot_est_dans_quart_haut_gauche(){
    if(Localisation_get().x_mm < 1000 && Localisation_get().y_mm > 1500){
        return 1;
    }
    return 0;
}

/// @brief Renvoi 1 si le robot est dans le quart supérieur droit du terrain
/// @return 1 si oui, 0 si non.
int Robot_est_dans_quart_haut_droit(){
    if(Localisation_get().x_mm > 1000 && Localisation_get().y_mm > 1500){
        return 1;
    }
    return 0;
}

int Robot_est_dans_zone_cerise_gauche(){
    if(Localisation_get().x_mm  < 630 && 
    Localisation_get().y_mm >1000 && Localisation_get().y_mm < 2000){
        return 1;
    }
    return 0;

}

int Robot_est_dans_zone_cerise_droite(){
    if(Localisation_get().x_mm  > 2000 - 630 && 
    Localisation_get().y_mm >1000 && Localisation_get().y_mm < 2000){
        return 1;
    }
    return 0;

}

int Robot_est_dans_zone_cerise_haut(enum couleur_t couleur){
    if(couleur == COULEUR_BLEU){
        if(Localisation_get().y_mm  > 2500 && Localisation_get().x_mm < 1000){
            return 1;
        }
    }else{
        if(Localisation_get().y_mm  > 2500 && Localisation_get().x_mm > 1000){
            return 1;
        }
    }
    return 0;
}

/// @brief Renvoi 1 si le robot intersect une zone de dépose
/// @param  couleur du robot
/// @return 1 si oui, 0 si non.
int Robot_est_dans_zone_depose(enum couleur_t couleur){
    float x_mm, y_mm;
    x_mm = Localisation_get().x_mm;
    y_mm = Localisation_get().y_mm;
    if(couleur == COULEUR_BLEU){
        // Zone 1
        if(x_mm < 550 && y_mm > 2450){
            return 1;
        }
        // Zone 2
        if(x_mm < 550 && y_mm > 800 && y_mm < 1450){
            return 1;
        }
        // Zone 3
        if(x_mm > 400 && x_mm < 1050 && y_mm < 550){
            return 1;
        }
        // Zone 4
        if(x_mm > 1450 && y_mm < 550){
            return 1;
        }
        // Zone 5
        if(x_mm > 1450 && y_mm > 1550 && y_mm < 2200){
            return 1;
        }
        return 0;

    }else{
        // VERT
        // Zone 1
        if(x_mm > (2000 - 550) && y_mm > 2450){
            return 1;
        }
        // Zone 2
        if(x_mm > (2000 - 550) && y_mm > 800 && y_mm < 1450){
            return 1;
        }
        // Zone 3
        if(x_mm < (2000 - 400) && x_mm > 2000 - 1050 && y_mm < 550){
            return 1;
        }
        // Zone 4
        if(x_mm < (2000 - 1450) && y_mm < 550){
            return 1;
        }
        // Zone 5
        if(x_mm < (2000 - 1450) && y_mm > 1550 && y_mm < 2200){
            return 1;
        }
        return 0;
    }
}

enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms, uint32_t nb_cerises){
    static enum {
        CALAGE_PANIER_1,
        RECULE_PANIER,
        LANCE_DANS_PANIER,
    }etat_lance_balles_dans_panier;
    float recal_pos_x, recal_pos_y;
    float angle_depart, angle_arrivee;    
    enum longer_direction_t longer_direction;
    float point_x, point_y;

    enum etat_action_t etat_action = ACTION_EN_COURS;

    struct trajectoire_t trajectoire;

    switch(etat_lance_balles_dans_panier){
        case CALAGE_PANIER_1:
            if(couleur == COULEUR_BLEU){
                recal_pos_x = RAYON_ROBOT;
                longer_direction = LONGER_VERS_A;
            }else{
                recal_pos_x = 2000- RAYON_ROBOT;
                longer_direction = LONGER_VERS_C;
            }

            if(calage_angle(longer_direction, recal_pos_x, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2.)), 120. *DEGRE_EN_RADIAN) == ACTION_TERMINEE){
                etat_lance_balles_dans_panier = RECULE_PANIER;
            }
            break;
        
        case RECULE_PANIER:
            Trajet_config(120, 250);
            if(couleur == COULEUR_BLEU){
                point_x = 180;
                point_y = 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 80;
            }else{
                point_x = 1735;
                point_y = 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 80;
            }
            angle_depart = Geometrie_get_angle_optimal(Localisation_get().angle_radian, 120. * DEGRE_EN_RADIAN);
            angle_arrivee = Geometrie_get_angle_optimal(angle_depart, 270. * DEGRE_EN_RADIAN);
            Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 
                                point_x, point_y,
                                angle_depart, angle_arrivee);

            if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){
                etat_lance_balles_dans_panier = LANCE_DANS_PANIER;
                Trajet_config(500, 500);
            }
            break;

        case LANCE_DANS_PANIER:
            Asser_Position_maintien();
            if(lance_balles(step_ms, nb_cerises) == ACTION_TERMINEE){
                etat_action = ACTION_TERMINEE;
                etat_lance_balles_dans_panier = CALAGE_PANIER_1;
            }
            break;
    }
    return etat_action;

}

/// @brief Active le propulseur, ouvre la porte, attend qql secondes.
/// @param step_ms : pas de temps.
/// @return ACTION_EN_COURS ou ACTION_TERMINEE
enum etat_action_t lance_balles(uint32_t step_ms, uint32_t nb_cerises){
    enum etat_action_t etat_action = ACTION_EN_COURS;
    static uint32_t tempo_ms;
    static uint32_t nb_iteration;

    static enum{
        LANCE_PROPULSEUR_ON,
        LANCE_TEMPO_PROP_ON,
        LANCE_PORTE_OUVERTE,
        LANCE_OUVERTURE_INITIALE,
    } etat_lance_balle = LANCE_PROPULSEUR_ON;

    switch(etat_lance_balle){
        case LANCE_PROPULSEUR_ON:
            i2c_annexe_active_propulseur();
            tempo_ms = 1000;
            nb_iteration=0;
            etat_lance_balle = LANCE_OUVERTURE_INITIALE;
            break;
        
        case LANCE_OUVERTURE_INITIALE:
            if(temporisation_terminee(&tempo_ms, step_ms)){
                i2c_annexe_ouvre_porte();
                etat_lance_balle = LANCE_PORTE_OUVERTE;
                tempo_ms = 250;
            }
            break;
        
        case LANCE_TEMPO_PROP_ON:
            if(temporisation_terminee(&tempo_ms, step_ms)){
                i2c_annexe_ouvre_porte();
                nb_iteration++;
                if(nb_iteration > nb_cerises){
                    nb_iteration=0;
                    etat_action = ACTION_TERMINEE;
                    etat_lance_balle = LANCE_PROPULSEUR_ON;
                    i2c_annexe_desactive_propulseur();
                }else{
                    etat_lance_balle = LANCE_PORTE_OUVERTE;
                    tempo_ms = 150;
                }
            }
            break;
        
        case LANCE_PORTE_OUVERTE:
            if(temporisation_terminee(&tempo_ms, step_ms)){
                i2c_annexe_mi_ferme_porte();
                etat_lance_balle = LANCE_TEMPO_PROP_ON;
                tempo_ms = 500;
            }
            break;

    }
    return etat_action;
}

/// @brief Envoie le robot se caler dans l'angle en face de lui, recale la localisation
/// Ne fonctionne que contre les bordures haute et basse, pas contre les bordures gauche et droites
enum etat_action_t calage_angle(enum longer_direction_t longer_direction, float x_mm, float y_mm, float angle_radian){
    enum etat_action_t etat_action = ACTION_EN_COURS;
    struct position_t position;
    struct trajectoire_t trajectoire;
    static float y_pos_ref;

    static enum {
        CONTACT_AVANT,
        RECULE_BORDURE,
        CONTACT_LATERAL,
        RECALE_X
    }etat_calage_angle;

    switch(etat_calage_angle){
        case CONTACT_AVANT:
            if(cerise_accostage() == ACTION_TERMINEE){
                position = Localisation_get();
                //if(fabs(position.y_mm - y_mm) < SEUIL_RECAL_DIST_MM){
                    Localisation_set_y(y_mm);
                //}
                //if(fabs(position.angle_radian - angle_radian) < SEUIL_RECAL_ANGLE_RADIAN){
                    Localisation_set_angle(angle_radian);
                //}
                y_pos_ref = Localisation_get().y_mm;
                etat_calage_angle = RECULE_BORDURE;
            }
            break;

        case RECULE_BORDURE:
            commande_translation_recule_vers_trompe();
            position = Localisation_get();
            if(fabs(y_pos_ref - Localisation_get().y_mm) > 35){
                etat_calage_angle = CONTACT_LATERAL;
            }
            break;

        case CONTACT_LATERAL:
            if(longer_direction == LONGER_VERS_A){
                commande_translation_avance_vers_A();
                if(i2c_annexe_get_contacteur_butee_A() == CONTACTEUR_ACTIF){
                    etat_calage_angle = RECALE_X;
                }
            }else{
                commande_translation_avance_vers_C();
                if(i2c_annexe_get_contacteur_butee_C() == CONTACTEUR_ACTIF){
                    etat_calage_angle = RECALE_X;
                }
            }
            break;

        case RECALE_X:
            etat_action = ACTION_TERMINEE;
            commande_vitesse_stop();
            position = Localisation_get();
            //if(fabs(position.x_mm - x_mm) < SEUIL_RECAL_DIST_MM){
                Localisation_set_x(x_mm);
            //}
            etat_calage_angle = CONTACT_AVANT;
            break;

    }

    return etat_action;
}

/// @brief Renvoi 1 si on doit attendre le déclenchement de la tirette
uint attente_tirette(void){
    return !gpio_get(TIRETTE);
}

/// @brief Renvoi COULEUR_VERT ou COULEUR_BLEU
enum couleur_t lire_couleur(void){
    if (gpio_get(COULEUR))
        return COULEUR_VERT;
    return COULEUR_BLEU;

}

/// @brief Décremente la temps de step_ms, renvoie 1 si la temporisation est écoulée
/// @param tempo_ms 
/// @param step_ms 
/// @return 1 si la temporisation est écoulée, 0 sinon.
int temporisation_terminee(uint32_t * tempo_ms, uint32_t step_ms){
    if(*tempo_ms < step_ms){
        return 1;
    }else{
        *tempo_ms -= step_ms;
        return 0;
    }
}