Ajout du fichier README

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 : 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 !!!
+    // 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;

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*`.

Trajet.c

@@ -91,4 +91,4 @@ double Trajet_calcul_vitesse(double pas_de_temps_s){
         vitesse = VITESSE_MAX_MM_S;
     }
     return vitesse;
-}
+}

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);
 
 

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);
 
 }
-/*
+#endif
-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<NB_ACQ_CALIBRATION) && (erreur_gyro < NB_MAX_ERREUR_GYRO)){
-        while(!Gyro_commande_SensorData(0));
-        while(!SPI_recData(GyroscopeReception));
-        Gyro_traitementDonnees(GyroscopeReception);
-        if (Gyro_SensorData.SQ & 0x04){
-            calcul_angle0 += Gyro_SensorData.rateData;
-            erreur_gyro = 0;
-            i++;
-        }else{
-            erreur_gyro++;
-        }
-        __delay32(2000); // 50 µs
-    }
-    if (erreur_gyro < NB_MAX_ERREUR_GYRO){
-        erreur_gyro = 0;
-        // TODO : Améliorer la stabilitée en augmentant la précision
-        angle0 = (long)(calcul_angle0 / NB_ACQ_CALIBRATION);
-        Gyro_Pret=1;
-    }else{
-        erreur_gyro = 1;
-    }
-    
-
-    return erreur_gyro;
-}
-*/
-#endif

test.c

@@ -137,7 +137,7 @@ int main() {
 
 // Mode test : renvoie 0 pour quitter le mode test
 int mode_test(){
-    static int iteration = 3;
+    static int iteration = 2;
     printf("Appuyez sur une touche pour entrer en mode test :\n");
     printf("A - pour asser_moteurs (rotation)\n");
     printf("B - pour avance (asser_moteur)\n");