diff --git a/Strategie_2024_plante.c b/Strategie_2024_plante.c index 1bcd2dd..2a35382 100644 --- a/Strategie_2024_plante.c +++ b/Strategie_2024_plante.c @@ -21,15 +21,14 @@ struct position_t liste_zone_plante[]= -enum etat_action_t Strat_2024_aller_zone_plante(uint32_t step_ms){ - int zone_plante = 2; +enum etat_action_t Strat_2024_aller_zone_plante(enum zone_plante_t zone_plante, uint32_t step_ms){ 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); + angle = atan2f(position_zone_plante.y_mm - position_robot.y_mm, position_zone_plante.x_mm - position_robot.x_mm); + return Strategie_tourner_a(angle - ANGLE_PINCE, step_ms); } enum etat_action_t Strat_2024_aller_a_plante(void){ @@ -115,6 +114,11 @@ enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot 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){ + 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_plante.h b/Strategie_2024_plante.h index 27d6efe..15a8e16 100644 --- a/Strategie_2024_plante.h +++ b/Strategie_2024_plante.h @@ -1,4 +1,14 @@ #include "Strategie.h" +enum zone_plante_t{ + ZONE_PLANTE_1=0, + ZONE_PLANTE_2=1, + ZONE_PLANTE_3=2, + ZONE_PLANTE_4=3, + ZONE_PLANTE_5=4, + ZONE_PLANTE_6=5, +}; + +enum etat_action_t Strat_2024_aller_zone_plante(enum zone_plante_t zone_plante, uint32_t step_ms); enum etat_action_t Strat_2024_aller_a_plante(); -enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot); \ No newline at end of file +enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot); diff --git a/Strategie_2024_pots.c b/Strategie_2024_pots.c index 9aa3ee7..74ff47a 100644 --- a/Strategie_2024_pots.c +++ b/Strategie_2024_pots.c @@ -7,7 +7,7 @@ #include "Localisation.h" #define DISTANCE_APPROCHE_POT_MM 300. -#define DISTANCE_ATTRAPE_POT_MM 180. +#define DISTANCE_ATTRAPE_POT_MM 185. float angle_bras[6] = { diff --git a/Test_strategie_2024.c b/Test_strategie_2024.c index c5a519e..4c1ca2f 100644 --- a/Test_strategie_2024.c +++ b/Test_strategie_2024.c @@ -25,6 +25,8 @@ int test_calage_debut(void); int test_attrape_pot(void); int test_aller_a_plante(void); int test_attrape_plante(void); +int test_aller_zone_plante(); +int test_pseudo_homologation(void); void affichage_test_strategie_2024(void); int test_strategie_2024(){ @@ -33,6 +35,8 @@ int test_strategie_2024(){ printf("C - Attrape pot.\n"); printf("D - Aller a plante.\n"); printf("E - Attrape plante.\n"); + printf("F - Aller zone plante.\n"); + printf("G - Pseudo homologation.\n"); int lettre; do{ @@ -64,6 +68,16 @@ int test_strategie_2024(){ while(test_attrape_plante()); break; + case 'f': + case 'F': + while(test_aller_zone_plante()); + break; + + case 'g': + case 'G': + while(test_pseudo_homologation()); + break; + case 'q': case 'Q': return 0; @@ -293,6 +307,70 @@ int test_aller_a_plante(){ } + + +int test_aller_zone_plante(){ + 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; + + printf("test_aller_a_plante\n"); + + i2c_maitre_init(); + Trajet_init(); + Balise_VL53L1X_init(); + + + set_position_avec_gyroscope(0); + if(get_position_avec_gyroscope()){ + printf("Init gyroscope\n"); + Gyro_Init(); + } + + stdio_flush(); + + multicore_launch_core1(affichage_test_strategie_2024); + + Localisation_set(800, 200, 0); + + + 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_aller_zone_plante(ZONE_PLANTE_3, _step_ms); + + + } + lettre = getchar_timeout_us(0); + //}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0)); + }while(etat_action == ACTION_EN_COURS); + Moteur_Stop(); + + return 0; + +} + + int test_attrape_pot(){ int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init; struct trajectoire_t trajectoire; @@ -373,6 +451,102 @@ int test_attrape_pot(){ } +int test_pseudo_homologation(){ + 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; + + enum { + TAP_CALAGE, + TAP_POT, + TAP_PLANTE_ORIENTATION, + TAP_PLANTE_ATTRAPE + } etat_test = TAP_CALAGE; + + 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(1); + 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); + } + } + switch(etat_test){ + case TAP_CALAGE: + if(Strategie_calage_debut(COULEUR_BLEU, _step_ms) == ACTION_TERMINEE){ + etat_test=TAP_POT; + } + break; + case TAP_POT: + if(Strat_2024_attrape_pot(GROUPE_POT_B1, _step_ms) == ACTION_TERMINEE){ + etat_test=TAP_PLANTE_ORIENTATION; + } + break; + + case TAP_PLANTE_ORIENTATION: + if(Strat_2024_aller_zone_plante(ZONE_PLANTE_3, _step_ms) == ACTION_TERMINEE){ + etat_test=TAP_PLANTE_ATTRAPE; + } + break; + + case TAP_PLANTE_ATTRAPE: + if(Strat_2024_plante_dans_pot(_step_ms, PLANTE_BRAS_1) == ACTION_TERMINEE){ + etat_action= ACTION_TERMINEE; + } + break; + } + + } + 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 5de8480..44c1aa0 100644 --- a/i2c_annexe.c +++ b/i2c_annexe.c @@ -173,7 +173,7 @@ void i2c_annexe_get_distances(uint8_t *distance_capteur_cm){ } /// @brief Envoie la consigne de position du servomoteur à la carte des servomoteurs -/// @param actionneur de 1 à 6, pour le "bras" correspondant". +/// @param actionneur de 0 à 5, pour le "bras" correspondant". /// @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){