Premier "scénario" de démonstration fonctionnel, encapsulation des fonctions

CMakeLists.txt

@@ -14,6 +14,7 @@ Asser_Moteurs.c
 Asser_Position.c
 Balise_VL53L1X.c
 Commande_vitesse.c
+Demonstration.c
 Evitement.c
 QEI.c
 Geometrie.c

Demonstration.c

@@ -0,0 +1,82 @@
+#include "Holonome2023.h"
+#include "Demonstration.h"
+
+#define TEST_TIMEOUT_US 10000000
+
+int Demonstration_calage();
+
+uint32_t temps_ms_demo = 0, temps_ms_old;
+
+int Demonstration_menu(void){
+    static int iteration = 2;
+    printf("Mode demo\n");
+    printf("A - Calage dans l'angle\n");
+    printf("B - Trajets unitaires\n");
+    printf("C - Trajets enchaines - manuels\n");
+    printf("D - Trajets enchaines - auto\n");
+    printf("E - Asservissement angulaire\n");
+
+    printf("Q - Quitter\n");
+
+    stdio_flush();
+    int rep = getchar_timeout_us(TEST_TIMEOUT_US);
+    switch (rep)
+    {
+    case 'a':
+    case 'A':
+        while(Demonstration_calage());
+        break;
+
+    case 'b':
+    case 'B':
+        break;
+
+    }
+    return 1;
+}
+
+int Demonstration_init(){
+    
+    Holonome2023_init();
+
+    temps_ms_demo = Temps_get_temps_ms();
+    temps_ms_old = temps_ms_demo;   
+}
+
+
+int Demonstration_calage(){
+    Demonstration_init();
+    enum {
+        CALAGE,
+        CALAGE_TERMINE
+    } etat_calage = CALAGE;
+    while(true){
+        Holonome_cyclique();
+
+        // Toutes les 1 ms.
+        if(temps_ms_demo != Temps_get_temps_ms()){
+            temps_ms_demo != Temps_get_temps_ms();
+
+        
+            switch (etat_calage)
+            {
+            case CALAGE:
+                if(calage_angle(LONGER_VERS_C, RAYON_ROBOT, PETIT_RAYON_ROBOT_MM, -60 * DEGRE_EN_RADIAN) == ACTION_TERMINEE){
+                    etat_calage = CALAGE_TERMINE;
+
+                }
+                break;
+            
+            case CALAGE_TERMINE:
+                etat_calage = CALAGE;
+                Moteur_Stop();
+                return 0;
+            
+            default:
+                break;
+            }
+        }
+        
+    }
+    return 0;
+}

Demonstration.h

@@ -0,0 +1 @@
+ int Demonstration_menu(void);

Holonome2023.c

@@ -1,33 +1,4 @@
-#include <stdio.h>
+#include "Holonome2023.h"
-#include "pico/multicore.h"
-#include "pico/stdlib.h"
-#include "hardware/gpio.h"
-#include "hardware/i2c.h"
-#include "hardware/timer.h"
-#include "pico/binary_info.h"
-#include "math.h"
-#include "Test.h"
-
-#include "gyro.h"
-#include "Asser_Moteurs.h"
-#include "Asser_Position.h"
-#include "Balise_VL53L1X.h"
-#include "Commande_vitesse.h"
-#include "Evitement.h"
-#include "i2c_annexe.h"
-#include "i2c_maitre.h"
-#include "Localisation.h"
-#include "Monitoring.h"
-#include "Moteurs.h"
-#include "QEI.h"
-#include "Robot_config.h"
-#include "Score.h"
-#include "Servomoteur.h"
-#include "spi_nb.h"
-#include "Strategie.h"
-#include "Temps.h"
-#include "Trajectoire.h"
-#include "Trajet.h"
 
 const uint LED_PIN = 25;
 #define LED_PIN_ROUGE 28
@@ -39,6 +10,7 @@ uint temps_cycle;
 #define V_INIT -999.0
 #define TEST_TIMEOUT_US 10000000
 
