From d539c6846c44b126e0c334f3f6d72fda3daccf6c Mon Sep 17 00:00:00 2001 From: Samuel Date: Wed, 24 Apr 2024 22:11:56 +0200 Subject: [PATCH] Gestion du doigt pour attrapper la plante --- Strategie_2024_plante.c | 53 ++++++++++++++++++++++++++++++++++++++++- Strategie_2024_pots.c | 25 +++++++++++++++---- Test_i2c.c | 25 +++++++++++++++++++ Test_strategie_2024.c | 2 +- i2c_annexe.c | 17 +++++++++++-- i2c_annexe.h | 7 ++++-- 6 files changed, 118 insertions(+), 11 deletions(-) diff --git a/Strategie_2024_plante.c b/Strategie_2024_plante.c index 565591f..1bcd2dd 100644 --- a/Strategie_2024_plante.c +++ b/Strategie_2024_plante.c @@ -9,6 +9,29 @@ #define ASSER_ANGLE_GAIN_PLANTE_P 1.5 #define ASSER_DISTANCE_GAIN_PLANTE_P 10 +struct position_t liste_zone_plante[]= +{ + {.x_mm = 1500, .y_mm = 1500 }, + {.x_mm = 1000, .y_mm = 1300 }, + {.x_mm = 1000, .y_mm = 700 }, + {.x_mm = 1500, .y_mm = 500 }, + {.x_mm = 2000, .y_mm = 700 }, + {.x_mm = 2000, .y_mm = 1300 } +}; + + + +enum etat_action_t Strat_2024_aller_zone_plante(uint32_t step_ms){ + int zone_plante = 2; + struct position_t position_robot, position_zone_plante; + float angle; + + position_zone_plante = liste_zone_plante[zone_plante]; + position_robot = Localisation_get(); + angle = atan2f(position_zone_plante.x_mm - position_robot.x_mm, position_zone_plante.y_mm - position_robot.y_mm); + return Strategie_tourner_a(angle, step_ms); +} + enum etat_action_t Strat_2024_aller_a_plante(void){ static enum { SAAP_INIT_DETECTION, @@ -55,6 +78,9 @@ enum etat_action_t Strat_2024_aller_a_plante(void){ enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot){ static enum{ PDP_ALLER_PLANTE, + PDP_PRE_PRISE_PLANTE, + PDP_PRE_PRISE_TEMPO, + PDP_PRE_PRISE_RECULE, PDP_ATTRAPE_PLANTE, PDP_RECULE, PDP_TEMPO, @@ -66,13 +92,38 @@ enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot { case PDP_ALLER_PLANTE: if (Strat_2024_aller_a_plante() == ACTION_TERMINEE){ + etat_plante_dans_pot=PDP_PRE_PRISE_PLANTE; + } + break; + + case PDP_PRE_PRISE_PLANTE: + i2c_annexe_ferme_doigt_plante(); + tempo_ms = 250; + etat_plante_dans_pot=PDP_PRE_PRISE_TEMPO; + break; + + case PDP_PRE_PRISE_TEMPO: + tempo_ms--; + if(tempo_ms <= 0){ + etat_plante_dans_pot=PDP_PRE_PRISE_RECULE; + } + break; + + case PDP_PRE_PRISE_RECULE: + position_initiale = Localisation_get(); + position_initiale.angle_radian += ANGLE_PINCE; + position_recule = Geometrie_deplace(position_initiale, -50); + Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT); + if(Strategie_aller_a(position_recule.x_mm, position_recule.y_mm, step_ms) == ACTION_TERMINEE){ + commande_vitesse_stop(); + i2c_annexe_ouvre_doigt_plante(); i2c_annexe_attrape_plante(bras_depose_pot); tempo_ms = 2000; - etat_plante_dans_pot=PDP_ATTRAPE_PLANTE; } break; + case PDP_ATTRAPE_PLANTE: tempo_ms--; if(tempo_ms <= 0){ diff --git a/Strategie_2024_pots.c b/Strategie_2024_pots.c index d9939ac..9aa3ee7 100644 --- a/Strategie_2024_pots.c +++ b/Strategie_2024_pots.c @@ -107,7 +107,8 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step AP_ORIENTE, AP_APPROCHE_POT, AP_ATTRAPE_POT, - AP_RETOUR_ET_LEVE_POT + AP_RETOUR_ET_LEVE_POT, + AP_FINALISE } etat_attrape_pot = AP_ALLER_VERS_GROUPE_POT; // Pour chaque pot @@ -170,7 +171,6 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step SANS_EVITEMENT, step_ms); if (etat_action == ACTION_TERMINEE){ - printf("pot x:%.2f, %2f\n", position_approche_pot.x_mm, position_approche_pot.y_mm); etat_attrape_pot = AP_ATTRAPE_POT; } break; @@ -201,16 +201,31 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step etat_attrape_pot = AP_APPROCHE_POT; pot = get_pot_suivant(pot); if(pot > 4){ - etat_attrape_pot = AP_ALLER_VERS_GROUPE_POT; - return ACTION_TERMINEE; + tempo_ms=250; + etat_attrape_pot = AP_FINALISE; + break; } position_pot = groupe_pot_get_pot(groupe_pot, pot); position_approche_pot = Geometrie_deplace(position_pot, -DISTANCE_APPROCHE_POT_MM); position_attrape_pot = Geometrie_deplace(position_pot, -DISTANCE_ATTRAPE_POT_MM); - printf("pot x:%.2f, %2f\n", position_approche_pot.x_mm, position_approche_pot.y_mm); } break; + case AP_FINALISE: + 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; + return ACTION_TERMINEE; + } + + break; default: break; diff --git a/Test_i2c.c b/Test_i2c.c index f832447..732cff2 100644 --- a/Test_i2c.c +++ b/Test_i2c.c @@ -302,6 +302,8 @@ int test_i2c_ecriture_pico_annex_nb_2(){ printf("H - Detection plante\n"); printf("I - Detection stop\n"); printf("J - Detection distance loin\n"); + printf("K - Ouvre doigt plante\n"); + printf("L - Ferme doigt plante\n"); printf("S - Score + 1\n"); printf("\nQ - Quitter\n"); @@ -393,6 +395,28 @@ int test_i2c_ecriture_pico_annex_nb_2(){ printf("=> Detection distance loin\n"); break; + case 'k': + case 'K': + i2c_annexe_ouvre_doigt_plante(); + printf("=> Ouvre doigt plante\n"); + break; + + case 'l': + case 'L': + i2c_annexe_ferme_doigt_plante(); + printf("=> ferme doigt plante\n"); + break; + + case 'm': + case 'M': + i2c_annexe_attrape_plante(PLANTE_BRAS_1); + i2c_annexe_ouvre_doigt_plante(); + + printf("=> ouvre doigt plante + attrape plante\n"); + break; + + + case 'q': case 'Q': multicore_reset_core1(); @@ -408,6 +432,7 @@ int test_i2c_ecriture_pico_annex_nb_2(){ printf("lettre non reconnue: %d %c\n", lettre, lettre); } } + //sleep_ms(1); i2c_gestion(i2c0); i2c_annexe_gestion(); diff --git a/Test_strategie_2024.c b/Test_strategie_2024.c index 3c62e77..c5a519e 100644 --- a/Test_strategie_2024.c +++ b/Test_strategie_2024.c @@ -221,7 +221,7 @@ int test_attrape_plante(){ } } - etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1); + etat_action = Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_6); } diff --git a/i2c_annexe.c b/i2c_annexe.c index 0e1fc7f..5de8480 100644 --- a/i2c_annexe.c +++ b/i2c_annexe.c @@ -89,7 +89,7 @@ void i2c_annexe_gestion(){ retour_i2c = i2c_ecrire_registre_nb(ADRESSE_PIC18F4550, ADRESSE_PIC18F4550_DEBUT_W, donnees_emission_pic18f4550, TAILLE_DONNEES_PIC18F4550_EMISSION); if(retour_i2c == I2C_SUCCES){ - etat_i2c_annexe = EMISSION_DONNEES_RPi; + etat_i2c_annexe = EMISSION_TEMPO_PIC18F; temps = time_us_32(); donnees_a_envoyer_pic=0; donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE]=0; @@ -188,7 +188,20 @@ void i2c_annexe_actionneur_pot(int actionneur, uint8_t pos_bras, uint8_t pos_doi /// @brief Envoi l'ordre d'attraper une plance /// @param action : PLANTE_BRAS_1 ou PLANTE_BRAS_6 void i2c_annexe_attrape_plante(uint8_t action){ - donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE] = action; + donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE] = + (donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE] & 0xF0) | action; + donnees_a_envoyer_pic=1; +} + +void i2c_annexe_ouvre_doigt_plante(void){ + donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE] = + (donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE] & 0x0F) | 0x10; + donnees_a_envoyer_pic=1; +} + +void i2c_annexe_ferme_doigt_plante(void){ + donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE] = + (donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE] & 0x0F) | 0x20; donnees_a_envoyer_pic=1; } diff --git a/i2c_annexe.h b/i2c_annexe.h index e11c69e..fb5ce3c 100644 --- a/i2c_annexe.h +++ b/i2c_annexe.h @@ -15,8 +15,8 @@ #define NB_BRAS 6 -#define PLANTE_BRAS_1 '1' -#define PLANTE_BRAS_6 '6' +#define PLANTE_BRAS_1 0x01 +#define PLANTE_BRAS_6 0x02 enum validite_vl53l8_t{ VL53L8_INVALIDE=0, @@ -45,3 +45,6 @@ void i2c_annexe_get_VL53L8(enum validite_vl53l8_t *validite, float * angle, floa void i2c_annexe_actionneur_pot(int actionneur, uint8_t pos_bras, uint8_t pos_doigt); void i2c_annexe_attrape_plante(uint8_t action); void i2c_annexe_init(void); + +void i2c_annexe_ferme_doigt_plante(void); +void i2c_annexe_ouvre_doigt_plante(void);