Implémentation de La Boucle D'asservissement D'un Drone Bicopter Avec Arduino - Cours - Projets Divers

30/12/2023 10:03 Implémentation de la Boucle d’Asservissement d’un Drone Bicopter avec Arduino – Cours | Projets Divers
“ Découvrez notre Chaîne YouTube "Ingénierie et Projets"

Implémentation de la Boucle d’Asservissement d’un Drone
Bicopter avec Arduino
‘ Découvrez notre Chaîne Secondaire "Information Neuronale et l'Ingénierie du
Cerveau"
Publié par ElecM le 2021-05-23
 Obtenir le fichier PDF
https://www.electronique-mixte.fr/implementation-de-la-boucle-dasservissement-dun-drone-bicopter-avec-arduino/ 1/32
Drone | Arduino #17: Contrôleur Proportionnel (P) d’un Drone Bi-copter [TU…
[TU…
Objectifs
1. Savoir configurer les registres du module MPU-6050 GY-521
2. Savoir implémenter la boucle d’asservissement d’un drone Bicopter (voir la figure ci-dessous)
3. Savoir l’effet de l’action proportionnelle sur l’équilibre du drone
4. Savoir la différence entre l’asservissement d’angle avec l’accéléromètre ou bien le gyroscope
5. Savoir implémenter une intégration numérique avec Arduino
6. Voir le tuto pour plus de détails
ARDUINO
+ M1
Consigne
+
Angle Consigne
+ P, PI, PID Vitesse
(V0)
- -
+ M2
CAPTEUR
Câblage et initialisation du Module MPU-6050 GY-521
‘ Veuillez consulter l’article dédié au module pour plus de détails
Câblage
VCC -> 3.3 V / 5 V (préférable)
GND -> GND
SCL -> A5 (Uno, Nano, Mini) (Pin SCL pour le Mega)
SDA -> A4 (Uno, Nano, Mini) (Pin SCL pour le Mega)
XDA -> NC (non connecté)
XCL -> NC
ADO -> NC
INT -> NC
Définition de l’adresse du module et LSB du Gyroscope
1 #define MPU 0x68

2
3 #define LSB_Gyro 65.5 // +/- 500°/s sur 16 bits
Configuration de l’Horloge interne à 8MHz
Wire.begin(); // Début transmission
Wire.beginTransmission(MPU); // Adresse du Module
Wire.write(0x6B); // Adresse du registre de control
Wire.write(0); // Valeur
Wire.endTransmission(true); // Fin de transmission
delay(10); // Retard avant la nouvelle transmission
Configuration de la sensibilité du Gyroscope à +/- 500°/s
1 Wire.beginTransmission(MPU);
2
3 Wire.write(0x1B);
4
5 Wire.write(0x08);
6
7 Wire.endTransmission();
8
9 delay(10);
Configuration de la sensibilité d’Accéléromètre à : +/- 8g
2
3 Wire.write(0x1C);
4
5 Wire.write(0x10);
6
8
9 delay(10);
Configuration de la bande passante du Filtre passe-bas

(Gyro/Accelo): 260Hz/ <1ms Delay
2
3 Wire.write(0x1A);
4
5 Wire.write(0x00);
6
8
9 delay(10);
Initialisation du port série (Vitesse >2500000 afin de réduire la

latence de transmission)
1 Serial.begin(250000);
Affectation du pin au servomoteur
1 M1.attach(PinM1);
2
3 M2.attach(PinM2);
La mesure de l’angle par le Gyro

Principe
Les gyroscopes sont utilisés comme capteur de position angulaire, alors que les gyromètres sont
des capteurs de vitesse angulaire. Le gyroscope donne la position angulaire (selon un ou deux
axes uniquement) de son référentiel par rapport à un référentiel inertiel (ou galiléen). Lire la suite…
L’obtention de l’angle peut être donc réaliser en utilisant l’intégration numérique de la vitesse
angulaire. Dans ce tuto on va utiliser la technique de Simpson pour intégrer une fonction. La
technique est analogue à la moyenne glissante d’une fonction sur 3 échantillons (voir la figure ci-
dessous).
‘
h=b-a=2*Te, Te étant la période d’échantillonnage
‘ Voir le tuto « Comment intégrer une fonction avec Arduino » pour plus de
détails.
La fonction AngleGyro()
La fonction AngleGyro() permet de convertir les données bruts issues du gyroscope en angles
dans les trois axes. Elle est basée sur l’intégration de Simpson dans l’axe x, y et z. Ci-dessous les
étapes de la mesure des angles :
Calcul des valeurs réelles (division par la sensibilité)
1 x= (double)GyAccTempp[4]/LSB_Gyro;
2
3 y= (double)GyAccTempp[5]/LSB_Gyro;
4
5
z= (double)GyAccTempp[6]/LSB_Gyro;
Intégration – Simpson:
‘ F(n)=[ f(n-1) + 4*f(n) + f(n+1)]*Te/6
1 h=2.0*Te_s;
2 X= (xyz[0][1] + 4.0*xyz[0][0] + x); X=X*h/6.0;
3 Y= (xyz[1][1] + 4.0*xyz[1][0] + y); Y=Y*h/6.0;
4 Z= (xyz[2][1] + 4.0*xyz[2][0] + z); Z=Z*h/6.0;
Mise à jour des échantillons dans les trois axes
1 xyz[0][1]=xyz[0][0];
2
3 xyz[0][0]=x;
4
5
6
7
8 xyz[1][1]=xyz[1][0];
9
10 xyz[1][0]=y;
11
12
13
14
15 xyz[2][1]=xyz[2][0];
16
17 xyz[2][0]=z;
Conversion des angles du Radian en degré
1 GyPitchRol[0] = X * (180.0/PI);
2
3 GyPitchRol[1] = Y * (180.0/PI) ;
4
5 GyPitchRol[2] = Z * (180.0/PI) ;
‘ La période d’intégration est l’inverse de la période d’échantillonnage! Elle doit

Être assez grande pour une bonne intégration et assez petite pour un système
rapide!
1 void AngleGyro(long *GyAccTempp, double *GyPitchRol, double Te_s)

2 {
3 double x,y,z, X, Y, Z, h;
4 static double xyz[3][2];
5
6 // 1. Calcul des valeurs réelles
7 x= (double)GyAccTempp[4]/LSB_Gyro;
8 y= (double)GyAccTempp[5]/LSB_Gyro;
9 z= (double)GyAccTempp[6]/LSB_Gyro;
10
11 // 2.1 Integration - Simpson: F(n)=[ f(n-1) + 4*f(n) + f(n+1)]*Te/6
12 h=2.0*Te_s;
13 X= (xyz[0][1] + 4.0*xyz[0][0] + x); X=X*h/6.0;
14 Y= (xyz[1][1] + 4.0*xyz[1][0] + y); Y=Y*h/6.0;
15 Z= (xyz[2][1] + 4.0*xyz[2][0] + z); Z=Z*h/6.0;
16
17 // 2.2 Mise à jour des échantillons
18 xyz[0][1]=xyz[0][0];
19 xyz[0][0]=x;
20
21 xyz[1][1]=xyz[1][0];
22 xyz[1][0]=y;
23
24 xyz[2][1]=xyz[2][0];
25 xyz[2][0]=z;
26
27 // 3. Conversion des angles du Radian en degré
28
29 GyPitchRol[0] = X * (180.0/PI);
30 GyPitchRol[1] = Y * (180.0/PI) ;
31 GyPitchRol[2] = Z * (180.0/PI) ;
}
A retenir
L’action proportionnel est facile à mettre en pratique. Elle ne nécessite pas la connaissance
des paramètres physiques du système pour effectuer la commande (Wn, dépassement, etc.).
Elle est utilisée pour augmenter la rapidité du système et réduire un peu l’erreur statique
(k>1). En revanche elle a tendance à déstabiliser le système.
La mesure de l’ange basé sur l’accélération semble inefficace. Le capteur est très sensible
aux vibrations durant le décollage du drone. Les mesures sont inutiles face aux vibrations
des moteurs. En revanche, la mesure de l’angle par le gyroscope reste peu sensible aux bruits
des vibrations. On peut encore améliorer la stabilité des mesure en utilisant une moyenne
glissante (voir le tuto). On maintient la mesure par le Gyro dans les tutos à venir pour
l’asservissement de l’angle.
‘ Une intégration numérique d’ordre supérieur (plus de points) peut être utilisée
pour réduire naturellement l’effet du bruit !
L’action (P) n’améliore pas la stabilité du drone. On verra dans le tuto prochain l’action PI et
avance de phase.
Le programme complet
#include <math.h>
#include <Servo.h>
#include <Wire.h>
#define NumData 7
#define pi 3.1415926535897932384626433832795
#define MPU 0x68
#define LSB_Gyro 65.5 // +/- 500°/s sur 16 bits
#define PinM1 11
#define PinM2 12
#define Fn 10.00
#define Zeta 0.05 //0.70710678118
#define K 1.0
#define T_ms 1
double Wn=2.0*PI*Fn;
double T_s=(double)T_ms/1000.0;
// Pinout
/*
VCC -> 3.3 V / 5 V (préférable)
GND -> GND
SCL -> A5 (Uno, Nano, Mini)
SDA -> A4 (Uno, Nano, Mini)
XDA -> NC (non connecté)

XCL -> NC
ADO -> NC
INT -> NC
*/
Servo M1, M2;

double Vitesse_M1,Vitesse_M2;
long GyAccTemp[NumData];
double PitchRoll[3];
double x_nn=0.0; // Consigne de l'Angle à l'équilibre (Fixe: 0°)!

double V0=40.0; // Consigne de la Vitesse (Variable par l'utilisa
double y_n;
double eps_n; // Erreur
double y_capt; // Sortie du capteur
double y_corr; // Sortie du correcteur
// Variables internes du système

double x1[2], y1[3];
// Variables internes du correcteur

double x_c[2], y_c[2];
void setup()
{
// Init module GY-512

// 1. Horloge interne: 8MHz
Wire.begin(); // Début transmission
Wire.beginTransmission(MPU); // Adresse du Module
Wire.write(0x6B); // Adresse du registre de control
Wire.write(0); // Valeur
Wire.endTransmission(true); // Fin de transmission
delay(10); // Retard avant la nouvelle transmission
// 2. Gryro +/- 500°/s

Wire.beginTransmission(MPU);
Wire.write(0x1B);
Wire.write(0x08);
Wire.endTransmission();
delay(10);
// 3. Accelo: +/- 8g
Wire.write(0x1C);
Wire.write(0x10);
delay(10);
// 4. Filtre passe-bas (Gyro/Accelo): 260Hz/ <1ms Delay

Wire.write(0x1A);
Wire.write(0x00);
delay(10);
// Init port série

Serial.begin(250000);
// Affectation du pin au servomoteur

M1.attach(PinM1);
M2.attach(PinM2);
}
void loop()
{
// 1. Définition de la consigne: Angle d'équilibre (0°) :)
x_nn=0.0;
// 2.1 Lecture du capteur: Accélo + Temp + Gyro

ReadGY521_0(GyAccTemp); // Lecture des données bruts
// 2.2 Calcul des Angles (Pitch / Roll / Yaw)

//AngleAcc(GyAccTemp, PitchRoll); //Accélo +/-90
AngleGyro(GyAccTemp, PitchRoll, (double)T_ms/1000.0); //Gyro +/-90°
// 2.3 Affectation de la sortie du capteur

//y_capt=PitchRoll[0]-1.0; // Accélo
y_capt=PitchRoll[1]+1.0; // Gyro
// 2.4 Filtrage de la mesure du capteur (Moyenne glissante)

/*
const byte N=5;
static byte j=0;
double somme =0.0, Cap[N];
Cap[j]=y_capt; j++; j%=N;

for (int i=0; i<N; i++) somme+=Cap[i];
y_capt=somme/double(N); somme=0.0;
*/
// 3. Soustracteur: Calcul de l'erreur eps(n)

eps_n=x_nn-y_capt;
// 4. Correcteur P (voir le lien en commentaire)

const double k0=0.03;
y_corr= k0*eps_n;
// 5. Définition de consigne de la vitesse V0 (0(0%, 180(100%))

V0=V0+0.01;
if (V0> 110.0) V0=V0-0.01;
// 6.1 Mise à jour de la vitesse

Vitesse_M1=V0-y_corr;
Vitesse_M2=V0+y_corr;
// 6.2. Réglage du Problème de Saturation et Valeurs <0

const double Vmax=180.0; // Rapport Cyclique Maximal
Vitesse_M1=Vmax*tanh(abs(Vitesse_M1)/Vmax);
Vitesse_M2=Vmax*tanh(abs(Vitesse_M2)/Vmax);
// 6.3 Affection des vitesses aux Moteurs

M1.write(Vitesse_M1);
M2.write(Vitesse_M2);
// Affichage: Vérification du Module GY-512 (A commenter)

Serial.print(y_corr); Serial.print(",");
Serial.println(-y_corr); Serial.print(",");
//Serial.println(y_capt);
//Serial.print(PitchRoll[0]-1.0); Serial.print(",");
//Serial.print(PitchRoll[1]); Serial.print(",");
//Serial.println(y_capt);
// Période d'échantillonnage
delay(T_ms);
}
// 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);
}
// Lecture des données des capteurs avec Corrections

