Arduino-->Gyro-->Servos ????

Bonjour,

Voilà 3 mois que je bataille, que je cherche à faire un programme en C.

J'ai acheté une carte Arduino Uno, un accéléromètre gyroscope MPU6050 et 2 servomoteurs, pensant naïvement que la programmation allait être simple même pour un novice comme moi ! Car je n'y connais rien en électronique et en programation :blush:

Mon projet est de faire manoeuvrer les servomoteurs suivant une inclinaison donnée (ex : à partir d'une inclinaison de 5° vers la gauche, je voudrais que mon servomoteur de gauche manoeuvre de 10°; idem pour la droite).

J'ai bien eu quelques bribes de réponses mais même avec ça je n'avance pas...
J'imagine bien que l'idéal serait d'apprendre le langage C dans sa globalité mais j'en ai besoin uniquement pour une partie précise de mon projet et n'en aurait plus besoin par la suite...
J'ai bien trouvé un code qui se rapproche de ce que je veux faire mais c'est pas encore ça...

#include <Servo.h>
 
Servo myservo;
int pos = 0;
const int MAX_MESURES = 20;
int mesures[MAX_MESURES];
int mesuresTriees[MAX_MESURES];
int nbMesures = 0;
 
 
void setup()
{
    myservo.attach(9);
    //Serial.begin(9600);
}
 
 
void loop()
 
{
    nbMesures = mesurer(mesures, nbMesures, MAX_MESURES);
    recopier(mesures,mesuresTriees,nbMesures);
    trier(mesuresTriees,nbMesures);
 
    //debugTab(mesuresTriees,nbMesures);
    pos = moyenneSansExtremes(mesuresTriees,nbMesures);
 
    //Serial.println(pos);
    myservo.write(pos);
    delay(20);
}
 
 
 
 
int mesurer(int tab[], int nb, int max)
{
    int sensorValue = analogRead(A0);
    pos = map(sensorValue, 165, 510, 0, 180);
    pos = constrain(pos, 0, 180);
 
    if(nb < max)
    {
        tab[nb] = pos;
        nb++;
    }
    else
    {
        for(int i=0; i<max-1; i++)
        {
            tab[i] = tab[i+1];
        }
 
        tab[max-1] = pos;
    }
 
    return nb;
}
 
 
void debugTab(int tab[], int nb)
{
    Serial.print("[");
 
    for(int i=0; i<nb; i++)
    {
        Serial.print(tab[i],DEC);
        if(i<nb-1)
        {
            Serial.print(",");
        }
    }
 
    Serial.print("]");
    Serial.println();
}
 
 
void recopier(int source[], int destination[], int nb)
{
    for(int i=0; i<nb; i++)
    {
        destination[i] = source[i];
    }
}
 
 
void trier(int tab[], int nb)
{
    int min, imin, tmp;
 
    for(int i=0; i<nb-1; i++)
    {
        min = tab[i];
        imin = i;
        for(int j=i+1; j<nb; j++)
        {
            if(min > tab[j])
            {
                min = tab[j];
                imin = j;
                break;
            }
        }
 
        if(i != imin)
        {
            tmp = tab[i];
            tab[i] = tab[imin];
            tab[imin] = tmp;
        }
    }
}
 
 
int moyenneSansExtremes(int tab[], int nb)
{
    int min = nb/4;
    int max = nb - (nb/4);
    int moyenne = 0;
    int j = 0;
 
    for(int i=min; i<max; i++)
    {
        moyenne += tab[i];
        j++;
    }
 
    moyenne = moyenne / j;
    return moyenne;
}

J'ai bien cherché des assos ou des clubs d'électronique pour m'épauller mais il n'y a rien dans les Landes. Y aurait-il une âme charitable pour m'aider?!

D'avance merci.

Heu, tu essai de lire la valeur du MPU6050 en faisant un analogRead() ?

Le module fonctionne avec i2c, il y a un exemple d'utilisation ici :
http://playground.arduino.cc/Main/MPU-6050

Après, tu auras beaucoup de bruit ou de pics de variation, il faut soit utiliser un filtre complementaire (complementary filter en anglais) soit un filtre de kalman, ce dernier est bien plus compliqué à mettre en place, moi j'utilise le code suivant (il faut installer la lib) :

J'imagine bien que l'idéal serait d'apprendre le langage C

et la physique.

En complément à la remarque de Viproz
Ton capteur a un gyromètre et un accéléromètre donc il retourne une vitesse et une accélération selon les trois axes mesurés. Il ne donne pas un angle. Tu ne peux donc pas passer comme ça de ta mesure à un angle pour le servo.

Heu, tu essai de lire la valeur du MPU6050 en faisant un analogRead() ?

C'est juste un exemple pour illustrer ma demande, Je ne sais même pas par quoi commencer !
Ce que j'ai déjà fait (et pas sans mal):

Installé des progrmmes qui sont en exemples sur la carte Arduino, (Basic Blink, Servo sweep...) récuprer les données brutes du gyro sur le moniteur série. Mais après......?

timy:
Installé des progrmmes qui sont en exemples sur la carte Arduino, (Basic Blink, Servo sweep...) récuprer les données brutes du gyro sur le moniteur série. Mais après......?

Si tu as essayé le code que j'ai envoyé, tu n'as pas du brut mais un angle, tu peux le récuperer dans le code, si il est envoyé sur le serial c'est que dans le code de l'arduino, il y a une ligne qui envoi les données

moi j'utilise le code suivant (il faut installer la lib) :

Quelle librairie faut il installer ? C'est en plus de celle MPU6050.ccp et MPU6050.h ?

celle ci : https://github.com/TKJElectronics/KalmanFilter/archive/master.zip

Je ne te garanti pas que ça mache, moi j'ai l’équivalent d'un GY-521

je fais le test ce soir ! Je vous tiens au courant :smiley:

Ça ne marche pas :~

A la compilation j'ai une erreur.

Il paraît manquer une librairie I2C... Qu'as-tu dans les INCLUDE au début du sketch ?

Voilà ce que j'ai en début de programme:

#include <Wire.h>
#include "Kalman.h"

Peux-tu STP mettre le code complet ?

il faut ouvrir les trois fichiers dans l'ide arduino, kalman.h, MPU6050.ino et i2c.ino

Voici le programme complet:

/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
 
 This software may be distributed and modified under the terms of the GNU
 General Public License version 2 (GPL2) as published by the Free Software
 Foundation and appearing in the file GPL2.TXT included in the packaging of
 this file. Please note that GPL2 Section 2[b] requires that all works based
 on this software must also be made publicly available under the terms of
 the GPL2 ("Copyleft").
 
 Contact information
 -------------------
 
 Kristian Lauszus, TKJ Electronics
 Web      :  http://www.tkjelectronics.com
 e-mail   :  kristianl@tkjelectronics.com
 */

#include <Wire.h>
#include "Kalman.h" // Source: https://github.com/TKJElectronics/KalmanFilter

Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;

/* IMU Data */
int16_t accX, accY, accZ;
int16_t tempRaw;
int16_t gyroX, gyroY, gyroZ;

double accXangle, accYangle; // Angle calculate using the accelerometer
double temp; // Temperature
double gyroXangle, gyroYangle; // Angle calculate using the gyro
double compAngleX, compAngleY; // Calculate the angle using a complementary filter
double kalAngleX, kalAngleY; // Calculate the angle using a Kalman filter

uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data

void setup() {  
  Serial.begin(115200);
  Wire.begin();
  i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
  i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
  i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
  i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
  while(i2cWrite(0x19,i2cData,4,false)); // Write to all four registers at once
  while(i2cWrite(0x6B,0x01,true)); // PLL with X axis gyroscope reference and disable sleep mode 
  
  while(i2cRead(0x75,i2cData,1));
  if(i2cData[0] != 0x68) { // Read "WHO_AM_I" register
    Serial.print(F("Error reading sensor"));
    while(1);
  }
  
  delay(100); // Wait for sensor to stabilize
  
  /* Set kalman and gyro starting angle */
  while(i2cRead(0x3B,i2cData,6));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  // atan2 outputs the value of -? to ? (radians) - see http://en.wikipedia.org/wiki/Atan2
  // We then convert it to 0 to 2? and then from radians to degrees
  accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
  accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
  
  kalmanX.setAngle(accXangle); // Set starting angle
  kalmanY.setAngle(accYangle);
  gyroXangle = accXangle;
  gyroYangle = accYangle;
  compAngleX = accXangle;
  compAngleY = accYangle;
  
  timer = micros();
}

void loop() {
  /* Update all the values */  
  while(i2cRead(0x3B,i2cData,14));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  tempRaw = ((i2cData[6] << 8) | i2cData[7]);  
  gyroX = ((i2cData[8] << 8) | i2cData[9]);
  gyroY = ((i2cData[10] << 8) | i2cData[11]);
  gyroZ = ((i2cData[12] << 8) | i2cData[13]);
  
  // atan2 outputs the value of -? to ? (radians) - see http://en.wikipedia.org/wiki/Atan2
  // We then convert it to 0 to 2? and then from radians to degrees
  accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
  accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
  
  double gyroXrate = (double)gyroX/131.0;
  double gyroYrate = -((double)gyroY/131.0);
  gyroXangle += gyroXrate*((double)(micros()-timer)/1000000); // Calculate gyro angle without any filter  
  gyroYangle += gyroYrate*((double)(micros()-timer)/1000000);
  //gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate
  //gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/1000000);
  
  compAngleX = (0.93*(compAngleX+(gyroXrate*(double)(micros()-timer)/1000000)))+(0.07*accXangle); // Calculate the angle using a Complimentary filter
  compAngleY = (0.93*(compAngleY+(gyroYrate*(double)(micros()-timer)/1000000)))+(0.07*accYangle);
  
  kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000); // Calculate the angle using a Kalman filter
  kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);
  timer = micros();
  
  temp = ((double)tempRaw + 12412.0) / 340.0;
  
  /* Print Data */
  /*
  Serial.print(accX);Serial.print("\t");
  Serial.print(accY);Serial.print("\t");
  Serial.print(accZ);Serial.print("\t");
  
  Serial.print(gyroX);Serial.print("\t");
  Serial.print(gyroY); Serial.print("\t");
  Serial.print(gyroZ);Serial.print("\t");
  */
  Serial.print(accXangle);Serial.print("\t");
  Serial.print(gyroXangle);Serial.print("\t");
  Serial.print(compAngleX);Serial.print("\t");
  Serial.print(kalAngleX);Serial.print("\t");
  
  Serial.print("\t");
  
  Serial.print(accYangle);Serial.print("\t");
  Serial.print(gyroYangle);Serial.print("\t");
  Serial.print(compAngleY); Serial.print("\t");
  Serial.print(kalAngleY);Serial.print("\t");
  
  //Serial.print(temp);Serial.print("\t");
   
  Serial.print("\r\n");
  delay(1);
}

il faut ouvrir les trois fichiers dans l'ide arduino, kalman.h, MPU6050.ino et i2c.ino

Exusez mon ignorance :* mais comment fais tu pour ouvrir les trois fichiers ?

Tu double clique sur MPU6050.ino :wink:

Edit : En ayant extrait l'archive bien sur

Je teste ce soir merci Viproz ! La suite au prochain épisode...

Plus d'erreurs de compilation mais le calme plat, rien ne s'affiche sur le moniteur séries :roll_eyes: Une idée ? Je vais me coucher demain il fera jour.... :sleeping:

quel est l'adresse de ton gyro ?

Déja est ce que mon branchement est OK ?