Fonction echange pot avant<->arriere ok + amélioration de la prise des plantes

Holonome2023.c

@@ -44,7 +44,7 @@ int main() {
     stdio_init_all();
     
     //Demonstration_init();Demonstration_auto();
-    //while(mode_test());
+    while(mode_test());
     test_pseudo_homologation();
     Holonome2023_init();
 

Strategie_2024_plante.c

@@ -61,13 +61,19 @@ enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante){
     } etat_aller_a_plante = SAAP_INIT_DETECTION;
     enum validite_vl53l8_t validite;
     float angle_rad, distance_mm, commande_vitesse_plante;
-    static int tempo_ms;
+    static float distance_min_mm;
+    static int tempo_ms, tempo_asserv;
     static bool entree_dans_zone;
 
     switch(etat_aller_a_plante){
         case SAAP_INIT_DETECTION:
+            i2c_annexe_actionneur_pot(0, BRAS_LEVITE, DOIGT_TIENT);
+            i2c_annexe_actionneur_pot(5, BRAS_LEVITE, DOIGT_TIENT);
+            i2c_annexe_mi_ferme_doigt_plante();
             i2c_annexe_set_mode_VL53L8(VL53L8_PLANTE);
             tempo_ms = 2000;
+            distance_min_mm = 2000;
+            tempo_asserv = 500;
             entree_dans_zone=false;
             etat_aller_a_plante = SAAP_ASSERV;
             break;
@@ -100,7 +106,21 @@ enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante){
                 }
 
                 // 2 on s'assure qu'il n'y a pas de robot en face (TODO)
-                commande_vitesse_plante = (distance_mm - 70) * ASSER_DISTANCE_GAIN_PLANTE_P;
+
+                // 2 bis, on s'assure que le robot se rapproche de la plante. Si ce n'est pas le cas, on arrête
+                if(distance_mm < distance_min_mm){
+                    distance_min_mm = distance_mm;
+                    tempo_asserv = 500;
+                }else{
+                    tempo_asserv--;
+                    if(tempo_asserv <= 0){
+                        commande_vitesse(0, 0, 0);
+                        return ACTION_ECHEC;
+                    }
+                }
+
+                // 3 on asservi
+                commande_vitesse_plante = (distance_mm - 83) * ASSER_DISTANCE_GAIN_PLANTE_P;
                 if(commande_vitesse_plante > 300){
                     commande_vitesse_plante = 300;
                 }
@@ -111,8 +131,8 @@ enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante){
                     return ACTION_TERMINEE;
                 }
                 
