Travail sur l'homologation - fonctions d'émission i2c non fonctionnelles

Holonome2023.c

@@ -16,6 +16,7 @@
 #include "QEI.h"
 #include "Servomoteur.h"
 #include "spi_nb.h"
+#include "Strategie.h"
 #include "Temps.h"
 #include "Trajectoire.h"
 #include "Trajet.h"
@@ -51,6 +52,12 @@ int main() {
     gpio_set_dir(LED_PIN_ROUGE, GPIO_OUT);
     gpio_put(LED_PIN_ROUGE, 0);
 
+    gpio_init(COULEUR);
+    gpio_init(TIRETTE);
+    gpio_set_dir(COULEUR, GPIO_IN);
+    gpio_set_dir(TIRETTE, GPIO_IN);
+
+
     // Il faut neutraliser cette broche qui pourrait interférer avec 
     // la lecture des codeurs. (problème sur la carte électrique)...
     gpio_init(LED_PIN_NE_PAS_UTILISER);

Strategie.c

@@ -1,23 +1,25 @@
-
+#include "hardware/gpio.h"
+#include "i2c_annexe.h"
+#include "Geometrie_robot.h"
 #include "Localisation.h"
+#include "Moteurs.h"
 #include "Strategie_prise_cerises.h"
 #include "Strategie.h"
 #include "Trajet.h"
 #include "math.h"
 
 #define DEGREE_EN_RADIAN  (M_PI / 180.)
+#define SEUIL_RECAL_DIST_MM 75
+#define SEUIL_RECAL_ANGLE_RADIAN (5 * DEGREE_EN_RADIAN)
+
+enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint32_t step_ms);
+enum etat_action_t calage_angle(enum longer_direction_t longer_direction, double x_mm, double y_mm, double angle_radian);
+enum etat_action_t lance_balles(uint32_t step_ms);
+
+enum etat_strategie_t etat_strategie=STRATEGIE_INIT;
 
 void Homologation(uint32_t step_ms){
-    static enum etat_strategie_t{
+    
-        STRATEGIE_INIT,
-        APPROCHE_CERISE_1_A,
-        APPROCHE_CERISE_1_B,
-        ATTRAPE_CERISE_1,
-        APPROCHE_PANIER_1_A,
-        APPROCHE_PANIER_1_B,
-        CALAGE_PANIER_1,
-        STRATEGIE_FIN
-    }etat_strategie=STRATEGIE_INIT;
 
     enum etat_action_t etat_action;
     enum etat_trajet_t etat_trajet;
@@ -47,30 +49,157 @@ void Homologation(uint32_t step_ms){
         case ATTRAPE_CERISE_1:
             etat_action = cerise_attraper_bordure(LONGER_VERS_C, step_ms);
             if(etat_action == ACTION_TERMINEE){
-                etat_strategie = APPROCHE_PANIER_1_A;
+                etat_strategie = APPROCHE_PANIER_1;
             }
             break;
 
-        case APPROCHE_PANIER_1_A:
+        case APPROCHE_PANIER_1:
+            Trajet_config(500, 500);
             Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
                                 485, Localisation_get().y_mm,
                                 465, 857,
                                 465,2830,
                                 +30. * DEGREE_EN_RADIAN, +120. * DEGREE_EN_RADIAN);
-            Trajet_debut_trajectoire(trajectoire);
+
-            etat_strategie = APPROCHE_PANIER_1_B;
+            if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
+                etat_strategie = CALAGE_PANIER_1;
+            }
+
             break;
 
