diff --git a/Holonome2023.c b/Holonome2023.c index a0ab85c..e5bbd37 100644 --- a/Holonome2023.c +++ b/Holonome2023.c @@ -1,4 +1,5 @@ #include "Holonome2023.h" +#include "Strategie_2024.h" #include "Demonstration.h" const uint LED_PIN = 25; diff --git a/Strategie_2024.c b/Strategie_2024.c index a797a8c..8c09b18 100644 --- a/Strategie_2024.c +++ b/Strategie_2024.c @@ -62,6 +62,7 @@ enum zone_plante_t get_zone_plante(enum couleur_t couleur){ void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){ int lettre, _step_ms = 1, _step_ms_gyro=2,temps_ms_init; + float angle_destination; struct trajectoire_t trajectoire; enum evitement_t evitement; enum etat_action_t etat_action=ACTION_EN_COURS; @@ -77,7 +78,10 @@ void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms) TAP_PLANTE_ATTRAPE_3, TAP_PLANTE_ATTRAPE_4, TAP_RENTRE, - TAP_DEPOSE + TAP_DEPOSE_1, + TAP_DEPOSE_2, + TAP_DEPOSE_3, + TAP_FINI } etat_test = TAP_CALAGE; @@ -150,25 +154,47 @@ void Strategie_2024(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms) break; case TAP_RENTRE: - float angle_destination; - angle_destination = 15 * DEGRE_EN_RADIAN; + if(couleur == COULEUR_BLEU){ + angle_destination = 15 * DEGRE_EN_RADIAN; + }else{ + angle_destination = (90-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; + if(Strategie_tourner_et_aller_a(300, 300, angle_destination, EVITEMENT_PAUSE_DEVANT_OBSTACLE, _step_ms) == ACTION_TERMINEE){ + etat_test=TAP_DEPOSE_1; } break; - case TAP_DEPOSE: - tempo_ms--; + case TAP_DEPOSE_1: 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); + if(Strat_2024_depose_pot(MASQUE_POT_1 | MASQUE_POT_6, _step_ms)== ACTION_TERMINEE){ + etat_test=TAP_DEPOSE_2; } break; + case TAP_DEPOSE_2: + if(couleur == COULEUR_BLEU){ + angle_destination = 180+15 * DEGRE_EN_RADIAN; + }else{ + angle_destination = (90-(180+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_3; + } + break; + + case TAP_DEPOSE_3: + commande_vitesse_stop(); + if(Strat_2024_depose_pot(MASQUE_POT_3 | MASQUE_POT_4, _step_ms)== ACTION_TERMINEE){ + etat_test=TAP_FINI; + } + break; + + case TAP_FINI: + break; + + } } \ No newline at end of file diff --git a/Strategie_2024_pots.c b/Strategie_2024_pots.c index e74dc46..36a71cb 100644 --- a/Strategie_2024_pots.c +++ b/Strategie_2024_pots.c @@ -348,7 +348,7 @@ enum etat_action_t Strat_2024_echange_pot_avant_arriere(uint32_t step_ms){ case EPAA_LEVE: tempo_ms--; if(tempo_ms <= 0){ - etat_echange_pot = EPAA_LEVE; + etat_echange_pot = EPAA_RETOURNE; } break; @@ -385,7 +385,7 @@ enum etat_action_t Strat_2024_depose_pot(uint8_t masque_pot, uint32_t step_ms){ masque =1; masque = masque << i; if(masque_pot & masque){ - i2c_annexe_actionneur_pot(i, BRAS_POT_SOL, DOIGT_TIENT); + i2c_annexe_actionneur_pot(i, BRAS_ECARTE, DOIGT_TIENT); } } tempo_ms=350; @@ -399,7 +399,7 @@ enum etat_action_t Strat_2024_depose_pot(uint8_t masque_pot, uint32_t step_ms){ masque =1; masque = masque << i; if(masque_pot & masque){ - i2c_annexe_actionneur_pot(i, BRAS_POT_SOL, DOIGT_LACHE); + i2c_annexe_actionneur_pot(i, BRAS_ECARTE, DOIGT_LACHE); } } tempo_ms=250; diff --git a/Strategie_2024_pots.h b/Strategie_2024_pots.h index 0afe90e..e4e063a 100644 --- a/Strategie_2024_pots.h +++ b/Strategie_2024_pots.h @@ -21,6 +21,13 @@ #define BRAS_5 4 #define BRAS_6 5 +#define MASQUE_POT_1 1 +#define MASQUE_POT_2 2 +#define MASQUE_POT_3 4 +#define MASQUE_POT_4 8 +#define MASQUE_POT_5 16 +#define MASQUE_POT_6 32 + #define DISTANCE_APPROCHE_POT_MM 300. #define DISTANCE_ATTRAPE_POT_MM 200. @@ -29,3 +36,4 @@ 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); +enum etat_action_t Strat_2024_depose_pot(uint8_t masque_pot, uint32_t step_ms); diff --git a/Test_i2c.c b/Test_i2c.c index 3666d9b..c98875d 100644 --- a/Test_i2c.c +++ b/Test_i2c.c @@ -305,7 +305,10 @@ int test_i2c_ecriture_pico_annex_nb_2(){ 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("N - Pots avant levite\n"); + printf("O - Pots ecarte et tient\n"); + printf("P - Pots ecarte et lache\n"); + printf("S - Score + 1\n"); printf("\nQ - Quitter\n"); @@ -425,6 +428,22 @@ int test_i2c_ecriture_pico_annex_nb_2(){ printf("=> Levite pots avant\n"); break; + case 'o': + case 'O': + for(int i=0; i<6; i++){ + i2c_annexe_actionneur_pot(i, BRAS_ECARTE, DOIGT_TIENT); + } + printf("=> Ecarte et tient pot\n"); + break; + + case 'p': + case 'P': + for(int i=0; i<6; i++){ + i2c_annexe_actionneur_pot(i, BRAS_ECARTE, DOIGT_LACHE); + } + printf("=> Ecarte et lache pot\n"); + break; + case 'q':