Corection nommage contacteur + début homologation

CMakeLists.txt

@@ -23,6 +23,7 @@ Localisation.c
 Moteurs.c
 Robot_config.c
 Servomoteur.c
+Strategie.c
 Strategie_prise_cerises.c
 Temps.c
 Test.c

Strategie.c

@@ -1,12 +1,56 @@
 
+#include "Localisation.h"
+#include "Strategie_prise_cerises.h"
+#include "Strategie.h"
+#include "Trajet.h"
+#include "math.h"
 
-void Homologation(void){
+#define DEGREE_EN_RADIAN  (M_PI / 180.)
+
+void Homologation(uint32_t step_ms){
     static enum etat_strategie_t{
-        STRATEGIE_INIT
+        STRATEGIE_INIT,
-    }etat_strategie;
+        APPROCHE_CERISE_1_A,
+        APPROCHE_CERISE_1_B,
+        ATTRAPE_CERISE_1,
+        STRATEGIE_FIN
+    }etat_strategie=STRATEGIE_INIT;
+
+    enum etat_action_t etat_action;
+    enum trajet_etat_t etat_trajet;
+
+    struct trajectoire_t trajectoire;
 
     switch(etat_strategie){
         case STRATEGIE_INIT:
-            
+            Localisation_set(775., 109., -60. * DEGREE_EN_RADIAN);
+            etat_strategie = APPROCHE_CERISE_1_A;
+            break;
+
+        case APPROCHE_CERISE_1_A:
+            Trajet_config(250, 500);
+            Trajectoire_droite(&trajectoire,775, 109, 857, 156, -60. * DEGREE_EN_RADIAN, +30. * DEGREE_EN_RADIAN);
+            Trajet_debut_trajectoire(trajectoire);
+            etat_strategie = APPROCHE_CERISE_1_B;
+            break;
+
+        case APPROCHE_CERISE_1_B:
+            etat_trajet = Trajet_avance(step_ms/1000.);
+            if(etat_trajet == TRAJET_TERMINE){
+                etat_strategie = ATTRAPE_CERISE_1;
+            }
+            break;
+        
+        case ATTRAPE_CERISE_1:
+            etat_action = cerise_attraper_bordure(LONGER_VERS_C, step_ms);
+            if(etat_action == ACTION_TERMINEE){
+                etat_strategie = STRATEGIE_FIN;
+            }
+            break;
+
+        case STRATEGIE_FIN:
+            break;
+        
+
     }
 }

Strategie.h

