Go Down

Topic: Arduino-->Gyro-->Servos ???? (Read 3 times) previous topic - next topic

Viproz

Tu double clique sur MPU6050.ino ;)


Edit : En ayant extrait l'archive bien sur
http://stilldrinking.org/programming-sucks

timy

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

timy

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

Viproz

quel est l'adresse de ton gyro ?
http://stilldrinking.org/programming-sucks

timy

Déja est ce que mon branchement est OK ?


timy


Viproz

Non, pas du tout :smiley-roll-sweat:
il faut que tu utilise du 5V pour pouvoir communiquer aver l'arduino
Ensuite, les port i2c sont A5 et A4 (comme spécifié ici : http://arduino.cc/en/reference/wire)
http://stilldrinking.org/programming-sucks

timy

je suis une saucisse le tout c'est de le savoir qu'on en est une  :smiley-mr-green:! Dans tous les cas ça fait plaisir (pas d'être une saucisse) d'être épaullé  :D je refais mes branchements.

timy

C'est Ok !, les valeurs s'affichent à vitesse grand V sur le moniteur série. Après cette petite victoire reste le coté obscur (un de plus) du programe:  :smiley-roll:

Comment procéder pour commander mes servos lorsque:

_j'incline vers la gauche, à partir de 5° par rapport à l'horizontale et jusqu'à 90°, mon servo de gauche bouge de 5° et garde cette position. Le servo de droite ne bouge pas. Lorsque je reviens à l'horizontale, le servo reprend sa position initiale à 0°.

Et idem lorsque j'incline vers la droite, c'est le servos de droite qui bouge (...). Pas simple l'histoire !?

UniseV

Les librairie t'aident déjà pas mal, tu as les valeurs du MPU6050, tu sais piloter les servos... il ne te reste plus que la rédaction de la partie "intelligente" du programme, qui se trouve principalement dans le loop.

Personnellement, si tu débute je te déconseille d'utiliser trop de fonctions.

C'est maintenant que tu écris TON programme...  ;)

Bien sur on reste là pour t'aider, mais il faut que tu te lance  XD
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 !

timy

Je suis d'accord, il faut que je me lance mais il me faudrait une base pour que je puisse comprendre l'architecture du programme et le faire à ma sauce car là je ne sais pas par quoi commencer.. :~

Je ne veux pas pas bêtement faire copier collé, j'ai besoin de comprendre les chôses pour savoir ou et comment modifier mon programme voire l'améliorer.

UniseV

Tu sais ce que ton programme doit faire, alors ce que je te propose c'est de commencer par écrire ton programme ici, sur le forum, un peu comme ça :

IF GyroX>5° SERVO1=SERVO1+3
IF GyroX<5° SERVO2=SERVO2+3

Et on aura déjà une base de discussion un peu plus solide ;)
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 !

timy

Je me lance donc, attention les yeux  :smiley-eek::
Code: [Select]
#include <Servo.h>

Servo myservog;
Servo myservod;
int pos = 0;

const int MAX_MESURES = 20;
int mesures[MAX_MESURES];
int mesuresTriees[MAX_MESURES];
int nbMesures = 0;

void setup()
{
  myservod.attach(8);
  myservog.attach(9);
  //Serial.begin(9600);
}


void loop()
{
  {
if gyroX>5°(à)90° myservog.write(pos=10)
}
{
if gyroX<355°(à)270° myservod.write(pos=10)
}
}

Viproz

#28
Jun 23, 2013, 10:56 am Last Edit: Jun 23, 2013, 10:58 am by Viproz Reason: 1

if gyroX>5°(à)90° myservog.write(pos=10)


Il va quand même falloir que tu apprennes les bases du c/c++...

PS : La variable gyroX n'existe pas dans ton code, j'imagine que c'est parce que tu comptes ajouter ce code à l'exemple d'utilisation du MPU6050
http://stilldrinking.org/programming-sucks

timy

voilà le programme completé, il manque le plus gros mais j'y travaille. Jusque là tout va bien ?

Code: [Select]
#include <Servo.h>
#include <Wire.h>
#include "Kalman.h" // Source: https://github.com/TKJElectronics/KalmanFilter

Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;
Servo myservog;
Servo myservod;
int pos = 0;

/* 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() { 
  myservod.attach(8);
  myservog.attach(9);
  Serial.begin(9600);
  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);
}

Go Up