Reprise de la fonction de calage dans un angle.

Geometrie.c

@@ -35,6 +35,22 @@ unsigned int Geometrie_compare_angle(float angle, float angle_min, float angle_m
     }
 }
 
+/// @brief A partir de l'orientation actuelle du robot et de l'orientation souhaitée, 
+/// donne l'angle consigne pour limiter les rotations inutiles.
+/// Tous les angles sont en radian
+/// @param angle_depart 
+/// @param angle_souhaite 
+/// @return angle_optimal en radian
+float Geometrie_get_angle_optimal(float angle_depart, float angle_souhaite){
+    while((angle_depart - angle_souhaite) > M_PI){
+        angle_souhaite += 2* M_PI;
+    }
+    while((angle_depart - angle_souhaite) < -M_PI){
+        angle_souhaite -= 2* M_PI;
+    }
+    return angle_souhaite;
+}
+
 /// @brief Indique si les deux plages d'angle se recoupent
 /// @param angle1_min Début de la première plage
 /// @param angle1_max Fin de la première plage

Geometrie.h

@@ -16,5 +16,6 @@ struct position_t{
 float Geometrie_get_angle_normalisee(float angle);
 unsigned int Geometrie_compare_angle(float angle, float angle_min, float angle_max);
 unsigned int Geometrie_intersecte_plage_angle(float angle1_min, float angle1_max, float angle2_min, float angle2_max);
+float Geometrie_get_angle_optimal(float angle_depart, float angle_souhaite);
 
 #endif

Monitoring.c

@@ -1,6 +1,7 @@
 #include "pico/stdlib.h"
 #include <stdio.h>
 #include "Monitoring.h"
+#include "Localisation.h"
 
 uint32_t temps_cycle_min = UINT32_MAX;
 uint32_t temps_cycle_max=0;
@@ -32,7 +33,16 @@ void temps_cycle_reset(){
 
 void Monitoring_display(){
     while(1){
+        uint32_t temps;
+        temps = time_us_32()/1000;
         temps_cycle_display();
+        printf(">DebugVar:%ld:%d\n", temps, debug_var);
+        printf(">DebugVarf:%ld:%f\n", temps, debug_varf);
+        struct position_t position = Localisation_get();
+        printf(">pos_x:%ld:%f\n", temps, position.x_mm);
+        printf(">pos_y:%ld:%f\n", temps, position.y_mm);
+        
+
     }
 }
 
@@ -41,8 +51,6 @@ void temps_cycle_display(){
     temps = time_us_32()/1000;
     printf(">T_cycle_min(us):%ld:%d\n", temps, temps_cycle_min);
     printf(">T_cycle_max(us):%ld:%d\n", temps, temps_cycle_max);
-    printf(">DebugVar:%ld:%d\n", temps, debug_var);
-    printf(">DebugVarf:%ld:%f\n", temps, debug_varf);
     temps_cycle_reset();
 }
 

Strategie.c

@@ -25,14 +25,26 @@ struct objectif_t{
 } objectifs[NB_OBJECTIFS];
 struct objectif_t * objectif_courant;
 
+uint32_t Score_nb_cerises = 0;
+uint32_t Score_cerises_dans_robot=0;
+
+void Score_set_cerise_dans_robot(uint32_t nb_cerises){
+    Score_cerises_dans_robot = nb_cerises;
+}
+
+uint32_t Score_get_cerise_dans_robot(void){
+    return Score_cerises_dans_robot;
+}
+
 
 
 // TODO: Peut-être à remetttre en variable locale après
 float distance_obstacle;
 
-enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms);
+enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms, uint32_t nb_cerises);
+enum etat_action_t lance_balles(uint32_t step_ms, uint32_t nb_cerises);
 enum etat_action_t calage_angle(enum longer_direction_t longer_direction, float x_mm, float y_mm, float angle_radian);
-enum etat_action_t lance_balles(uint32_t step_ms);
+
 
 int Robot_est_dans_quart_haut_gauche();
 int Robot_est_dans_quart_haut_droit();
@@ -124,6 +136,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){
 
             etat_action = cerise_attraper_bordure(longer_direction, step_ms, recal_pos_x, recal_pos_y);
             if(etat_action == ACTION_TERMINEE){
+                Score_set_cerise_dans_robot(6);
                 etat_strategie = ALLER_PANIER;
             }
             break;
@@ -144,6 +157,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){
 
             if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){
                 etat_strategie = ATTRAPER_CERISE_HAUT;
+                Score_set_cerise_dans_robot(6);
             }
             break;
 
