From e79b7614f5d77ce75f56e67b3d1bb7ff52184cb9 Mon Sep 17 00:00:00 2001 From: Samuel Date: Sat, 4 May 2024 11:22:39 +0200 Subject: [PATCH] =?UTF-8?q?Ajout=20des=20corrections=20en=20distance=20par?= =?UTF-8?q?=20pot=20+=20renommage=20des=20variables=20d'=C3=A9vitement?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Demonstration.c | 20 ++++++++++---------- Holonome2023.c | 6 +++++- Monitoring.c | 2 ++ Strategie.c | 14 +++++++------- Strategie.h | 10 +++++----- Strategie_2024_plante.c | 15 +++++++++------ Strategie_2024_pots.c | 27 +++++++++++++++++++-------- Strategie_deplacement.c | 14 +++++++------- Test_i2c.c | 10 ++++++++++ Test_strategie.c | 10 +++++----- Test_strategie_2024.c | 22 +++++++++++++++++----- i2c_annexe.h | 1 + 12 files changed, 97 insertions(+), 54 deletions(-) diff --git a/Demonstration.c b/Demonstration.c index 3810752..a2e390b 100644 --- a/Demonstration.c +++ b/Demonstration.c @@ -210,7 +210,7 @@ enum etat_action_t Demonstration_calage(){ Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, RAYON_ROBOT + 150, PETIT_RAYON_ROBOT_MM + 150, Localisation_get().angle_radian, Localisation_get().angle_radian); - if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){ + if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ etat_calage = CALAGE_TERMINE; } break; @@ -260,7 +260,7 @@ enum etat_action_t Demonstration_rectangle(int avance_x_mm, int avance_y_mm){ Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().x_mm, Localisation_get().y_mm + avance_y_mm, Localisation_get().angle_radian, Localisation_get().angle_radian); - if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){ + if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ etat_rectangle = AVANCE_X; } break; @@ -269,7 +269,7 @@ enum etat_action_t Demonstration_rectangle(int avance_x_mm, int avance_y_mm){ Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().x_mm + avance_x_mm, Localisation_get().y_mm, Localisation_get().angle_radian, Localisation_get().angle_radian); - if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){ + if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ etat_rectangle = RECULE_Y; } break; @@ -278,7 +278,7 @@ enum etat_action_t Demonstration_rectangle(int avance_x_mm, int avance_y_mm){ Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().x_mm, Localisation_get().y_mm - avance_y_mm, Localisation_get().angle_radian, Localisation_get().angle_radian); - if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){ + if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ etat_rectangle = RECULE_X; } break; @@ -287,7 +287,7 @@ enum etat_action_t Demonstration_rectangle(int avance_x_mm, int avance_y_mm){ Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().x_mm - avance_x_mm, Localisation_get().y_mm, Localisation_get().angle_radian, Localisation_get().angle_radian); - if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){ + if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ etat_rectangle = RECTANGLE_TERMINE; } break; @@ -338,7 +338,7 @@ enum etat_action_t Demonstration_avance_puis_tourne(int avance_x_mm, int avance_ Trajectoire_droite(&trajectoire, pos_x_init_mm, pos_y_init_mm, pos_x_init_mm + avance_x_mm, pos_y_init_mm + avance_y_mm, Localisation_get().angle_radian, Localisation_get().angle_radian); - if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){ + if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ etat_avance_puis_tourne = APT_TOURNE; } break; @@ -347,7 +347,7 @@ enum etat_action_t Demonstration_avance_puis_tourne(int avance_x_mm, int avance_ Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian, Localisation_get().angle_radian + (angle_degrees * DEGRE_EN_RADIAN) ); Trajet_config(TRAJECT_CONFIG_ROTATION_PURE); - if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){ + if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ etat_avance_puis_tourne = APT_RECULE; Trajet_config(TRAJECT_CONFIG_STD); } @@ -357,7 +357,7 @@ enum etat_action_t Demonstration_avance_puis_tourne(int avance_x_mm, int avance_ Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().x_mm - avance_x_mm, Localisation_get().y_mm - avance_y_mm, Localisation_get().angle_radian, Localisation_get().angle_radian); - if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){ + if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ etat_avance_puis_tourne = APT_TERMINE; } break; @@ -404,7 +404,7 @@ enum etat_action_t Demonstration_bezier(){ -576, 1500 - 276, 545, 1500 - 276, Localisation_get().angle_radian, Localisation_get().angle_radian); - if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){ + if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ etat_bezier = BEZIER_RETOUR; } break; @@ -416,7 +416,7 @@ enum etat_action_t Demonstration_bezier(){ -76, 1500 - 788, 242, 1500 - 1289, Localisation_get().angle_radian, Localisation_get().angle_radian); - if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){ + if(Strategie_parcourir_trajet(trajectoire, 1, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ etat_bezier = BEZIER_TERMINE; } break; diff --git a/Holonome2023.c b/Holonome2023.c index e5bee3e..3b32d9d 100644 --- a/Holonome2023.c +++ b/Holonome2023.c @@ -11,6 +11,9 @@ uint temps_cycle; #define V_INIT -999.0 #define TEST_TIMEOUT_US 10000000 +// A supprimer +int test_pseudo_homologation(void); + int mode_test(); @@ -41,7 +44,8 @@ int main() { stdio_init_all(); //Demonstration_init();Demonstration_auto(); - while(mode_test()); + //while(mode_test()); + test_pseudo_homologation(); Holonome2023_init(); multicore_launch_core1(Monitoring_display); diff --git a/Monitoring.c b/Monitoring.c index e37c956..d4f48a9 100644 --- a/Monitoring.c +++ b/Monitoring.c @@ -1,4 +1,5 @@ #include "pico/stdlib.h" +#include "pico/multicore.h" #include #include "Monitoring.h" #include "Localisation.h" @@ -92,6 +93,7 @@ void set_debug_varf(float variable){ void Monitoring_Error(char * msg){ gpio_put(LED_PIN_ROUGE, 1); Log_message(msg, ERROR); + multicore_reset_core1(); while(1){ sleep_ms(500); printf(msg); diff --git a/Strategie.c b/Strategie.c index 7334e08..185a765 100644 --- a/Strategie.c +++ b/Strategie.c @@ -276,7 +276,7 @@ enum etat_action_t Strategie_aller_a(float pos_x, float pos_y, uint32_t step_ms) pos_x, pos_y, Localisation_get().angle_radian, Localisation_get().angle_radian); - return Strategie_parcourir_trajet(trajectoire, step_ms, ARRET_DEVANT_OBSTACLE); + return Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_ARRET_DEVANT_OBSTACLE); } 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){ @@ -318,7 +318,7 @@ enum etat_action_t Strategie_aller_a_puis_tourner(float pos_x, float pos_y, floa Localisation_get().angle_radian, Geometrie_get_angle_optimal(Localisation_get().angle_radian,angle_radian)); - if(Strategie_parcourir_trajet(trajectoire, step_ms, SANS_EVITEMENT) == ACTION_TERMINEE){ + if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ etat_aller_a_puis_tourner = AAPT_ALLER; return ACTION_TERMINEE; } @@ -337,7 +337,7 @@ enum etat_action_t Strategie_tourner_a(float angle_radian,uint32_t step_ms){ Localisation_get().angle_radian, Geometrie_get_angle_optimal(Localisation_get().angle_radian,angle_radian)); - return Strategie_parcourir_trajet(trajectoire, step_ms, SANS_EVITEMENT); + return Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT); } @@ -503,7 +503,7 @@ enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_ Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian, (0 * DEGRE_EN_RADIAN) - ANGLE_PINCE); } - if(Strategie_parcourir_trajet(trajectoire, step_ms, SANS_EVITEMENT) == ACTION_TERMINEE){ + if(Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT) == ACTION_TERMINEE){ i2c_annexe_set_mode_VL53L8(VL53L8_BORDURE); etat_calage_debut = CD_LECTURE_BORDURE_X; } @@ -529,9 +529,9 @@ enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_ case CD_ALLER_POSITION_INIT: Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); if(couleur == COULEUR_BLEU){ - etat_action = Strategie_tourner_et_aller_a(225, 225, Localisation_get().angle_radian, SANS_EVITEMENT, step_ms); + etat_action = Strategie_tourner_et_aller_a(225, 225, Localisation_get().angle_radian, EVITEMENT_SANS_EVITEMENT, step_ms); }else{ - etat_action = Strategie_tourner_et_aller_a(3000 - 225, 225, Localisation_get().angle_radian, SANS_EVITEMENT, step_ms); + etat_action = Strategie_tourner_et_aller_a(3000 - 225, 225, Localisation_get().angle_radian, EVITEMENT_SANS_EVITEMENT, step_ms); } if(etat_action == ACTION_TERMINEE){ etat_calage_debut = CD_ROTATION_POSITION_INIT; @@ -543,7 +543,7 @@ enum etat_action_t Strategie_calage_debut(enum couleur_t couleur, uint32_t step_ (45. * DEGRE_EN_RADIAN) - ANGLE_PINCE);*/ Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian, 0); - return Strategie_parcourir_trajet(trajectoire, step_ms, SANS_EVITEMENT); + return Strategie_parcourir_trajet(trajectoire, step_ms, EVITEMENT_SANS_EVITEMENT); //etat_calage_debut = CD_ENVOI_CDE_BORDURE; } diff --git a/Strategie.h b/Strategie.h index 77e9ee0..74e171a 100644 --- a/Strategie.h +++ b/Strategie.h @@ -31,11 +31,11 @@ enum couleur_t{ }; enum evitement_t{ - SANS_EVITEMENT, - PAUSE_DEVANT_OBSTACLE, - ARRET_DEVANT_OBSTACLE, - RETOUR_SI_OBSTABLE, - CONTOURNEMENT + EVITEMENT_SANS_EVITEMENT, + EVITEMENT_PAUSE_DEVANT_OBSTACLE, + EVITEMENT_ARRET_DEVANT_OBSTACLE, + EVITEMENT_RETOUR_SI_OBSTABLE, + EVITEMENT_CONTOURNEMENT }; struct objectif_t{ diff --git a/Strategie_2024_plante.c b/Strategie_2024_plante.c index b712d70..012a9db 100644 --- a/Strategie_2024_plante.c +++ b/Strategie_2024_plante.c @@ -145,6 +145,8 @@ enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot 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_plante_dans_pot=PDP_PRE_PRISE_PLANTE; } @@ -159,21 +161,22 @@ enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot case PDP_PRE_PRISE_TEMPO: tempo_ms--; if(tempo_ms <= 0){ + tempo_ms=500; etat_plante_dans_pot=PDP_PRE_PRISE_RECULE; } break; case PDP_PRE_PRISE_RECULE: + tempo_ms--; + if(tempo_ms <= 0){ + i2c_annexe_actionneur_pot(0, BRAS_HAUT, DOIGT_TIENT); + i2c_annexe_actionneur_pot(5, BRAS_HAUT, DOIGT_TIENT); + } position_initiale = Localisation_get(); position_initiale.angle_radian += ANGLE_PINCE; - position_recule = Geometrie_deplace(position_initiale, -50); + 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(bras_depose_pot == PLANTE_BRAS_1){ - i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_TIENT); - }else{ - i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_TIENT); - } commande_vitesse_stop(); i2c_annexe_ouvre_doigt_plante(); i2c_annexe_attrape_plante(bras_depose_pot); diff --git a/Strategie_2024_pots.c b/Strategie_2024_pots.c index a15cbff..8d462b5 100644 --- a/Strategie_2024_pots.c +++ b/Strategie_2024_pots.c @@ -28,6 +28,16 @@ float angle_bras_correction[6] = 7 * DEGRE_EN_RADIAN }; +float distance_bras_correction_mm[6] = +{ + 0, + 0, + -10, + -15, + 0, + 0 +}; + enum etat_bras_t{ BRAS_LIBRE, BRAS_OCCUPE, @@ -131,19 +141,19 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step switch (etat_attrape_pot) { case AP_ALLER_VERS_GROUPE_POT: + bras = get_bras_libre(); 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); + position_attrape_pot = Geometrie_deplace(position_pot, -(DISTANCE_ATTRAPE_POT_MM + distance_bras_correction_mm[bras])); 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);*/ etat_action = Strategie_tourner_et_aller_a( position_approche_pot.x_mm, position_approche_pot.y_mm, (-30. *DEGRE_EN_RADIAN), - SANS_EVITEMENT, step_ms); + EVITEMENT_SANS_EVITEMENT, step_ms); if (etat_action == ACTION_TERMINEE){ etat_attrape_pot = AP_RECALE; - bras = get_bras_libre(); i2c_annexe_set_mode_VL53L8(VL53L8_DISTANCE_LOIN); } break; @@ -161,7 +171,7 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step }*/ etat_attrape_pot = AP_ORIENTE; }else{ - printf("Erreur - recalage trop loin\n"); + //printf("Erreur - recalage trop loin\n"); } } break; @@ -181,9 +191,10 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step SANS_EVITEMENT, step_ms);*/ etat_action = Strategie_aller_a_puis_tourner( position_approche_pot.x_mm, position_approche_pot.y_mm, position_approche_pot.angle_radian - angle_bras[bras], - SANS_EVITEMENT, step_ms); + EVITEMENT_SANS_EVITEMENT, step_ms); if (etat_action == ACTION_TERMINEE){ + position_attrape_pot = Geometrie_deplace(position_pot, -(DISTANCE_ATTRAPE_POT_MM + distance_bras_correction_mm[bras])); etat_attrape_pot = AP_ATTRAPE_POT; } break; @@ -193,7 +204,7 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step 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); + EVITEMENT_SANS_EVITEMENT, step_ms); if (etat_action == ACTION_TERMINEE){ tempo_ms=250; etat_attrape_pot = AP_RETOUR_ET_LEVE_POT; @@ -209,7 +220,7 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step } 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); + EVITEMENT_SANS_EVITEMENT, step_ms); if (etat_action == ACTION_TERMINEE){ etat_attrape_pot = AP_APPROCHE_POT; pot = get_pot_suivant(pot); @@ -232,7 +243,7 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step } 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); + EVITEMENT_SANS_EVITEMENT, step_ms); if (etat_action == ACTION_TERMINEE){ etat_attrape_pot = AP_ALLER_VERS_GROUPE_POT; return ACTION_TERMINEE; diff --git a/Strategie_deplacement.c b/Strategie_deplacement.c index bfd9779..94456a8 100644 --- a/Strategie_deplacement.c +++ b/Strategie_deplacement.c @@ -22,7 +22,7 @@ enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire, break; case PARCOURS_AVANCE: - if(evitement != SANS_EVITEMENT){ + if(evitement != EVITEMENT_SANS_EVITEMENT){ angle_avancement = Trajet_get_orientation_avance(); distance_obstacle = Balise_VL53L1X_get_distance_obstacle_mm(angle_avancement); Trajet_set_obstacle_mm(distance_obstacle); @@ -30,23 +30,23 @@ enum etat_action_t Strategie_parcourir_trajet(struct trajectoire_t trajectoire, if(Evitement_get_statu() == OBSTACLE_CONFIRME || Evitement_get_statu() == OBSTACLE_NON_CONFIRME){ switch(evitement){ - case SANS_EVITEMENT: - //printf("Evitement lors trajet SANS_EVITEMENT: ERREUR\n"); + case EVITEMENT_SANS_EVITEMENT: + //printf("Evitement lors trajet EVITEMENT_SANS_EVITEMENT: ERREUR\n"); break; - case PAUSE_DEVANT_OBSTACLE: + case EVITEMENT_PAUSE_DEVANT_OBSTACLE: // Rien à faire ici break; - case ARRET_DEVANT_OBSTACLE: + case EVITEMENT_ARRET_DEVANT_OBSTACLE: etat_parcourt = PARCOURS_INIT; return ACTION_ECHEC; - case RETOUR_SI_OBSTABLE: + case EVITEMENT_RETOUR_SI_OBSTABLE: trajet_inverse = !trajet_inverse; Trajet_inverse(); break; - case CONTOURNEMENT: // TODO + case EVITEMENT_CONTOURNEMENT: // TODO break; } diff --git a/Test_i2c.c b/Test_i2c.c index 314af75..3666d9b 100644 --- a/Test_i2c.c +++ b/Test_i2c.c @@ -304,6 +304,8 @@ int test_i2c_ecriture_pico_annex_nb_2(){ printf("J - Detection distance loin\n"); printf("K - Ouvre doigt plante\n"); printf("L - Ferme doigt plante\n"); + printf("M - ouvre doigt + attrape plante\n"); + printf("L - Pots avant levite\n"); printf("S - Score + 1\n"); printf("\nQ - Quitter\n"); @@ -415,6 +417,14 @@ int test_i2c_ecriture_pico_annex_nb_2(){ printf("=> ouvre doigt plante + attrape plante\n"); break; + case 'n': + case 'N': + i2c_annexe_actionneur_pot(0, BRAS_LEVITE, DOIGT_TIENT); + i2c_annexe_actionneur_pot(5, BRAS_LEVITE, DOIGT_TIENT); + + printf("=> Levite pots avant\n"); + break; + case 'q': diff --git a/Test_strategie.c b/Test_strategie.c index 0950be5..65c41f7 100644 --- a/Test_strategie.c +++ b/Test_strategie.c @@ -126,15 +126,15 @@ int test_evitement(){ }while(lettre == PICO_ERROR_TIMEOUT || lettre == 0); switch(lettre){ case 'a': - case 'A':evitement=SANS_EVITEMENT;break; + case 'A':evitement=EVITEMENT_SANS_EVITEMENT;break; case 'b': - case 'B':evitement=PAUSE_DEVANT_OBSTACLE;break; + case 'B':evitement=EVITEMENT_PAUSE_DEVANT_OBSTACLE;break; case 'c': - case 'C':evitement=ARRET_DEVANT_OBSTACLE;break; + case 'C':evitement=EVITEMENT_ARRET_DEVANT_OBSTACLE;break; case 'd': - case 'D':evitement=RETOUR_SI_OBSTABLE;break; + case 'D':evitement=EVITEMENT_RETOUR_SI_OBSTABLE;break; case 'e': - case 'E':evitement=CONTOURNEMENT;break; + case 'E':evitement=EVITEMENT_CONTOURNEMENT;break; } i2c_maitre_init(); diff --git a/Test_strategie_2024.c b/Test_strategie_2024.c index 740b400..cac2b6e 100644 --- a/Test_strategie_2024.c +++ b/Test_strategie_2024.c @@ -3,6 +3,7 @@ #include "pico/error.h" #include "pico/multicore.h" #include "hardware/i2c.h" +#include "Commande_vitesse.h" #include "i2c_annexe.h" #include "i2c_maitre.h" #include "Evitement.h" @@ -463,6 +464,7 @@ int test_pseudo_homologation(){ struct trajectoire_t trajectoire; enum evitement_t evitement; enum etat_action_t etat_action=ACTION_EN_COURS; + static int tempo_ms; enum { TAP_CALAGE, @@ -518,7 +520,7 @@ int test_pseudo_homologation(){ } switch(etat_test){ case TAP_CALAGE: - if(Strategie_calage_debut(COULEUR_BLEU, _step_ms) == ACTION_TERMINEE){ + if(Strategie_calage_debut_manuel(COULEUR_BLEU, _step_ms) == ACTION_TERMINEE){ etat_test=TAP_POT; } break; @@ -557,14 +559,24 @@ int test_pseudo_homologation(){ break; case TAP_RENTRE: - if(Strategie_aller_a(250, 250, _step_ms) == ACTION_TERMINEE){ + float angle_destination; + angle_destination = 15 * DEGRE_EN_RADIAN; + Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE); + if(Strategie_tourner_et_aller_a(450, 450, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){ etat_test=TAP_DEPOSE; + i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_TIENT); + i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_TIENT); + tempo_ms=500; } break; case TAP_DEPOSE: - i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_LACHE); - i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_LACHE); + tempo_ms--; + commande_vitesse_stop(); + if(tempo_ms<= 0){ + i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_LACHE); + i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_LACHE); + } break; } @@ -737,7 +749,7 @@ int test_attrape_1_pot(void){ i2c_annexe_actionneur_pot(bras, BRAS_POT_SOL, DOIGT_TIENT); if( Strategie_tourner_et_aller_a( position_attrape_pot.x_mm, position_attrape_pot.y_mm, position_attrape_pot.angle_radian - angle_bras[bras] + angle_bras_correction[bras], - SANS_EVITEMENT, _step_ms) == ACTION_TERMINEE){ + EVITEMENT_SANS_EVITEMENT, _step_ms) == ACTION_TERMINEE){ i2c_annexe_actionneur_pot(bras, BRAS_HAUT, DOIGT_TIENT); tempo_ms = 2000; etat_test = TAP_ATTRAPE_TEMPO; diff --git a/i2c_annexe.h b/i2c_annexe.h index fb5ce3c..8d225b5 100644 --- a/i2c_annexe.h +++ b/i2c_annexe.h @@ -12,6 +12,7 @@ #define BRAS_ECARTE 3 #define BRAS_DEPOSE_JARDINIERE 4 #define BRAS_HAUT 5 +#define BRAS_LEVITE 6 #define NB_BRAS 6