void ReadGY521( long *GyAccTempp, long *GATCorrr)
{
// Init du module GY-521
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 gy

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;
}
}
}
//Conversion des données accéléromètre en pitch/roll/yaw

void AngleAcc(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) ;
}
//Conversion des données du gyro en pitch/roll/yaw

void AngleGyro(long *GyAccTempp, double *GyPitchRol, double Te_s)
{
double x,y,z, X, Y, Z, h;
static double xyz[3][2];
// 1. Calcul des valeurs réelles

x= (double)GyAccTempp[4]/LSB_Gyro;
y= (double)GyAccTempp[5]/LSB_Gyro;
// 2.1 Integration - Simpson: F(n)=[ f(n-1) + 4*f(n) + f(n+1)]*Te/6
h=2.0*Te_s;
X= (xyz[0][1] + 4.0*xyz[0][0] + x); X=X*h/6.0;
Y= (xyz[1][1] + 4.0*xyz[1][0] + y); Y=Y*h/6.0;
Z= (xyz[2][1] + 4.0*xyz[2][0] + z); Z=Z*h/6.0;
// 2.2 Mise à jour des échantillons

xyz[0][1]=xyz[0][0];
xyz[0][0]=x;
xyz[1][1]=xyz[1][0];
xyz[1][0]=y;
xyz[2][1]=xyz[2][0];
xyz[2][0]=z;
// 3. Conversion des angles du Radian en degré

GyPitchRol[0] = X * (180.0/PI);
GyPitchRol[1] = Y * (180.0/PI) ;
GyPitchRol[2] = Z * (180.0/PI) ;
}
void AngleGyroNC52(long *GyAccTempp, double *GyPitchRol, double Te_s)

