Ajout de l'erreur critique pour le gyro qui arrete la strategie

Monitoring.c

@@ -10,6 +10,7 @@
 
 uint32_t temps_cycle_min = UINT32_MAX;
 uint32_t temps_cycle_max=0;
+int erreur_critique = 0;
 int lock=0;
 uint32_t debug_var=0;
 float debug_varf=0;
@@ -88,4 +89,13 @@ void set_debug_varf(float variable){
 void Monitoring_Error(char * msg){
     gpio_put(LED_PIN_ROUGE, 1);
 
-}
+}
+
+void Monitoring_set_erreur_critique(){
+    Monitoring_Error("");
+    erreur_critique=1;
+}
+
+int Monitoring_get_erreur_critique(){
+    return erreur_critique;
+}

Monitoring.h

@@ -8,4 +8,6 @@ uint32_t temps_cycle_get_max();
 void set_debug_var(uint32_t variable);
 void set_debug_varf(float variable);
 void Monitoring_display();
-void Monitoring_Error(char * msg);
+void Monitoring_Error(char * msg);
+void Monitoring_set_erreur_critique();
+int Monitoring_get_erreur_critique();

Strategie.c

@@ -4,6 +4,7 @@
 #include "Commande_vitesse.h"
 #include "Geometrie_robot.h"
 #include "Localisation.h"
+#include "Monitoring.h"
 #include "Moteurs.h"
 #include "Score.h"
 #include "Strategie_prise_cerises.h"
@@ -53,8 +54,6 @@ int Robot_est_dans_zone_cerise_droite();
 enum etat_action_t Strategie_aller_cerises_laterales_proches(enum couleur_t couleur, uint32_t step_ms);
 enum etat_action_t Strategie_aller_cerises_laterales_opposees(enum couleur_t couleur, uint32_t step_ms);
 
-enum etat_homologation_t etat_homologation=STRATEGIE_INIT;
-
 
 void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
     
@@ -80,8 +79,13 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
         ALLER_PANIER,
         LANCER_PANIER,
         DECISION_STRATEGIE,
+        STRATEGIE_FIN
     }etat_strategie;
 
+    if(Monitoring_get_erreur_critique()){
+        etat_strategie=STRATEGIE_FIN;
+    }
+
     switch(etat_strategie){
         case STRATEGIE_INIT:
             if(couleur == COULEUR_BLEU){
@@ -121,7 +125,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
             }
             angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
 
-            Trajet_config(250, 500);            
+            Trajet_config(250, 250);            
             Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 156,
                      Localisation_get().angle_radian, angle_fin);
 
@@ -158,7 +162,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
             }
             angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, angle_fin);
 
-            Trajet_config(250, 500);            
+            Trajet_config(250, 250);            
             Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, point_x, 3000 - 175,
                      Localisation_get().angle_radian, angle_fin);
 
@@ -185,7 +189,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
             break;
 
         case ALLER_CERISE_GAUCHE:
-            Trajet_config(300, 250);
+            Trajet_config(250, 250);
             angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN);
             if(couleur == COULEUR_BLEU){
                 if(Strategie_aller_cerises_laterales_proches(couleur, step_ms)== ACTION_TERMINEE){
@@ -199,7 +203,6 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
 
             if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
                 etat_strategie = ATTRAPER_CERISE_GAUCHE;
-                Trajet_config(500, 500);
             }
             break;
 
@@ -211,7 +214,7 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
             break;
 
         case ALLER_CERISE_DROITE:
-            Trajet_config(300, 250);
+            Trajet_config(250, 250);
             angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, -150. * DEGRE_EN_RADIAN);
             if(couleur == COULEUR_BLEU){
                 if(Strategie_aller_cerises_laterales_opposees(couleur, step_ms)== ACTION_TERMINEE){
@@ -225,7 +228,6 @@ void Strategie(enum couleur_t couleur, uint32_t step_ms, uint32_t temps_ms){
 
             if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
                 etat_strategie = ATTRAPER_CERISE_GAUCHE;
-                Trajet_config(500, 500);
             }
             break;
 
@@ -298,14 +300,14 @@ enum etat_action_t Strategie_aller_cerises_laterales_proches(enum couleur_t coul
         Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
                             740, 3000 - 550,
                             510, 3000 - 1580,
-                            180, 1400,
+                            500, 1400,
                             Localisation_get().angle_radian, angle_fin);
     }else{
         angle_fin = Geometrie_get_angle_optimal(Localisation_get().angle_radian, 30. * DEGRE_EN_RADIAN);
         Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm,
                             2000 - 740, 3000 - 550,
                             2000 - 510, 3000 - 1580,
-                            2000 - 180, 1400,
+                            2000 - 500, 1400,
                             Localisation_get().angle_radian, angle_fin);
 
     }
