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
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 Une idée ? Je vais me coucher demain il fera jour....
quel est l'adresse de ton gyro ?
Non, pas du tout
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 : Wire - Arduino Reference)
je suis une saucisse le tout c'est de le savoir qu'on en est une ! Dans tous les cas ça fait plaisir (pas d'être une saucisse) d'être épaullé je refais mes branchements.
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:
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 !?
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
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.
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
Je me lance donc, attention les yeux :
#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)
}
}
timy:
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
voilà le programme completé, il manque le plus gros mais j'y travaille. Jusque là tout va bien ?
#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);
}