-        case APPROCHE_PANIER_1_B:
+        case CALAGE_PANIER_1:
-            etat_trajet = Trajet_avance(step_ms/1000.);
+            if(calage_angle(LONGER_VERS_A, RAYON_ROBOT, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)), 120. *DEGREE_EN_RADIAN) == ACTION_TERMINEE){
-            if(etat_trajet == TRAJET_TERMINE){
+                etat_strategie = RECULE_PANIER;
-                etat_strategie = ATTRAPE_CERISE_1;
             }
             break;
         
+        case RECULE_PANIER:
+            Trajet_config(250, 500);
+            Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
+                                225, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 120,
+                                120. * DEGREE_EN_RADIAN, +240. * DEGREE_EN_RADIAN);
+
+            if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
+                etat_strategie = LANCE_DANS_PANIER;
+            }
+            break;
+
+        case LANCE_DANS_PANIER:
+            if(lance_balles(step_ms) == ACTION_TERMINEE){
+                etat_strategie = STRATEGIE_FIN;
+            }
+            break;
 
 
         case STRATEGIE_FIN:
+            Moteur_Stop();
             break;
     }
 }
+
+
+/// @brief Active le propulseur, ouvre la porte, attend qql secondes.
+/// @param step_ms : pas de temps.
+/// @return ACTION_EN_COURS ou ACTION_TERMINEE
+enum etat_action_t lance_balles(uint32_t step_ms){
+    enum etat_action_t etat_action = ACTION_EN_COURS;
+    uint32_t tempo_ms;
+
+    static enum{
+        LANCE_PROPULSEUR_ON,
+        LANCE_TEMPO_PROP_ON,
+        LANCE_PORTE_OUVERTE,
+    } etat_lance_balle = LANCE_PROPULSEUR_ON;
+
+    switch(etat_lance_balle){
+        case LANCE_PROPULSEUR_ON:
+            i2c_annexe_active_propulseur();
+            tempo_ms = 2000;
+            etat_lance_balle = LANCE_TEMPO_PROP_ON;
+            break;
+        
+        case LANCE_TEMPO_PROP_ON:
+            if (tempo_ms < step_ms){
+                etat_lance_balle = LANCE_PORTE_OUVERTE;
+                i2c_annexe_ouvre_porte();
+                tempo_ms = 6000;
+            }else{
+                tempo_ms -= step_ms;
+            }
+            break;
+        
+        case LANCE_PORTE_OUVERTE:
+            if (tempo_ms < step_ms){
+                etat_lance_balle = LANCE_PROPULSEUR_ON;
+                i2c_annexe_desactive_propulseur();
+                etat_action = ACTION_TERMINEE;
+            }
+            break;
+
+
+    }
+    return etat_action;
+}
+
+/// @brief Envoie le robot se caler dans l'angle en face de lui, recale la localisation
+enum etat_action_t calage_angle(enum longer_direction_t longer_direction, double x_mm, double y_mm, double angle_radian){
+    enum etat_action_t etat_action = ACTION_EN_COURS;
+    struct position_t position;
+
+    avance_puis_longe_bordure(longer_direction);
+    if( ((longer_direction == LONGER_VERS_A) && (i2c_annexe_get_contacteur_butee_A() == CONTACTEUR_ACTIF) ) ||
+        ((longer_direction == LONGER_VERS_C) && (i2c_annexe_get_contacteur_butee_C() == CONTACTEUR_ACTIF) ) ){
+        etat_action = ACTION_TERMINEE;
+
+        position = Localisation_get();
+        if(fabs(position.x_mm - x_mm) < SEUIL_RECAL_DIST_MM){
+            Localisation_set_x(x_mm);
+        }
+        if(fabs(position.y_mm - y_mm) < SEUIL_RECAL_DIST_MM){
+            Localisation_set_y(y_mm);
+        }
+        if(fabs(position.angle_radian - angle_radian) < SEUIL_RECAL_ANGLE_RADIAN){
+            Localisation_set_angle(angle_radian);
+        }
+    }
+    return etat_action;
+}
+
+enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint32_t step_ms){
+    enum etat_action_t etat_action = ACTION_EN_COURS;
+    enum etat_trajet_t etat_trajet;
+    static enum {
+        PARCOURS_INIT,
+        PARCOURS_AVANCE,
+    } etat_parcourt=PARCOURS_INIT;
+
+    switch (etat_parcourt){
+        case PARCOURS_INIT:
+            Trajet_debut_trajectoire(trajectoire);
+            etat_parcourt = PARCOURS_AVANCE;
+            break;
+        
+        case PARCOURS_AVANCE:
+            etat_trajet = Trajet_avance(step_ms/1000.);
+            if(etat_trajet == TRAJET_TERMINE){
+                etat_action = ACTION_TERMINEE;
+                etat_parcourt = PARCOURS_INIT;
+            }
+            break;
+    }
+
+    return etat_action;
+}
+
+/// @brief Renvoi 1 si on doit attendre le déclenchement de la tirette
+uint attente_tirette(void){
+    return gpio_get(TIRETTE);
+}
+
+/// @brief Renvoi COULEUR_VERT ou COULEUR_BLEU
+enum couleur_t lire_couleur(void){
+    if (gpio_get(COULEUR))
+        return COULEUR_VERT;
+    return COULEUR_BLEU;
+
+}