+
 int mode_test();
 
 void init_led(uint Numero_de_la_led, uint etat){
@@ -58,50 +30,20 @@ int main() {
     uint32_t score=0;
     uint32_t match_en_cours=1;
 
-    stdio_init_all();
+    set_position_avec_gyroscope(1);
-
-    init_led(LED_PIN, 1);
-    init_led(LED_PIN_ROUGE, 0);
-
-    gpio_init(COULEUR);
-    gpio_init(TIRETTE);
-    gpio_set_dir(COULEUR, GPIO_IN);
-    gpio_set_dir(TIRETTE, GPIO_IN);
-
 
     // Il faut neutraliser cette broche qui pourrait interférer avec 
     // la lecture des codeurs. (problème sur la carte électrique)...
     gpio_init(LED_PIN_NE_PAS_UTILISER);
     gpio_set_dir(LED_PIN_NE_PAS_UTILISER, GPIO_IN);
 
-    sleep_ms(3000);
+    stdio_init_all();
-    Servomoteur_Init();
-    //puts("Debut");
-    //spi_test();
-
-    //while(1);
-    Temps_init();
-    Moteur_Init();
-    QEI_init();
-    AsserMoteur_Init();
-    Localisation_init();
 
     while(mode_test());
-    i2c_maitre_init();
+    Holonome2023_init();
-    Trajet_init();
-    Balise_VL53L1X_init();
 
     multicore_launch_core1(Monitoring_display);
 
-    set_position_avec_gyroscope(1);
-    if(get_position_avec_gyroscope()){
-        Gyro_Init();
-    }else{
-        sleep_ms(5000);
-    }
-
-    
-
     temps_ms = Temps_get_temps_ms();
     temps_ms_old = temps_ms;
     while (1) { 
@@ -191,3 +133,65 @@ int main() {
         }
     }
 }
+
+void Holonome2023_init(){
+    init_led(LED_PIN, 1);
+    init_led(LED_PIN_ROUGE, 0);
+
+    gpio_init(COULEUR);
+    gpio_init(TIRETTE);
+    gpio_set_dir(COULEUR, GPIO_IN);
+    gpio_set_dir(TIRETTE, GPIO_IN);
+
+    Log_init();
+    
+    sleep_ms(3000);
+    Servomoteur_Init();
+
+    Temps_init();
+    Moteur_Init();
+    QEI_init();
+    AsserMoteur_Init();
+    Localisation_init();
+    i2c_maitre_init();
+    Trajet_init();
+    Balise_VL53L1X_init();
+
+    if(get_position_avec_gyroscope()){
+        Gyro_Init();
+    }else{
+        sleep_ms(5000);
+    }
+
+    for(int i=0; i<10; i++){
+        i2c_gestion(i2c0);
+        i2c_annexe_gestion();
+        sleep_ms(1);
+    }
+}
+
+/// @brief Fonction à appeler le plus souvent possible. Lit les codeurs, le gyroscope, et asservit les moteurs
+void Holonome_cyclique(void){
+    static uint32_t temps_ms = 0;
+    // Surveillance du temps d'execution
+    temps_cycle_check();
+
+    i2c_gestion(i2c0);
+    i2c_annexe_gestion();
+    Balise_VL53L1X_gestion();
+
+    if(temps_ms != Temps_get_temps_ms()){
+        temps_ms = Temps_get_temps_ms();
+        QEI_update();
+        Localisation_gestion();
+        AsserMoteur_Gestion(STEP_MS);
+        Evitement_gestion(STEP_MS);
+
+        // Routine à 2 ms
+        if(temps_ms % STEP_MS_GYRO == 0){
+            if(get_position_avec_gyroscope()){
+                Gyro_Read(STEP_MS_GYRO);
+            }
+        }
+    }
+}

Holonome2023.h

@@ -0,0 +1,38 @@
+#include <stdio.h>
+#include "pico/multicore.h"
+#include "pico/stdlib.h"
+#include "hardware/gpio.h"
+#include "hardware/i2c.h"
+#include "hardware/timer.h"
+#include "pico/binary_info.h"
+#include "math.h"
+#include "Test.h"
+
+#include "gyro.h"
+#include "Asser_Moteurs.h"
+#include "Asser_Position.h"
+#include "Balise_VL53L1X.h"
+#include "Commande_vitesse.h"
+#include "Evitement.h"
+#include "Geometrie_robot.h"
+#include "i2c_annexe.h"
+#include "i2c_maitre.h"
+#include "Localisation.h"
+#include "Log.h"
+#include "Monitoring.h"
+#include "Moteurs.h"
+#include "QEI.h"
+#include "Robot_config.h"
+#include "Score.h"
+#include "Servomoteur.h"
+#include "spi_nb.h"
+#include "Strategie.h"
+#include "Temps.h"
+#include "Trajectoire.h"
+#include "Trajet.h"
+
+#define STEP_MS_GYRO 2 /*ms*/
+#define STEP_MS 1 /*ms*/
+
+void Holonome2023_init(void);
+void Holonome_cyclique(void);

Strategie.c

@@ -40,7 +40,7 @@ float distance_obstacle;
 
 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);
+
 
 
 int Robot_est_dans_quart_haut_gauche();

Strategie.h

@@ -46,6 +46,8 @@ enum etat_action_t avance_puis_longe_bordure(enum longer_direction_t longer_dire
 enum etat_action_t parcourt_trajet_simple(struct trajectoire_t trajectoire, uint32_t step_ms);
 enum etat_action_t parcourt_trajet_simple_sans_evitement(struct trajectoire_t trajectoire, uint32_t step_ms);
 enum etat_action_t depose_cerises(uint32_t step_ms);
+enum etat_action_t calage_angle(enum longer_direction_t longer_direction, float x_mm, float y_mm, float angle_radian);
+
 void Homologation(uint32_t step_ms);
 enum couleur_t lire_couleur(void);
 uint attente_tirette(void);

Test.c

@@ -14,6 +14,7 @@
 #include "Asser_Position.h"
 #include "Balise_VL53L1X.h"
 #include "Commande_vitesse.h"
+#include "Demonstration.h"
 #include "Geometrie_robot.h"
 #include "i2c_annexe.h"
 #include "i2c_maitre.h"
@@ -93,6 +94,7 @@ int mode_test(){
     printf("U - Tests i2c...\n");
     printf("V - APDS_9960\n");
     printf("W - Endurance aller retour\n");
+    printf("Z - Codes de démonstration\n");
     stdio_flush();
     int rep = getchar_timeout_us(TEST_TIMEOUT_US);
     stdio_flush();
@@ -212,6 +214,11 @@ int mode_test(){
         while(test_endurance_aller_retour());
         break;
     
+    case 'Z':
+    case 'z':
+        while(Demonstration_menu());
+        break;
+
     case PICO_ERROR_TIMEOUT:
         iteration--;        
         if(iteration == 0){