Ajout score dans la stratégie + modification cerises latérales

CMakeLists.txt

@@ -25,6 +25,7 @@ Localisation.c
 Moteurs.c
 Monitoring.c
 Robot_config.c
+Score.c
 Servomoteur.c
 Strategie.c
 Strategie_prise_cerises.c

Holonome2023.c

@@ -82,7 +82,7 @@ int main() {
     AsserMoteur_Init();
     Localisation_init();
 
-    //while(mode_test());
+    while(mode_test());
     i2c_maitre_init();
     Trajet_init();
     Balise_VL53L1X_init();
@@ -154,7 +154,7 @@ int main() {
                         printf("MATCH_ARRET_EN_COURS\n");
                         statu_match = MATCH_ARRET_EN_COURS;
                     }
-                    Strategie(couleur, _step_ms);
+                    Strategie(couleur, _step_ms, timer_match_ms);
                     break;
                 
                 case MATCH_ARRET_EN_COURS:

Score.c

@@ -0,0 +1,40 @@
+#include "pico/stdlib.h"
+#include "i2c_annexe.h"
+#include "Score.h"
+
+#define SCORE_INITIAL 5
+
+uint32_t Score_nb_cerises=0;
+uint32_t Score_nb_points=0;
+uint32_t Score_funny_action=0;
+uint32_t Score_pieds_dans_plat=0;
+
+void Score_recalcule(){
+    Score_nb_points = SCORE_INITIAL;
+    if(Score_nb_cerises > 0){
+        Score_nb_points += Score_nb_cerises + 5;
+    }
+    Score_nb_points += Score_funny_action;
+    Score_nb_points += Score_pieds_dans_plat;
+
+}
+
+void Score_actualise(){
+    Score_recalcule();
+    i2c_annexe_envoie_score(Score_nb_points);
+}
+
+void Score_set_funny_action(){
+    Score_funny_action = 5;
+    Score_actualise();
+}
+
+void Score_set_pieds_dans_plat(){
+    Score_pieds_dans_plat = 15;
+    Score_actualise();
+}
+
+void Score_ajout_cerise(uint32_t nb_cerises){
+    Score_nb_cerises += nb_cerises;
+    Score_actualise();
+}

Score.h

@@ -0,0 +1,5 @@
+#include "pico/stdlib.h"
+
+void Score_set_funny_action();
+void Score_set_pieds_dans_plat();
+void Score_ajout_cerise(uint32_t nb_cerises);

Strategie.c

@@ -6,6 +6,7 @@
 #include "Geometrie_robot.h"
 #include "Localisation.h"
 #include "Moteurs.h"
+#include "Score.h"
 #include "Strategie_prise_cerises.h"
 #include "Strategie.h"
 #include "Trajet.h"
@@ -25,14 +26,13 @@ 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){
+void Strategie_set_cerise_dans_robot(uint32_t nb_cerises){
     Score_cerises_dans_robot = nb_cerises;
 }
 
-uint32_t Score_get_cerise_dans_robot(void){
+uint32_t Strategie_get_cerise_dans_robot(void){
     return Score_cerises_dans_robot;
 }
 
@@ -48,11 +48,13 @@ enum etat_action_t calage_angle(enum longer_direction_t longer_direction, float
 
 int Robot_est_dans_quart_haut_gauche();
 int Robot_est_dans_quart_haut_droit();
+int Robot_est_dans_zone_cerise_haut(enum couleur_t couleur);
+int Robot_est_dans_zone_cerise_gauche();
 
 enum etat_homologation_t etat_homologation=STRATEGIE_INIT;
 
 
-void Strategie(enum couleur_t couleur, uint32_t step_ms){
+void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
     
     float angle_fin;
     float recal_pos_x, recal_pos_y;
@@ -136,7 +138,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);
+                Strategie_set_cerise_dans_robot(6);
                 etat_strategie = ALLER_PANIER;
             }
             break;
@@ -157,7 +159,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);
+                Strategie_set_cerise_dans_robot(6);
             }
             break;
 
@@ -214,7 +216,9 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms){
             break;
 
         case LANCER_PANIER:
-            if(lance_balles_dans_panier(couleur, step_ms, Score_get_cerise_dans_robot())== ACTION_TERMINEE){
+            if(lance_balles_dans_panier(couleur, step_ms, Strategie_get_cerise_dans_robot())== ACTION_TERMINEE){
+                Score_ajout_cerise(Strategie_get_cerise_dans_robot());
+                Strategie_set_cerise_dans_robot(0);
                 objectif_courant->etat = FAIT;
                 etat_strategie = DECISION_STRATEGIE;
             }
@@ -265,13 +269,16 @@ enum etat_action_t  Strategie_aller_panier(enum couleur_t couleur, uint32_t step
 
     // Definition des trajectoires
     if(couleur == COULEUR_BLEU){
-        // Si le robot est déjà dans le quart haut gauche, 
+        // Si le robot est déjà dans la zone cerise haut
         // On va en ligne droite
-        if(Robot_est_dans_quart_haut_gauche()){
+        if(Robot_est_dans_zone_cerise_haut(couleur)){
             Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
                 465,2830,
                 Localisation_get().angle_radian, 
                 Geometrie_get_angle_optimal(Localisation_get().angle_radian, +120. * DEGRE_EN_RADIAN));
+        }else if (Robot_est_dans_zone_cerise_gauche()){
+
+            /// !!! TODO !!!
         }else{
         // Sinon, on a une courbe de bézier
             Trajectoire_bezier(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
@@ -307,6 +314,8 @@ enum etat_action_t  Strategie_aller_panier(enum couleur_t couleur, uint32_t step
 
 }
 
+/// Fonction de localisation aproximatives pour la stratégie.
+
 /// @brief Renvoi 1 si le robot est dans le quart supérieur gauche du terrain
 /// @return 1 si oui, 0 si non.
 int Robot_est_dans_quart_haut_gauche(){
@@ -325,6 +334,28 @@ int Robot_est_dans_quart_haut_droit(){
     return 0;
 }
 
+int Robot_est_dans_zone_cerise_gauche(){
+    if(Localisation_get().x_mm  < 630 && 
+    Localisation_get().y_mm >1000 && Localisation_get().y_mm < 2000){
+        return 1;
+    }
+    return 0;
+
+}
+
+int Robot_est_dans_zone_cerise_haut(enum couleur_t couleur){
+    if(couleur == COULEUR_BLEU){
+        if(Localisation_get().y_mm  > 2500 && Localisation_get().x_mm < 1000){
+            return 1;
+        }
+    }else{
+        if(Localisation_get().y_mm  > 2500 && Localisation_get().x_mm > 1000){
+            return 1;
+        }
+    }
+    return 0;
+}
+
 enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t step_ms, uint32_t nb_cerises){
     static enum {
         CALAGE_PANIER_1,

Strategie.h

@@ -54,7 +54,7 @@ enum couleur_t lire_couleur(void);
 uint attente_tirette(void);
 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);
+void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms);
 
 int temporisation_terminee(uint32_t * tempo_ms, uint32_t step_ms);
 enum etat_action_t  Strategie_aller_panier(enum couleur_t couleur, uint32_t step_ms);

Strategie_prise_cerises.c

@@ -7,6 +7,7 @@
 #include "Localisation.h"
 #include "math.h"
 #include "i2c_annexe.h"
+#include "Trajet.h"
 
 // Rotation en rad/s pour accoster les cerises
 #define ROTATION_CERISE 0.5f
@@ -18,9 +19,12 @@
 // Translation en mm/s pour aspirer les cerises
 #define TRANSLATION_CERISE 70
 
+// Seuil angulaire pour la détection de la fin de la barrette de cerises
+#define SEUIL_ANGLE_CERISE (2. * DEGRE_EN_RADIAN)
+
 void commande_rotation_contacteur_longer_A();
 void commande_rotation_contacteur_longer_C();
-enum etat_action_t cerises_attraper_demi_cerises_laterale(uint32_t step_ms, enum longer_direction_t longer_direction);
+enum etat_action_t cerises_attraper_demi_cerises_laterale(uint32_t step_ms, enum longer_direction_t longer_direction, float pos_recal_x_mm);
 enum etat_action_t demarre_turbine(uint32_t step_ms);
 
 enum longer_direction_t inverser_longe_direction(enum longer_direction_t direction);
@@ -39,10 +43,11 @@ enum etat_action_t cerises_attraper_cerises_gauches(uint32_t step_ms){
     }etat_attrappe_cerises_gauche = ATTRAPE_CERISE_DEMI_BAS;
     struct trajectoire_t trajectoire;
     float angle_fin;
+    float pos_recal_x_mm = PETIT_RAYON_ROBOT_MM + 30;
 
     switch (etat_attrappe_cerises_gauche){
         case ATTRAPE_CERISE_DEMI_BAS:
-            if(cerises_attraper_demi_cerises_laterale(step_ms, LONGER_VERS_A) == ACTION_TERMINEE){
+            if(cerises_attraper_demi_cerises_laterale(step_ms, LONGER_VERS_A, pos_recal_x_mm) == ACTION_TERMINEE){
                 etat_attrappe_cerises_gauche = REVENIR_CENTRE;
             }
             break;
@@ -59,7 +64,7 @@ enum etat_action_t cerises_attraper_cerises_gauches(uint32_t step_ms){
             break;
         
         case ATTRAPE_CERISE_DEMI_HAUT:
-            if(cerises_attraper_demi_cerises_laterale(step_ms, LONGER_VERS_C) == ACTION_TERMINEE){
+            if(cerises_attraper_demi_cerises_laterale(step_ms, LONGER_VERS_C, pos_recal_x_mm) == ACTION_TERMINEE){
                 etat_attrappe_cerises_gauche = ATTRAPE_CERISE_DEMI_BAS;
                 return ACTION_TERMINEE;
             }
@@ -70,7 +75,7 @@ enum etat_action_t cerises_attraper_cerises_gauches(uint32_t step_ms){
     
 }
 
-enum etat_action_t cerises_attraper_demi_cerises_laterale(uint32_t step_ms, enum longer_direction_t longer_direction){
+enum etat_action_t cerises_attraper_demi_cerises_laterale(uint32_t step_ms, enum longer_direction_t longer_direction, float pos_recal_x_mm){
     // 1 accoster
     // Demarrer la turbine
     // 2 Longer en aspirant
@@ -79,12 +84,19 @@ enum etat_action_t cerises_attraper_demi_cerises_laterale(uint32_t step_ms, enum
         ACCOSTAGE,
         DEMARRE_TURBINE,
         LONGE,
+        TOURNE,
         AVANCE,
     }etat_attrappe_demi_cerise=ACCOSTAGE;
+    const float continu_avance_mm = 60;
+    static float orientation_ref;
+    static float pos_y_ref;
+    struct trajectoire_t trajectoire;
 
     switch(etat_attrappe_demi_cerise){
         case ACCOSTAGE:
             if(cerise_accostage() == ACTION_TERMINEE){
+                Localisation_set_x(pos_recal_x_mm);
+                orientation_ref = Localisation_get().angle_radian;
                 etat_attrappe_demi_cerise = DEMARRE_TURBINE;
             }
             break;
@@ -101,7 +113,18 @@ enum etat_action_t cerises_attraper_demi_cerises_laterale(uint32_t step_ms, enum
             // Le robot n'a les deux contacteurs sur le support que tant qu'il est à moins de 240mm (MAX_LONGE_MM) de la bordure
             // ou 120 (MAX_LONGE_MM/2) du milieu de la bordure
             // En fonction du demi-terrain sur lequel se trouve le robot, on surveille la position en Y pour respecter cette condition
-            if( (Localisation_get().y_mm > 1500 + MAX_LONGE_MM/2 ) || (Localisation_get().y_mm < 1500 - MAX_LONGE_MM/2 )){
+            /*if( (Localisation_get().y_mm > 1500 + MAX_LONGE_MM/2 ) || (Localisation_get().y_mm < 1500 - MAX_LONGE_MM/2 )){
+                etat_attrappe_demi_cerise = AVANCE;
+            }*/
+            if(fabs(orientation_ref - Localisation_get().angle_radian) > SEUIL_ANGLE_CERISE){
+                etat_attrappe_demi_cerise = TOURNE;
+                pos_y_ref = Localisation_get().y_mm;
+            }
+            break;
+        
+        case TOURNE:
+            Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian, orientation_ref);
+            if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){
                 etat_attrappe_demi_cerise = AVANCE;
             }
             break;
@@ -113,7 +136,7 @@ enum etat_action_t cerises_attraper_demi_cerises_laterale(uint32_t step_ms, enum
                 commande_translation_longer_vers_C();
             }
 
-            if( (Localisation_get().y_mm > 1500 + MAX_ASPIRE_CERISE_MM/2 ) || (Localisation_get().y_mm < 1500 - MAX_ASPIRE_CERISE_MM/2 )){
+            if( (Localisation_get().y_mm > pos_y_ref + continu_avance_mm ) || (Localisation_get().y_mm < pos_y_ref - continu_avance_mm )){
                 etat_attrappe_demi_cerise = ACCOSTAGE;
                 i2c_annexe_desactive_turbine();
                 commande_vitesse_stop();

Test_strategie.c

@@ -139,7 +139,7 @@ int test_cerise_laterales(){
     Trajet_init();
     Balise_VL53L1X_init();
     //printf("Init gyroscope\n");
-    set_position_avec_gyroscope(0);
+    set_position_avec_gyroscope(1);
     if(get_position_avec_gyroscope()){
         Gyro_Init();
     }