Strategie.h

@@ -3,6 +3,9 @@
 #ifndef STRATEGIE_H
 #define STRATEGIE_H
 
+#define COULEUR 15
+#define TIRETTE 14
+
 enum etat_action_t{
     ACTION_EN_COURS,
     ACTION_TERMINEE
@@ -13,9 +16,30 @@ enum longer_direction_t{
     LONGER_VERS_C
 };
 
+enum couleur_t{
+    COULEUR_BLEU=0,
+    COULEUR_VERT
+};
+
+extern enum etat_strategie_t{
+    STRATEGIE_INIT=0,
+    APPROCHE_CERISE_1_A=1,
+    APPROCHE_CERISE_1_B=2,
+    ATTRAPE_CERISE_1=3,
+    APPROCHE_PANIER_1=4,
+    CALAGE_PANIER_1=5,
+    RECULE_PANIER=6,
+    LANCE_DANS_PANIER=7,
+    STRATEGIE_FIN=254,
+}etat_strategie;
+
 enum etat_action_t cerise_accostage(void);
-enum etat_action_t cerise_longer_bord(enum longer_direction_t longer_direction);
+enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_direction);
 void Homologation(uint32_t step_ms);
+enum couleur_t lire_couleur(void);
+uint attente_tirette(void);
+
+
 
 // STRATEGIE_H
 #endif

Strategie_prise_cerises.c

@@ -49,7 +49,7 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
             break;
         
         case ATTRAPE_VERS_BORDURE:
