From 1bc1514b236c4872e3d4f19d33f10121b14f804f Mon Sep 17 00:00:00 2001
From: Samuel <samuel.kay@poivron-robotique.fr>
Date: Wed, 29 Mar 2023 23:12:16 +0200
Subject: [PATCH] =?UTF-8?q?Strat=C3=A9gie=20d'homologation=20:=20sans=20?=
 =?UTF-8?q?=C3=A9vitement,=20sans=20tirette,=20sans=20gestion=20du=20c?=
 =?UTF-8?q?=C3=B4t=C3=A9.?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Strategie.c               | 23 +++++++++++++++++++----
 Strategie.h               |  7 ++++---
 Strategie_prise_cerises.c |  8 ++++----
 Test_strategie.c          |  7 +++++--
 4 files changed, 32 insertions(+), 13 deletions(-)

diff --git a/Strategie.c b/Strategie.c
index 4a9384f..18f10c0 100644
--- a/Strategie.c
+++ b/Strategie.c
@@ -1,5 +1,6 @@
 #include "hardware/gpio.h"
 #include "i2c_annexe.h"
+#include "Asser_Position.h"
 #include "Geometrie_robot.h"
 #include "Localisation.h"
 #include "Moteurs.h"
@@ -61,6 +62,16 @@ void Homologation(uint32_t step_ms){
                                 465,2830,
                                 +30. * DEGREE_EN_RADIAN, +120. * DEGREE_EN_RADIAN);
 
+            if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
+                etat_strategie = CALAGE_PANIER_1;
+            }
+            break;
+
+        case APPROCHE_PANIER_2:
+            Trajectoire_droite(&trajectoire,Localisation_get().x_mm, Localisation_get().y_mm, 
+                                265,2830,
+                                +120. * DEGREE_EN_RADIAN, +120. * DEGREE_EN_RADIAN);
+
             if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
                 etat_strategie = CALAGE_PANIER_1;
             }
@@ -68,7 +79,7 @@ void Homologation(uint32_t step_ms){
             break;
 
         case CALAGE_PANIER_1:
-            if(calage_angle(LONGER_VERS_A, RAYON_ROBOT, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)), 120. *DEGREE_EN_RADIAN) == ACTION_TERMINEE){
+            if(calage_angle(LONGER_VERS_A, RAYON_ROBOT, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2.)), 120. *DEGREE_EN_RADIAN) == ACTION_TERMINEE){
                 etat_strategie = RECULE_PANIER;
             }
             break;
@@ -76,8 +87,8 @@ void Homologation(uint32_t step_ms){
         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);
+                                180, 3000 - (RAYON_ROBOT/(RACINE_DE_3/2)) - 80,
+                                120. * DEGREE_EN_RADIAN, +270. * DEGREE_EN_RADIAN);
 
             if(parcourt_trajet_simple(trajectoire, step_ms) == ACTION_TERMINEE){
                 etat_strategie = LANCE_DANS_PANIER;
@@ -85,6 +96,7 @@ void Homologation(uint32_t step_ms){
             break;
 
         case LANCE_DANS_PANIER:
+            Asser_Position_maintien();
             if(lance_balles(step_ms) == ACTION_TERMINEE){
                 etat_strategie = STRATEGIE_FIN;
             }
@@ -120,7 +132,8 @@ enum etat_action_t lance_balles(uint32_t step_ms){
         
         case LANCE_TEMPO_PROP_ON:
             if (tempo_ms < step_ms){
-                etat_lance_balle = LANCE_PORTE_OUVERTE;
+                //etat_lance_balle = LANCE_PORTE_OUVERTE;
+                etat_lance_balle = LANCE_TEMPO_PROP_ON;
                 i2c_annexe_ouvre_porte();
                 tempo_ms = 6000;
             }else{
@@ -133,6 +146,8 @@ enum etat_action_t lance_balles(uint32_t step_ms){
                 etat_lance_balle = LANCE_PROPULSEUR_ON;
                 i2c_annexe_desactive_propulseur();
                 etat_action = ACTION_TERMINEE;
+            }else{
+                tempo_ms -= step_ms;
             }
             break;
 
diff --git a/Strategie.h b/Strategie.h
index 4666978..c09da19 100644
--- a/Strategie.h
+++ b/Strategie.h
@@ -27,9 +27,10 @@ extern enum etat_strategie_t{
     APPROCHE_CERISE_1_B=2,
     ATTRAPE_CERISE_1=3,
     APPROCHE_PANIER_1=4,
-    CALAGE_PANIER_1=5,
-    RECULE_PANIER=6,
-    LANCE_DANS_PANIER=7,
+    APPROCHE_PANIER_2=5,
+    CALAGE_PANIER_1=6,
+    RECULE_PANIER=7,
+    LANCE_DANS_PANIER=8,
     STRATEGIE_FIN=254,
 }etat_strategie;
 
diff --git a/Strategie_prise_cerises.c b/Strategie_prise_cerises.c
index b70c818..ea5ea69 100644
--- a/Strategie_prise_cerises.c
+++ b/Strategie_prise_cerises.c
@@ -15,7 +15,7 @@
 #define MAX_LONGE_MM 240
 
 // Translation en mm/s pour aspirer les cerises
-#define TRANSLATION_CERISE 45
+#define TRANSLATION_CERISE 70
 
 void commande_rotation_contacteur_longer_A();
 void commande_rotation_contacteur_longer_C();
@@ -58,10 +58,9 @@ 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();
+            i2c_annexe_active_turbine();
             commande_vitesse_stop();
-            tempo_ms = 2000;
+            tempo_ms = 1000;
             etat_attrape = TURBINE_DEMARRAGE_TEMPO;
 
             break;
@@ -90,6 +89,7 @@ enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direct
 
         case ASPIRE_LIBRE:
             commande_vitesse_stop();
+            i2c_annexe_desactive_turbine();
             etat_action = ACTION_TERMINEE;
             break;
     }
diff --git a/Test_strategie.c b/Test_strategie.c
index f6b1d99..48c5f26 100644
--- a/Test_strategie.c
+++ b/Test_strategie.c
@@ -103,11 +103,14 @@ int test_homologation(){
     i2c_maitre_init();
     Trajet_init();
     //printf("Init gyroscope\n");
-    //Gyro_Init();
+    set_position_avec_gyroscope(1);
+    if(get_position_avec_gyroscope()){
+        Gyro_Init();
+    }
 
     stdio_flush();
 
-    //set_position_avec_gyroscope(1);
+    
 
     multicore_launch_core1(affichage_test_strategie);