Pages: [1] 2   Go Down
Author Topic: Arduino-->Gyro-->Servos ????  (Read 2655 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Newbie
*
Karma: 0
Posts: 18
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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  smiley-red

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...
Code:
#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.
Logged

Offline Offline
Full Member
***
Karma: 0
Posts: 129
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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) :
https://github.com/TKJElectronics/KalmanFilter/blob/master/examples/MPU6050/MPU6050.ino
Logged


France
Offline Offline
Faraday Member
**
Karma: 38
Posts: 3539
There is an Arduino for that
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Quote
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.
« Last Edit: June 18, 2013, 02:20:33 pm by fdufnews » Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 18
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Quote
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......?
Logged

Offline Offline
Full Member
***
Karma: 0
Posts: 129
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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
Logged


Offline Offline
Newbie
*
Karma: 0
Posts: 18
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Code:
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 ?
Logged

Offline Offline
Full Member
***
Karma: 0
Posts: 129
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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
Logged


Offline Offline
Newbie
*
Karma: 0
Posts: 18
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

je fais le test ce soir ! Je vous tiens au courant  smiley-grin
Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 18
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Ça ne marche pas  smiley-confuse

A la compilation j'ai une erreur.
http://imageshack.us/photo/my-images/809/7qf8.jpg/
Logged

Paris
Offline Offline
Sr. Member
****
Karma: 2
Posts: 366
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

EN: Libraries are my Gurus, they make me believe anything they want !
FR: Les librairies sont mes gourous, elles me font croire ce qu'elles veulent !

Offline Offline
Newbie
*
Karma: 0
Posts: 18
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Voilà ce que j'ai en début de programme:
Code:
#include <Wire.h>
#include "Kalman.h"
Logged

Paris
Offline Offline
Sr. Member
****
Karma: 2
Posts: 366
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Peux-tu STP mettre le code complet ?
Logged

EN: Libraries are my Gurus, they make me believe anything they want !
FR: Les librairies sont mes gourous, elles me font croire ce qu'elles veulent !

Offline Offline
Full Member
***
Karma: 0
Posts: 129
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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


Offline Offline
Newbie
*
Karma: 0
Posts: 18
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Voici le programme complet:
Code:
/* 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);
}

Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 18
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Exusez mon ignorance  smiley-kiss mais comment fais tu pour ouvrir les trois fichiers ?
Logged

Pages: [1] 2   Go Up
Jump to: