Gestion du doigt pour attrapper la plante

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

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;
+                    tempo_ms=250;
-                    return ACTION_TERMINEE;
+                    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;

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

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

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

i2c_annexe.h

@@ -15,8 +15,8 @@
 
 #define NB_BRAS 6
 
-#define PLANTE_BRAS_1 '1'
+#define PLANTE_BRAS_1 0x01
-#define PLANTE_BRAS_6 '6'
+#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);