Fonctions de démonstration

Demonstration.c

@@ -2,35 +2,90 @@
 #include "Demonstration.h"
 
 #define TEST_TIMEOUT_US 10000000
+#define CAPTEUR_POUR_ATTENTE 11
+
+
+int Demonstration_init(void);
+enum etat_action_t Demonstration_calage();
+enum etat_action_t Demonstration_rectangle(int avance_x_mm, int avance_y_mm);
+enum etat_action_t Demonstration_avance_puis_tourne(int avance_x_mm, int avance_y_mm, float angle_degrees);
+enum etat_action_t Demonstration_bezier();
+enum etat_action_t Demonstration_attente();
 
-int Demonstration_calage();
 
 uint32_t temps_ms_demo = 0, temps_ms_old;
 
+void demo_affiche_localisation(){
+    struct position_t position;
+    while(1){
+        position = Localisation_get();
+        printf(">X:%f\n>Y:%f\n>angle:%f\n", position.x_mm, position.y_mm, position.angle_radian *180. / 3.141592654);
+        printf(">v_bat:%2.2f\n", i2c_annexe_get_tension_batterie() / 10.);
+        printf(">capteur:%d\n", Balise_VL53L1X_get_capteur_cm(CAPTEUR_POUR_ATTENTE));
+    }
+}
+
 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");
+    int rep;
+    printf("Mode demo - init\n");
+    Demonstration_init();
+    while(1){
+        do{
+            printf("A - Calage dans l'angle\n");
+            printf("B - Trajets unitaires - Rectangle\n");
+            printf("C - Trajets unitaires - Droit puis tourne\n");
+            printf("D - Trajets unitaires - Bezier\n");
+            printf("E - Test attente\n");
+            /*printf("C - Trajets enchaines - manuels\n");
+            printf("D - Trajets enchaines - auto\n");
+            printf("E - Asservissement angulaire\n");*/
+            printf("Q - Quitter\n");
+            rep = getchar_timeout_us(TEST_TIMEOUT_US);
+        }while(rep == 0 || rep == PICO_ERROR_TIMEOUT);
 
-    printf("Q - Quitter\n");
+        switch (rep)
+        {
+        case 'a':
+        case 'A':
+            Demonstration_calage();
+            break;
 
-    stdio_flush();
-    int rep = getchar_timeout_us(TEST_TIMEOUT_US);
-    switch (rep)
-    {
-    case 'a':
-    case 'A':
-        while(Demonstration_calage());
-        break;
+        case 'b':
+        case 'B':
+            printf("Demonstration rectangle\n");
+            Demonstration_rectangle(550, 1000);
+            printf("Recalage\n");
+            Demonstration_calage();
+            break;
 
-    case 'b':
-    case 'B':
-        break;
+        case 'c':
+        case 'C':
+            Demonstration_avance_puis_tourne(300, 1000, 720.);
+            break;
 
+        case 'd':
+        case 'D':
+            Demonstration_bezier();
+            break;
+
+        case 'E':
+            printf("Début attente\n");
+            Demonstration_attente();
+            printf("Fin attente\n");
+            break;
+
+        case 'q':
+        case 'Q':
+            return 0;
+            break;
+
+        case 'z':
+        case 'Z':
+            Demonstration_semiauto();
+            break;
+
+        }
     }
     return 1;
 }
