He estado trabajando en un proyecto reciente que involucra múltiples IMU (específicamente 5 MPU6050, GY-521 Breakout board). Todo funciona bien, desde el cálculo del ángulo hasta el manejo de la deriva giroscópica para DOS sensores. El problema viene cuando agrego un tercer sensor.
Básicamente, lo que sucede es que cuando se imprimen los valores X / Y (en el Monitor Serial) de dos sensores (S1 y S2) funcionan bien, pero cuando se incluye un tercer sensor (S3), los valores de ángulo de S1 completamente espejo para los valores de S3. Realicé varias pruebas (cortocircuitos, software / hardware, cambio / cambio de sensores, cambio de espera y dirección de lectura, etc.) pero los resultados son los mismos.
Cuando ejecuto la función llamada "ThirdGyro", incluso sin imprimirla, es cuando las cosas se vuelven locas. Estoy usando el Demux "CD4051" para cambiar entre los sensores.
El MPU6050 tiene 2 direcciones en las que se puede leer, 0x68 - Predeterminado y 0x69. La forma en que lo hago para 3 sensores es mediante la lectura de los valores del sensor que tiene su pin AD0 HIGH, (0x69) que he establecido como la dirección de lectura (0x68 como modo de espera / no de lectura). Con la salida común (3) configurada alta en el CD4051, y todas las patillas ADO de la IMU conectadas a sus respectivas E / S, mediante la demuxación puedo aislar cada sensor y hacer que lea.
No ejecuté los pines SCL y SDA de la IMU, a través de Demux solo porque pensé que era una molestia. También chicos esta es mi primera pregunta :)
//Necessary Libraries
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 accelgyroSB(0x68); // SB - Standby Gyro, Not Read
MPU6050 accelgyroR(0x69); // R - Reading gyro, AD0 = H then being Read
//Global Gyro/Accel Variables for RAW DATA
int16_t gx, gy, gz, gsx, gsy, gsz, ax, ay, az, asx, asy, asz;
double timeStep, time, timePrev;
double arx, ary, arz, grx, gry, grz, rx, ry, rz;
int i;
//Filtered Gyro/Accel Variables for Sensors: r[Axis] [Sensor#]
int rx1, rx2, rx3, ry1, ry2, ry3, rz1, rz2, rz3;
void setup(){
Wire.begin(); //Join 12c bus with lib
Serial.begin(9600);
accelgyroR.initialize();
accelgyroSB.initialize();
//Initialization of Gyroscope
time = millis();
i = 1;
pinMode(3, OUTPUT); //Select Channel Input A
pinMode(4, OUTPUT); //Select Channel Input B
pinMode(5, OUTPUT); //Select Channel Input C
pinMode(6, OUTPUT); //Common Output for HIGH
digitalWrite(6, HIGH);
//Connection Check
Serial.println("Preliminary Connection Check..");
Serial.println(accelgyroR.testConnection() ? "MPU6050 #1 connection
successful" : "Connection Failed");
}
void AngleCalc(){ //Universal Angle Calculation with Complimentary Filter
timePrev = time;
time = millis();
timeStep = (time - timePrev) / 1000; //Step in seconds
//Gathering Raw Gyroscope Data of Selected Gyro
accelgyroR.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
//Scaling Raw data with 131(Gyro) and 2048(Accelerometer) scale factors
from Datasheet, s=scaled
gsx = gx / 131;
gsy = gy / 131;
gsz = gz / 131;
asx = ax / 2048;
asy = ay / 2048;
asz = az / 2048;
//Accelerometer angle calculation
arx = (180/3.141592) * atan(asx / sqrt(square(asy) + square(asz)));
ary = (180/3.141592) * atan(asy / sqrt(square(asx) + square(asz)));
arz = (180/3.141592) * atan(sqrt(square(asy) + square(asx)) / asz);
if (i == 1){
grx = arx;
gry = ary;
grz = arz;
}
else{
grx = grx + (timeStep * gsx);
gry = gry + (timeStep * gsy);
grz = grz + (timeStep * gsz);
}
//Cocktail of Gyro and Accelerometer data ratioed ;)
rx = (0.96 * arx) + (0.04 * grx);
ry = (0.96 * ary) + (0.04 * gry);
rz = (0.96 * arz) + (0.04 * grz);
}
void FirstGyro(){
digitalWrite(3, LOW); //A ADDRESS : 000
digitalWrite(4, LOW); //B
digitalWrite(5, LOW); //C
AngleCalc();
rx1 = rx;
ry1 = ry;
rz1 = rz;
delay(20);
}
void SecondGyro(){
digitalWrite(3, HIGH); //A ADDRESS : 001
digitalWrite(4, LOW); //B
digitalWrite(5, LOW); //C
AngleCalc();
rx2 = rx;
ry2 = ry;
rz2 = rz;
delay(20);
}
void ThirdGyro(){
digitalWrite(3, LOW); //A ADDRESS : 010
digitalWrite(4, HIGH); //B
digitalWrite(5, LOW); //C
AngleCalc();
rx3 = rx;
ry3 = ry;
rz3 = rz;
delay(20);
}
void loop(){
//MUXING THE AD0 PIN SERVES AS IDENTIFYING UNIQUE SENSORS
//BELOW CODE REFERS TO ABOVE RESPECTIVE FUNCTIONS OF RESPECTIVE SENSORS
FirstGyro();
delay(10);
SecondGyro();
delay(10);
ThirdGyro();
Serial.println("");
Serial.print(ry1); Serial.print("\t");
Serial.print(ry2); Serial.print("\t");
Serial.print(ry3);
//Print out filtered data
}