Holonome_2024/Holonome2023.c

#include "Holonome2023.h"
#include "Strategie_2024.h"
#include "Demonstration.h"

const uint LED_PIN = 25;
#define LED_PIN_ROUGE 28
const uint LED_PIN_NE_PAS_UTILISER = 22;

uint temps_cycle;


#define V_INIT -999.0
#define TEST_TIMEOUT_US 10000000

// A supprimer 
int test_pseudo_homologation(void);


int mode_test();

void init_led(uint Numero_de_la_led, uint etat){
    gpio_init(Numero_de_la_led);
    gpio_set_dir(Numero_de_la_led, GPIO_OUT);
    gpio_put(Numero_de_la_led, etat);
}

int main() {
    bi_decl(bi_program_description("This is a test binary."));
    bi_decl(bi_1pin_with_name(LED_PIN, "On-board LED"));
    float vitesse_filtre_x=V_INIT, vitesse_filtre_y=V_INIT, vitesse_filtre_z=V_INIT;
    struct t_angle_gyro_float angle_gyro;

    uint32_t temps_ms = 0, temps_ms_old;
    uint32_t temps_us_debut_cycle;
    uint32_t score=0;
    uint32_t match_en_cours=1;

    set_position_avec_gyroscope(1);

    // Il faut neutraliser cette broche qui pourrait interférer avec 
    // la lecture des codeurs. (problème sur la carte électrique)...
    gpio_init(LED_PIN_NE_PAS_UTILISER);
    gpio_set_dir(LED_PIN_NE_PAS_UTILISER, GPIO_IN);

    stdio_init_all();
    
    //Demonstration_init();Demonstration_prise_plante();
    //while(1);
    while(mode_test());
    //test_pseudo_homologation();
    Holonome2023_init();

    multicore_launch_core1(Monitoring_display);

    temps_ms = Temps_get_temps_ms();
    temps_ms_old = temps_ms;
    while (1) { 
        static enum {
            MATCH_ATTENTE_TIRETTE,
            MATCH_EN_COURS,
            MATCH_ARRET_EN_COURS,
            MATCH_TERMINEE
        } statu_match=MATCH_ATTENTE_TIRETTE;
        uint16_t _step_ms_gyro = 2;
        const uint16_t _step_ms = 1;
        static uint32_t timer_match_ms=0;
        static uint32_t timer_match_init=0;
        static enum couleur_t couleur;

        // Surveillance du temps d'execution
        temps_cycle_check();

        i2c_gestion(i2c0);
        i2c_annexe_gestion();
        Balise_VL53L1X_gestion();
        
        
        if(temps_ms != Temps_get_temps_ms() && match_en_cours){
            timer_match_ms = (time_us_32() - timer_match_init) / 1000;
            temps_ms = Temps_get_temps_ms();
            QEI_update();
            Localisation_gestion();
            AsserMoteur_Gestion(_step_ms);
            Evitement_gestion(_step_ms);

            // Routine à 2 ms
            if(temps_ms % _step_ms_gyro == 0){
                if(get_position_avec_gyroscope()){
                    Gyro_Read(_step_ms_gyro);
                }
            }

            switch(statu_match){
                case MATCH_ATTENTE_TIRETTE:
                    if(lire_couleur() == COULEUR_JAUNE){
                        // Tout jaune
                        i2c_annexe_couleur_balise(0b11111100, 0x0FFF);
                    }else{
                        // Tout bleu
                        i2c_annexe_couleur_balise(0b00000011, 0x0FFF);
                    }
                    if(attente_tirette() == 0){
                        statu_match = MATCH_EN_COURS;
                        timer_match_init = time_us_32();
                        couleur = lire_couleur();
                        timer_match_ms = 0;
                        i2c_annexe_couleur_balise(0, 0x00);
                        score=5;
                        i2c_annexe_envoie_score(score);
                    }
                    break;

                case MATCH_EN_COURS:
                    if (timer_match_ms > 100000){ // 90 secondes
                        printf("MATCH_ARRET_EN_COURS\n");
                        statu_match = MATCH_ARRET_EN_COURS;
                    }
                    Strategie_2024(couleur, _step_ms, timer_match_ms);
                    break;
                
                case MATCH_ARRET_EN_COURS:
                    commande_vitesse_stop();

                    if (timer_match_ms > 100000){ // 100 secondes
                        statu_match = MATCH_TERMINEE;
                    }
                    
                    break;

                case MATCH_TERMINEE:
                    Moteur_Stop();
                    match_en_cours = 0;
                    break;
            }
        }
    }
}

void Holonome2023_init(){
    init_led(LED_PIN, 1);
    init_led(LED_PIN_ROUGE, 0);

    gpio_init(COULEUR);
    gpio_init(TIRETTE);
    gpio_set_dir(COULEUR, GPIO_IN);
    gpio_set_dir(TIRETTE, GPIO_IN);

    Log_init();
    
    sleep_ms(3000);
    Servomoteur_Init();

    Temps_init();
    Moteur_Init();
    QEI_init();
    AsserMoteur_Init();
    Localisation_init();
    i2c_maitre_init();
    Trajet_init();
    Balise_VL53L1X_init();

    if(get_position_avec_gyroscope()){
        Gyro_Init();
    }else{
        sleep_ms(5000);
    }

    for(int i=0; i<10; i++){
        i2c_gestion(i2c0);
        i2c_annexe_gestion();
        sleep_ms(1);
    }
}

/// @brief Fonction à appeler le plus souvent possible. Lit les codeurs, le gyroscope, et asservit les moteurs
/// @param param : Utiliser PARAM_DEFAULT, sinon, utiliser un "ou" avec les valeurs suivantes : PARAM_NO_MOTORS
void Holonome_cyclique(int param){
    static uint32_t temps_ms = 0;
    // Surveillance du temps d'execution
    temps_cycle_check();

    i2c_gestion(i2c0);
    i2c_annexe_gestion();
    Balise_VL53L1X_gestion();

    if(temps_ms != Temps_get_temps_ms()){
        temps_ms = Temps_get_temps_ms();
        QEI_update();
        Localisation_gestion();
        if(!(param & PARAM_NO_MOTORS)){
            AsserMoteur_Gestion(STEP_MS);
        }
        
        Evitement_gestion(STEP_MS);

        // Routine à 2 ms
        if(temps_ms % STEP_MS_GYRO == 0){
            if(get_position_avec_gyroscope()){
                Gyro_Read(STEP_MS_GYRO);
            }
        }
    }
}