diff --git a/Holonome2023.c b/Holonome2023.c index c770d92..a03c477 100644 --- a/Holonome2023.c +++ b/Holonome2023.c @@ -16,6 +16,7 @@ #include "QEI.h" #include "Servomoteur.h" #include "spi_nb.h" +#include "Strategie.h" #include "Temps.h" #include "Trajectoire.h" #include "Trajet.h" @@ -51,6 +52,12 @@ int main() { gpio_set_dir(LED_PIN_ROUGE, GPIO_OUT); gpio_put(LED_PIN_ROUGE, 0); + gpio_init(COULEUR); + gpio_init(TIRETTE); + gpio_set_dir(COULEUR, GPIO_IN); + gpio_set_dir(TIRETTE, GPIO_IN); + + // 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); diff --git a/Strategie.c b/Strategie.c index a40ff6d..4a9384f 100644 --- a/Strategie.c +++ b/Strategie.c @@ -1,23 +1,25 @@ - +#include "hardware/gpio.h" +#include "i2c_annexe.h" +#include "Geometrie_robot.h" #include "Localisation.h" +#include "Moteurs.h" #include "Strategie_prise_cerises.h" #include "Strategie.h" #include "Trajet.h" #include "math.h" #define DEGREE_EN_RADIAN (M_PI / 180.) +#define SEUIL_RECAL_DIST_MM 75 +#define SEUIL_RECAL_ANGLE_RADIAN (5 * DEGREE_EN_RADIAN) + +enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint32_t step_ms); +enum etat_action_t calage_angle(enum longer_direction_t longer_direction, double x_mm, double y_mm, double angle_radian); +enum etat_action_t lance_balles(uint32_t step_ms); + +enum etat_strategie_t etat_strategie=STRATEGIE_INIT; void Homologation(uint32_t step_ms){ - static enum etat_strategie_t{ - STRATEGIE_INIT, - APPROCHE_CERISE_1_A, - APPROCHE_CERISE_1_B, - ATTRAPE_CERISE_1, - APPROCHE_PANIER_1_A, - APPROCHE_PANIER_1_B, - CALAGE_PANIER_1, - STRATEGIE_FIN - }etat_strategie=STRATEGIE_INIT; + enum etat_action_t etat_action; enum etat_trajet_t etat_trajet; @@ -47,30 +49,157 @@ void Homologation(uint32_t step_ms){ case ATTRAPE_CERISE_1: etat_action = cerise_attraper_bordure(LONGER_VERS_C, step_ms); if(etat_action == ACTION_TERMINEE){ - etat_strategie = APPROCHE_PANIER_1_A; + etat_strategie = APPROCHE_PANIER_1; } break; - case APPROCHE_PANIER_1_A: + case APPROCHE_PANIER_1: + Trajet_config(500, 500); Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 485, Localisation_get().y_mm, 465, 857, 465,2830, +30. * DEGREE_EN_RADIAN, +120. * DEGREE_EN_RADIAN); - Trajet_debut_trajectoire(trajectoire); - etat_strategie = APPROCHE_PANIER_1_B; + + if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ + etat_strategie = CALAGE_PANIER_1; + } + + break; + + case CALAGE_PANIER_1: + if(calage_angle(LONGER_VERS_A, RAYON_ROBOT, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)), 120. *DEGREE_EN_RADIAN) == ACTION_TERMINEE){ + etat_strategie = RECULE_PANIER; + } break; - case APPROCHE_PANIER_1_B: - etat_trajet = Trajet_avance(step_ms/1000.); - if(etat_trajet == TRAJET_TERMINE){ - etat_strategie = ATTRAPE_CERISE_1; + case RECULE_PANIER: + Trajet_config(250, 500); + Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, + 225, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 120, + 120. * DEGREE_EN_RADIAN, +240. * DEGREE_EN_RADIAN); + + if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){ + etat_strategie = LANCE_DANS_PANIER; + } + break; + + case LANCE_DANS_PANIER: + if(lance_balles(step_ms) == ACTION_TERMINEE){ + etat_strategie = STRATEGIE_FIN; } break; - case STRATEGIE_FIN: + Moteur_Stop(); break; } } + + +/// @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){ + enum etat_action_t etat_action = ACTION_EN_COURS; + uint32_t tempo_ms; + + static enum{ + LANCE_PROPULSEUR_ON, + LANCE_TEMPO_PROP_ON, + LANCE_PORTE_OUVERTE, + } etat_lance_balle = LANCE_PROPULSEUR_ON; + + switch(etat_lance_balle){ + case LANCE_PROPULSEUR_ON: + i2c_annexe_active_propulseur(); + tempo_ms = 2000; + etat_lance_balle = LANCE_TEMPO_PROP_ON; + break; + + case LANCE_TEMPO_PROP_ON: + if (tempo_ms < step_ms){ + etat_lance_balle = LANCE_PORTE_OUVERTE; + i2c_annexe_ouvre_porte(); + tempo_ms = 6000; + }else{ + tempo_ms -= step_ms; + } + break; + + case LANCE_PORTE_OUVERTE: + if (tempo_ms < step_ms){ + etat_lance_balle = LANCE_PROPULSEUR_ON; + i2c_annexe_desactive_propulseur(); + etat_action = ACTION_TERMINEE; + } + break; + + + } + return etat_action; +} + +/// @brief Envoie le robot se caler dans l'angle en face de lui, recale la localisation +enum etat_action_t calage_angle(enum longer_direction_t longer_direction, double x_mm, double y_mm, double angle_radian){ + enum etat_action_t etat_action = ACTION_EN_COURS; + struct position_t position; + + avance_puis_longe_bordure(longer_direction); + if( ((longer_direction == LONGER_VERS_A) && (i2c_annexe_get_contacteur_butee_A() == CONTACTEUR_ACTIF) ) || + ((longer_direction == LONGER_VERS_C) && (i2c_annexe_get_contacteur_butee_C() == CONTACTEUR_ACTIF) ) ){ + etat_action = ACTION_TERMINEE; + + position = Localisation_get(); + if(fabs(position.x_mm - x_mm) < SEUIL_RECAL_DIST_MM){ + Localisation_set_x(x_mm); + } + 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); + } + } + return etat_action; +} + +enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint32_t step_ms){ + enum etat_action_t etat_action = ACTION_EN_COURS; + enum etat_trajet_t etat_trajet; + static enum { + PARCOURS_INIT, + PARCOURS_AVANCE, + } etat_parcourt=PARCOURS_INIT; + + switch (etat_parcourt){ + case PARCOURS_INIT: + Trajet_debut_trajectoire(trajectoire); + etat_parcourt = PARCOURS_AVANCE; + break; + + case PARCOURS_AVANCE: + etat_trajet = Trajet_avance(step_ms/1000.); + if(etat_trajet == TRAJET_TERMINE){ + etat_action = ACTION_TERMINEE; + etat_parcourt = PARCOURS_INIT; + } + 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; + +} \ No newline at end of file diff --git a/Strategie.h b/Strategie.h index 72778bd..4666978 100644 --- a/Strategie.h +++ b/Strategie.h @@ -3,6 +3,9 @@ #ifndef STRATEGIE_H #define STRATEGIE_H +#define COULEUR 15 +#define TIRETTE 14 + enum etat_action_t{ ACTION_EN_COURS, ACTION_TERMINEE @@ -13,9 +16,30 @@ enum longer_direction_t{ LONGER_VERS_C }; +enum couleur_t{ + COULEUR_BLEU=0, + COULEUR_VERT +}; + +extern enum etat_strategie_t{ + STRATEGIE_INIT=0, + APPROCHE_CERISE_1_A=1, + APPROCHE_CERISE_1_B=2, + ATTRAPE_CERISE_1=3, + APPROCHE_PANIER_1=4, + CALAGE_PANIER_1=5, + RECULE_PANIER=6, + LANCE_DANS_PANIER=7, + STRATEGIE_FIN=254, +}etat_strategie; + enum etat_action_t cerise_accostage(void); -enum etat_action_t cerise_longer_bord(enum longer_direction_t longer_direction); +enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_direction); void Homologation(uint32_t step_ms); +enum couleur_t lire_couleur(void); +uint attente_tirette(void); + + // STRATEGIE_H #endif \ No newline at end of file diff --git a/Strategie_prise_cerises.c b/Strategie_prise_cerises.c index b9d2d71..b70c818 100644 --- a/Strategie_prise_cerises.c +++ b/Strategie_prise_cerises.c @@ -49,7 +49,7 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct break; case ATTRAPE_VERS_BORDURE: - cerise_longer_bord(longer_direction); + avance_puis_longe_bordure(longer_direction); if( (longer_direction == LONGER_VERS_A) && (i2c_annexe_get_contacteur_butee_A() == CONTACTEUR_ACTIF) || (longer_direction == LONGER_VERS_C) && (i2c_annexe_get_contacteur_butee_C() == CONTACTEUR_ACTIF) ){ etat_attrape = TURBINE_DEMARRAGE; @@ -59,6 +59,7 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct case TURBINE_DEMARRAGE: i2c_annexe_ferme_porte(); //i2c_annexe_active_turbine(); + i2c_annexe_active_propulseur(); commande_vitesse_stop(); tempo_ms = 2000; etat_attrape = TURBINE_DEMARRAGE_TEMPO; @@ -76,7 +77,7 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct case ASPIRE_LONGE: longer_direction_aspire = inverser_longe_direction(longer_direction); - cerise_longer_bord(LONGER_VERS_A); + avance_puis_longe_bordure(LONGER_VERS_A); // La fonction cerise_longer_bord n'est efficace que tant que le robot a ses deux contacteur sur le support // Le robot n'a les deux contacteurs sur le support que tant qu'il est à moins de 240mm (MAX_LONGE_MM) de la bordure // En fonction du demi-terrain sur lequel se trouve le robot, on surveille la position en Z pour respecter cette condition @@ -99,7 +100,7 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct /// @brief Fonction pour accoster et longer une bordure /// @param longer_direction : direction dans laquelle le robot va aller une fois le long de la bordure /// @return ACTION_EN_COURS -enum etat_action_t cerise_longer_bord(enum longer_direction_t longer_direction){ +enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_direction){ static enum { LONGE_INIT, diff --git a/Test_strategie.c b/Test_strategie.c index 85268fe..f6b1d99 100644 --- a/Test_strategie.c +++ b/Test_strategie.c @@ -19,6 +19,7 @@ int test_accostage(void); int test_longe(void); int test_homologation(void); +int test_tirette_et_couleur(); void affichage_test_strategie(){ uint32_t temps; @@ -38,6 +39,20 @@ void affichage_test_strategie(){ printf(">c_pos_y:%ld:%f\n", temps, Trajet_get_consigne().y_mm); printf(">c_pos_angle:%ld:%f\n", temps, Trajet_get_consigne().angle_radian); + printf(">etat_strat:%d\n",etat_strategie); + + /*switch(etat_strategie){ + case STRATEGIE_INIT: printf(">etat_strat:STRATEGIE_INIT|t\n"); break; + case APPROCHE_CERISE_1_A: printf(">etat_strat:APPROCHE_CERISE_1_A|t\n");break; + case APPROCHE_CERISE_1_B: printf(">etat_strat:APPROCHE_CERISE_1_B|t\n");break; + case ATTRAPE_CERISE_1: printf(">etat_strat:ATTRAPE_CERISE_1|t\n");break; + case APPROCHE_PANIER_1: printf(">etat_strat:APPROCHE_PANIER_1|t\n");break; + case CALAGE_PANIER_1: printf(">etat_strat:CALAGE_PANIER_1|t\n");break; + case RECULE_PANIER: printf(">etat_strat:RECULE_PANIER|t\n");break; + case LANCE_DANS_PANIER: printf(">etat_strat:LANCE_DANS_PANIER|t\n");break; + case STRATEGIE_FIN: printf(">etat_strat:STRATEGIE_FIN|t\n");break; + }*/ + sleep_ms(100); } } @@ -47,6 +62,7 @@ int test_strategie(){ printf("L - longer.\n"); printf("A - Accoster.\n"); printf("H - Homologation.\n"); + printf("C - Couleur et tirette.\n"); int lettre; do{ lettre = getchar_timeout_us(0); @@ -57,6 +73,11 @@ int test_strategie(){ while(test_accostage()); break; + case 'c': + case 'C': + while(test_tirette_et_couleur()); + break; + case 'h': case 'H': while(test_homologation()); @@ -67,8 +88,6 @@ int test_strategie(){ while(test_longe()); break; - - case 'q': case 'Q': return 0; @@ -158,7 +177,7 @@ int test_longe(){ } if(temps_ms > temps_ms_init + 200){ - if(cerise_longer_bord(LONGER_VERS_A) == ACTION_TERMINEE){ + if(avance_puis_longe_bordure(LONGER_VERS_A) == ACTION_TERMINEE){ printf("Accostage_terminee\n"); } } @@ -220,4 +239,27 @@ int test_accostage(){ return 0; } return 1; +} + + +int test_tirette_et_couleur(){ + int lettre; + uint couleur, tirette; + printf("Tirette et couleur\n"); + + stdio_flush(); + + do{ + printf(">tirette:%d\n", attente_tirette()); + if(lire_couleur() == COULEUR_VERT){ + printf(">couleur:Vert|t\n"); + }else{ + printf(">couleur:Bleu|t\n"); + } + sleep_ms(10); + + lettre = getchar_timeout_us(0); + }while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0)); + + } \ No newline at end of file