From c5e76ba440dd8bc1012634d50297cd509df0786f Mon Sep 17 00:00:00 2001 From: Samuel Date: Sun, 7 Apr 2024 22:26:36 +0200 Subject: [PATCH] =?UTF-8?q?Premier=20pot=20attrap=C3=A9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Geometrie.c | 4 +- Geometrie.h | 1 + Strategie.h | 1 + Strategie_2024_pots.c | 85 ++++++++++++++++++++++++++++++++++++++++--- Strategie_2024_pots.h | 1 + Test_i2c.c | 10 ++--- Test_strategie_2024.c | 82 ++++++++++++++++++++++++++++++++++++++++- i2c_annexe.c | 6 +-- 8 files changed, 174 insertions(+), 16 deletions(-) diff --git a/Geometrie.c b/Geometrie.c index 723abfb..1d3bcaf 100644 --- a/Geometrie.c +++ b/Geometrie.c @@ -75,8 +75,8 @@ unsigned int Geometrie_intersecte_plage_angle(float angle1_min, float angle1_max struct position_t Geometrie_deplace(struct position_t position_depart, float distance_mm){ struct position_t position_arrivée; position_arrivée.angle_radian = position_depart.angle_radian; - position_arrivée.x_mm = cosf(position_depart.angle_radian) * distance_mm; - position_arrivée.y_mm = sinf(position_depart.angle_radian) * distance_mm; + position_arrivée.x_mm = position_depart.x_mm + cosf(position_depart.angle_radian) * distance_mm; + position_arrivée.y_mm = position_depart.y_mm + sinf(position_depart.angle_radian) * distance_mm; return position_arrivée; } diff --git a/Geometrie.h b/Geometrie.h index 7495042..0fd96fd 100644 --- a/Geometrie.h +++ b/Geometrie.h @@ -17,5 +17,6 @@ float Geometrie_get_angle_normalisee(float angle); unsigned int Geometrie_compare_angle(float angle, float angle_min, float angle_max); unsigned int Geometrie_intersecte_plage_angle(float angle1_min, float angle1_max, float angle2_min, float angle2_max); float Geometrie_get_angle_optimal(float angle_depart, float angle_souhaite); +struct position_t Geometrie_deplace(struct position_t position_depart, float distance_mm); #endif diff --git a/Strategie.h b/Strategie.h index cf76211..d28bcd7 100644 --- a/Strategie.h +++ b/Strategie.h @@ -67,6 +67,7 @@ enum etat_action_t Strategie_pieds_dans_plat(enum couleur_t couleur, uint32_t st enum etat_action_t Strategie_aller_a(float pos_x, float pos_y, uint32_t step_ms); enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_ms); +enum etat_action_t Strategie_tourner_et_aller_a(float pos_x, float pos_y, float angle_radian, enum evitement_t evitement,uint32_t step_ms); extern float distance_obstacle; diff --git a/Strategie_2024_pots.c b/Strategie_2024_pots.c index 856d3c4..58df870 100644 --- a/Strategie_2024_pots.c +++ b/Strategie_2024_pots.c @@ -2,8 +2,10 @@ #include "Strategie.h" #include "Geometrie_robot.h" #include "Strategie_2024_pots.h" +#include "i2c_annexe.h" -#define DISTANCE_APPROCHE_POT +#define DISTANCE_APPROCHE_POT_MM 250. +#define DISTANCE_ATTRAPE_POT_MM 100. float angle_bras[6] = { @@ -17,8 +19,8 @@ float angle_bras[6] = struct position_t position_pots_dans_groupe_pot[5] = { - {.x_mm = -40, .y_mm = 69.2, .angle_radian = 120 * DEGRE_EN_RADIAN}, - {.x_mm = 40, .y_mm = 69.2, .angle_radian = 60 * DEGRE_EN_RADIAN}, + {.x_mm = -40, .y_mm = 69.2, .angle_radian = -60 * DEGRE_EN_RADIAN}, + {.x_mm = 40, .y_mm = 69.2, .angle_radian = -120 * DEGRE_EN_RADIAN}, {.x_mm = -80, .y_mm = 0, .angle_radian = -90 * DEGRE_EN_RADIAN}, {.x_mm = 80, .y_mm = 0, .angle_radian = -90 * DEGRE_EN_RADIAN}, {.x_mm = 0, .y_mm = 0, .angle_radian = -90 * DEGRE_EN_RADIAN} @@ -53,14 +55,35 @@ struct position_t groupe_pot_get_pot(unsigned int groupe_pot, unsigned int num_p sinf(angle_groupe_pot) * position_pots_dans_groupe_pot[num_pot].x_mm + cosf(angle_groupe_pot) * position_pots_dans_groupe_pot[num_pot].y_mm; - position_pot.angle_radian = my_position_groupe_pot.angle_radian + angle_groupe_pot; + position_pot.angle_radian = position_pots_dans_groupe_pot[num_pot].angle_radian + angle_groupe_pot; return position_pot; } + +int get_bras_libre(void){ + return BRAS_1; +} + /// @brief Fonction qui déplace le robot jusqu'à la zone pour attraper les pots et qui attrape les 5 pots -enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot){ +enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step_ms){ // Parcourir la trajectoire pour aller jusqu'au premier pot + struct position_t position_pot, position_approche_pot, position_attrape_pot; + enum etat_action_t etat_action; + + static int bras, tempo_ms; + + // Pour le 1er pot + position_pot = groupe_pot_get_pot(groupe_pot, POT_1); + position_approche_pot = Geometrie_deplace(position_pot, -DISTANCE_APPROCHE_POT_MM); + position_attrape_pot = Geometrie_deplace(position_pot, -DISTANCE_ATTRAPE_POT_MM); + + static enum { + AP_ALLER_VERS_GROUPE_POT, + AP_APPROCHE_POT, + AP_ATTRAPE_POT, + AP_RETOUR_ET_LEVE_POT + } etat_attrape_pot = AP_ALLER_VERS_GROUPE_POT; // Pour chaque pot // Baisser le bras correspondant @@ -69,4 +92,56 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot){ // Reculer dans l'axe de prise et rejoindre le point de prise suivant // Pendant le mouvement, apres 1 sec (à confirmer) Lever le bras + switch (etat_attrape_pot) + { + case AP_ALLER_VERS_GROUPE_POT: + printf("position_pot X:%f Y:%f r:%f\n", position_pot.x_mm, position_pot.y_mm, + position_pot.angle_radian / DEGRE_EN_RADIAN); + printf("position_approche_pot X:%f Y:%f r:%f\n", position_approche_pot.x_mm, position_approche_pot.y_mm, + position_approche_pot.angle_radian / DEGRE_EN_RADIAN); + etat_attrape_pot = AP_APPROCHE_POT; + case AP_APPROCHE_POT: + Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); + etat_action = Strategie_tourner_et_aller_a( + position_approche_pot.x_mm, position_approche_pot.y_mm, position_approche_pot.angle_radian - angle_bras[0], + SANS_EVITEMENT, step_ms); + if (etat_action == ACTION_TERMINEE){ + etat_attrape_pot = AP_ATTRAPE_POT; + bras = get_bras_libre(); + } + break; + + case AP_ATTRAPE_POT: + i2c_annexe_actionneur_pot(bras, BRAS_POT_SOL, DOIGT_TIENT); + etat_action = Strategie_tourner_et_aller_a( + position_attrape_pot.x_mm, position_attrape_pot.y_mm, position_attrape_pot.angle_radian - angle_bras[bras], + SANS_EVITEMENT, step_ms); + if (etat_action == ACTION_TERMINEE){ + tempo_ms=250; + etat_attrape_pot = AP_RETOUR_ET_LEVE_POT; + } + break; + + case AP_RETOUR_ET_LEVE_POT: + if(tempo_ms >= 0){ + tempo_ms -= step_ms; + }else{ + i2c_annexe_actionneur_pot(bras, BRAS_HAUT, DOIGT_TIENT); + } + etat_action = Strategie_tourner_et_aller_a( + position_approche_pot.x_mm, position_approche_pot.y_mm, position_attrape_pot.angle_radian - angle_bras[bras], + SANS_EVITEMENT, step_ms); + if (etat_action == ACTION_TERMINEE){ + etat_attrape_pot = AP_ALLER_VERS_GROUPE_POT; + // TODO: pot suivant + return ACTION_TERMINEE; + } + break; + + default: + break; + } + + return ACTION_EN_COURS; + } diff --git a/Strategie_2024_pots.h b/Strategie_2024_pots.h index 867991e..028bc05 100644 --- a/Strategie_2024_pots.h +++ b/Strategie_2024_pots.h @@ -21,3 +21,4 @@ #define BRAS_6 5 struct position_t groupe_pot_get_pot(unsigned int groupe_pot, unsigned int num_pot); +enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step_ms); diff --git a/Test_i2c.c b/Test_i2c.c index c70ae1c..3d19f2f 100644 --- a/Test_i2c.c +++ b/Test_i2c.c @@ -327,7 +327,7 @@ int test_i2c_ecriture_pico_annex_nb_2(){ switch(lettre){ case 'a': case 'A': - for(int i=1; i<7; i++){ + for(int i=0; i<6; i++){ i2c_annexe_actionneur_pot(i, BRAS_PLIE, DOIGT_LACHE); } printf("=> Lache pot\n"); @@ -335,7 +335,7 @@ int test_i2c_ecriture_pico_annex_nb_2(){ case 'b': case 'B': - for(int i=1; i<7; i++){ + for(int i=0; i<6; i++){ i2c_annexe_actionneur_pot(i, BRAS_POT_SOL, DOIGT_TIENT); } printf("=> Attrape pot\n"); @@ -343,7 +343,7 @@ int test_i2c_ecriture_pico_annex_nb_2(){ case 'c': case 'C': - for(int i=1; i<7; i++){ + for(int i=0; i<6; i++){ i2c_annexe_actionneur_pot(i, BRAS_PLIE, DOIGT_TIENT); } printf("=> Tient pot\n"); @@ -351,10 +351,10 @@ int test_i2c_ecriture_pico_annex_nb_2(){ case 'D': case 'd': - for(int i=1; i<7; i++){ + for(int i=0; i<6; i++){ i2c_annexe_actionneur_pot(i, BRAS_HAUT, DOIGT_TIENT); } - printf("=> Attrape pot\n"); + printf("=>Pot haut\n"); break; case 'E': diff --git a/Test_strategie_2024.c b/Test_strategie_2024.c index 126a912..a75bc8a 100644 --- a/Test_strategie_2024.c +++ b/Test_strategie_2024.c @@ -21,10 +21,13 @@ int test_calcul_position_pot(void); int test_calage_debut(void); +int test_attrape_pot(); void affichage_test_strategie_2024(void); int test_strategie_2024(){ printf("A - Position groupes pot.\n"); + printf("B - Calage debut.\n"); + printf("C - Attrape pot.\n"); int lettre; do{ @@ -41,6 +44,15 @@ int test_strategie_2024(){ while(test_calage_debut()); break; + case 'c': + case 'C': + while(test_attrape_pot()); + break; + + case 'q': + case 'Q': + return 0; + } } @@ -99,7 +111,7 @@ int test_calage_debut(){ i2c_maitre_init(); Trajet_init(); Balise_VL53L1X_init(); - Localisation_set(200,200,0); + Localisation_set(250,250,0); set_position_avec_gyroscope(0); @@ -154,6 +166,74 @@ int test_calage_debut(){ } +int test_attrape_pot(){ + int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init; + struct trajectoire_t trajectoire; + enum evitement_t evitement; + enum etat_action_t etat_action; + + + printf("test_attrape_pot\n"); + + + i2c_maitre_init(); + Trajet_init(); + Balise_VL53L1X_init(); + Localisation_set(250, 250, (45. * DEGRE_EN_RADIAN) - ANGLE_PINCE); + + + set_position_avec_gyroscope(0); + if(get_position_avec_gyroscope()){ + printf("Init gyroscope\n"); + Gyro_Init(); + } + + stdio_flush(); + Trajet_config(TRAJECT_CONFIG_STD); + + multicore_launch_core1(affichage_test_strategie_2024); + + + temps_ms = Temps_get_temps_ms(); + temps_ms_init = temps_ms; + do{ + i2c_gestion(i2c0); + i2c_annexe_gestion(); + Balise_VL53L1X_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); + Evitement_gestion(_step_ms); + + // Routine à 2 ms + if(temps_ms % _step_ms_gyro == 0){ + if(get_position_avec_gyroscope()){ + Gyro_Read(_step_ms_gyro); + } + } + + etat_action = Strat_2024_attrape_pot(GROUPE_POT_B1, _step_ms); + + } + lettre = getchar_timeout_us(0); + //}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0)); + }while(etat_action == ACTION_EN_COURS); + printf("STRATEGIE_LOOP_2\n"); + printf("Lettre : %d; %c\n", lettre, lettre); + + while(1){Moteur_Stop();} + + if(lettre == 'q' && lettre == 'Q'){ + return 0; + } + return 0; + +} + void affichage_test_strategie_2024(){ uint32_t temps; diff --git a/i2c_annexe.c b/i2c_annexe.c index 1095b71..cb66a18 100644 --- a/i2c_annexe.c +++ b/i2c_annexe.c @@ -164,11 +164,11 @@ void i2c_annexe_get_distances(uint8_t *distance_capteur_cm){ /// @param pos_bras Code de position du bras, voir les #define BRAS_PLIE, ... définis plus haut ou dans le .h /// @param pos_doigt Code de position du doigt, voir les #define DOIGT_LACHE, ... définis plus haut ou dans le .h void i2c_annexe_actionneur_pot(int actionneur, uint8_t pos_bras, uint8_t pos_doigt){ - if(actionneur < 1 || actionneur > 6){ + if(actionneur < 0 || actionneur > 5){ printf("Erreur: i2c_annexe_actionneur_pot\n" ); return; } - donnees_emission_pic18f4550[actionneur-1] = (pos_bras << 2) | pos_doigt; + donnees_emission_pic18f4550[actionneur] = (pos_bras << 2) | pos_doigt; donnees_a_envoyer_pic=1; } @@ -178,7 +178,7 @@ void i2c_annexe_attrape_plante(uint8_t action){ } void i2c_annexe_init(void){ - for(unsigned int actionneur=1; actionneur<= NB_BRAS; actionneur++){ + for(unsigned int actionneur=0; actionneur< NB_BRAS; actionneur++){ i2c_annexe_actionneur_pot(actionneur, BRAS_PLIE, DOIGT_LACHE); } }