|
#include "Wire.h" // Librairie Permettant d'utiliser la communication I2C
|
|
|
|
const int MPU_ADDR = 0x68; // Addresse I2C du module MPU-6050. SI AD0 est relié a Vcc, l'addresse devient 0x69.
|
|
int16_t accelerometer_x, accelerometer_y, accelerometer_z; // Valeur des accelerations retournées par le MPU-6050
|
|
char tmp_str[7]; // Variable temporaire pour la convertion
|
|
|
|
float alpha, beta; // Angles mesurés en degrés
|
|
|
|
//Fonction permettant la convertion d'entier codé sur 16 bits en string
|
|
char* convert_int16_to_str(int16_t i)
|
|
{
|
|
sprintf(tmp_str, "%6d", i);
|
|
return tmp_str;
|
|
}
|
|
|
|
void setup()
|
|
{
|
|
Serial.begin(9600);
|
|
|
|
Wire.begin();
|
|
Wire.beginTransmission(MPU_ADDR); // Debut de la transimission avec le MPU-6050
|
|
Wire.write(0x6B); // Envoie de l'adresse du registre PWR_MGMT_1
|
|
Wire.write(0); // Actif en continue, Aquisition des mesures en continue, Choix de l'horloge interne pour le fonctionnement du MPU-6050
|
|
Wire.endTransmission(true); // Fin de transimision
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
// Debut de communication et envoie de la demande d'information
|
|
Wire.beginTransmission(MPU_ADDR); // Debut de la transimission avec le MPU-6050
|
|
Wire.write(0x3B); // On commence par pointer le registre 0x3B. Registre contenant les 8 bits de poids fort de l'acceleration sur l'axe x du MPU-6050 [MPU-6050 Register Map and Descriptions Revision 4.2, p.7]
|
|
Wire.endTransmission(false); // Fin de transmission et attente de la reponse de la par de l'eclave (MOU-6050). La connection est maintenu active.
|
|
Wire.requestFrom(MPU_ADDR, 3*2, true); // Lecture de 6 registres 8bits. Chaque acceleration mesurée est codée sur 16 bits, d'où 3*2.
|
|
// La lecture des registres se fait automatiquement. Les registres renvoyés se suivent.
|
|
// Il suffit d'indiquer l'adresse du premier et les autres suivent en fonction du nombre de registre demandés
|
|
|
|
// Re assemblage des registres remandés pour reconsitué les informations.
|
|
// "Wire.read()<<8 | Wire.read();"
|
|
// Comme les 8 bits de poids fort sont envoyés en premier, il est necessaire de faire un decalage pour les remettre a la bonne place.
|
|
// Ensuite il suffit d'ajouter les 8 bits de poids faible.
|
|
|
|
accelerometer_x = Wire.read()<<8 | Wire.read(); // Lecture des registres 0x3B (ACCEL_XOUT_H) et 0x3C (ACCEL_XOUT_L)
|
|
accelerometer_y = Wire.read()<<8 | Wire.read(); // Lecture des registres 0x3D (ACCEL_YOUT_H) et 0x3E (ACCEL_YOUT_L)
|
|
accelerometer_z = Wire.read()<<8 | Wire.read(); // Lecture des registres 0x3F (ACCEL_ZOUT_H) et 0x40 (ACCEL_ZOUT_L)
|
|
|
|
// Calcul des angles
|
|
// Alpha pour l'angle autour de l'axe x du MPU-6050
|
|
alpha = atan((float)accelerometer_y/(float)accelerometer_z)*(180/PI); // Le coefficient 180/PI permet d'obtenir alpha en degrés
|
|
// Beta pour l'angle autour de l'axe y du MPU-6050
|
|
beta = atan((float)accelerometer_x/(float)accelerometer_z)*(180/PI); // Le coefficient 180/PI permet d'obtenir beta en degrés
|
|
|
|
// Affichage des angles mesurés
|
|
Serial.print("Alpha = ");
|
|
Serial.print(alpha,0);
|
|
Serial.print(" | Beta = ");
|
|
Serial.println(beta,0);
|
|
|
|
// Actualisation des mesures toutes les 50ms
|
|
delay(50);
|
|
}
|