diff --git a/CMakeLists.txt b/CMakeLists.txt index 385dd39..873fdc9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,6 +23,7 @@ Localisation.c Moteurs.c Robot_config.c Servomoteur.c +Strategie.c Strategie_prise_cerises.c Temps.c Test.c diff --git a/Strategie.c b/Strategie.c index 15c4188..d0b1468 100644 --- a/Strategie.c +++ b/Strategie.c @@ -1,12 +1,56 @@ +#include "Localisation.h" +#include "Strategie_prise_cerises.h" +#include "Strategie.h" +#include "Trajet.h" +#include "math.h" -void Homologation(void){ +#define DEGREE_EN_RADIAN (M_PI / 180.) + +void Homologation(uint32_t step_ms){ static enum etat_strategie_t{ - STRATEGIE_INIT - }etat_strategie; + STRATEGIE_INIT, + APPROCHE_CERISE_1_A, + APPROCHE_CERISE_1_B, + ATTRAPE_CERISE_1, + STRATEGIE_FIN + }etat_strategie=STRATEGIE_INIT; + + enum etat_action_t etat_action; + enum trajet_etat_t etat_trajet; + + struct trajectoire_t trajectoire; switch(etat_strategie){ case STRATEGIE_INIT: - + Localisation_set(775., 109., -60. * DEGREE_EN_RADIAN); + etat_strategie = APPROCHE_CERISE_1_A; + break; + + case APPROCHE_CERISE_1_A: + Trajet_config(250, 500); + Trajectoire_droite(&trajectoire,775, 109, 857, 156, -60. * DEGREE_EN_RADIAN, +30. * DEGREE_EN_RADIAN); + Trajet_debut_trajectoire(trajectoire); + etat_strategie = APPROCHE_CERISE_1_B; + break; + + case APPROCHE_CERISE_1_B: + etat_trajet = Trajet_avance(step_ms/1000.); + if(etat_trajet == TRAJET_TERMINE){ + etat_strategie = ATTRAPE_CERISE_1; + } + break; + + case ATTRAPE_CERISE_1: + etat_action = cerise_attraper_bordure(LONGER_VERS_C, step_ms); + if(etat_action == ACTION_TERMINEE){ + etat_strategie = STRATEGIE_FIN; + } + break; + + case STRATEGIE_FIN: + break; + + } } \ No newline at end of file diff --git a/Strategie.h b/Strategie.h index eba172a..72778bd 100644 --- a/Strategie.h +++ b/Strategie.h @@ -1,3 +1,8 @@ +#include "pico/stdlib.h" + +#ifndef STRATEGIE_H +#define STRATEGIE_H + enum etat_action_t{ ACTION_EN_COURS, ACTION_TERMINEE @@ -10,4 +15,7 @@ enum longer_direction_t{ enum etat_action_t cerise_accostage(void); enum etat_action_t cerise_longer_bord(enum longer_direction_t longer_direction); -void Homologation(void); \ No newline at end of file +void Homologation(uint32_t step_ms); + +// STRATEGIE_H +#endif \ No newline at end of file diff --git a/Strategie_prise_cerises.c b/Strategie_prise_cerises.c index ce754a1..5355086 100644 --- a/Strategie_prise_cerises.c +++ b/Strategie_prise_cerises.c @@ -4,25 +4,104 @@ #include "Commande_vitesse.h" #include "Geometrie.h" #include "Geometrie_robot.h" +#include "Localisation.h" #include "math.h" #include "i2c_annexe.h" // Rotation en rad/s pour accoster les cerises #define ROTATION_CERISE 0.5f +// Distance la plus éloignée du bord où le robot est capable d'aspirer les cerises en longeant la bodure. +#define MAX_LONGE_MM 240 + // Translation en mm/s pour aspirer les cerises -#define TRANSLATION_CERISE 15 +#define TRANSLATION_CERISE 45 void commande_rotation_contacteur_longer_A(); void commande_rotation_contacteur_longer_C(); void commande_translation_longer_vers_A(); void commande_translation_longer_vers_C(); +enum longer_direction_t inverser_longe_direction(enum longer_direction_t direction); double vitesse_accostage_mm_s=100; -enum etat_action_t cerise_longer_bord(enum longer_direction_t longer_direction){ +/// @brief Fonction pour attraper les cerises sur les supports perpendiculaires à la bordure. +/// Le robot accoste, longe le support cerise vers la bordure, active la turbine, puis longe le support cerise jusqu'à son bout. +/// @param longer_direction : direction dans laquelle se trouve la bordure +/// @return ACTION_EN_COURS ou ACTION_TERMINEE +enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms){ enum etat_action_t etat_action = ACTION_EN_COURS; + enum longer_direction_t longer_direction_aspire; + static uint32_t tempo_ms = 0; + static enum { + ATTRAPE_INIT, + ATTRAPE_VERS_BORDURE, + TURBINE_DEMARRAGE, + TURBINE_DEMARRAGE_TEMPO, + ASPIRE_LONGE, + ASPIRE_LIBRE + } etat_attrape=ATTRAPE_INIT; + + switch(etat_attrape){ + case ATTRAPE_INIT: + etat_attrape=ATTRAPE_VERS_BORDURE; + printf("ATTRAPE_VERS_BORDURE\n"); + break; + + case ATTRAPE_VERS_BORDURE: + cerise_longer_bord(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; + printf("TURBINE_DEMARRAGE\n"); + } + break; + + case TURBINE_DEMARRAGE: + i2c_annexe_ferme_porte(); + //i2c_annexe_active_turbine(); + commande_vitesse_stop(); + tempo_ms = 2000; + etat_attrape = TURBINE_DEMARRAGE_TEMPO; + printf("TURBINE_DEMARRAGE_TEMPO\n"); + break; + + case TURBINE_DEMARRAGE_TEMPO: + if(tempo_ms < step_ms){ + etat_attrape = ASPIRE_LONGE; + printf("ASPIRE_LONGE\n"); + } + tempo_ms -= step_ms; + break; + + case ASPIRE_LONGE: + longer_direction_aspire = inverser_longe_direction(longer_direction); + cerise_longer_bord(longer_direction_aspire); + // 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 + if( ((Localisation_get().y_mm > 1500) && (Localisation_get().y_mm < (3000 - MAX_LONGE_MM) )) || + ((Localisation_get().y_mm < 1500) && (Localisation_get().y_mm > (MAX_LONGE_MM))) ){ + etat_attrape = ASPIRE_LIBRE; + printf("ASPIRE_LIBRE\n"); + } + break; + + case ASPIRE_LIBRE: + commande_vitesse_stop(); + etat_action = ACTION_TERMINEE; + break; + } + return etat_action; +} + + +/// @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){ + static enum { LONGE_INIT, LONGE_VERS_A, @@ -64,7 +143,7 @@ enum etat_action_t cerise_longer_bord(enum longer_direction_t longer_direction){ printf("Longer INIT\n"); } } - return etat_action; + return ACTION_EN_COURS; } @@ -143,11 +222,11 @@ enum etat_action_t cerise_accostage(void){ } void commande_rotation_contacteur_longer_A(){ - commande_rotation(-ROTATION_CERISE, RAYON_ROBOT, 0); + commande_rotation(ROTATION_CERISE, RAYON_ROBOT, 0); } void commande_rotation_contacteur_longer_C(){ - commande_rotation(ROTATION_CERISE, RAYON_ROBOT/2.0, -RAYON_ROBOT* RACINE_DE_3/2.0); + commande_rotation(-ROTATION_CERISE, RAYON_ROBOT/2.0, -RAYON_ROBOT* RACINE_DE_3/2.0); } void commande_translation_longer_vers_A(){ @@ -161,3 +240,10 @@ void commande_translation_longer_vers_C(){ // V_y : -V * sin (60°) = -V * RACINE(3) / 2 commande_vitesse(-TRANSLATION_CERISE/2., -TRANSLATION_CERISE / 2. * RACINE_DE_3, 0); } + +enum longer_direction_t inverser_longe_direction(enum longer_direction_t direction){ + if(direction ==LONGER_VERS_A){ + return LONGER_VERS_C; + } + return LONGER_VERS_A; +} \ No newline at end of file diff --git a/Strategie_prise_cerises.h b/Strategie_prise_cerises.h index 738766b..5ef63bc 100644 --- a/Strategie_prise_cerises.h +++ b/Strategie_prise_cerises.h @@ -1 +1,2 @@ #include "Strategie.h" +enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms); diff --git a/Test.c b/Test.c index 27076c0..75c140e 100644 --- a/Test.c +++ b/Test.c @@ -68,7 +68,7 @@ int mode_test(){ printf("C - pour les codeurs\n"); printf("D - pour les codeurs (somme en mm)\n"); printf("E - Commande en vitesse...\n"); - printf("F - Accostage\n"); + printf("F - Strategie...\n"); printf("H - Asser Position - avance\n"); printf("I - Asser Position - avance et tourne (gyro)\n"); printf("J - Asser Position - avance et tourne (sans gyro)\n"); diff --git a/Test_strategie.c b/Test_strategie.c index d4c675f..85268fe 100644 --- a/Test_strategie.c +++ b/Test_strategie.c @@ -1,6 +1,7 @@ #include "pico/multicore.h" #include "stdio.h" #include "hardware/i2c.h" +#include "math.h" #include "Asser_Moteurs.h" #include "i2c_annexe.h" @@ -21,14 +22,24 @@ int test_homologation(void); void affichage_test_strategie(){ uint32_t temps; - temps = time_us_32()/1000; - printf(">contacteur_butee_A:%ld:%d\n", temps, i2c_annexe_get_contacteur_butee_A()); - printf(">contacteur_butee_C:%ld:%d\n", temps, i2c_annexe_get_contacteur_butee_C()); - printf(">contacteur_longer_A:%ld:%d\n", temps, i2c_annexe_get_contacteur_longer_A()); - printf(">contacteur_longer_C:%ld:%d\n", temps, i2c_annexe_get_contacteur_longer_C()); - printf(">V_consigne_A:%ld:%f\n>V_consigne_B:%ld:%f\n>V_consigne_C:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_A), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_B), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_C)); + + while(true){ + temps = time_us_32()/1000; + printf(">contacteur_butee_A:%ld:%d\n", temps, i2c_annexe_get_contacteur_butee_A()); + printf(">contacteur_butee_C:%ld:%d\n", temps, i2c_annexe_get_contacteur_butee_C()); + printf(">contacteur_longer_A:%ld:%d\n", temps, i2c_annexe_get_contacteur_longer_A()); + printf(">contacteur_longer_C:%ld:%d\n", temps, i2c_annexe_get_contacteur_longer_C()); + printf(">V_consigne_A:%ld:%f\n>V_consigne_B:%ld:%f\n>V_consigne_C:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_A), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_B), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_C)); + printf(">pos_x:%ld:%f\n", temps, Localisation_get().x_mm); + printf(">pos_y:%ld:%f\n", temps, Localisation_get().y_mm); + printf(">pos_angle:%ld:%f\n", temps, Localisation_get().angle_radian/M_PI*180.); - sleep_ms(10); + printf(">c_pos_x:%ld:%f\n", temps, Trajet_get_consigne().x_mm); + 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); + + sleep_ms(100); + } } @@ -68,15 +79,18 @@ int test_strategie(){ int test_homologation(){ int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init; + printf("Homologation\n"); i2c_maitre_init(); Trajet_init(); //printf("Init gyroscope\n"); - Gyro_Init(); + //Gyro_Init(); stdio_flush(); - set_position_avec_gyroscope(1); + //set_position_avec_gyroscope(1); + + multicore_launch_core1(affichage_test_strategie); temps_ms = Temps_get_temps_ms(); temps_ms_init = temps_ms; @@ -92,24 +106,24 @@ int test_homologation(){ // Routine à 2 ms if(temps_ms % _step_ms_gyro == 0){ - // Gyro_Read(_step_ms_gyro); - } - - if(temps_ms > temps_ms_init + 200){ - if(cerise_longer_bord(LONGER_VERS_A) == ACTION_TERMINEE){ - printf("Accostage_terminee\n"); + if(get_position_avec_gyroscope()){ + Gyro_Read(_step_ms_gyro); } } + Homologation(_step_ms); + } lettre = getchar_timeout_us(0); - }while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0)); + //}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0)); + }while(1); + printf("STRATEGIE_LOOP_2\n"); printf("Lettre : %d; %c\n", lettre, lettre); if(lettre == 'q' && lettre == 'Q'){ return 0; } - return 1; + return 0; }