Go Down

Topic: Gyroscope (Read 317 times) previous topic - next topic

bartolipier

Bonjour,

je construis une tourelle stabilisée par gyroscope (Gy-521) qui fonctionne. Je veux pouvoir modifier le position du servo de l'axe X par un potentiomètre (Pour éviter une obstacle). Lorsque je rajoute les lignes de code correspondant au potar, je neutralise l'action du gyro sur cet axe. Pouvez-vous m'aider car je ne maîtrise pas assez la programmation pour comprendre ce qui manque. Je vous joins mon code.

Code: [Select]

#include <Servo.h> // Installation de la bibliothèque de servo
#include "I2Cdev.h" // Installation de la bibliothèque I2Cdev
#include "MPU6050_6Axis_MotionApps20.h" // Installation dela bibliothèque MPU6050
#include "Wire.h"

const int potar = 0; //déclaration du potentiometre

Servo myservoY; // Déclaration de la fonction du servo roulis
Servo myservoX; // Déclaratio de la fonction du servo tangage

MPU6050 mpu; // Mise a jour de l'adresse par défaut

/* =========================================================================
   Branchements du gyroscope:              Les branchement des servos sont:
        VCC --> 3.3V ou 5v                    Fil rouge --> + alim externe
        GND --> GND                           Fil noir --> - Commun
        SCL --> A5                            Fil signal servo axe X --> D9
        SDA --> A4                            Fil signal servo axe Y --> D10
        INT --> D2
   * ========================================================================= */

#define OUTPUT_READABLE_YAWPITCHROLL

#define LED_PIN 13 // Arduino is 13
bool blinkState = false;

// Contrôle de l'état de la MPU
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // Vrai si l'initialisation a réussie
uint8_t devStatus;      // retourne l'état après chaque opération de l'appareil (0 = succès! 0 = erreur)
uint16_t packetSize;    // attendu taille de paquet DMP (valeur par défaut est 42 octets)
uint16_t fifoCount;     // compte de tous les octets actuellement en FIFO
uint8_t fifoBuffer[64]; // Tampon de stockage FIFO

// orientation / mouvement vars
Quaternion q;           // [w, x, y, z] Conteneur de quaternion
VectorInt16 aa;         // [x, y, z] mesures des capteurs Accel
VectorInt16 aaReal;     // [x, y, z] mesures des capteurs d'accélération sans gravité
VectorInt16 aaWorld;    // [x, y, z] monde-cadre des mesures de capteur d'accélération
VectorFloat gravity;    // [x, y, z] vecteur de gravité
float euler[3];         // [psi, theta, phi] Conteneur angle d'Euler
float ypr[3];           // [yaw, pitch, roll] lacet / tangage / roll-conteneurs et vecteur de gravité

// Structure de paquet pour InvenSense démo théière
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };

volatile bool mpuInterrupt = false;     // indique si la broche MPU d'interruption est allé haute
void dmpDataReady() {
    mpuInterrupt = true;
}

// ================================================================
// ===                        VOID SETUP                        ===
// ================================================================

void setup() {
    // rejoindre le bus I2C (la bibliothèque I2Cdev ne le fait pas automatiquement)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        TWBR = 24; // 400kHz horloge de l'I2C (200kHz si le CPU est de 8 MHz)
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif

  // Branchement des servos
  myservoY.attach(9); // Servo Y asservit à la broche 9
  myservoX.attach(10);// Servo X asservit à la broche 10

    mpu.initialize(); // Initialisation du dispositif
    devStatus = mpu.dmpInitialize(); // Charge et configure le DMP

    // fourni vos propres décalages gyroscopiques ici, à l'échelle de la sensibilité min
    mpu.setXGyroOffset(220);
    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-85);
    mpu.setZAccelOffset(1788); // 1688 réglage d'usine par défaut pour ma puce de test

    // assurez-vous qu'il a travaillé (0 si oui)
    if (devStatus == 0) {
        mpu.setDMPEnabled(true); // allume le DMP, maintenant qu'il est prêt

        attachInterrupt(0, dmpDataReady, RISING); // permet la détection d'interruption Arduino
        mpuIntStatus = mpu.getIntStatus();

        dmpReady = true; // met notre DMP prèt si la fonction main loop() sait qu'il est prèt

        // obtention de la taille de paquet DMP pour une comparaison ultérieure
        packetSize = mpu.dmpGetFIFOPacketSize();
    } else {
        // ERREUR!
         // 1 = la charge initiale de mémoire a échoué
         // 2 = mises à jour de configuration DMP échouée (Si elle va se casser, généralement le code sera 1)
    }

    // Configurer la LED de sortie
    pinMode(LED_PIN, OUTPUT);
}

// ================================================================
// ===                         VOID LOOP                        ===
// ================================================================

void loop() {
    
    int val = analogRead(potar); //lecture de la position du potar
    int angle = val/5.7; // Transforme en angle
    myservoX.write(angle); // Envoie la valeur au servo des X
  
  
    if (!dmpReady) return; // si la programmation a échoué, ne pas essayer de faire quelque chose

    while (!mpuInterrupt && fifoCount < packetSize) { // attendre interruption de MPU ou un paquet supplémentaire disponible
      }

    mpuInterrupt = false; // réinitialiser l'indicateur d'interruption et obtenir INT_STATUS octet
    mpuIntStatus = mpu.getIntStatus();

    fifoCount = mpu.getFIFOCount(); // obtenir le nombre de FIFO actuel

    if ((mpuIntStatus & 0x10) || fifoCount == 1024) { // vérifier en cas de débordement
       mpu.resetFIFO(); // RESET pour que nous puissions continuer proprement

    } else if (mpuIntStatus & 0x02) { // Sinon, vérifier les données DMP interruption prêt
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); // attendre une longueur de données correcte disponible
        mpu.getFIFOBytes(fifoBuffer, packetSize); //lire un paquet de FIFO
        
        // suivre le compte FIFO. au cas où il est > 1 paquets disponibles
        // (Cela nous permet de lire tout de suite sans attendre une interruption)
        fifoCount -= packetSize;

        #ifdef OUTPUT_READABLE_YAWPITCHROLL // affichage en degrés angles d'Euler           
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            
            myservoY.write(int(ypr[1] * -180/M_PI)+90);   // Rotation autour de Y
            myservoX.write(int(ypr[2] * -180/M_PI)+90);   // Rotation autour de X
        #endif

        blinkState = !blinkState; // faire clignoter la LED pour indiquer l'activité
        digitalWrite(LED_PIN, blinkState);
    }
    
}



Merci d'avance

Go Up