@@ -1,3 +1,8 @@
+#include "pico/stdlib.h"
+
+#ifndef STRATEGIE_H
+#define STRATEGIE_H
+
 enum etat_action_t{
     ACTION_EN_COURS,
     ACTION_TERMINEE
@@ -10,4 +15,7 @@ enum longer_direction_t{
 
 enum etat_action_t cerise_accostage(void);
 enum etat_action_t cerise_longer_bord(enum longer_direction_t longer_direction);
-void Homologation(void);
+void Homologation(uint32_t step_ms);
+
+// STRATEGIE_H
+#endif

Strategie_prise_cerises.c

@@ -4,25 +4,104 @@
 #include "Commande_vitesse.h"
 #include "Geometrie.h"
 #include "Geometrie_robot.h"
+#include "Localisation.h"
 #include "math.h"
 #include "i2c_annexe.h"
 
 // Rotation en rad/s pour accoster les cerises
 #define ROTATION_CERISE 0.5f
 
+// Distance la plus éloignée du bord où le robot est capable d'aspirer les cerises en longeant la bodure.
+#define MAX_LONGE_MM 240
+
 // Translation en mm/s pour aspirer les cerises
-#define TRANSLATION_CERISE 15
+#define TRANSLATION_CERISE 45
 
 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);
 
 
 double vitesse_accostage_mm_s=100;
 
-enum etat_action_t cerise_longer_bord(enum longer_direction_t longer_direction){
+/// @brief Fonction pour attraper les cerises sur les supports perpendiculaires à la bordure.
+/// Le robot accoste, longe le support cerise vers la bordure, active la turbine, puis longe le support cerise jusqu'à son bout.
+/// @param longer_direction : direction dans laquelle se trouve la bordure
+/// @return ACTION_EN_COURS ou ACTION_TERMINEE
+enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms){
     enum etat_action_t etat_action = ACTION_EN_COURS;
+    enum longer_direction_t longer_direction_aspire;
+    static uint32_t tempo_ms = 0;
+    static enum {
+        ATTRAPE_INIT,
+        ATTRAPE_VERS_BORDURE,
+        TURBINE_DEMARRAGE,
+        TURBINE_DEMARRAGE_TEMPO,
+        ASPIRE_LONGE,
+        ASPIRE_LIBRE
+    } etat_attrape=ATTRAPE_INIT;
+    
+    switch(etat_attrape){
+        case ATTRAPE_INIT:
+            etat_attrape=ATTRAPE_VERS_BORDURE;
+            printf("ATTRAPE_VERS_BORDURE\n");
+            break;
+        
+        case ATTRAPE_VERS_BORDURE:
+            cerise_longer_bord(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;
+                    printf("TURBINE_DEMARRAGE\n");
+            }
+            break;
+        
+        case TURBINE_DEMARRAGE:
+            i2c_annexe_ferme_porte();
+            //i2c_annexe_active_turbine();
+            commande_vitesse_stop();
+            tempo_ms = 2000;
+            etat_attrape = TURBINE_DEMARRAGE_TEMPO;
+            printf("TURBINE_DEMARRAGE_TEMPO\n");
+            break;
+
+        case TURBINE_DEMARRAGE_TEMPO:
+            if(tempo_ms < step_ms){
+                etat_attrape = ASPIRE_LONGE;
+                printf("ASPIRE_LONGE\n");
+            }
+            tempo_ms -= step_ms;
+            break;
+
+        case ASPIRE_LONGE:
+            longer_direction_aspire = inverser_longe_direction(longer_direction);
+            cerise_longer_bord(longer_direction_aspire);
+            // 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
+            if( ((Localisation_get().y_mm > 1500) && (Localisation_get().y_mm < (3000 - MAX_LONGE_MM) )) ||
+                ((Localisation_get().y_mm < 1500) && (Localisation_get().y_mm > (MAX_LONGE_MM))) ){
+                etat_attrape = ASPIRE_LIBRE;
+                printf("ASPIRE_LIBRE\n");
+            }
+            break;
+
+        case ASPIRE_LIBRE:
+            commande_vitesse_stop();
+            etat_action = ACTION_TERMINEE;
+            break;
+    }
+    return etat_action;
+}
+
+
+/// @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){
+
     static enum {
         LONGE_INIT,
         LONGE_VERS_A,
@@ -64,7 +143,7 @@ enum etat_action_t cerise_longer_bord(enum longer_direction_t longer_direction){
                 printf("Longer INIT\n");
             }
     }
-    return etat_action;
+    return ACTION_EN_COURS;
 
 }
 
