void SetSpeedMotor(double *PitchRollYaw, byte Vitesse_0, double Gain)
{
// Calcul de la partie variable de la vitesse
double P=PitchRollYaw[0];
double R=PitchRollYaw[1];
double Y=PitchRollYaw[2];
double Vitesse=Vitesse_0+Gain*(P+R); // Dépendance R&P: -180<R+P<180
//double Vitesse=Vitesse_0+Gain*(R); // Dépendance R: -90<R<90
//double Vitesse=Vitesse_0+Gain*(P); // Dépendance P: -90<P<90
// Mise à jour de la vitesse du moteur
M1.write(Vitesse); // 0-180
}
Fonction de lecture de données brutes du capteur
void ReadGY521_0( long *GyAccTempp)
{
long GATCorr_0[NumData]={0,0,0,0,0,0,0};
ReadGY521(GyAccTemp,GATCorr_0);
}
Le programme complet
#include<Wire.h>
#include <math.h>
#include <Servo.h>
#define NumData 7
#define pi 3.1415926535897932384626433832795
#define MPU 0x68
#define PinM1 9
// Pinout
/*
VCC -> 3.3 V / 5 V (préférable)
GND -> GND
SCL -> A5
SDA -> A4
XDA -> NC (non connecté)
XCL -> NC
ADO -> NC
INT -> NC
*/
Servo M1;
long GyAccTemp[NumData];
long GATCorr[NumData]={0,0,0,0,0,0,0};
double PitchRoll[3];
void setup()
{
// Init module GY-512
Wire.begin();
Wire.beginTransmission(MPU);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
// Init port série
Serial.begin(19200);
// Affectation du pin au servomoteur
M1.attach(PinM1);
}
void loop()
{
// Lecture des données: Acce + Temp + Gyro
//ReadGY521(GyAccTemp,GATCorr); // Lecture avec correction
ReadGY521_0(GyAccTemp); // Lecture des données bruts
// Calcul du Pitch / Roll / Yaw atan(x/sqt(y²+z²)
ComputeAngle(GyAccTemp, PitchRoll); //+-90
// Mise à jour de la vitesse du moteur
SetSpeedMotor(PitchRoll,90, 0.5); // 0(0%) - 180(100%)
}
void SetSpeedMotor(double *PitchRollYaw, byte Vitesse_0, double Gain)
{
// Calcul de la partie variable de la vitesse
double P=PitchRollYaw[0];
double R=PitchRollYaw[1];
double Y=PitchRollYaw[2];
double Vitesse=Vitesse_0+Gain*(P+R); // Dépendance R&P: -180<R+P<180
//double Vitesse=Vitesse_0+Gain*(R); // Dépendance R: -90<R<90
//double Vitesse=Vitesse_0+Gain*(P); // Dépendance P: -90<P<90
// Mise à jour de la vitesse du moteur
M1.write(Vitesse); // 0-180
}
// Lecture des données des capteurs avec Corrections
void ReadGY521( long *GyAccTempp, long *GATCorrr)
{
// Init du module GY-521
Wire.beginTransmission(MPU);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU,14,true);
// Lecture des données (3 axes accéléromètre + température + 3 axes gyroscope
for (long i=0;i<NumData;i++)
{
if(i!=3)
{
GyAccTempp[i]=(Wire.read()<<8|Wire.read()) - GATCorrr[i];
}
else
{
GyAccTempp[i]=(Wire.read()<<8|Wire.read()) - GATCorrr[i];
GyAccTempp[i] = GyAccTempp[i]/340 + 36.53;
}
}
}
// Lecture des données des capteurs sans Corrections
void ReadGY521_0( long *GyAccTempp)
{
long GATCorr_0[NumData]={0,0,0,0,0,0,0};
ReadGY521(GyAccTemp,GATCorr_0);
}
//Conversion des données accéléromètre en pitch/roll/yaw
void ComputeAngle(long *GyAccTempp, double *PitchRol)
{
double x,y,z, X, Y, Z;
x= (double)GyAccTempp[0];
y= (double)GyAccTempp[1];
z= (double)GyAccTempp[2];
X=sqrt((y*y) + (z*z));
Y=sqrt((x*x) + (z*z));
Z=sqrt((x*x) + (y*y));
PitchRol[0] = atan(x/X); // pitch
PitchRol[1] = atan(y/Y); // roll
PitchRol[2] = atan(z/Z); // yaw
//Conversion Radian en degré
PitchRol[0] = PitchRol[0] * (180.0/pi);
PitchRol[1] = PitchRol[1] * (180.0/pi) ;
PitchRol[2] = PitchRol[2] * (180.0/pi) ;
}