@@ -38,39 +93,190 @@ int Demonstration_menu(void){
 int Demonstration_init(){
     
     Holonome2023_init();
-
     temps_ms_demo = Temps_get_temps_ms();
     temps_ms_old = temps_ms_demo;   
+    multicore_launch_core1(demo_affiche_localisation);
+}
+
+void Demonstration_semiauto(){
+    Demonstration_calage();
+    Demonstration_attente();
+
+    while(true){
+        Demonstration_rectangle(550, 1000);
+        Demonstration_calage();
+        Demonstration_attente();
+
+        Demonstration_avance_puis_tourne(300, 1000, 720.);
+        Demonstration_calage();
+        Demonstration_attente();
+
+        Demonstration_bezier();
+        Demonstration_calage();
+        Demonstration_attente();
+    }
+}
+
+enum etat_action_t Demonstration_attente(){
+    enum {
+        ATTENTE_DETECTION,
+        DETECTION_PROCHE,
+        FIN_ATTENTE
+    } etat_attente = ATTENTE_DETECTION;
+    uint32_t temps_debut_tempo, duree_tempo_ms = 50;
+
+    while(true){
+        Holonome_cyclique(PARAM_NO_MOTORS);
+        
+        switch(etat_attente){
+            case ATTENTE_DETECTION:
+                if(Balise_VL53L1X_get_capteur_cm(CAPTEUR_POUR_ATTENTE) < 15 && Balise_VL53L1X_get_capteur_cm(CAPTEUR_POUR_ATTENTE) > 1){
+                    /// Sans obstacle, le capteur peut renvoyer 0;
+                    etat_attente=DETECTION_PROCHE;
+                    temps_debut_tempo = time_us_32();
+                }
+                break;
+            
+            case DETECTION_PROCHE:
+                if(Balise_VL53L1X_get_capteur_cm(CAPTEUR_POUR_ATTENTE) > 15 || Balise_VL53L1X_get_capteur_cm(CAPTEUR_POUR_ATTENTE) < 1){
+                    // On a perdu la detection avant le temps écoulé
+                    etat_attente=ATTENTE_DETECTION;
+                }
+                if((temps_debut_tempo + (duree_tempo_ms * 1000)) < time_us_32()){
+                    // temps écoulé
+                    etat_attente=FIN_ATTENTE;
+                }
+                break;
+
+            case FIN_ATTENTE:
+                if(Balise_VL53L1X_get_capteur_cm(CAPTEUR_POUR_ATTENTE) > 15 || Balise_VL53L1X_get_capteur_cm(CAPTEUR_POUR_ATTENTE) < 1){
+                    // On a perdu la detection après le temps écoulé
+                    etat_attente=ATTENTE_DETECTION;
+                    return ACTION_TERMINEE;
+                }
+                break;
+                
+        }
+        //sleep_ms(20);
+    }
+    
+
 }
 
 
-int Demonstration_calage(){
-    Demonstration_init();
+enum etat_action_t Demonstration_calage(){
     enum {
         CALAGE,
+        DECALAGE,
         CALAGE_TERMINE
     } etat_calage = CALAGE;
     while(true){
-        Holonome_cyclique();
+        Holonome_cyclique(PARAM_DEFAULT);
+        struct trajectoire_t trajectoire;
 
         // Toutes les 1 ms.
         if(temps_ms_demo != Temps_get_temps_ms()){
-            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;
-
+                    etat_calage = DECALAGE;
                 }
                 break;
+
+            case DECALAGE:
+                Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 
+                    RAYON_ROBOT + 150, PETIT_RAYON_ROBOT_MM + 150,
+                    Localisation_get().angle_radian, Localisation_get().angle_radian);
+                if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){
+                    etat_calage = CALAGE_TERMINE;
+                }
+                break;
+
             
             case CALAGE_TERMINE:
                 etat_calage = CALAGE;
                 Moteur_Stop();
-                return 0;
+                return ACTION_TERMINEE;
+                
+            
+            default:
+                break;
+            }
+        }
+        
+    }
+    return ACTION_ECHEC;
+}
+
+
+/// @brief Deplacement suivant un rectangle.
+/// @param avance_x_mm : Distance en X (conseillé 620)
+/// @param avance_y_mm : Distance en Y (conseillé 1000)
+/// @return ACTION_TERMINEE
+enum etat_action_t Demonstration_rectangle(int avance_x_mm, int avance_y_mm){
+    enum {
+        AVANCE_Y,
+        AVANCE_X,
+        RECULE_Y,
+        RECULE_X,
+        RECTANGLE_TERMINE
+    } etat_rectangle = AVANCE_Y;
+    while(true){
+        struct trajectoire_t trajectoire;
+
+        Holonome_cyclique(PARAM_DEFAULT);
+
+        // Toutes les 1 ms.
+        if(temps_ms_demo != Temps_get_temps_ms()){
+            temps_ms_demo = Temps_get_temps_ms();
+
+        
+            switch (etat_rectangle)
+            {
+            case AVANCE_Y:
+                Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 
+                    Localisation_get().x_mm, Localisation_get().y_mm + avance_y_mm,
+                    Localisation_get().angle_radian, Localisation_get().angle_radian);
+                if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){
+                    etat_rectangle = AVANCE_X;
+                }
+                break;
+            
+            case AVANCE_X:
+                Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 
+                    Localisation_get().x_mm + avance_x_mm, Localisation_get().y_mm,
+                    Localisation_get().angle_radian, Localisation_get().angle_radian);
+                if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){
+                    etat_rectangle = RECULE_Y;
+                }
+                break;
+
+            case RECULE_Y:
+                Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 
+                    Localisation_get().x_mm, Localisation_get().y_mm - avance_y_mm,
+                    Localisation_get().angle_radian, Localisation_get().angle_radian);
+                if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){
+                    etat_rectangle = RECULE_X;
+                }
+                break;
+            
+            case RECULE_X:
+                Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 
+                    Localisation_get().x_mm - avance_x_mm, Localisation_get().y_mm,
+                    Localisation_get().angle_radian, Localisation_get().angle_radian);
+                if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){
+                    etat_rectangle = RECTANGLE_TERMINE;
+                }
+                break;
+            
+            case RECTANGLE_TERMINE:
+                etat_rectangle = AVANCE_Y;
+                Moteur_Stop();
+                return ACTION_TERMINEE;
             
             default:
                 break;
