Je travaille sur un système avec 2 mpu (calcul valeurs entrée et sortie) et un servomoteur.
Il calcul l'angle du mpu 1 (valeur entrée) et renvoie l'angle inverse au servomoteur pour compenser l'angle. Le mpu 2 calcul alors l'angle de sorti (posé sur une pièce au bout du servomoteur supposé être le plus nul possible).
//Libraries
#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Servo.h>
//Objects
Adafruit_MPU6050 mpu1;
Adafruit_MPU6050 mpu2;
Servo servo;
int temps;
float angx1, angy1, angz1;
float angx2, angy2, angz2;
float acc_total_vector1, acc_total_vector2;
int val;
int preVal;
void setup() {
//Init Serial USB
Serial.begin(9600);
servo.attach(6);
Serial.println(F("Initialize System"));
if (!mpu1.begin(0x68) && !mpu2.begin(0x69)) { // Change address if needed
Serial.println("Failed to find MPU6050 chip");
while (1) {
delay(10);
}
}
mpu1.setAccelerometerRange(MPU6050_RANGE_16_G);
mpu2.setAccelerometerRange(MPU6050_RANGE_16_G);
mpu1.setGyroRange(MPU6050_RANGE_250_DEG);
mpu2.setGyroRange(MPU6050_RANGE_250_DEG);
mpu1.setFilterBandwidth(MPU6050_BAND_21_HZ);
mpu2.setFilterBandwidth(MPU6050_BAND_21_HZ);
}
void loop() {
readMPU();
delay(0);
}
void readMPU( ) { /* function readMPU */
////Read acceleromter data
sensors_event_t a1, g1, temp1;
sensors_event_t a2, g2, temp2;
mpu1.getEvent(&a1, &g1, &temp1);
mpu2.getEvent(&a2, &g2, &temp2);
acc_total_vector1 = sqrt(pow(a1.acceleration.x, 2) + pow(a1.acceleration.y, 2) + pow(a1.acceleration.z, 2));
// To prevent asin to produce a NaN, make sure the input value is within [-1;+1]
if (abs(a1.acceleration.x) < acc_total_vector1) {
angx1 = asin((float)a1.acceleration.y / acc_total_vector1) * (180 / PI) - 11.70; // asin gives angle in radian. Convert to degree multiplying by 180/pi
}
if (abs(a1.acceleration.y) < acc_total_vector1) {
angy1 = asin((float)a1.acceleration.x / acc_total_vector1) * (180 / PI);
}
acc_total_vector2 = sqrt(pow(a2.acceleration.x, 2) + pow(a2.acceleration.y, 2) + pow(a2.acceleration.z, 2));
// To prevent asin to produce a NaN, make sure the input value is within [-1;+1]
if (abs(a2.acceleration.x) < acc_total_vector2) {
angx2 = asin((float)a2.acceleration.y / acc_total_vector2) * (180 / PI) - 11.70; // asin gives angle in radian. Convert to degree multiplying by 180/pi
}
if (abs(a2.acceleration.y) < acc_total_vector2) {
angy2 = asin((float)a2.acceleration.x / acc_total_vector2) * (180 / PI);
}
val = angx1;
if (val != preVal)
{
servo.write(val);
preVal = val;
}
/* Print out the values */
Serial.print(millis());
Serial.print("/");
Serial.print(angx1);
Serial.print("/");
Serial.println(angx2);
}
Je rencontre deux problèmes :
lorsque j'affiche les valeurs d'angles sur l'axe x pour le mpu 1 et le mpu 2, les deux valeurs correspondent au mpu 1 ; les deux valeurs sont pourtant bien séparées dans mon code
mon servo-moteur reçoit les valeurs de l'angle du mpu 1 mais il ne tourne que sur une plage de environ 45 degrés
A savoir que mon servomoteur fonctionne correctement avec d'autres codes.
Si MPU1 est statique et que MPU2 bouge est-ce que les courbes sont pertinentes?
Si MPU2 est statique et que MPU1 bouge est-ce que les courbes sont pertinentes?
Les adresses de MPU1 et MPU2 sont bien programmées sur les modules pour correspondre aux adresses déclarées dans le code?
Un petit coup de I2Cscanner permet de le vérifier.
Si la condition n'est pas remplie, les angles ne sont pas calculés et tu ne le sais pas.
Il faudrait peut-être mettre des else avec soit affichage d'un message, soit retour de la fonction avec un booleen qui indique une erreur.
D'après la doc du MPU la broche AD0 doit être soit à GND, soit à VCC
The LSB bit of the 7 bit address is determined by the logic level on pin AD0. This allows two MPU-60X0s to be connected to the same I2C bus.
When used in this configuration, the address of the one of the devices should be b1101000 (pin AD0 is logic low) and the address of the other should be b1101001 (pin AD0 is logic high).
Si tu regardes avec un voltmètre sur la broche AD0, sur un module elle devrait être à 0 et sur l'autre à 3,3V