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];
// 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];
// 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 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];