@@ -170,7 +184,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){
             break;
 
         case LANCER_PANIER:
-            if(lance_balles_dans_panier(couleur, step_ms)== ACTION_TERMINEE){
+            if(lance_balles_dans_panier(couleur, step_ms, Score_get_cerise_dans_robot())== ACTION_TERMINEE){
                 objectif_courant->etat = FAIT;
                 etat_strategie = DECISION_STRATEGIE;
             }
@@ -235,7 +249,7 @@ enum etat_action_t  Strategie_aller_panier(enum couleur_t couleur, uint32_t step
                 465,2830,
                 +30. * DEGRE_EN_RADIAN, +120. * DEGRE_EN_RADIAN);
         }
-    }else{
+    }else{ // COULEUR_VERT
         // Si le robot est déjà dans le quart haut droit, 
         // On va en ligne droite
         if(Robot_est_dans_quart_haut_droit()){
@@ -245,7 +259,7 @@ enum etat_action_t  Strategie_aller_panier(enum couleur_t couleur, uint32_t step
         }else{
         // Sinon, on a une courbe de bézier
             Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
-                            485, Localisation_get().y_mm,
+                            2000-485, Localisation_get().y_mm,
                             2000-465, 857,
                             2000-465, 2830,
                             -150. * DEGRE_EN_RADIAN, -240. * DEGRE_EN_RADIAN);
@@ -279,113 +293,14 @@ int Robot_est_dans_quart_haut_droit(){
     return 0;
 }
 
-
+enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms, uint32_t nb_cerises){
-void Homologation(uint32_t step_ms){
-    
-
-    enum etat_action_t etat_action;
-    enum etat_trajet_t etat_trajet;
-
-    struct trajectoire_t trajectoire;
-
-    switch(etat_homologation){
-        case STRATEGIE_INIT:
-            Localisation_set(775., 109., (-60.+CORR_ANGLE_DEPART_DEGREE) * DEGRE_EN_RADIAN);
-            etat_homologation = ATTENTE_TIRETTE;
-            break;
-
-        case ATTENTE_TIRETTE:
-            if(attente_tirette() == 0){
-                etat_homologation = APPROCHE_CERISE_1_A;
-            }
-            break;
-
-        case APPROCHE_CERISE_1_A:
-            Trajet_config(250, 500);
-            Trajectoire_droite(&trajectoire,775, 109, 857, 156, -60. * DEGRE_EN_RADIAN, 30. * DEGRE_EN_RADIAN);
-            if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == TRAJET_TERMINE){
-                etat_homologation = ATTRAPE_CERISE_1;
-            }
-            break;
-        
-        case ATTRAPE_CERISE_1:
-            etat_action = cerise_attraper_bordure(LONGER_VERS_C, step_ms, 1000-15-PETIT_RAYON_ROBOT_MM, RAYON_ROBOT);
-            if(etat_action == ACTION_TERMINEE){
-                etat_homologation = APPROCHE_PANIER_1;
-            }
-            break;
-
-        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. * DEGRE_EN_RADIAN, +120. * DEGRE_EN_RADIAN);
-
-            if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
-                etat_homologation = CALAGE_PANIER_1;
-            }
-            break;
-
-        case CALAGE_PANIER_1:
-            if(calage_angle(LONGER_VERS_A, RAYON_ROBOT, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2.)), 120. *DEGRE_EN_RADIAN) == ACTION_TERMINEE){
-                etat_homologation = RECULE_PANIER;
-            }
-            break;
-        
-        case RECULE_PANIER:
-            Trajet_config(250, 500);
-            Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
-                                180, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 80,
-                                120. * DEGRE_EN_RADIAN, +270. * DEGRE_EN_RADIAN);
-
-            if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){
-                etat_homologation = LANCE_DANS_PANIER;
-            }
-            break;
-
-        case LANCE_DANS_PANIER:
-            Asser_Position_maintien();
-            if(lance_balles(step_ms) == ACTION_TERMINEE){
-                etat_homologation = STRATEGIE_FIN;
-            }
-            break;
-
-        case APPROCHE_CERISE_2:
-            Trajet_config(250, 500);
-            Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
-                                830, 3000 - 156,
-                                Localisation_get().angle_radian, 30. * DEGRE_EN_RADIAN);
-
-            if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){
-                etat_homologation = STRATEGIE_FIN;
-            }
-            break;
-
-        case ATTRAPPE_CERISE_2:
-            etat_action = cerise_attraper_bordure(LONGER_VERS_A, step_ms, 1000-15-PETIT_RAYON_ROBOT_MM, 3000-RAYON_ROBOT);
-            if(etat_action == ACTION_TERMINEE){
-                etat_homologation = APPROCHE_PANIER_2;
-            }
-            break;
-
-
-        case STRATEGIE_FIN:
-            i2c_annexe_desactive_propulseur();
-            commande_vitesse_stop();
-            break;
-    }
-}
-
-
-enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms){
     static enum {
         CALAGE_PANIER_1,
         RECULE_PANIER,
         LANCE_DANS_PANIER,
     }etat_lance_balles_dans_panier;
     float recal_pos_x, recal_pos_y;
+    float angle_depart, angle_arrivee;    
     enum longer_direction_t longer_direction;
     float point_x, point_y;
 
@@ -417,9 +332,11 @@ enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t ste
                 point_x = 1735;
                 point_y = 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 80;
             }