@@ -338,7 +340,7 @@ enum etat_action_t Strategie_aller_cerises_laterales_opposees(enum couleur_t cou
 enum etat_action_t  Strategie_aller_panier(enum couleur_t couleur, uint32_t step_ms){
     enum etat_action_t etat_action = ACTION_EN_COURS;
     struct trajectoire_t trajectoire;
-    Trajet_config(500, 500);
+    Trajet_config(250, 250);
 
     // Definition des trajectoires
     if(couleur == COULEUR_BLEU){
@@ -552,7 +554,7 @@ enum etat_action_t lance_balles_dans_panier(enum couleur_t couleur, uint32_t ste
 
             if(parcourt_trajet_simple_sans_evitement(trajectoire, step_ms) == ACTION_TERMINEE){
                 etat_lance_balles_dans_panier = LANCE_DANS_PANIER;
-                Trajet_config(500, 500);
+                Trajet_config(250, 250);
             }
             break;
 

Strategie.h

@@ -25,25 +25,6 @@ enum couleur_t{
     COULEUR_INCONNUE,
 };
 
-extern enum etat_homologation_t{
-    STRATEGIE_INIT,
-    ATTENTE_TIRETTE,
-    APPROCHE_CERISE_1_A,
-    APPROCHE_CERISE_1_B,
-    ATTRAPE_CERISE_1,
-    APPROCHE_PANIER_1,
-    APPROCHE_PANIER_2,
-    CALAGE_PANIER_1,
-    RECULE_PANIER,
-    TOURNE_PANIER,
-    LANCE_DANS_PANIER,
-    APPROCHE_CERISE_2,
-    ATTRAPPE_CERISE_2,
-    APPROCHE_PANIER_3,
-    STRATEGIE_FIN,
-    
-}etat_homologation;
-
 enum evitement_t{
     SANS_EVITEMENT,
     PAUSE_DEVANT_OBSTACLE,

Test_strategie.c

@@ -51,7 +51,6 @@ void affichage_test_strategie(){
 
         printf(">tirette:%ld:%d\n", temps, attente_tirette());
 
-        printf(">etat_strat:%ld:%d\n", temps, etat_homologation);
         printf(">distance_obstacle:%ld:%f\n", temps, Trajet_get_obstacle_mm());
         printf(">angle_avance:%ld:%f\n", temps, Trajet_get_orientation_avance());
         

gyro_ADXRS453.c

@@ -64,19 +64,21 @@ int gyro_get_sensor_data(uint16_t tampon_envoi[], uint8_t tampon_reception[]){
             //printf("SPI ERROR\n");
             return 1;
         }else{
-            /*while(1){
+            while(1){
                 affiche_tampon_32bits(tampon_reception);
                 printf("Gyro Failed - SQ bits (%#3x)!= 0x4\n", Gyro_SensorData.SQ);
-            }*/
+            }
             //set_position_avec_gyroscope(0);
         }
     }
     if(Gyro_SensorData.ST != 0x1){
         Monitoring_Error("Gyro Failed - Status != 0x1\n");
+        set_position_avec_gyroscope(0);
         /*while(1){
             printf("Gyro Failed - Status (%#3x)!= 0x1\n", Gyro_SensorData.ST);
             affiche_tampon_32bits(tampon_reception);
         }*/
+        Monitoring_set_erreur_critique();
         //set_position_avec_gyroscope(0);
         return 1;
     }