From c46a7e50f990a0fa7b11d680108ee5d8e4356f40 Mon Sep 17 00:00:00 2001 From: Samuel Date: Sun, 11 Dec 2022 15:39:30 +0100 Subject: [PATCH] Ajout du fichier README --- QEI.c | 8 ++-- Readme.md | 66 +++++++++++++++++++++++++++++ Trajet.c | 2 +- gyro.c | 3 +- gyro_ADXRS453.c | 107 +----------------------------------------------- test.c | 2 +- 6 files changed, 76 insertions(+), 112 deletions(-) create mode 100644 Readme.md diff --git a/QEI.c b/QEI.c index ed7867a..5869733 100644 --- a/QEI.c +++ b/QEI.c @@ -48,12 +48,12 @@ void QEI_init(){ gpio_set_dir(CODEUR_1_B, GPIO_IN); // Initialisation des "machines à états" : - // QEI1 : broche 31 et 32 - pio : pio0, sm : 0, Offset : 0, broches (GPIO) 26 et 27, clock div : 0 pour commencer - // QEI1 : !!! Attention, il faudra modifier la carte élec !!! + // QEI1 : broche 31 et 32 - pio : pio0, sm : 0, Offset : 0, GPIO 26 et 27, clock div : 0 pour commencer + // QEI1 : !!! Attention, il faudra modifier la carte élec !!! => Fait. quadrature_encoder_program_init(pio_QEI, 0, offset, 26, 0); - // QEI2 : broche 26 et 27 - pio : pio0, sm : 1, Offset : 0, broches (GPIO) 20 et 21, clock div : 0 pour commencer + // QEI2 : broche 26 et 27 - pio : pio0, sm : 1, Offset : 0, GPIO 20 et 21, clock div : 0 pour commencer quadrature_encoder_program_init(pio_QEI, 1, offset, 20, 0); - // QEI3 : broche 24 et 25 - pio : pio0, sm : 1, Offset : 0, broches (GPIO) 18 et 19, clock div : 0 pour commencer + // QEI3 : broche 24 et 25 - pio : pio0, sm : 1, Offset : 0, GPIO 18 et 19, clock div : 0 pour commencer quadrature_encoder_program_init(pio_QEI, 2, offset, 18, 0); QEI_A.value=0; diff --git a/Readme.md b/Readme.md new file mode 100644 index 0000000..5222099 --- /dev/null +++ b/Readme.md @@ -0,0 +1,66 @@ +Code de déplacement du robot holonome 2023 +========================================== +Ce dépôt contient de code de déplacement du robot holonome de l'équipe [Poivron Robotique](http://poivron-robotique.fr). Ce robot a pour ambition de participer à la coupe de France de Robotique en 2023. + +Le but est de présenter un code assurant toutes les fonctions de déplacement du robot, allant de la commande PWM des moteurs jusqu'au suivi d'une trajectoire (droite, portion de cercle, courbe de Bézier) en passant par le contrôle de l'accélération et de décélération. + +Ce code est conçu pour s'exécuter sur un Raspberry Pi Pico. + +![Architecture du programme](doc/ProgrammeHolonome2023.png) + +Voici un bref paragraphe explicatif pour chaque bloc fonctionnel. + +Moteurs +------- +Initialise les modules PWM et propose les fonctions pour les piloter. + +QEI +--- +Décode les signaux renvoyés par les codeurs et les convertit en distance parcoure par les roues en millimètres. + +L'interprétation des signaux se fait grâce aux PIOs et à l'[exemple fourni dans le SDK](https://github.com/raspberrypi/pico-examples/tree/master/pio/quadrature_encoder). + +SPI +--- +Tentative de créer des fonctions non bloquantes pour le SPI. Mais vu la vitesse du SPI, l'intérêt est limité. Une partie du code n'a pas été éprouvé. + +Gyro ADXRS453 +---- +Initialise le Gyroscope ADXRS453. Décode les données fournies par le gyroscope et les converti en radian/seconde. Les biais éventuels ne sont pas compensés. + +Gyro +---- +Compensation du biais du gyroscope et intégration de la vitesse pour obtenir l'orientation. + +Localisation +------------ + +Utilisation des équations définies dans [l'étude](http://poivron-robotique.fr/Robot-holonome-localisation-partie-2.html) pour obtenir la position du robot à partir du déplacement mesuré de ses roues. + +Devrait à terme aussi utiliser les valeurs du gyroscope pour gagner en précision. + +Asservissement des moteurs +-------------------------- +À partir d'une consigne de vitesse pour chaque moteur, utilisation d'un correcteur proportionnel et intégrale (PI) pour déterminer la commande des moteurs. + +Commande de vitesse +------------------- +À partir de la rotation désirée et des vitesses X et Y dans le référentiel du robot, détermine la consigne de vitesse pour chaque moteur grâce aux [lois de commande](http://poivron-robotique.fr/Robot-holonome-lois-de-commande.html). + + +Asservissement en position +-------------------------- +À partir d'une consigne de position, détermine la rotation désirée et les vitesses X et Y dans le référentiel du robot. Ceci en deux étapes : +1. Asservissement avec un simple gain proportionnel +2. Projection des vitesses X et Y dans le référentiel du robot. + +Gestion des trajets +------------------- +Cette fonction permet de parcourir une trajectoire en tenant compte des contraintes de vitesse. +1. Calcul de la vitesse + - Prise en compte de l'accélération maximale + - Prise en compte de la décélération pour s'arrêter. Voir [cet article](http://poivron-robotique.fr/Consigne-de-vitesse.html). +2. Calcul de l'avancement sur la trajectoire. +3. Obtention de la nouvelle consigne de position. + +Pour se déplacer sur une trajectoire, cette fonction utilise les outils de gestion des trajectoires définis dans les fichiers `trajectoire*`. \ No newline at end of file diff --git a/Trajet.c b/Trajet.c index e1a62c8..569f96a 100644 --- a/Trajet.c +++ b/Trajet.c @@ -91,4 +91,4 @@ double Trajet_calcul_vitesse(double pas_de_temps_s){ vitesse = VITESSE_MAX_MM_S; } return vitesse; -} \ No newline at end of file +} diff --git a/gyro.c b/gyro.c index 5af85d0..4e4a68d 100644 --- a/gyro.c +++ b/gyro.c @@ -57,8 +57,9 @@ void Gyro_Init(void){ vitesse_calibration = NULL; vitesse_angulaire = &_vitesse_angulaire; - //spi_init(spi0, 100 * 1000); // SPI init @ 100 kHz + //uint speed = spi_init(spi0, 10 * 1000); // SPI init @ 10 kHz uint speed = spi_init(spi0, 2 * 1000 * 1000); // SPI init @ 2 MHz + printf("vitesse SPI : %d\n", speed); diff --git a/gyro_ADXRS453.c b/gyro_ADXRS453.c index 3655951..5783f02 100644 --- a/gyro_ADXRS453.c +++ b/gyro_ADXRS453.c @@ -162,7 +162,7 @@ void gyro_get_vitesse_brute(struct t_angle_gyro* angle_gyro, struct t_angle_gyro return; } - rot_z = Gyro_SensorData.rateData; + rot_z = -Gyro_SensorData.rateData; if(angle_gyro_moy == NULL){ angle_gyro->rot_x = 0; @@ -241,107 +241,4 @@ void Gyro_traitementDonnees(uint8_t * tamponRecu){ Gyro_SensorData.P1 = (tamponRecu[3] & 0x01); } -/* -unsigned char Gyro_gestion(void){ - Gyro_commande_SensorData(0); - while(!SPI_recData(GyroscopeReception)); - - Gyro_traitementDonnees(GyroscopeReception); - if (Gyro_SensorData.SQ & 0x04){ - Gyro_Angle +=(long) (Gyro_SensorData.rateData - angle0); - //Gyro_Angle = angle0; - }else{ - Gyro_Angle = (long)0x3333; - } - return 0; -} -inline unsigned char Gyro_gestion_nb(){ - if(SPI_recData(GyroscopeReception)){ - Gyro_traitementDonnees(GyroscopeReception); - if (Gyro_SensorData.SQ & 0x04) - // calcul du nouvel angle - // TODO : Améliorer la stabilitée en augmentant la précision - Gyro_Angle +=(long) ((long)Gyro_SensorData.rateData - (long)angle0); - return 0; - } - return 1; - -} - -int Gyro_getAngle(void){ - // 80° par secondes - // 5 kHz => 200 µs - // Gyro_Angle en 1,6 e-2 degré - return (int)(-Gyro_Angle / 5000 / 80); -} -long Gyro_getRawAngle(void){ - // 80° par secondes - // 5 kHz => 200 µs - // Gyro_Angle en 1,6 e-2 degré - return Gyro_Angle ; -} -long double Gyro_getAngleRadian(void){ - // 80° par secondes - // 5 kHz => 200 µs - // Gyro_Angle en 1,6 e-2 degré - return -Gyro_Angle * GYRO_COEF_RADIAN_5kHz; -} -unsigned char * Gyro_getRawData(){ - return GyroscopeReception; -} - -int Gyro_init(){ - - long long calcul_angle0; - - int i, erreur_gyro; - - Gyro_Timer_ms=100; - while(Gyro_Timer_ms); - // Envoie message auto-test des test - while(!Gyro_commande_SensorData(1)); - while(!SPI_recData(GyroscopeReception)); - - // Attente 50 ms - les tests doivent indiquer des erreur - Gyro_Timer_ms=50; - while(Gyro_Timer_ms); - while(!Gyro_commande_SensorData(0)); - while(!SPI_recData(GyroscopeReception)); - - // Attente 50 ms - les erreurs doivent s'être effacées - Gyro_Timer_ms=50; - while(Gyro_Timer_ms); - while(!Gyro_commande_SensorData(0)); - while(!SPI_recData(GyroscopeReception)); - - // Calibration du gyroscope - calcul_angle0 = 0; - i=0; - erreur_gyro = 0; - while((i