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);