From 3d64e7ae94c2d27a472dbb8d4ddd8ffe7d46994a Mon Sep 17 00:00:00 2001 From: Samuel Date: Sun, 5 Nov 2023 13:02:02 +0100 Subject: [PATCH] =?UTF-8?q?Fonctions=20de=20d=C3=A9monstration?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Demonstration.c | 389 ++++++++++++++++++++++++++++++++++++++++++++---- Demonstration.h | 4 +- Holonome2023.c | 12 +- Holonome2023.h | 5 +- Trajet.c | 3 +- 5 files changed, 380 insertions(+), 33 deletions(-) diff --git a/Demonstration.c b/Demonstration.c index 286d88e..081dec1 100644 --- a/Demonstration.c +++ b/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; +} diff --git a/Demonstration.h b/Demonstration.h index 19bce18..a9928e7 100644 --- a/Demonstration.h +++ b/Demonstration.h @@ -1 +1,3 @@ - int Demonstration_menu(void); \ No newline at end of file + int Demonstration_menu(void); + void Demonstration_semiauto(void); + int Demonstration_init(void); \ No newline at end of file diff --git a/Holonome2023.c b/Holonome2023.c index f7f942e..5e793a4 100644 --- a/Holonome2023.c +++ b/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 diff --git a/Holonome2023.h b/Holonome2023.h index 00bd782..af4b8b0 100644 --- a/Holonome2023.h +++ b/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); \ No newline at end of file +void Holonome_cyclique(int); \ No newline at end of file diff --git a/Trajet.c b/Trajet.c index a719628..bbaa24b 100644 --- a/Trajet.c +++ b/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;