-            cerise_longer_bord(longer_direction);
+            avance_puis_longe_bordure(longer_direction);
             if( (longer_direction == LONGER_VERS_A) && (i2c_annexe_get_contacteur_butee_A() == CONTACTEUR_ACTIF) ||
                 (longer_direction == LONGER_VERS_C) && (i2c_annexe_get_contacteur_butee_C() == CONTACTEUR_ACTIF) ){
                     etat_attrape = TURBINE_DEMARRAGE;
@@ -59,6 +59,7 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
         case TURBINE_DEMARRAGE:
             i2c_annexe_ferme_porte();
             //i2c_annexe_active_turbine();
+            i2c_annexe_active_propulseur();
             commande_vitesse_stop();
             tempo_ms = 2000;
             etat_attrape = TURBINE_DEMARRAGE_TEMPO;
@@ -76,7 +77,7 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
 
         case ASPIRE_LONGE:
             longer_direction_aspire = inverser_longe_direction(longer_direction);
-            cerise_longer_bord(LONGER_VERS_A);
+            avance_puis_longe_bordure(LONGER_VERS_A);
             // La fonction cerise_longer_bord n'est efficace que tant que le robot a ses deux contacteur sur le support
             // Le robot n'a les deux contacteurs sur le support que tant qu'il est à moins de 240mm (MAX_LONGE_MM) de la bordure
             // En fonction du demi-terrain sur lequel se trouve le robot, on surveille la position en Z pour respecter cette condition
@@ -99,7 +100,7 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
 /// @brief Fonction pour accoster et longer une bordure
 /// @param longer_direction : direction dans laquelle le robot va aller une fois le long de la bordure
 /// @return ACTION_EN_COURS
-enum etat_action_t cerise_longer_bord(enum longer_direction_t longer_direction){
+enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_direction){
 
     static enum {
         LONGE_INIT,

Test_strategie.c

@@ -19,6 +19,7 @@
 int test_accostage(void);
 int test_longe(void);
 int test_homologation(void);
+int test_tirette_et_couleur();
 
 void affichage_test_strategie(){
     uint32_t temps;
@@ -38,6 +39,20 @@ void affichage_test_strategie(){
         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);
 
+        printf(">etat_strat:%d\n",etat_strategie);
+
+        /*switch(etat_strategie){
+            case STRATEGIE_INIT: printf(">etat_strat:STRATEGIE_INIT|t\n"); break;
+            case APPROCHE_CERISE_1_A: printf(">etat_strat:APPROCHE_CERISE_1_A|t\n");break;
+            case APPROCHE_CERISE_1_B: printf(">etat_strat:APPROCHE_CERISE_1_B|t\n");break;
+            case ATTRAPE_CERISE_1: printf(">etat_strat:ATTRAPE_CERISE_1|t\n");break;
+            case APPROCHE_PANIER_1: printf(">etat_strat:APPROCHE_PANIER_1|t\n");break;
+            case CALAGE_PANIER_1: printf(">etat_strat:CALAGE_PANIER_1|t\n");break;
+            case RECULE_PANIER: printf(">etat_strat:RECULE_PANIER|t\n");break;
+            case LANCE_DANS_PANIER: printf(">etat_strat:LANCE_DANS_PANIER|t\n");break;
+            case STRATEGIE_FIN: printf(">etat_strat:STRATEGIE_FIN|t\n");break;
+        }*/
+
         sleep_ms(100);
     }
 }
@@ -47,6 +62,7 @@ int test_strategie(){
     printf("L - longer.\n");
     printf("A - Accoster.\n");
     printf("H - Homologation.\n");
+    printf("C - Couleur et tirette.\n");
     int lettre;
     do{
         lettre = getchar_timeout_us(0);
@@ -57,6 +73,11 @@ int test_strategie(){
             while(test_accostage());
             break;
 
+        case 'c':
+        case 'C':
+            while(test_tirette_et_couleur());
+            break;
+
         case 'h':
         case 'H':
             while(test_homologation());
@@ -67,8 +88,6 @@ int test_strategie(){
             while(test_longe());
             break;
 
-        
-
         case 'q':
         case 'Q':
             return 0;
@@ -158,7 +177,7 @@ int test_longe(){
             }
 
             if(temps_ms > temps_ms_init + 200){
-                if(cerise_longer_bord(LONGER_VERS_A) == ACTION_TERMINEE){
+                if(avance_puis_longe_bordure(LONGER_VERS_A) == ACTION_TERMINEE){
                     printf("Accostage_terminee\n");
                 }
             }
@@ -221,3 +240,26 @@ int test_accostage(){
     }
     return 1;
 }
+
+
+int test_tirette_et_couleur(){
+    int lettre;
+    uint couleur, tirette;
+    printf("Tirette et couleur\n");
+
+    stdio_flush();
+
+    do{
+        printf(">tirette:%d\n", attente_tirette());
+        if(lire_couleur() == COULEUR_VERT){
+            printf(">couleur:Vert|t\n");
+        }else{
+            printf(">couleur:Bleu|t\n");
+        }
+        sleep_ms(10);
+    
+        lettre = getchar_timeout_us(0);
+    }while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
+    
+
+}