-                commande_vitesse(cosf(ANGLE_PINCE) * commande_vitesse_plante ,
+                commande_vitesse(cosf(ANGLE_PINCE + 0.04) * commande_vitesse_plante ,
-                    sinf(ANGLE_PINCE) * commande_vitesse_plante , angle_rad * ASSER_ANGLE_GAIN_PLANTE_P);
+                    sinf(ANGLE_PINCE + 0.04) * commande_vitesse_plante , (angle_rad - 0.04) * ASSER_ANGLE_GAIN_PLANTE_P);
             }else{
                 tempo_ms--;
                 if(tempo_ms <= 0){
@@ -128,7 +148,7 @@ enum etat_action_t Strat_2024_aller_a_plante(enum zone_plante_t zone_plante){
 /// @brief S'approche d'une plante et la dépose dans un pot déjà attrapé
 /// @param step_ms 
 /// @param bras_depose_pot : PLANTE_BRAS_1 ou PLANTE_BRAS_6
-/// @return ACTION_EN_COURS ou ACTION_TERMINEE
+/// @return ACTION_EN_COURS ou ACTION_TERMINEE ou ACTION_ECHEC (plante tombée devant le robot)
 enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot, enum zone_plante_t zone_plante){
     static enum{
         PDP_ALLER_PLANTE,
@@ -141,18 +161,23 @@ enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot
     } etat_plante_dans_pot = PDP_ALLER_PLANTE;
     static int tempo_ms;
     struct position_t position_recule, position_initiale;
+    enum validite_vl53l8_t validite;
+    enum etat_action_t etat_action;
+    float angle_rad, distance_mm;
 
     switch (etat_plante_dans_pot)
     {
         case PDP_ALLER_PLANTE:
-            i2c_annexe_actionneur_pot(0, BRAS_LEVITE, DOIGT_TIENT);
+            etat_action = Strat_2024_aller_a_plante(zone_plante);
-            i2c_annexe_actionneur_pot(5, BRAS_LEVITE, DOIGT_TIENT);
+            if (etat_action == ACTION_TERMINEE){
-            if (Strat_2024_aller_a_plante(zone_plante) == ACTION_TERMINEE){
                 etat_plante_dans_pot=PDP_PRE_PRISE_PLANTE;
+            }else if (etat_action == ACTION_ECHEC){
+                return ACTION_ECHEC;
             }
             break;
 
         case PDP_PRE_PRISE_PLANTE:
+            i2c_annexe_set_mode_VL53L8(VL53L8_PLANTE);
             i2c_annexe_ferme_doigt_plante();
             tempo_ms = 250;
             etat_plante_dans_pot=PDP_PRE_PRISE_TEMPO;
@@ -167,6 +192,7 @@ enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot
             break;
         
         case PDP_PRE_PRISE_RECULE:
+            i2c_annexe_get_VL53L8(&validite, &angle_rad, &distance_mm);
             tempo_ms--;
             if(tempo_ms <= 0){
                 i2c_annexe_actionneur_pot(0, BRAS_HAUT, DOIGT_TIENT);
@@ -177,10 +203,20 @@ enum etat_action_t Strat_2024_plante_dans_pot(uint step_ms, uint bras_depose_pot
             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(validite == VL53L8_PLANTE){
+                    if(distance_mm > 90){
+                        // Nous n'avons pas attrapé la plante avec le doigt, on recommence:
+                        etat_plante_dans_pot = PDP_ALLER_PLANTE;
+                        i2c_annexe_ouvre_doigt_plante();
+                        i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
+                        break;
+                    }
+                }
                 commande_vitesse_stop();
                 i2c_annexe_ouvre_doigt_plante();
                 i2c_annexe_attrape_plante(bras_depose_pot);
                 tempo_ms = 5000;
+                i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
                 etat_plante_dans_pot=PDP_ATTRAPE_PLANTE;
             }
             break;

Strategie_2024_pots.c

@@ -63,6 +63,8 @@ struct position_t position_groupe_pot[6] =
     };
 
 
+enum etat_action_t Strat_2024_depose_pot(uint8_t masque_pot, uint32_t step_ms);
+
 /// @brief renvoie la position du centre du pot ainsi que l'ange par lequel l'attraper
 /// @param groupe_pot Position du groupe de pot
 /// @param num_pot Pot à prendre, entre 0 et 4 (ou utiliser les macros POT_x)
@@ -258,3 +260,183 @@ enum etat_action_t Strat_2024_attrape_pot(unsigned int groupe_pot, uint32_t step
     return ACTION_EN_COURS;
 
 }
+
+
+enum etat_action_t Strat_2024_echange_pot_avant_arriere(uint32_t step_ms){
+    static struct position_t position_initiale;
+    struct position_t position_but, position_tmp;
+    enum etat_action_t etat_action;
+
+    static int tempo_ms;
+
+    static enum {
+        EPAA_DEPOSE_POT,
+        EPAA_TOURNE,
+        EPAA_ATTRAPE_POT,
+        EPAA_AVANCE_1,
+        EPAA_RECULE_1,
+        EPAA_RECENTRE,
+        EPAA_AVANCE_2,
+        EPAA_RECULE_2,
+        EPAA_LEVE,
+        EPAA_RETOURNE,
+
+    } etat_echange_pot = EPAA_DEPOSE_POT;
+
+    switch (etat_echange_pot)
+    {
+    case EPAA_DEPOSE_POT:
+        position_initiale = Localisation_get();
+        // On dépose les pots 1, 3, 4 et 6 : 0b00101101 : 0x2D
+        if(Strat_2024_depose_pot(0x2D, step_ms) == ACTION_TERMINEE){
+            etat_echange_pot = EPAA_TOURNE;
+        }
+        break;
+
+    case EPAA_TOURNE:
+        if(Strategie_tourner_a(position_initiale.angle_radian + M_PI, step_ms) == ACTION_TERMINEE){
+            etat_echange_pot = EPAA_ATTRAPE_POT;
+            position_initiale.angle_radian += M_PI;
+            i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_TIENT);
+            i2c_annexe_actionneur_pot(2, BRAS_POT_SOL, DOIGT_TIENT);
+            i2c_annexe_actionneur_pot(3, BRAS_POT_SOL, DOIGT_TIENT);
+            i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_TIENT);
+            tempo_ms = 500;
+        }
+        break;
+
+    case EPAA_ATTRAPE_POT:
+        tempo_ms--;
+        if(tempo_ms <= 0){
+            etat_echange_pot = EPAA_AVANCE_1;
+        }
+        break;
+
+    case EPAA_AVANCE_1:
+        position_but = Geometrie_deplace(position_initiale, DISTANCE_ECHANGE_POT);
+        Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
+        if(Strategie_tourner_et_aller_a(position_but.x_mm, position_but.y_mm, position_initiale.angle_radian, EVITEMENT_ARRET_DEVANT_OBSTACLE,
+                step_ms) == ACTION_TERMINEE){
+            etat_echange_pot = EPAA_RECULE_1;
+        }
+        break;
+
+    case EPAA_RECULE_1:
+        position_but = Geometrie_deplace(position_initiale, -DISTANCE_ECHANGE_POT);
+        Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
+        if(Strategie_tourner_et_aller_a(position_but.x_mm, position_but.y_mm, position_initiale.angle_radian, EVITEMENT_ARRET_DEVANT_OBSTACLE,
+                step_ms) == ACTION_TERMINEE){
+            etat_echange_pot = EPAA_RECENTRE;
+        }
+        break;
+
+    case EPAA_RECENTRE:
+        Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
+        if(Strategie_tourner_et_aller_a(position_initiale.x_mm, position_initiale.y_mm, position_initiale.angle_radian, EVITEMENT_ARRET_DEVANT_OBSTACLE,
+                step_ms) == ACTION_TERMINEE){
+            etat_echange_pot = EPAA_AVANCE_2;
+        }
+        break;
+
+    case EPAA_AVANCE_2:
+        position_tmp = position_initiale;
+        position_tmp.angle_radian += 40. * DEGRE_EN_RADIAN ;
+
+        position_but = Geometrie_deplace(position_tmp, DISTANCE_ECHANGE_POT);
+        Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
+        if(Strategie_tourner_et_aller_a(position_but.x_mm, position_but.y_mm, position_initiale.angle_radian, EVITEMENT_ARRET_DEVANT_OBSTACLE,
+                step_ms) == ACTION_TERMINEE){
+            etat_echange_pot = EPAA_RECULE_2;
+        }
+        break;
+
+    case EPAA_RECULE_2:
+        position_tmp = position_initiale;
+        position_tmp.angle_radian += 60. * DEGRE_EN_RADIAN ;
+        position_but = Geometrie_deplace(position_tmp, -DISTANCE_ECHANGE_POT);
+        Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
+        if(Strategie_tourner_et_aller_a(position_but.x_mm, position_but.y_mm, position_initiale.angle_radian, EVITEMENT_ARRET_DEVANT_OBSTACLE,
+                step_ms) == ACTION_TERMINEE){
+            etat_echange_pot = EPAA_LEVE;
+            i2c_annexe_actionneur_pot(0, BRAS_HAUT, DOIGT_TIENT);
+            i2c_annexe_actionneur_pot(2, BRAS_HAUT, DOIGT_TIENT);
+            i2c_annexe_actionneur_pot(3, BRAS_HAUT, DOIGT_TIENT);
+            i2c_annexe_actionneur_pot(5, BRAS_HAUT, DOIGT_TIENT);
+            tempo_ms = 500;
+        }
+        break;
+
+    case EPAA_LEVE:
+        tempo_ms--;
+        if(tempo_ms <= 0){
+            etat_echange_pot = EPAA_LEVE;
+        }
+        break;
+
+    case EPAA_RETOURNE:
+        if(Strategie_tourner_a(position_initiale.angle_radian - M_PI, step_ms) == ACTION_TERMINEE){
+            etat_echange_pot = EPAA_DEPOSE_POT;
+            return ACTION_TERMINEE;
+        }
+        break;
+
+    
+    default:
+        break;
+    }
+    return ACTION_EN_COURS;
+
+}
+
+/// @brief dépose 1 ou plusieurs pot au pied du robot.
+/// @param masque_pot: choisi les pots à déposer, 0x01 pour le bras 1, 0x02 pour le bras 2, 0x03 pour les bras 1 & 2
+enum etat_action_t Strat_2024_depose_pot(uint8_t masque_pot, uint32_t step_ms){
+    static int tempo_ms;
+    uint8_t masque=1;
+    static enum {
+        DP_BAISSE_BRAS,
+        DP_BAISSE_BRAS_TEMPO,
+        DP_RANGE_DOIGT,
+    } etat_depose_pot = DP_BAISSE_BRAS;
+    switch (etat_depose_pot)
+    {
+    case DP_BAISSE_BRAS:
+        for (int i=0; i< NB_BRAS; i++){
+            masque =1;
+            masque = masque << i;
+            if(masque_pot & masque){
+                i2c_annexe_actionneur_pot(i, BRAS_POT_SOL, DOIGT_TIENT);
+            }
+        }
+        tempo_ms=350;
+        etat_depose_pot = DP_BAISSE_BRAS_TEMPO;
+        break;
+    
+    case DP_BAISSE_BRAS_TEMPO:
+        tempo_ms--;
+        if(tempo_ms < 0){
+            for (int i=0; i< NB_BRAS; i++){
+                masque =1;
+                masque = masque << i;
+                if(masque_pot & masque){
+                    i2c_annexe_actionneur_pot(i, BRAS_POT_SOL, DOIGT_LACHE);
+                }
+            }
+            tempo_ms=250;
+            etat_depose_pot = DP_RANGE_DOIGT;
+        }
+        break;
+
+    case DP_RANGE_DOIGT:
+        tempo_ms--;
+        if(tempo_ms < 0){
+            etat_depose_pot = DP_BAISSE_BRAS;
+            return ACTION_TERMINEE;
+        }
+        break;
+    
+    default:
+        break;
+    }
+    return ACTION_EN_COURS;
+}

Strategie_2024_pots.h

@@ -24,5 +24,8 @@
 #define DISTANCE_APPROCHE_POT_MM 300. 
 #define DISTANCE_ATTRAPE_POT_MM 200.
 
+#define DISTANCE_ECHANGE_POT 50.
+
 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);

Test_strategie_2024.c

@@ -29,6 +29,7 @@ int test_attrape_plante(void);
 int test_aller_zone_plante();
 int test_pseudo_homologation(void);
 int test_attrape_1_pot(void);
+int test_echange_pot(void);
 void affichage_test_strategie_2024(void);
 
 int test_strategie_2024(){
@@ -39,7 +40,8 @@ int test_strategie_2024(){
     printf("E - Attrape plante.\n");
     printf("F - Aller zone plante.\n");
     printf("G - Pseudo homologation.\n");
-    printf("H - réglage pots.\n");
+    printf("H - reglage pots.\n");
+    printf("I - echange pots.\n");
     
     int lettre;
     do{
@@ -86,6 +88,12 @@ int test_strategie_2024(){
             while(test_attrape_1_pot());
             break;
 
+        case 'i':
+        case 'I':
+            while(test_echange_pot());
+            break;
+            
+
         case 'q':
         case 'Q':
             return 0;
@@ -476,7 +484,7 @@ int test_pseudo_homologation(){
         TAP_DEPOSE
     } etat_test = TAP_CALAGE;
 
-    printf("test_attrape_pot\n");
+    printf("test_pseudo_homologation\n");
 
 
     i2c_maitre_init();
@@ -597,7 +605,7 @@ int test_pseudo_homologation(){
 
 }
 
-
+extern float abscisse;
 void affichage_test_strategie_2024(){
     uint32_t temps;
     enum validite_vl53l8_t validite_vl53l8;
@@ -615,6 +623,8 @@ void affichage_test_strategie_2024(){
         printf(">c_pos_y:%ld:%f\n", temps, Trajet_get_consigne().y_mm);
         printf(">c_pos_angle:%ld:%f\n", temps, (Trajet_get_consigne().angle_radian+ ANGLE_PINCE) / DEGRE_EN_RADIAN);
 
+        printf(">abscisse:%ld:%f\n", temps, abscisse);
+
         printf(">tirette:%ld:%d\n", temps, attente_tirette());
 
         printf(">distance_obstacle:%ld:%f\n", temps, Trajet_get_obstacle_mm());
@@ -634,7 +644,7 @@ void affichage_test_strategie_2024(){
                 break;
         }
         
-        sleep_ms(100);
+        sleep_ms(10);
     }
 }
 
@@ -779,3 +789,89 @@ int test_attrape_1_pot(void){
     return 0;
 
 }
+
+
+int test_echange_pot(){
+    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;
+    static int tempo_ms;
+
+    static enum {
+        TEP_INIT,
+        TEP_ACTION,
+    } etat_echange_pot = TEP_INIT;
+
+    printf("test_echange_pot\n");
+
+    i2c_maitre_init();
+    Trajet_init();
+    Balise_VL53L1X_init();
+
+    
+    set_position_avec_gyroscope(1);
+    if(get_position_avec_gyroscope()){
+        printf("Init gyroscope\n");
+        Gyro_Init();
+    }
+
+    stdio_flush();
+
+    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_echange_pot){
+                case TEP_INIT:
+                    i2c_annexe_actionneur_pot(0, BRAS_POT_SOL, DOIGT_TIENT);
+                    i2c_annexe_actionneur_pot(1, BRAS_HAUT, DOIGT_TIENT);
+                    i2c_annexe_actionneur_pot(2, BRAS_POT_SOL, DOIGT_TIENT);
+                    i2c_annexe_actionneur_pot(3, BRAS_POT_SOL, DOIGT_TIENT);
+                    i2c_annexe_actionneur_pot(4, BRAS_HAUT, DOIGT_TIENT);
+                    i2c_annexe_actionneur_pot(5, BRAS_POT_SOL, DOIGT_TIENT);
+                    tempo_ms=3000;
+                    etat_echange_pot = TEP_ACTION;
+                    break;
+
+                case TEP_ACTION:
+                    tempo_ms--;
+                    if (tempo_ms <= 0)
+                    {
+                        etat_action = Strat_2024_echange_pot_avant_arriere(_step_ms);
+                    }
+                    break;
+
+
+            }
+
+        }
+        lettre = getchar_timeout_us(0);
+    //}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
+    }while(etat_action == ACTION_EN_COURS);
+    Moteur_Stop();
+    
+    return 0;
+
+}

i2c_annexe.c

@@ -205,6 +205,12 @@ void i2c_annexe_ferme_doigt_plante(void){
     donnees_a_envoyer_pic=1;
 }
 
+void i2c_annexe_mi_ferme_doigt_plante(void){
+    donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE] = 
+        (donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE] & 0x0F) | 0x30;
+    donnees_a_envoyer_pic=1;
+}
+
 void i2c_annexe_init(void){
     for(unsigned int actionneur=0; actionneur< NB_BRAS; actionneur++){
         i2c_annexe_actionneur_pot(actionneur, BRAS_PLIE, DOIGT_LACHE);

i2c_annexe.h

@@ -49,3 +49,4 @@ void i2c_annexe_init(void);
 
 void i2c_annexe_ferme_doigt_plante(void);
 void i2c_annexe_ouvre_doigt_plante(void);
+void i2c_annexe_mi_ferme_doigt_plante(void);