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
#define MPU 0x68
#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
Wire.beginTransmission(MPU);
Wire.write(0x1B);
Wire.write(0x08);
Wire.endTransmission();
delay(10);
Configuration de la sensibilité d’Accéléromètre à : +/- 8g
Wire.beginTransmission(MPU);
Wire.write(0x1C);
Wire.write(0x10);
Wire.endTransmission();
delay(10);
Configuration de la bande passante du Filtre passe-bas (Gyro/Accelo): 260Hz/ <1ms Delay
Wire.beginTransmission(MPU);
Wire.write(0x1A);
Wire.write(0x00);
Wire.endTransmission();
delay(10);
Initialisation du port série (Vitesse >2500000 afin de réduire la latence de transmission)
Serial.begin(250000);
Affectation du pin au servomoteur
M1.attach(PinM1);
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).
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é)
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!
// 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) ; }
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.
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'utilisateur: Télécommande) 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
// 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);
// 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.beginTransmission(MPU); Wire.write(0x3B); Wire.endTransmission(false); Wire.requestFrom(MPU,14,true);
// 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) ; }