@@ -80,3 +286,132 @@ int Demonstration_calage(){
     }
     return 0;
 }
+
+/// @brief Deplacement suivant une droite, rotation sur lui-même du robot une fois arrivée à destination,
+/// puis retour en ligne droite
+/// @param avance_x_mm : Distance en X (conseillé 300)
+/// @param avance_y_mm : Distance en Y (conseillé 1000)
+/// @param angle_degrees : Rotation du robot sur lui-même
+/// @return ACTION_TERMINEE
+enum etat_action_t Demonstration_avance_puis_tourne(int avance_x_mm, int avance_y_mm, float angle_degrees){
+    enum {
+        APT_AVANCE,
+        APT_TOURNE,
+        APT_RECULE,
+        APT_TERMINE
+    } etat_avance_puis_tourne = APT_AVANCE;
+    int pos_x_init_mm, pos_y_init_mm;
+    while(true){
+        struct trajectoire_t trajectoire;
+
+        Holonome_cyclique(PARAM_DEFAULT);
+
+        // Toutes les 1 ms.
+        if(temps_ms_demo != Temps_get_temps_ms()){
+            temps_ms_demo = Temps_get_temps_ms();
+
+        
+            switch (etat_avance_puis_tourne)
+            {
+            case APT_AVANCE:
+                pos_x_init_mm = Localisation_get().x_mm;
+                pos_y_init_mm = Localisation_get().y_mm;
+                Trajectoire_droite(&trajectoire, pos_x_init_mm, pos_y_init_mm, 
+                    pos_x_init_mm + avance_x_mm, pos_y_init_mm + avance_y_mm,
+                    Localisation_get().angle_radian, Localisation_get().angle_radian);
+                if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){
+                    etat_avance_puis_tourne = APT_TOURNE;
+                }
+                break;
+            
+            case APT_TOURNE:
+                Trajectoire_rotation(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 
+                    Localisation_get().angle_radian, Localisation_get().angle_radian + (angle_degrees * DEGRE_EN_RADIAN) );
+                Trajet_config(TRAJECT_CONFIG_ROTATION_PURE);
+                if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){
+                    etat_avance_puis_tourne = APT_RECULE;
+                    Trajet_config(TRAJECT_CONFIG_STD);
+                }
+                break;
+
+            case APT_RECULE:
+                Trajectoire_droite(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 
+                    Localisation_get().x_mm - avance_x_mm, Localisation_get().y_mm - avance_y_mm,
+                    Localisation_get().angle_radian, Localisation_get().angle_radian);
+                if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){
+                    etat_avance_puis_tourne = APT_TERMINE;
+                }
+                break;
+            
+            case APT_TERMINE:
+                etat_avance_puis_tourne = APT_AVANCE;
+                Moteur_Stop();
+                return ACTION_TERMINEE;
+            
+            default:
+                break;
+            }
+        }
+        
+    }
+    return ACTION_ECHEC;
+}
+
+/// @brief Déplacement suivant deux courbes de Bézier. Recommandé pour une démo sur une planche de 1m x 1,5m
+/// @return ACTION_TERMINEE
+enum etat_action_t Demonstration_bezier(){
+    enum {
+        BEZIER_ALLER,
+        BEZIER_RETOUR,
+        BEZIER_TERMINE
+    } etat_bezier = BEZIER_ALLER;
+    while(true){
+        struct trajectoire_t trajectoire;
+        static int pos_x_init_mm, pos_y_init_mm;
+
+        Holonome_cyclique(PARAM_DEFAULT);
+        Trajet_config(200, 200);
+
+        // Toutes les 1 ms.
+        if(temps_ms_demo != Temps_get_temps_ms()){
+            temps_ms_demo = Temps_get_temps_ms();
+
+        
+            switch (etat_bezier)
+            {
+            case BEZIER_ALLER:
+                Trajectoire_bezier(&trajectoire, Localisation_get().x_mm, Localisation_get().y_mm, 
+                    1386, Localisation_get().y_mm,
+                    -576, 1500 - 276,
+                    545,  1500 - 276,
+                    Localisation_get().angle_radian, Localisation_get().angle_radian);
+                if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){
+                    etat_bezier = BEZIER_RETOUR;
+                }
+                break;
+            
+            case BEZIER_RETOUR:
+                Trajectoire_bezier(&trajectoire, 
+                    Localisation_get().x_mm, Localisation_get().y_mm, 
+                    1391, Localisation_get().y_mm,
+                    -76, 1500 - 788,
+                    242,  1500 - 1289,
+                    Localisation_get().angle_radian, Localisation_get().angle_radian);
+                if(Strategie_parcourir_trajet(trajectoire, 1, SANS_EVITEMENT) == ACTION_TERMINEE){
+                    etat_bezier = BEZIER_TERMINE;
+                }
+                break;
+            
+            case BEZIER_TERMINE:
+                etat_bezier = BEZIER_ALLER;
+                Trajet_config(TRAJECT_CONFIG_STD);
+                return ACTION_TERMINEE;
+            
+            default:
+                break;
+            }
+        }
+        
+    }
+    return ACTION_ECHEC;
+}

