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, al imprimir los valores X/Y (en el monitor en serie) de dos sensores (S1 y S2), funcionan bien, pero cuando se incluye un tercer sensor (S3), los valores de ángulo de S1 reflejan completamente para los valores de S3. Realicé varias pruebas (cortos, software/hardware, intercambio/cambio de sensores, intercambio 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 leyendo los valores del sensor que tiene su pin AD0 configurado en ALTO (0x69) que configuré como la dirección de lectura (0x68 en espera/no leído). Con la salida común (3) configurada en lo alto del CD4051, y todos los pines ADO de la IMU conectados a sus respectivas E/S, mediante la demultiplexación puedo aislar cada sensor y hacer que se lea.
No ejecuté los pines SCL y SDA de la IMU, a través del 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
}
simular este circuito : esquema creado con CircuitLab
Su software implica que los pines 3, 4 y 5 de Arduino están conectados al CD4051, pero su esquema no muestra esto. ¿Estás seguro de que esas conexiones son correctas? ¿Puedes verificar que las tres líneas AD0 están cambiando de la manera que deseas? El CD4051 es un interruptor analógico , lo que significa que los pines deseleccionados son de circuito abierto . ¿Está utilizando resistencias desplegables para asegurarse de que las líneas AD0 se vean como "0" lógicos cuando no están seleccionadas?
La resistencia pulldown debe ser lo suficientemente pequeña para que pueda superar cualquier corriente de fuga que intente elevar el pin, y también descargar la capacitancia del nodo razonablemente rápido, pero no tan pequeña como para que la unidad que pasa por el CD4051 no pueda. superar la resistencia. Para la lógica CMOS, los valores en el rango de 10K a 100K suelen ser buenos.
simular este circuito : esquema creado con CircuitLab
Como alternativa, podría considerar cambiar a un decodificador que no trimite sus salidas, como un 74HC138. Este chip en particular tiene salidas bajas activas, lo que significa que tendría que cambiar la dirección que considera que es la dirección "activa" en su firmware.
Hay un problema secundario más que no puedo resistir comentar. Ha escrito su software utilizando exclusivamente variables globales. Supongo que su experiencia en programación es principalmente en el lenguaje BASIC. Esto está bien para proyectos pequeños, pero te meterá en problemas con algo más complejo. En el lenguaje C y sus derivados, querrá aprender cómo usar variables locales dentro de las funciones y cómo usar parámetros y devolver valores para pasar datos dentro y fuera de las funciones sin usar variables globales.
wesley lee
A.Gomes
david tweed
A.Gomes
david tweed