+            angle_depart = Geometrie_get_angle_optimal(Localisation_get().angle_radian, 120. * DEGRE_EN_RADIAN);
+            angle_arrivee = Geometrie_get_angle_optimal(angle_depart, 270. * DEGRE_EN_RADIAN);
             Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 
                                 point_x, point_y,
-                                120. * DEGRE_EN_RADIAN, +270. * DEGRE_EN_RADIAN);
+                                angle_depart, angle_arrivee);
 
             if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){
                 etat_lance_balles_dans_panier = LANCE_DANS_PANIER;
@@ -428,7 +345,7 @@ enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t ste
 
         case LANCE_DANS_PANIER:
             Asser_Position_maintien();
-            if(lance_balles(step_ms) == ACTION_TERMINEE){
+            if(lance_balles(step_ms, nb_cerises) == ACTION_TERMINEE){
                 etat_action = ACTION_TERMINEE;
                 etat_lance_balles_dans_panier = CALAGE_PANIER_1;
             }
@@ -441,7 +358,7 @@ enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t ste
 /// @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 lance_balles(uint32_t step_ms, uint32_t nb_cerises){
     enum etat_action_t etat_action = ACTION_EN_COURS;
     static uint32_t tempo_ms;
     static uint32_t nb_iteration;
@@ -473,7 +390,7 @@ enum etat_action_t lance_balles(uint32_t step_ms){
             if(temporisation_terminee(&tempo_ms, step_ms)){
                 i2c_annexe_ouvre_porte();
                 nb_iteration++;
-                if(nb_iteration > 10){
+                if(nb_iteration > nb_cerises){
                     nb_iteration=0;
                     etat_action = ACTION_TERMINEE;
                     etat_lance_balle = LANCE_PROPULSEUR_ON;
@@ -498,26 +415,69 @@ enum etat_action_t lance_balles(uint32_t step_ms){
 }
 
 /// @brief Envoie le robot se caler dans l'angle en face de lui, recale la localisation
+/// Ne fonctionne que contre les bordures haute et basse, pas contre les bordures gauche et droites
 enum etat_action_t calage_angle(enum longer_direction_t longer_direction, float x_mm, float y_mm, float angle_radian){
     enum etat_action_t etat_action = ACTION_EN_COURS;
     struct position_t position;
+    struct trajectoire_t trajectoire;
+    static float y_pos_ref;
 
-    avance_puis_longe_bordure(longer_direction);
+    static enum {
-    if( ((longer_direction == LONGER_VERS_A) && (i2c_annexe_get_contacteur_butee_A() == CONTACTEUR_ACTIF) ) ||
+        CONTACT_AVANT,
-        ((longer_direction == LONGER_VERS_C) && (i2c_annexe_get_contacteur_butee_C() == CONTACTEUR_ACTIF) ) ){
+        RECULE_BORDURE,
-        etat_action = ACTION_TERMINEE;
+        CONTACT_LATERAL,
+        RECALE_X
+    }etat_calage_angle;
 
+    switch(etat_calage_angle){
+        case CONTACT_AVANT:
+            if(cerise_accostage() == ACTION_TERMINEE){
                 position = Localisation_get();
-        if(fabs(position.x_mm - x_mm) < SEUIL_RECAL_DIST_MM){
+                //if(fabs(position.y_mm - y_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){
+                //if(fabs(position.angle_radian - angle_radian) < SEUIL_RECAL_ANGLE_RADIAN){
                     Localisation_set_angle(angle_radian);
+                //}
+                y_pos_ref = Localisation_get().y_mm;
+                etat_calage_angle = RECULE_BORDURE;
+            }
+            break;
+
+        case RECULE_BORDURE:
+            commande_translation_recule_vers_trompe();
+            position = Localisation_get();
+            if(fabs(y_pos_ref - Localisation_get().y_mm) > 35){
+                etat_calage_angle = CONTACT_LATERAL;
+            }
+            break;
+
+        case CONTACT_LATERAL:
+            if(longer_direction == LONGER_VERS_A){
+                commande_translation_avance_vers_A();
+                if(i2c_annexe_get_contacteur_butee_A() == CONTACTEUR_ACTIF){
+                    etat_calage_angle = RECALE_X;
+                }
+            }else{
+                commande_translation_avance_vers_C();
+                if(i2c_annexe_get_contacteur_butee_C() == CONTACTEUR_ACTIF){
+                    etat_calage_angle = RECALE_X;
                 }
             }
+            break;
+
+        case RECALE_X:
+            etat_action = ACTION_TERMINEE;
+            commande_vitesse_stop();
+            position = Localisation_get();
+            //if(fabs(position.x_mm - x_mm) < SEUIL_RECAL_DIST_MM){
+                Localisation_set_x(x_mm);
+            //}
+            etat_calage_angle = CONTACT_AVANT;
+            break;
+
+    }
+
     return etat_action;
 }
 

Strategie.h

@@ -52,7 +52,7 @@ enum etat_action_t depose_cerises(uint32_t step_ms);
 void Homologation(uint32_t step_ms);
 enum couleur_t lire_couleur(void);
 uint attente_tirette(void);
-enum etat_action_t lance_balles(uint32_t step_ms);
+enum etat_action_t lance_balles(uint32_t step_ms, uint32_t nb_cerises);
 int test_panier(void);
 void Strategie(enum couleur_t couleur, uint32_t step_ms);
 

Strategie_prise_cerises.c

@@ -20,8 +20,7 @@
 
 void commande_rotation_contacteur_longer_A();
 void commande_rotation_contacteur_longer_C();
-void commande_translation_longer_vers_A();
+
-void commande_translation_longer_vers_C();
 enum longer_direction_t inverser_longe_direction(enum longer_direction_t direction);
 
 
@@ -160,6 +159,7 @@ enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_dire
 
 }
 
+/// @brief Viens position le robot contre une bordure ou un support cerise devant lui.
 enum etat_action_t cerise_accostage(void){
     enum etat_action_t etat_action = ACTION_EN_COURS;
     float rotation;
@@ -174,7 +174,7 @@ enum etat_action_t cerise_accostage(void){
     switch (etat_accostage)
     {
     case CERISE_AVANCE_DROIT:
-        commande_vitesse(vitesse_accostage_mm_s * cos(-M_PI/6), vitesse_accostage_mm_s * sin(-M_PI/6), 0);
+        commande_translation_avance_vers_trompe();
         if(i2c_annexe_get_contacteur_longer_A() == CONTACTEUR_ACTIF){
             etat_accostage=CERISE_TOURNE_CONTACTEUR_LONGER_A;
         }
@@ -245,6 +245,26 @@ void commande_translation_longer_vers_C(){
     commande_vitesse(-TRANSLATION_CERISE/2., -TRANSLATION_CERISE / 2. * RACINE_DE_3, 0);
 }
 
+void commande_translation_avance_vers_trompe(){
+    commande_vitesse(vitesse_accostage_mm_s * cos(-M_PI/6), vitesse_accostage_mm_s * sin(-M_PI/6), 0);
+}
+
+void commande_translation_recule_vers_trompe(){
+    commande_vitesse(-vitesse_accostage_mm_s * cos(-M_PI/6), -vitesse_accostage_mm_s * sin(-M_PI/6), 0);
+}
+
+void commande_translation_avance_vers_A(){
+    // V_x : V * cos (60°) = V / 2
+    // V_y : V * sin (60°) = V * RACINE(3) / 2
+    commande_vitesse(vitesse_accostage_mm_s/2., vitesse_accostage_mm_s / 2. * RACINE_DE_3, 0);
+}
+
+void commande_translation_avance_vers_C(){
+    // V_x : -V * cos (60°) = -V / 2
+    // V_y : -V * sin (60°) = -V * RACINE(3) / 2
+    commande_vitesse(-vitesse_accostage_mm_s/2., -vitesse_accostage_mm_s / 2. * RACINE_DE_3, 0);
+}
+
 enum longer_direction_t inverser_longe_direction(enum longer_direction_t direction){
     if(direction ==LONGER_VERS_A){
         return LONGER_VERS_C;

Strategie_prise_cerises.h

@@ -1,2 +1,8 @@
 #include "Strategie.h"
 enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms, float pos_x_mm, float pos_y_mm);
+void commande_translation_longer_vers_A();
+void commande_translation_longer_vers_C();
+void commande_translation_avance_vers_trompe();
+void commande_translation_recule_vers_trompe();
+void commande_translation_avance_vers_A();
+void commande_translation_avance_vers_C();

Test_strategie.c

@@ -331,7 +331,7 @@ int test_panier(){
                     break;
 
                 case TEST_PANIER_LANCE_BALLES:
-                    if(lance_balles(_step_ms) == ACTION_TERMINEE){
+                    if(lance_balles(_step_ms, 10) == ACTION_TERMINEE){
                         etat_test_panier=TEST_PANIER_PORTE_OUVERTE;
                     }
                     break;