From ba9a1e5411c54e32531e0ceab8ddcb416a4192b7 Mon Sep 17 00:00:00 2001 From: Samuel Date: Sun, 5 May 2024 22:43:19 +0200 Subject: [PATCH] =?UTF-8?q?Fonction=20echange=20pot=20avant<->arriere=20ok?= =?UTF-8?q?=20+=20am=C3=A9lioration=20de=20la=20prise=20des=20plantes?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Holonome2023.c | 2 +- Strategie_2024_plante.c | 54 ++++++++++-- Strategie_2024_pots.c | 182 ++++++++++++++++++++++++++++++++++++++++ Strategie_2024_pots.h | 3 + Test_strategie_2024.c | 104 ++++++++++++++++++++++- i2c_annexe.c | 6 ++ i2c_annexe.h | 1 + 7 files changed, 338 insertions(+), 14 deletions(-) diff --git a/Holonome2023.c b/Holonome2023.c index 3b32d9d..b6600d5 100644 --- a/Holonome2023.c +++ b/Holonome2023.c @@ -44,7 +44,7 @@ int main() { stdio_init_all(); //Demonstration_init();Demonstration_auto(); - //while(mode_test()); + while(mode_test()); test_pseudo_homologation(); Holonome2023_init(); diff --git a/Strategie_2024_plante.c b/Strategie_2024_plante.c index 012a9db..558012e 100644 --- a/Strategie_2024_plante.c +++ b/Strategie_2024_plante.c @@ -61,13 +61,19 @@ enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante){ } etat_aller_a_plante = SAAP_INIT_DETECTION; enum validite_vl53l8_t validite; float angle_rad, distance_mm, commande_vitesse_plante; - static int tempo_ms; + static float distance_min_mm; + static int tempo_ms, tempo_asserv; static bool entree_dans_zone; switch(etat_aller_a_plante){ case SAAP_INIT_DETECTION: + i2c_annexe_actionneur_pot(0, BRAS_LEVITE, DOIGT_TIENT); + i2c_annexe_actionneur_pot(5, BRAS_LEVITE, DOIGT_TIENT); + i2c_annexe_mi_ferme_doigt_plante(); i2c_annexe_set_mode_VL53L8(VL53L8_PLANTE); - tempo_ms = 2000; + tempo_ms = 2000; + distance_min_mm = 2000; + tempo_asserv = 500; entree_dans_zone=false; etat_aller_a_plante = SAAP_ASSERV; break; @@ -100,7 +106,21 @@ enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante){ } // 2 on s'assure qu'il n'y a pas de robot en face (TODO) - commande_vitesse_plante = (distance_mm - 70) * ASSER_DISTANCE_GAIN_PLANTE_P; + + // 2 bis, on s'assure que le robot se rapproche de la plante. Si ce n'est pas le cas, on arrête + if(distance_mm < distance_min_mm){ + distance_min_mm = distance_mm; + tempo_asserv = 500; + }else{ + tempo_asserv--; + if(tempo_asserv <= 0){ + commande_vitesse(0, 0, 0); + return ACTION_ECHEC; + } + } + + // 3 on asservi + commande_vitesse_plante = (distance_mm - 83) * ASSER_DISTANCE_GAIN_PLANTE_P; if(commande_vitesse_plante > 300){ commande_vitesse_plante = 300; } @@ -111,8 +131,8 @@ enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante){ return ACTION_TERMINEE; } - commande_vitesse(cosf(ANGLE_PINCE) * commande_vitesse_plante , - sinf(ANGLE_PINCE) * commande_vitesse_plante , angle_rad * ASSER_ANGLE_GAIN_PLANTE_P); + commande_vitesse(cosf(ANGLE_PINCE + 0.04) * commande_vitesse_plante , + sinf(ANGLE_PINCE + 0.04) * commande_vitesse_plante , (angle_rad - 0.04) * ASSER_ANGLE_GAIN_PLANTE_P); }else{ tempo_ms--; if(tempo_ms <= 0){ @@ -128,7 +148,7 @@ enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante){ /// @brief S'approche d'une plante et la dépose dans un pot déjà attrapé /// @param step_ms /// @param bras_depose_pot : PLANTE_BRAS_1 ou PLANTE_BRAS_6 -/// @return ACTION_EN_COURS ou ACTION_TERMINEE +/// @return ACTION_EN_COURS ou ACTION_TERMINEE ou ACTION_ECHEC (plante tombée devant le robot) enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot, enum zone_plante_t zone_plante){ static enum{ PDP_ALLER_PLANTE, @@ -141,18 +161,23 @@ enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot } etat_plante_dans_pot = PDP_ALLER_PLANTE; static int tempo_ms; struct position_t position_recule, position_initiale; + enum validite_vl53l8_t validite; + enum etat_action_t etat_action; + float angle_rad, distance_mm; switch (etat_plante_dans_pot) { case PDP_ALLER_PLANTE: - i2c_annexe_actionneur_pot(0, BRAS_LEVITE, DOIGT_TIENT); - i2c_annexe_actionneur_pot(5, BRAS_LEVITE, DOIGT_TIENT); - if (Strat_2024_aller_a_plante(zone_plante) == ACTION_TERMINEE){ + etat_action = Strat_2024_aller_a_plante(zone_plante); + if (etat_action == ACTION_TERMINEE){ etat_plante_dans_pot=PDP_PRE_PRISE_PLANTE; + }else if (etat_action == ACTION_ECHEC){ + return ACTION_ECHEC; } break; case PDP_PRE_PRISE_PLANTE: + i2c_annexe_set_mode_VL53L8(VL53L8_PLANTE); i2c_annexe_ferme_doigt_plante(); tempo_ms = 250; etat_plante_dans_pot=PDP_PRE_PRISE_TEMPO; @@ -167,6 +192,7 @@ enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot break; case PDP_PRE_PRISE_RECULE: + i2c_annexe_get_VL53L8(&validite, &angle_rad, &distance_mm); tempo_ms--; if(tempo_ms <= 0){ i2c_annexe_actionneur_pot(0, BRAS_HAUT, DOIGT_TIENT); @@ -177,10 +203,20 @@ enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot position_recule = Geometrie_deplace(position_initiale, -100); Trajet_config(TRAJECT_CONFIG_AVANCE_DROIT); if(Strategie_aller_a(position_recule.x_mm, position_recule.y_mm, step_ms) == ACTION_TERMINEE){ + if(validite == VL53L8_PLANTE){ + if(distance_mm > 90){ + // Nous n'avons pas attrapé la plante avec le doigt, on recommence: + etat_plante_dans_pot = PDP_ALLER_PLANTE; + i2c_annexe_ouvre_doigt_plante(); + i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE); + break; + } + } commande_vitesse_stop(); i2c_annexe_ouvre_doigt_plante(); i2c_annexe_attrape_plante(bras_depose_pot); tempo_ms = 5000; + i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE); etat_plante_dans_pot=PDP_ATTRAPE_PLANTE; } break; diff --git a/Strategie_2024_pots.c b/Strategie_2024_pots.c index 8d462b5..0720020 100644 --- a/Strategie_2024_pots.c +++ b/Strategie_2024_pots.c @@ -63,6 +63,8 @@ struct position_t position_groupe_pot[6] = }; +enum etat_action_t Strat_2024_depose_pot(uint8_t masque_pot, uint32_t step_ms); + /// @brief renvoie la position du centre du pot ainsi que l'ange par lequel l'attraper /// @param groupe_pot Position du groupe de pot /// @param num_pot Pot à prendre, entre 0 et 4 (ou utiliser les macros POT_x) @@ -258,3 +260,183 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step return ACTION_EN_COURS; } + + +enum etat_action_t Strat_2024_echange_pot_avant_arriere(uint32_t step_ms){ + static struct position_t position_initiale; + struct position_t position_but, position_tmp; + enum etat_action_t etat_action; + + static int tempo_ms; + + static enum { + EPAA_DEPOSE_POT, + EPAA_TOURNE, + EPAA_ATTRAPE_POT, + EPAA_AVANCE_1, + EPAA_RECULE_1, + EPAA_RECENTRE, + EPAA_AVANCE_2, + EPAA_RECULE_2, + EPAA_LEVE, + EPAA_RETOURNE, + + } etat_echange_pot = EPAA_DEPOSE_POT; + + switch (etat_echange_pot) + { + case EPAA_DEPOSE_POT: + position_initiale = Localisation_get(); + // On dépose les pots 1, 3, 4 et 6 : 0b00101101 : 0x2D + if(Strat_2024_depose_pot(0x2D, step_ms) == ACTION_TERMINEE){ + etat_echange_pot = EPAA_TOURNE; + } + break; + + case EPAA_TOURNE: + if(Strategie_tourner_a(position_initiale.angle_radian + M_PI, step_ms) == ACTION_TERMINEE){ + etat_echange_pot = EPAA_ATTRAPE_POT; + position_initiale.angle_radian += M_PI; + i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_TIENT); + i2c_annexe_actionneur_pot(2, BRAS_POT_SOL, DOIGT_TIENT); + i2c_annexe_actionneur_pot(3, BRAS_POT_SOL, DOIGT_TIENT); + i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_TIENT); + tempo_ms = 500; + } + break; + + case EPAA_ATTRAPE_POT: + tempo_ms--; + if(tempo_ms <= 0){ + etat_echange_pot = EPAA_AVANCE_1; + } + break; + + case EPAA_AVANCE_1: + position_but = Geometrie_deplace(position_initiale, DISTANCE_ECHANGE_POT); + Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); + if(Strategie_tourner_et_aller_a(position_but.x_mm, position_but.y_mm, position_initiale.angle_radian, EVITEMENT_ARRET_DEVANT_OBSTACLE, + step_ms) == ACTION_TERMINEE){ + etat_echange_pot = EPAA_RECULE_1; + } + break; + + case EPAA_RECULE_1: + position_but = Geometrie_deplace(position_initiale, -DISTANCE_ECHANGE_POT); + Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); + if(Strategie_tourner_et_aller_a(position_but.x_mm, position_but.y_mm, position_initiale.angle_radian, EVITEMENT_ARRET_DEVANT_OBSTACLE, + step_ms) == ACTION_TERMINEE){ + etat_echange_pot = EPAA_RECENTRE; + } + break; + + case EPAA_RECENTRE: + Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); + if(Strategie_tourner_et_aller_a(position_initiale.x_mm, position_initiale.y_mm, position_initiale.angle_radian, EVITEMENT_ARRET_DEVANT_OBSTACLE, + step_ms) == ACTION_TERMINEE){ + etat_echange_pot = EPAA_AVANCE_2; + } + break; + + case EPAA_AVANCE_2: + position_tmp = position_initiale; + position_tmp.angle_radian += 40. * DEGRE_EN_RADIAN ; + + position_but = Geometrie_deplace(position_tmp, DISTANCE_ECHANGE_POT); + Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); + if(Strategie_tourner_et_aller_a(position_but.x_mm, position_but.y_mm, position_initiale.angle_radian, EVITEMENT_ARRET_DEVANT_OBSTACLE, + step_ms) == ACTION_TERMINEE){ + etat_echange_pot = EPAA_RECULE_2; + } + break; + + case EPAA_RECULE_2: + position_tmp = position_initiale; + position_tmp.angle_radian += 60. * DEGRE_EN_RADIAN ; + position_but = Geometrie_deplace(position_tmp, -DISTANCE_ECHANGE_POT); + Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); + if(Strategie_tourner_et_aller_a(position_but.x_mm, position_but.y_mm, position_initiale.angle_radian, EVITEMENT_ARRET_DEVANT_OBSTACLE, + step_ms) == ACTION_TERMINEE){ + etat_echange_pot = EPAA_LEVE; + i2c_annexe_actionneur_pot(0, BRAS_HAUT, DOIGT_TIENT); + i2c_annexe_actionneur_pot(2, BRAS_HAUT, DOIGT_TIENT); + i2c_annexe_actionneur_pot(3, BRAS_HAUT, DOIGT_TIENT); + i2c_annexe_actionneur_pot(5, BRAS_HAUT, DOIGT_TIENT); + tempo_ms = 500; + } + break; + + case EPAA_LEVE: + tempo_ms--; + if(tempo_ms <= 0){ + etat_echange_pot = EPAA_LEVE; + } + break; + + case EPAA_RETOURNE: + if(Strategie_tourner_a(position_initiale.angle_radian - M_PI, step_ms) == ACTION_TERMINEE){ + etat_echange_pot = EPAA_DEPOSE_POT; + return ACTION_TERMINEE; + } + break; + + + default: + break; + } + return ACTION_EN_COURS; + +} + +/// @brief dépose 1 ou plusieurs pot au pied du robot. +/// @param masque_pot: choisi les pots à déposer, 0x01 pour le bras 1, 0x02 pour le bras 2, 0x03 pour les bras 1 & 2 +enum etat_action_t Strat_2024_depose_pot(uint8_t masque_pot, uint32_t step_ms){ + static int tempo_ms; + uint8_t masque=1; + static enum { + DP_BAISSE_BRAS, + DP_BAISSE_BRAS_TEMPO, + DP_RANGE_DOIGT, + } etat_depose_pot = DP_BAISSE_BRAS; + switch (etat_depose_pot) + { + case DP_BAISSE_BRAS: + for (int i=0; i< NB_BRAS; i++){ + masque =1; + masque = masque << i; + if(masque_pot & masque){ + i2c_annexe_actionneur_pot(i, BRAS_POT_SOL, DOIGT_TIENT); + } + } + tempo_ms=350; + etat_depose_pot = DP_BAISSE_BRAS_TEMPO; + break; + + case DP_BAISSE_BRAS_TEMPO: + tempo_ms--; + if(tempo_ms < 0){ + for (int i=0; i< NB_BRAS; i++){ + masque =1; + masque = masque << i; + if(masque_pot & masque){ + i2c_annexe_actionneur_pot(i, BRAS_POT_SOL, DOIGT_LACHE); + } + } + tempo_ms=250; + etat_depose_pot = DP_RANGE_DOIGT; + } + break; + + case DP_RANGE_DOIGT: + tempo_ms--; + if(tempo_ms < 0){ + etat_depose_pot = DP_BAISSE_BRAS; + return ACTION_TERMINEE; + } + break; + + default: + break; + } + return ACTION_EN_COURS; +} \ No newline at end of file diff --git a/Strategie_2024_pots.h b/Strategie_2024_pots.h index 5d6ec46..0afe90e 100644 --- a/Strategie_2024_pots.h +++ b/Strategie_2024_pots.h @@ -24,5 +24,8 @@ #define DISTANCE_APPROCHE_POT_MM 300. #define DISTANCE_ATTRAPE_POT_MM 200. +#define DISTANCE_ECHANGE_POT 50. + 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); +enum etat_action_t Strat_2024_echange_pot_avant_arriere(uint32_t step_ms); diff --git a/Test_strategie_2024.c b/Test_strategie_2024.c index cac2b6e..f4473df 100644 --- a/Test_strategie_2024.c +++ b/Test_strategie_2024.c @@ -29,6 +29,7 @@ int test_attrape_plante(void); int test_aller_zone_plante(); int test_pseudo_homologation(void); int test_attrape_1_pot(void); +int test_echange_pot(void); void affichage_test_strategie_2024(void); int test_strategie_2024(){ @@ -39,7 +40,8 @@ int test_strategie_2024(){ printf("E - Attrape plante.\n"); printf("F - Aller zone plante.\n"); printf("G - Pseudo homologation.\n"); - printf("H - réglage pots.\n"); + printf("H - reglage pots.\n"); + printf("I - echange pots.\n"); int lettre; do{ @@ -86,6 +88,12 @@ int test_strategie_2024(){ while(test_attrape_1_pot()); break; + case 'i': + case 'I': + while(test_echange_pot()); + break; + + case 'q': case 'Q': return 0; @@ -476,7 +484,7 @@ int test_pseudo_homologation(){ TAP_DEPOSE } etat_test = TAP_CALAGE; - printf("test_attrape_pot\n"); + printf("test_pseudo_homologation\n"); i2c_maitre_init(); @@ -597,7 +605,7 @@ int test_pseudo_homologation(){ } - +extern float abscisse; void affichage_test_strategie_2024(){ uint32_t temps; enum validite_vl53l8_t validite_vl53l8; @@ -615,6 +623,8 @@ void affichage_test_strategie_2024(){ 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+ ANGLE_PINCE) / DEGRE_EN_RADIAN); + printf(">abscisse:%ld:%f\n", temps, abscisse); + printf(">tirette:%ld:%d\n", temps, attente_tirette()); printf(">distance_obstacle:%ld:%f\n", temps, Trajet_get_obstacle_mm()); @@ -634,7 +644,7 @@ void affichage_test_strategie_2024(){ break; } - sleep_ms(100); + sleep_ms(10); } } @@ -779,3 +789,89 @@ int test_attrape_1_pot(void){ return 0; } + + +int test_echange_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=ACTION_EN_COURS; + static int tempo_ms; + + static enum { + TEP_INIT, + TEP_ACTION, + } etat_echange_pot = TEP_INIT; + + printf("test_echange_pot\n"); + + i2c_maitre_init(); + Trajet_init(); + Balise_VL53L1X_init(); + + + set_position_avec_gyroscope(1); + if(get_position_avec_gyroscope()){ + printf("Init gyroscope\n"); + Gyro_Init(); + } + + stdio_flush(); + + 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); + } + } + + switch(etat_echange_pot){ + case TEP_INIT: + i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_TIENT); + i2c_annexe_actionneur_pot(1, BRAS_HAUT, DOIGT_TIENT); + i2c_annexe_actionneur_pot(2, BRAS_POT_SOL, DOIGT_TIENT); + i2c_annexe_actionneur_pot(3, BRAS_POT_SOL, DOIGT_TIENT); + i2c_annexe_actionneur_pot(4, BRAS_HAUT, DOIGT_TIENT); + i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_TIENT); + tempo_ms=3000; + etat_echange_pot = TEP_ACTION; + break; + + case TEP_ACTION: + tempo_ms--; + if (tempo_ms <= 0) + { + etat_action = Strat_2024_echange_pot_avant_arriere(_step_ms); + } + break; + + + } + + } + lettre = getchar_timeout_us(0); + //}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0)); + }while(etat_action == ACTION_EN_COURS); + Moteur_Stop(); + + return 0; + +} \ No newline at end of file diff --git a/i2c_annexe.c b/i2c_annexe.c index 44c1aa0..8518cfd 100644 --- a/i2c_annexe.c +++ b/i2c_annexe.c @@ -205,6 +205,12 @@ void i2c_annexe_ferme_doigt_plante(void){ donnees_a_envoyer_pic=1; } +void i2c_annexe_mi_ferme_doigt_plante(void){ + donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE] = + (donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE] & 0x0F) | 0x30; + donnees_a_envoyer_pic=1; +} + void i2c_annexe_init(void){ for(unsigned int actionneur=0; actionneur< NB_BRAS; actionneur++){ i2c_annexe_actionneur_pot(actionneur, BRAS_PLIE, DOIGT_LACHE); diff --git a/i2c_annexe.h b/i2c_annexe.h index 8d225b5..eab1356 100644 --- a/i2c_annexe.h +++ b/i2c_annexe.h @@ -49,3 +49,4 @@ void i2c_annexe_init(void); void i2c_annexe_ferme_doigt_plante(void); void i2c_annexe_ouvre_doigt_plante(void); +void i2c_annexe_mi_ferme_doigt_plante(void);