Demonstration.h

@@ -1 +1,3 @@
- int Demonstration_menu(void);
+ int Demonstration_menu(void);
+ void Demonstration_semiauto(void);
+ int Demonstration_init(void);

Holonome2023.c

@@ -1,4 +1,5 @@
 #include "Holonome2023.h"
+#include "Demonstration.h"
 
 const uint LED_PIN = 25;
 #define LED_PIN_ROUGE 28
@@ -38,7 +39,8 @@ int main() {
     gpio_set_dir(LED_PIN_NE_PAS_UTILISER, GPIO_IN);
 
     stdio_init_all();
-
+    
+    Demonstration_init();Demonstration_semiauto();
     while(mode_test());
     Holonome2023_init();
 
@@ -171,7 +173,8 @@ void Holonome2023_init(){
 }
 
 /// @brief Fonction à appeler le plus souvent possible. Lit les codeurs, le gyroscope, et asservit les moteurs
-void Holonome_cyclique(void){
+/// @param param : Utiliser PARAM_DEFAULT, sinon, utiliser un "ou" avec les valeurs suivantes : PARAM_NO_MOTORS
+void Holonome_cyclique(int param){
     static uint32_t temps_ms = 0;
     // Surveillance du temps d'execution
     temps_cycle_check();
@@ -184,7 +187,10 @@ void Holonome_cyclique(void){
         temps_ms = Temps_get_temps_ms();
         QEI_update();
         Localisation_gestion();
-        AsserMoteur_Gestion(STEP_MS);
+        if(!(param & PARAM_NO_MOTORS)){
+            AsserMoteur_Gestion(STEP_MS);
+        }
+        
         Evitement_gestion(STEP_MS);
 
         // Routine à 2 ms

Holonome2023.h

@@ -34,5 +34,8 @@
 #define STEP_MS_GYRO 2 /*ms*/
 #define STEP_MS 1 /*ms*/
 
+#define PARAM_DEFAULT 0
+#define PARAM_NO_MOTORS 1
+
 void Holonome2023_init(void);
-void Holonome_cyclique(void);
+void Holonome_cyclique(int);

Trajet.c

@@ -27,8 +27,8 @@ float vitesse_max_contrainte_obstacle;
 void Trajet_init(){
     abscisse = 0;
     vitesse_mm_s = 0;
-    acceleration_mm_ss = 500;
     position_mm = 0;
+    Trajet_config(TRAJECT_CONFIG_STD);
 }
 
 /// @brief Configure la vitesse maximale et l'acceleration pour les futurs trajets
@@ -49,6 +49,7 @@ void Trajet_debut_trajectoire(struct trajectoire_t trajectoire){
 
 /// @brief Avance la consigne de position sur la trajectoire
 /// @param pas_de_temps_s : temps écoulé depuis le dernier appel en seconde 
+/// @return TRAJET_EN_COURS ou TRAJET_TERMINE
 enum etat_trajet_t Trajet_avance(float pas_de_temps_s){
     float distance_mm;
     enum etat_trajet_t trajet_etat = TRAJET_EN_COURS;