@@ -143,11 +222,11 @@ enum etat_action_t cerise_accostage(void){
 }
 
 void commande_rotation_contacteur_longer_A(){
-    commande_rotation(-ROTATION_CERISE, RAYON_ROBOT, 0);
+    commande_rotation(ROTATION_CERISE, RAYON_ROBOT, 0);
 }
 
 void commande_rotation_contacteur_longer_C(){
-    commande_rotation(ROTATION_CERISE, RAYON_ROBOT/2.0, -RAYON_ROBOT* RACINE_DE_3/2.0);
+    commande_rotation(-ROTATION_CERISE, RAYON_ROBOT/2.0, -RAYON_ROBOT* RACINE_DE_3/2.0);
 }
 
 void commande_translation_longer_vers_A(){
@@ -161,3 +240,10 @@ void commande_translation_longer_vers_C(){
     // V_y : -V * sin (60°) = -V * RACINE(3) / 2
     commande_vitesse(-TRANSLATION_CERISE/2., -TRANSLATION_CERISE / 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;
+    }
+    return LONGER_VERS_A;
+}

Strategie_prise_cerises.h

@@ -1 +1,2 @@
 #include "Strategie.h"
+enum etat_action_t cerise_attraper_bordure(enum longer_direction_t longer_direction, uint32_t step_ms);

Test.c

@@ -68,7 +68,7 @@ int mode_test(){
     printf("C - pour les codeurs\n");
     printf("D - pour les codeurs (somme en mm)\n");
     printf("E - Commande en vitesse...\n");
-    printf("F - Accostage\n");
+    printf("F - Strategie...\n");
     printf("H - Asser Position - avance\n");
     printf("I - Asser Position - avance et tourne (gyro)\n");
     printf("J - Asser Position - avance et tourne (sans gyro)\n");

Test_strategie.c

@@ -1,6 +1,7 @@
 #include "pico/multicore.h"
 #include "stdio.h"
 #include "hardware/i2c.h"
+#include "math.h"
 
 #include "Asser_Moteurs.h"
 #include "i2c_annexe.h"
@@ -21,14 +22,24 @@ int test_homologation(void);
 
 void affichage_test_strategie(){
     uint32_t temps;
-    temps = time_us_32()/1000;
+    
-    printf(">contacteur_butee_A:%ld:%d\n", temps, i2c_annexe_get_contacteur_butee_A());
+    while(true){
-    printf(">contacteur_butee_C:%ld:%d\n", temps, i2c_annexe_get_contacteur_butee_C());
+        temps = time_us_32()/1000;
-    printf(">contacteur_longer_A:%ld:%d\n", temps, i2c_annexe_get_contacteur_longer_A());
+        printf(">contacteur_butee_A:%ld:%d\n", temps, i2c_annexe_get_contacteur_butee_A());
-    printf(">contacteur_longer_C:%ld:%d\n", temps, i2c_annexe_get_contacteur_longer_C());
+        printf(">contacteur_butee_C:%ld:%d\n", temps, i2c_annexe_get_contacteur_butee_C());
-    printf(">V_consigne_A:%ld:%f\n>V_consigne_B:%ld:%f\n>V_consigne_C:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_A), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_B), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_C));
+        printf(">contacteur_longer_A:%ld:%d\n", temps, i2c_annexe_get_contacteur_longer_A());
+        printf(">contacteur_longer_C:%ld:%d\n", temps, i2c_annexe_get_contacteur_longer_C());
+        printf(">V_consigne_A:%ld:%f\n>V_consigne_B:%ld:%f\n>V_consigne_C:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_A), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_B), temps, AsserMoteur_getConsigne_mm_s(MOTEUR_C));
+        printf(">pos_x:%ld:%f\n", temps, Localisation_get().x_mm);
+        printf(">pos_y:%ld:%f\n", temps, Localisation_get().y_mm);
+        printf(">pos_angle:%ld:%f\n", temps, Localisation_get().angle_radian/M_PI*180.);
 
-    sleep_ms(10);
+        printf(">c_pos_x:%ld:%f\n", temps, Trajet_get_consigne().x_mm);
+        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);
+
+        sleep_ms(100);
+    }
 }
 
 
@@ -68,15 +79,18 @@ int test_strategie(){
 
 int test_homologation(){
     int lettre, _step_ms = 1, temps_ms=0, _step_ms_gyro=2,temps_ms_init;
+    printf("Homologation\n");
 
     i2c_maitre_init();
     Trajet_init();
     //printf("Init gyroscope\n");
-    Gyro_Init();
+    //Gyro_Init();
 
     stdio_flush();
 
-    set_position_avec_gyroscope(1);
+    //set_position_avec_gyroscope(1);
+
+    multicore_launch_core1(affichage_test_strategie);
 
     temps_ms = Temps_get_temps_ms();
     temps_ms_init = temps_ms;
@@ -92,24 +106,24 @@ int test_homologation(){
 
             // Routine à 2 ms
             if(temps_ms % _step_ms_gyro == 0){
-            //    Gyro_Read(_step_ms_gyro);
+                if(get_position_avec_gyroscope()){
-            }
+                    Gyro_Read(_step_ms_gyro);
-
-            if(temps_ms > temps_ms_init + 200){
-                if(cerise_longer_bord(LONGER_VERS_A) == ACTION_TERMINEE){
-                    printf("Accostage_terminee\n");
                 }
             }
 
+            Homologation(_step_ms);
+
         }
         lettre = getchar_timeout_us(0);
-    }while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
+    //}while((lettre == PICO_ERROR_TIMEOUT) || (lettre == 0));
+    }while(1);
+    printf("STRATEGIE_LOOP_2\n");
     printf("Lettre : %d; %c\n", lettre, lettre);
     
     if(lettre == 'q' && lettre == 'Q'){
         return 0;
     }
-    return 1;
+    return 0;
 
 }