From 0237ccb8fe4b8d2fed8cb940ddcaca0d7f92bae0 Mon Sep 17 00:00:00 2001 From: Samuel Date: Sat, 18 Mar 2023 17:59:15 +0100 Subject: [PATCH] Accostage OK --- CMakeLists.txt | 4 ++- Commande_vitesse.c | 9 ++++++ Commande_vitesse.h | 3 +- Strategie.h | 4 ++- Strategie_prise_cerises.c | 55 ++++++++++++++++++++++++++------- Test.c | 11 ++++++- Test.h | 4 ++- Test_strategie.c | 64 +++++++++++++++++++++++++++++++++++++++ Test_strategie.h | 1 + i2c_annexe.h | 3 ++ 10 files changed, 142 insertions(+), 16 deletions(-) create mode 100644 Test_strategie.c create mode 100644 Test_strategie.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 19693bf..385dd39 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,15 +22,17 @@ i2c_annexe.c Localisation.c Moteurs.c Robot_config.c +Servomoteur.c +Strategie_prise_cerises.c Temps.c Test.c +Test_strategie.c Trajet.c Trajectoire.c Trajectoire_bezier.c Trajectoire_circulaire.c Trajectoire_droite.c Trajectoire_rotation.c -Servomoteur.c spi_nb.c) pico_generate_pio_header(test ${CMAKE_CURRENT_LIST_DIR}/quadrature_encoder.pio) diff --git a/Commande_vitesse.c b/Commande_vitesse.c index 2a28bd7..7f15a50 100644 --- a/Commande_vitesse.c +++ b/Commande_vitesse.c @@ -2,6 +2,8 @@ #include "Geometrie_robot.h" #include "Commande_vitesse.h" + + /// @brief Commande une rotation autour d'un point dans le référentiel du robot. /// Cette fonction déplace le torseur cinétique du centre de rotation vers le centre du robot /// pour obtenir la vitesse de ce dernier. @@ -33,4 +35,11 @@ void commande_vitesse(double vitesse_x_mm_s, double vitesse_y_mm_s, double orien AsserMoteur_setConsigne_mm_s(MOTEUR_B, vitesse_roue_b); AsserMoteur_setConsigne_mm_s(MOTEUR_C, vitesse_roue_c); +} + +/// @brief Arrête le robot. +void commande_vitesse_stop(){ + AsserMoteur_setConsigne_mm_s(MOTEUR_A, 0); + AsserMoteur_setConsigne_mm_s(MOTEUR_B, 0); + AsserMoteur_setConsigne_mm_s(MOTEUR_C, 0); } \ No newline at end of file diff --git a/Commande_vitesse.h b/Commande_vitesse.h index 61d0cdb..2dfb34f 100644 --- a/Commande_vitesse.h +++ b/Commande_vitesse.h @@ -1,2 +1,3 @@ void commande_vitesse(double vitesse_x_mm_s, double vitesse_y_mm_s, double orientation_radian_s); -void commande_rotation(double rotation_rad_s, double centre_x, double centre_y); \ No newline at end of file +void commande_rotation(double rotation_rad_s, double centre_x, double centre_y); +void commande_vitesse_stop(void); \ No newline at end of file diff --git a/Strategie.h b/Strategie.h index 2c399f4..ce302a3 100644 --- a/Strategie.h +++ b/Strategie.h @@ -1,4 +1,6 @@ enum etat_action_t{ ACTION_EN_COURS, ACTION_TERMINEE -}; \ No newline at end of file +}; + +enum etat_action_t cerise_accostage(void); \ No newline at end of file diff --git a/Strategie_prise_cerises.c b/Strategie_prise_cerises.c index 7f419e4..9a316d1 100644 --- a/Strategie_prise_cerises.c +++ b/Strategie_prise_cerises.c @@ -3,22 +3,25 @@ #include "Geometrie.h" #include "Geometrie_robot.h" #include "math.h" +#include "i2c_annexe.h" // Rotation en rad/s pour accoster les cerises #define ROTATION_CERISE 0.5f +void commande_rotation_contacteur_longer_A(); +void commande_rotation_contacteur_longer_C(); + double vitesse_accostage_mm_s=100; enum etat_action_t cerise_accostage(void){ enum etat_action_t etat_action = ACTION_EN_COURS; - struct position_t contacteur_gauche, contacteur_droit; double rotation; static enum { CERISE_AVANCE_DROIT, - CERISE_TOURNE_CONTACTEUR_GAUCHE, - CERISE_TOURNE_CONTACTEUR_DROIT, + CERISE_TOURNE_CONTACTEUR_LONGER_A, + CERISE_TOURNE_CONTACTEUR_LONGER_C, CERISE_ACCOSTE } etat_accostage=CERISE_AVANCE_DROIT; @@ -26,17 +29,47 @@ enum etat_action_t cerise_accostage(void){ { case CERISE_AVANCE_DROIT: commande_vitesse(vitesse_accostage_mm_s * cos(-M_PI/6), vitesse_accostage_mm_s * sin(-M_PI/6), 0); + if(i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_ACTIF){ + etat_accostage=CERISE_TOURNE_CONTACTEUR_LONGER_A; + } + if(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_ACTIF){ + etat_accostage=CERISE_TOURNE_CONTACTEUR_LONGER_C; + } + if (i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_ACTIF && i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_ACTIF){ + etat_accostage=CERISE_ACCOSTE; + } break; - case CERISE_TOURNE_CONTACTEUR_GAUCHE: - rotation = ROTATION_CERISE; - commande_rotation(ROTATION_CERISE, contacteur_gauche.x_mm, contacteur_gauche.y_mm); + case CERISE_TOURNE_CONTACTEUR_LONGER_A: + commande_rotation_contacteur_longer_A(); + if(i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF){ + if(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF){ + etat_accostage = CERISE_AVANCE_DROIT; + }else{ + etat_accostage = CERISE_TOURNE_CONTACTEUR_LONGER_A; + } + }else if(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_ACTIF){ + etat_accostage = CERISE_ACCOSTE; + } break; - case CERISE_TOURNE_CONTACTEUR_DROIT: - rotation = ROTATION_CERISE; - commande_rotation(-ROTATION_CERISE, contacteur_droit.x_mm, contacteur_droit.y_mm); + case CERISE_TOURNE_CONTACTEUR_LONGER_C: + commande_rotation_contacteur_longer_C(); + if(i2c_annexe_get_contacteur_longer_C() == CONTACTEUR_INACTIF){ + if(i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_INACTIF){ + etat_accostage = CERISE_AVANCE_DROIT; + }else{ + etat_accostage = CERISE_TOURNE_CONTACTEUR_LONGER_C; + } + }else if(i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_ACTIF){ + etat_accostage = CERISE_ACCOSTE; + } break; + + case CERISE_ACCOSTE: + commande_vitesse_stop(); + //etat_accostage = CERISE_AVANCE_DROIT; + etat_action = ACTION_TERMINEE; default: break; @@ -46,9 +79,9 @@ 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, -RAYON_ROBOT* RACINE_DE_3/2); + commande_rotation(ROTATION_CERISE, RAYON_ROBOT/2, -RAYON_ROBOT* RACINE_DE_3/2); } \ No newline at end of file diff --git a/Test.c b/Test.c index 2d2789c..a3ba22d 100644 --- a/Test.c +++ b/Test.c @@ -26,6 +26,9 @@ #include "Trajectoire.h" #include "Trajet.h" +#include "Test_strategie.h" +#include "Test.h" + #define V_INIT -999.0 #define TEST_TIMEOUT_US 10000000 @@ -65,6 +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("H - Asser Position - avance\n"); printf("I - Asser Position - avance et tourne (gyro)\n"); printf("J - Asser Position - avance et tourne (sans gyro)\n"); @@ -107,6 +111,11 @@ int mode_test(){ while(test_cde_vitesse()); break; + case 'F': + case 'f': + while(test_accostage()); + break; + case 'H': case 'h': while(test_asser_position_avance()); @@ -695,7 +704,7 @@ int test_aller_retour(){ case 'e': case 'E': Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); - Trajectoire_droite(&trajectoire, 0, 0, 0, 700, 0, M_PI); + Trajectoire_droite(&trajectoire, 0, 0, 0, 1500, 0, M_PI); printf("Trajectoire droite avec rotation (OK)\n"); break; diff --git a/Test.h b/Test.h index 019bb5e..fe54c20 100644 --- a/Test.h +++ b/Test.h @@ -1 +1,3 @@ -int mode_test(); \ No newline at end of file + +int mode_test(); +void test_trajectoire_teleplot(); \ No newline at end of file diff --git a/Test_strategie.c b/Test_strategie.c new file mode 100644 index 0000000..bcf480d --- /dev/null +++ b/Test_strategie.c @@ -0,0 +1,64 @@ +#include "pico/multicore.h" +#include "stdio.h" +#include "hardware/i2c.h" + +#include "Asser_Moteurs.h" +#include "i2c_annexe.h" +#include "i2c_maitre.h" +#include "gyro.h" +#include "Localisation.h" +#include "QEI.h" +#include "Robot_config.h" +#include "Strategie.h" +#include "Temps.h" +#include "Trajet.h" +#include "Trajectoire.h" +#include "Test.h" + +int test_accostage(){ + int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init; + + i2c_maitre_init(); + Trajet_init(); + //printf("Init gyroscope\n"); + //Gyro_Init(); + + stdio_flush(); + + //set_position_avec_gyroscope(1); + + temps_ms = Temps_get_temps_ms(); + temps_ms_init = temps_ms; + do{ + i2c_gestion(i2c0); + i2c_annexe_gestion(); + // Routines à 1 ms + if(temps_ms != Temps_get_temps_ms()){ + temps_ms = Temps_get_temps_ms(); + QEI_update(); + Localisation_gestion(); + AsserMoteur_Gestion(_step_ms); + + // Routine à 2 ms + if(temps_ms % _step_ms_gyro == 0){ + // Gyro_Read(_step_ms_gyro); + } + + if(temps_ms > temps_ms_init + 200){ + if(cerise_accostage() == ACTION_TERMINEE){ + printf("Accostage_terminee\n"); + } + } + + /*printf(">contacteur_butee_A:%d\n", i2c_annexe_get_contacteur_butee_A()); + printf(">contacteur_butee_C:%d\n", i2c_annexe_get_contacteur_butee_C()); + printf(">contacteur_longer_A:%d\n", i2c_annexe_get_contacteur_longer_A()); + printf(">contacteur_longer_C:%d\n", i2c_annexe_get_contacteur_longer_C());*/ + + + } + lettre = getchar_timeout_us(0); + }while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0)); + printf("Lettre : %d; %c\n", lettre, lettre); + +} \ No newline at end of file diff --git a/Test_strategie.h b/Test_strategie.h new file mode 100644 index 0000000..cf42ebd --- /dev/null +++ b/Test_strategie.h @@ -0,0 +1 @@ +int test_accostage(void); \ No newline at end of file diff --git a/i2c_annexe.h b/i2c_annexe.h index d7d8e77..550747d 100644 --- a/i2c_annexe.h +++ b/i2c_annexe.h @@ -1,5 +1,8 @@ #include "pico/stdlib.h" +#define CONTACTEUR_ACTIF 0 +#define CONTACTEUR_INACTIF 1 + void i2c_annexe_gestion(void); void i2c_annexe_active_turbine(void); void i2c_annexe_desactive_turbine(void);