{
double x,y,z, X, Y, Z;
static double xyz[3][4];
// 1. Calcul des valeurs réelles

x= (double)GyAccTempp[4]/LSB_Gyro;
y= (double)GyAccTempp[5]/LSB_Gyro;
// 2.1 Integration - Formule NC-5-2 d’ordre 7

X= 434.0*(xyz[0][3]+x)+1024.0*(xyz[0][2]+xyz[0][0])+864.0*xyz[0][1] ;
Y= 434.0*(xyz[1][3]+y)+1024.0*(xyz[1][2]+xyz[1][0])+864.0*xyz[1][1] ;
Z= 434.0*(xyz[2][3]+z)+1024.0*(xyz[2][2]+xyz[2][0])+864.0*xyz[2][1] ;
// 2.2 Mise à jour des échantillons

for(int i=0; i<3; i++)
{
xyz[0][i+1]=xyz[0][i];
xyz[1][i+1]=xyz[1][i];
xyz[2][i+1]=xyz[2][i];
}
xyz[0][0]=x;
xyz[1][0]=y;
xyz[2][0]=z;
// 3. Conversion des angles du Radian en degré

GyPitchRol[0] = X * (180.0/PI);
GyPitchRol[1] = Y * (180.0/PI) ;
GyPitchRol[2] = Z * (180.0/PI) ;
}
double Sys2All(double x_nn, double *x, double *y, double zeta, double wn
{
// Paramètre du système
double a1=2.0*zeta/wn;
double a2=1.0/(wn*wn);
const double b0=(a1/(2.0*T))+(a2/(T*T));

const double b1=-2.0*a2/(T*T);
const double b2=(-1.0*a1/(2.0*T))+(a2/(T*T));
const double b[3]={b0,b1,b2};
// Variables de l'entrée et la sortie

double y_nn=0.0;
// Calcul de la nouvelle sortie

y_nn= -(y[0]*(1.0+b[1]))-(y[1]*b[2])+(k*x[0]); // y[1]: y(n-2), y[0]:
y_nn/=b[0];
// Mise à jour de la sortie

y[1]=y[0];
y[0]=y_nn;

x[0]=x_nn;
// Renvoie du résultat
return y_nn;
}
double CorrPI(double x_nn, double *xpi, double *ypi, double kp, double k
{
double y_nn=0.0;

y_nn=ypi[1] + kp*x_nn + (2.0*T*ki)*xpi[0] -kp*xpi[1];
// y(n)=y(n-2)+ k1*x(n) + 2*T*k2*x(n-1) - k1*x(n-2)

ypi[1]=ypi[0];
ypi[0]=y_nn;

xpi[1]=xpi[0];
xpi[0]=x_nn;
return y_nn;
}
double CorrPD_AP(double x_nn, double *x_cc, double *y_cc, double a, doub

{
double y_nn=0.0;
// Paramètres du correcteur
double alfa=a*t0;
double beta=t0;

// Modèle Analogique: C(p)= k*(1+aTp)/(1+Tp)
// Modèle numérique: y(n)=[alfa*k 2kT -alfa*k]*[x(n) x(n-1) x(n-2)]'
// -[2T -beta]*[y(n-1) y(n-2)]'
// y(n)=y(n)/beta
y_nn=(alfa*k0*x_nn)+(2*k0*T*x_cc[0]) - (alfa*k0*x_cc[1]);
y_nn=y_nn- (2*T*y_cc[0]) - (beta*y_cc[1]);
y_nn/=beta;

y_cc[1]=y_cc[0];
y_cc[0]=y_nn;

x_cc[1]=x_cc[0];
x_cc[0]=x_nn;
return y_nn;
}
double PID(double x_nn, double *x_cc, double *y_cc, double N_fil, double
{
double t1=1.0/(w*(z+sqrt(z*z-1.0)));
double t2=1.0/(w*(z-sqrt(z*z-1.0)));
double td=t1+t2;
double ti=t1*t2/td;
/*
Serial.print(t1);
Serial.print("\t");
Serial.print(t2);
Serial.print("\t");
Serial.print(td);
Serial.print("\t");
Serial.println(ti);
*/
double a1=2.0*z/w;
double a2=1.0/(w*w);
double b1=ti;
double b2=ti*t2/N_fil;
double b[3];
b[0]= a1/(2*T)+a2/(T*T);
b[1]= 1.0-2.0*a2/(T*T);
b[2]=-a1/(2*T)+a2/(T*T);
double c[3];
c[0]= b1/(2*T)+b2/(T*T);
c[1]= -2.0*b2/(T*T);
c[2]= -b1/(2*T)+b2/(T*T);

double y_nn=0.0;
405
406
407
y_nn=k*(b[0]*x_nn + b[1]*x_cc[0] + b[2]*x_cc[1]);
408
y_nn=y_nn-(c[1]*y_cc[0] + c[2]*y_cc[1]);
409
y_nn=y_nn/c[0];
410
411
412
413
y_cc[1]=y_cc[0];
414
y_cc[0]=y_nn;
415
416
// Mise à jour de l'entrée
x_cc[1]=x_cc[0];
x_cc[0]=x_nn;
return y_nn;
}
Accueil Drone avec Arduino
Click to rate this post!
[Total: 2 Average: 5]
‘
Nous Soutenir
Catégories : ALGORITHME ARDUINO ASSERVISSEMENT ASSERVISSEMENT ARDUINO AUTOMATIQUE
COMMANDE DES MOTEURS DRONE PROJETS ÉLECTRONIQUES
0 commentaire
Laisser un commentaire
Nom *
E-mail *
Site internet
Anti-Robot *
+ = seize 
Qu’avez vous à l’esprit ?
LAISSER UN COMMENTAIRE
Arduino & Grafcet
Langage C et VHDL pour les débutants
Traitement d’Images avec Matlab
Tout les livres pour les Électroniciens
Série GRAFCET
Projets électroniques
Projets Arduino
Série Analyse fréquentielle
Série Drone | Arduino
Nos kITS
Série Capteurs | Détecteurs
Série Arduino
Série Asservissement | Arduino
Série Traitement d’Image
Série Proteus
Série Matlab
Kits Arduino
Rejoindre la team des IR
Kits drone
Devenir Ingénieur
YouTube 25 k
Articles similaires
ALGORITHME
Livre Arduino & Grafcet: Codage en C du Grafcet avec Arduino

Découvrez notre Chaîne YouTube "Ingénierie et Projets" Découvrez notre Chaîne Secondaire
"Information Neuronale et l'Ingénierie du Cerveau" Obtenir le fichier PDF Broché: 230 pages Date de
publication : 2 juillet 2023 Collection : Arduino Langue : Français ISBN-10: 8397581509 ISBN-
13: 9798397581509 Dimensions du Lire la suite…
ALGORITHME
GRAFCET | ARDUINO #45: La Commande d’un Ascenseur – Architecture

matérielle II
"Information Neuronale et l'Ingénierie du Cerveau" Obtenir le fichier PDF Objectifs du projet Comprendre
l’architecture matérielle du système Savoir le schéma de puissance de Lire la suite…
ARDUINO
GRAFCET | ARDUINO #44: La Commande d’un Ascenseur – Introduction I

"Information Neuronale et l'Ingénierie du Cerveau" Obtenir le fichier PDF Objectifs du projet Savoir les
composantes dans d’un ascenseur Savoir le principe de Lire la suite…
Traitement d’Images avec Matlab
Tout les livres pour les Électroniciens
Langage C et VHDL pour les débutants
Besoin d’Aide ?
Projets-FPGA Pour les Électroniciens
Rejoindre Notre chaîne
Rechercher
Rechercher…
ACCUEIL CONTACT À PROPOS NEWSLETTER AVIS VISITEURS ENVOYEZ-NOUS UN FICHIER
JE FAIS UN DON NOS LIVRES FAQ CONCOURS LOGIN
Hestia | Développé par ThemeIsle

Implémentation de La Boucle D'asservissement D'un Drone Bicopter Avec Arduino - Cours - Projets Divers

Transféré par

Informations du document

Titre original

Copyright

Formats disponibles

Partager ce document

Partager ou intégrer le document

Options de partage

Avez-vous trouvé ce document utile ?

Ce contenu est-il inapproprié ?

Droits d'auteur :

Formats disponibles

Implémentation de La Boucle D'asservissement D'un Drone Bicopter Avec Arduino - Cours - Projets Divers

Transféré par

Droits d'auteur :

Formats disponibles

30/12/2023 10:03 Implémentation de la Boucle d’Asservissement d’un Drone Bicopter avec Arduino – Cours | Projets Divers

“ Découvrez notre Chaîne YouTube "Ingénierie et Projets"

 Obtenir le fichier PDF

Câblage et initialisation du Module MPU-6050 GY-521

‘ Veuillez consulter l’article dédié au module pour plus de détails

Définition de l’adresse du module et LSB du Gyroscope

1 #define MPU 0x68

Configuration de l’Horloge interne à 8MHz

Wire.begin(); // Début transmission

Wire.beginTransmission(MPU); // Adresse du Module

Wire.write(0x6B); // Adresse du registre de control

Wire.endTransmission(true); // Fin de transmission

delay(10); // Retard avant la nouvelle transmission

Configuration de la sensibilité du Gyroscope à +/- 500°/s

Configuration de la sensibilité d’Accéléromètre à : +/- 8g

Configuration de la bande passante du Filtre passe-bas

Initialisation du port série (Vitesse >2500000 afin de réduire la

Affectation du pin au servomoteur

La mesure de l’angle par le Gyro

Calcul des valeurs réelles (division par la sensibilité)

‘ F(n)=[ f(n-1) + 4*f(n) + f(n+1)]*Te/6

Mise à jour des échantillons dans les trois axes

Conversion des angles du Radian en degré

‘ La période d’intégration est l’inverse de la période d’échantillonnage! Elle doit

1 void AngleGyro(long *GyAccTempp, double *GyPitchRol, double Te_s)

XDA -> NC (non connecté)

Servo M1, M2;

double x_nn=0.0; // Consigne de l'Angle à l'équilibre (Fixe: 0°)!

// Variables internes du système

// Variables internes du correcteur

// Init module GY-512

// 2. Gryro +/- 500°/s

// 4. Filtre passe-bas (Gyro/Accelo): 260Hz/ <1ms Delay

// Init port série

// Affectation du pin au servomoteur

// 2.1 Lecture du capteur: Accélo + Temp + Gyro

// 2.2 Calcul des Angles (Pitch / Roll / Yaw)

// 2.3 Affectation de la sortie du capteur

// 2.4 Filtrage de la mesure du capteur (Moyenne glissante)

Cap[j]=y_capt; j++; j%=N;

// 3. Soustracteur: Calcul de l'erreur eps(n)

// 4. Correcteur P (voir le lien en commentaire)

// 5. Définition de consigne de la vitesse V0 (0(0%, 180(100%))

// 6.1 Mise à jour de la vitesse

// 6.2. Réglage du Problème de Saturation et Valeurs <0

// 6.3 Affection des vitesses aux Moteurs

// Affichage: Vérification du Module GY-512 (A commenter)

// Lecture des données des capteurs sans Corrections

// Lecture des données des capteurs avec Corrections

// Lecture des données (3 axes accéléromètre + température + 3 axes gy

//Conversion des données accéléromètre en pitch/roll/yaw

PitchRol[0] = atan(x/X); // pitch

//Conversion Radian en degré

//Conversion des données du gyro en pitch/roll/yaw

// 1. Calcul des valeurs réelles

// 2.1 Integration - Simpson: F(n)=[ f(n-1) + 4*f(n) + f(n+1)]*Te/6

// 2.2 Mise à jour des échantillons

// 3. Conversion des angles du Radian en degré

void AngleGyroNC52(long *GyAccTempp, double *GyPitchRol, double Te_s)

// 1. Calcul des valeurs réelles

// 2.1 Integration - Formule NC-5-2 d’ordre 7

// 2.2 Mise à jour des échantillons

// 3. Conversion des angles du Radian en degré

‘ F(n)=[ f(n-1) + 4f(n) + f(n+1)]Te/6

1 void AngleGyro(long GyAccTempp, double GyPitchRol, double Te_s)

// 2.1 Integration - Simpson: F(n)=[ f(n-1) + 4f(n) + f(n+1)]Te/6

void AngleGyroNC52(long GyAccTempp, double GyPitchRol, double Te_s)

const double b0=(a1/(2.0T))+(a2/(TT));

double CorrPD_AP(double x_nn, double x_cc, double y_cc, double a, doub