Buenas, estoy queriendo controlar 4 servos MG 996R con 2 sensores MPU6050, para ellos uso las direcciones I2C 0x68 y 0x69, pero estoy teniendo problemas con el control de los servos, ya que con un solo sensor puedo usar las 3 rotaciones libremente y todo funciona bien, pero cuando quiero usar dos sensores, cada uno con su respectiva direccion I2C, ambos controlan los 4 servos por igual por algun motivo, no discriminan los servos cuando estan juntos, en cambio cuando quito un giroscopo del programa el giroscopo restante controla los servos que le indico a la perfección.
El codigo es este:
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
Adafruit_PWMServoDriver servos = Adafruit_PWMServoDriver(0x40);
const int MPU2 = 0x69, MPU1=0x68;
long accelX, accelY, accelZ;
float gForceX, gForceY, gForceZ, gyroX, gyroY, gyroZ,rotX, rotY, rotZ;
long accelX2, accelY2, accelZ2;
float gForceX2, gForceY2, gForceZ2;
//agregados
float Acc[3];
float Gy[3];
float acond[3];
unsigned int pos0=135; // ancho de pulso en cuentas para pocicion 0°
unsigned int pos180=635; // ancho de pulso en cuentas para la pocicion 180°
int servoX, servoY;
void setServo(uint8_t n_servo, int angulo) {
int duty;
duty=map(angulo,0,180,pos0, pos180);
servos.setPWM(n_servo, 0, duty);
}
void setup(){
servos.begin();
servos.setPWMFreq(60); //Frecuecia PWM de 60Hz o T=16,66ms
Wire.begin();
Wire.beginTransmission(MPU1);
Wire.write(0x6B);
Wire.write(0b00000000);
Wire.endTransmission();
Wire.beginTransmission(MPU1);
Wire.write(0x1B);
Wire.write(0x00000000);
Wire.endTransmission();
Wire.beginTransmission(MPU1);
Wire.write(0x1C);
Wire.write(0b00000000);
Wire.endTransmission();
Wire.begin();
Wire.beginTransmission(MPU2);
Wire.write(0x6B);
Wire.write(0b00000000);
Wire.endTransmission();
Wire.beginTransmission(MPU2);
Wire.write(0x1B);
Wire.write(0x00000000);
Wire.endTransmission();
Wire.beginTransmission(MPU2);
Wire.write(0x1C);
Wire.write(0b00000000);
Wire.endTransmission();
Serial.begin(9600);
}
void loop(){
GetMpuValue(MPU1,0);
Serial.print("\t ||| \t");
GetMpuValue(MPU2,1);
Serial.println("");
}
void GetMpuValue(const int MPU,int i){
Wire.beginTransmission(MPU);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(MPU,6);
while(Wire.available() < 6);
accelX = Wire.read()<<8|Wire.read();
accelY = Wire.read()<<8|Wire.read();
accelZ = Wire.read()<<8|Wire.read();
Wire.beginTransmission(MPU);
Wire.write(0x43);
Wire.endTransmission();
Wire.requestFrom(MPU,6);
while(Wire.available() < 6);
gyroX = Wire.read()<<8|Wire.read();
gyroY = Wire.read()<<8|Wire.read();
gyroZ = Wire.read()<<8|Wire.read();
Acc[0] = accelX / 16384.0;
Acc[1] = accelY / 16384.0;
Acc[2] = accelZ / 16384.0;
Gy[0] = gyroX / 131.0;
Gy[1] = gyroY / 131.0;
Gy[2] = gyroZ / 131.0;
//Aplicar el Filtro Complementario----AGREGADO DEL OTRO PROGRAMA---ELIMINA TEMBLEQUES
rotX = 0.98 *(rotX+Gy[0]*0.010) + 0.02*Acc[0];
rotY = 0.98 *(rotY+Gy[1]*0.010) + 0.02*Acc[1];
rotZ = 0.98 *(rotZ+Gy[2]*0.010) + 0.02*Acc[2];
acond[0]=map(rotX,-10,10,0,180);
acond[1]=map(rotY,-10,10,0,180);
acond[2]=map(rotZ,-10,10,0,180);
Serial.print("X=\t");
Serial.print(acond[0]);
Serial.print("\t");
Serial.print("Y=\t");
Serial.print(acond[1]);
Serial.print("\t");
Serial.print("Z=\t");
Serial.print(acond[2]);
servoX = 2*i;
servoY = (2*i)+1;
setServo(servoX,acond[0]);
setServo(servoY,acond[1]);
delay(50);
}
Ahora de esta forma ambos giroscopos controlan los 4 servos, luego, si comento, por ejemplo, las siguientes lineas, deshabilitando la llamada a un giroscopo, el giroscopo restante ya no controla los 4 servos, sino que controla los servos que se le indice, entonces en este ejemplo el MPU2 controla los servos 2 y 3.
//GetMpuValue(MPU1,0);
Serial.print("\t ||| \t");
GetMpuValue(MPU2,1);
Serial.println("");
Ahora si comento la otra linea, el MPU2 estara deshabilitado y el MPU1 controlara solo los servos 0 y 1:
GetMpuValue(MPU1,0);
Serial.print("\t ||| \t");
\\GetMpuValue(MPU2,1);
Serial.println("");
Mi objetivo es poder controlar dos servos con un giroscopo y dos servos con otro.
Adjunto las conexiones, las cuales son sencillas y pocas.
En cuanto a las conecciones de los MPU conecte la alimentacion Vcc de uno al pin AD0 del giroscopo para que ese giroscopo tenga la direccion 0X69, en cambio el giroscopo que esta alimentado en su pin Vcc tiene la direccion 0X68.
