MPU605; Cuadricoptero

[albenegra]

Muy buenas a todos, es mi primer post pero este foro me a ayudado muchisimas veces.

Estoy montando un cuadricoptero en arduino y tengo un MPU6050, necesito ayuda me veo en un tunel sin salida.
Se leer los valores, se que tengo que hacer un filtro KALMAN (duda: para los 3 ejes o solo X e Y) y despues el PID que es donde me pierdo se para que sirve pero ni se hacerle ni encuentro un codigo mas o menos apto y nose como atacar a las ESC despues.
Mis motores son de 1200Kv y las ESC hasta 25A nose si necesitais mas datos. este es nuestro bichillo  

El codigo KALMAN que e probado:
https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/MPU6050

Si almenos me podeis ayudar con algo en plan mira con los valores del kalman haces tal en el PID y despues lo que sea aunque sea poca cosa  

gracias de antemano  

PD: Espero no haber incumplido ninguna norma en el post jeje

[miquel]

Hola Albenegra!

Te recomiendo que utilices el Filtro Complementario en lugar del Kalman y es muy simple de implementar http://web.mit.edu/scolton/www/filter.pdf
Hay mucha información al respecto en la web, por ejemplo http://robottini.altervista.org/kalman-filter-vs-complementary-filter
El filtro Kalman y el complementario sirven para "unir" las informaciones del giro y el acelerometro, para el eje yaw solo tienes el giro y por lo tanto no necesitas el filtro.
El pid es lo más difícil de implementar en un quad y sobre todo lo más difícil de de ajustar. En la web hay mucho código especialmente para Arduino, por ejemplo http://aeroquad.com/content.php
Los ESC se controlan como un servo, necesitaras un pic con 4 pwm.


Saludos

Miquel

[albenegra]

Lo de las ESC esta mas o menos controlado.
¿Se podria decir que la funcion del PID es como una especie de joystick autonomo?

Voy a mirar a fondo las webs que me has puesto, aunque ya e buscado por mil lados tambien jaja
muchas gracias miquel!

[elgarbe]

En principio te recomiendo leer el post sobre acelerómetro y el de giróscopos en este mismo sub foro. Ahí explico más o menos para que sirve cada uno, como se usa y la necesidad de fusionar los datos.

Es casi imposible que puedas desarrollar un sistema de control completo, implementarlo y que funcione. A no ser que tengas mucha experiencia.
Yo estoy en el mismo camino, pero estoy organizado de la siguiente forma:
1° Leer los datos de los sensores (acelerómetro, giróscopo y magnetómetro). En tu caso no tienes magnetómetro.
2° Probar algoritmos de fusion de datos, ya sea kalman, filtro complementario u otros.
3° Controlar 1 ESC con 1 motor, haciendo un PWM sobre la entrada del ESC.
4° Montar 1 motor con 1 hélice en un brazo que pivotee en un extremo para tratar de mantenerlo de forma horizontal.
5° Implementar un PID que reciba como entrada el ángulo del brazo deseado y en funcion de las constantes del PID, de los sensores y de la respuesta del brazo module el PWM para variar la velocidad del motor y que este gire el ángulo deseado.
6° Montar 2 motores uno en cada extremo de uno de los brazos del quad y poner un pibot en el centro.
7° Implementar el PID sobre los dos motores para equilibrar el brazo o hacerlo girar a gusto.
8° Leer los comandos del transmisor de radio control, leyendo las salidas del receptor.
9° Comandar el angulo de giro del brazo con solo 2 motores con el radio control.
10° Implementar un algoritmo tipo DCM que permita saber con exactitud la rotacion respecto de la tierra que tiene el Quad.
11° Armar el quad completo y probar todo eso.

Como veras hay como mínimo 11 pasos para mí antes de poder probar algo. En tu caso me parece que te entusiasmaste un poco y te adelantaste unos cuantos pasos... Yo voy a seguir publicando los avances de mi controladora. Tambien puedes revisar estos links:

https://sites.google.com/site/kuadricoptero/home
https://sites.google.com/site/mikuadricoptero/home
http://www.starlino.com/quadcopter_acc_gyro.html

Cualquier duda estamos por aquí para ayudate!

Saludos

[albenegra]

Algo asi es lo que buscaba, mil gracias.
Debo tenerlo montado en cosa de 1 mes al menos que suba y baje crees que se puede o estoy jodido jaja por eso creo que intente ir tan de golpe, me lo tomare con mas calma y seguire los consejos que me dais

Un saludo ! 

[elgarbe]

Si le dedicas muchas horas diarias y solo tiene que levantar y bajar y tienes buenos conocimientos en programación podrías obtener algo...
Ya tienes seleccionado el micro que usará? el distema de radio control? si querés publicá lo que tenes y vemos si te podemos ayudar.

Saludos!

[PCCM]

Miguel y leonardo ya te dijeron todo, podria aportar a tu pregunta de que tienes razón de que  el PID sería como un joyistick autónomo, la sintonización por prueba y error te simplificará mucho el tiempo.He intenta como te dicen, el filtro complementario ya que para la aplicación del quad es mas que suficiente y lo aplicarías solo para los ejes X e Y, ya que para el eje Z necesitarías otro sensor(como dice leonardo, el magnetómetro) ya que el gyro solo te puede dar errores exagerados después de varios minutos.

Saludos, y espero que sigas compartiendo tu proyecto.

[albenegra]

Muchas gracias a todos enserio 

Tenemos (somos dos es un proyecto de clase) el modulo de RF joystick y dos pulsadores mas o menos controlados con una placa de fabricacion propia que hemos echo hoy, falta cargar el bootloader y ver que funciona. Ya hemos mandado datos.

En la parte del cuadricoptero; Hoy hemos construido un balancin para probar codigo y demas giroscopio-ESC en cuanto tenga datos yo posteo
Aunque contamos con la ayuda de los profesores no son expertos en este tema en concreto y lo malo, solo podemos trabajar en clase aunque son 6h.

Un saludo a todos   

[elgarbe]

Hola, quería comentarte que he adquirido una placa de prueba con el MPU6050 asi que si necesitas que probemos algo me avisas.

Recuerda que si quieres poner el código que estas testeando siempre es bienvenido!

Saludos!

[albenegra]

He probado algo sencillo con el PID y bueno no a sido algo muy ideal pero a sido un comienzo.
Cuando tengo un codigo mas o menos funcional hos lo pondre sin dudarlo porque de momento voy un poco lento jeje

Otra cosa, hasta que punto me sirve un modulo RF para mi cuadricoptero esque todo el mundo usa mandos comerciales 

Que tal te a ido con la nueva adquisicion elgarbe

[albenegra]

Este es el mejor codigo que tengo hasta ahora, los palores PID estan sin ajustar pero la verdad estabiliza 'bien'
Esta echo apartir del codigo para el filtro KALMAN que encontre jeje
Os pasare un video con este codigo en el cuadricoptero 

Código: [Seleccionar]

#include <PID_v1.h>
#include <Wire.h>
#include "Kalman.h" // Source: https://github.com/TKJElectronics/KalmanFilter
#include <Servo.h>

#define PINMOTOR 8      //pin conectado al ESC
#define MAXIMOPWM 160   
#define MINIMOPWM 30   
#define PINMOTOR2 9

Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;
float inverOutput;
int X;
/* Motor Data */
int pulsoMotor = 30;
int pulsoMotor2 = 30;
Servo myservo; 
Servo myservo2;
 byte recibiendoByte ;
 boolean iniciado = false;
 boolean carga = false;
 
/* PID Data */
double Setpoint, Input, Output;    //Define Variables we'll be connecting to
PID myPID(&Input, &Output, &Setpoint,1,5,1, DIRECT);   //Specify the links and initial tuning parameters

/* IMU Data */
int16_t accX, accY, accZ;
int16_t gyroX, gyroY, gyroZ;

double accXangle, accYangle; // Angle calculate using the accelerometer
double gyroXangle, gyroYangle; // Angle calculate using the gyro
double compAngleX, compAngleY; // Calculate the angle using a complementary filter
double kalAngleX, kalAngleY; // Calculate the angle using a Kalman filter

uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data

void setup() {
  Serial.begin(115200);
  Setpoint = 180;

  myservo.attach(PINMOTOR);  // inicializo el ESC
  myservo2.attach(PINMOTOR2);
   Serial.println(" Comienzo del test");  //
   Serial.println (" Pulsar 'A' para arrancar  \n Cuando escuche el pitido de confirmación");
   while ( iniciado==false ){
          myservo.write(0);
          myservo2.write(0);   // Armado
          recibiendoByte = Serial.read(); // Leemos el Byte recibido
          if (recibiendoByte == 65 || recibiendoByte ==97) {    // A o a  Mayusculas o minusculas
              iniciado=true;
            }
}
   Serial.println (" Pulsar 'C' para cargar");

  while ( carga==false ){
          myservo.write(30);   // Armado
          myservo2.write(30);
          recibiendoByte = Serial.read(); // Leemos el Byte recibido
          if (recibiendoByte == 67 || recibiendoByte ==99) {    // A o a  Mayusculas o minusculas
              carga=true;
            }
}
  //turn the PID on
  myPID.SetMode(AUTOMATIC);
 // myPID.SetOutputLimits(90,179);
  myPID.SetSampleTime(50);
 
  Wire.begin();
  TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz

  i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
  i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
  i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
  i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
  while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
  while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode

  while (i2cRead(0x75, i2cData, 1));
  if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
    Serial.print(F("Error reading sensor"));
    while (1);
  }

  delay(100); // Wait for sensor to stabilize

  /* Set kalman and gyro starting angle */
  while (i2cRead(0x3B, i2cData, 6));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
  // We then convert it to 0 to 2π and then from radians to degrees
  accYangle = (atan2(accX, accZ) + PI) * RAD_TO_DEG;
  accXangle = (atan2(accY, accZ) + PI) * RAD_TO_DEG;

  kalmanX.setAngle(accXangle); // Set starting angle
  kalmanY.setAngle(accYangle);
  gyroXangle = accXangle;
  gyroYangle = accYangle;
  compAngleX = accXangle;
  compAngleY = accYangle;

  timer = micros();
}

void loop() {
  /* Update all the values */
  while (i2cRead(0x3B, i2cData, 14));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  gyroX = ((i2cData[8] << 8) | i2cData[9]);
  gyroY = ((i2cData[10] << 8) | i2cData[11]);
  gyroZ = ((i2cData[12] << 8) | i2cData[13]);

  // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
  // We then convert it to 0 to 2π and then from radians to degrees
  accXangle = (atan2(accY, accZ) + PI) * RAD_TO_DEG;
  accYangle = (atan2(accX, accZ) + PI) * RAD_TO_DEG;

  double gyroXrate = (double)gyroX / 131.0;
  double gyroYrate = -((double)gyroY / 131.0);
  gyroXangle += gyroXrate * ((double)(micros() - timer) / 1000000); // Calculate gyro angle without any filter
  gyroYangle += gyroYrate * ((double)(micros() - timer) / 1000000);
  //gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate
  //gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/1000000);

  compAngleX = (0.93 * (compAngleX + (gyroXrate * (double)(micros() - timer) / 1000000))) + (0.07 * accXangle); // Calculate the angle using a Complimentary filter
  compAngleY = (0.93 * (compAngleY + (gyroYrate * (double)(micros() - timer) / 1000000))) + (0.07 * accYangle);

  kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros() - timer) / 1000000); // Calculate the angle using a Kalman filter
  kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros() - timer) / 1000000);
  timer = micros();


  /* Print Data */
#if 0 // Set to 1 to activate
  Serial.print(accX); Serial.print("\t");
  Serial.print(accY); Serial.print("\t");
  Serial.print(accZ); Serial.print("\t");

  Serial.print(gyroX); Serial.print("\t");
  Serial.print(gyroY); Serial.print("\t");
  Serial.print(gyroZ); Serial.print("\t");
#endif
  //Serial.print(accXangle); Serial.print("\t");
  //Serial.print(gyroXangle); Serial.print("\t");
  //Serial.print(compAngleX); Serial.print("\t");
  Serial.print(kalAngleX); Serial.print("\t");

  Serial.print("\t");

  //Serial.print(accYangle); Serial.print("\t");
  //Serial.print(gyroYangle); Serial.print("\t");
  //Serial.print(compAngleY); Serial.print("\t");
  //Serial.print(kalAngleY); Serial.print("\t");


  Serial.print("\r\n");
  delay(1);

  // PID
  Input = kalAngleX;
    myPID.Compute();   
    pulsoMotor2= map (Output,0,255,90,94);
    pulsoMotor= 184 - pulsoMotor2 ;
    myservo.write(pulsoMotor);
    myservo2.write(pulsoMotor2);
   
  delay(250);
  Serial.print("Velocidad del pulso-1->  ");
  Serial.println (pulsoMotor);
  Serial.print("PID 11111 -->  ");
  Serial.println (inverOutput);
  Serial.print("Velocidad del pulso-2->  ");
  Serial.println (pulsoMotor2);
  Serial.print("PID 22222 -->  ");
  Serial.println (Output);
   
}


[elgarbe]

Muy bueno!

tendrás las librerías de tus #includes?

Saludos!

[albenegra]

Estan incluidas el codigo compila y funciona fisicamente.
Si te refieres a que te las pase puedo pasartelas sin problema

[elgarbe]

Claro, entiendo, lo que te decía es si quieres compartirlas con nosotros 

Sds.

[albenegra]

Las del MPU son estas:
https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU6DOF/MPU6050  

PID:
https://github.com/br3ttb/Arduino-PID-Library/zipball/master

Y las librerias servo.h y wire.h vienen en arduino

Saludos y espero haberte ayudado  


Por cierto, creo que el MPU6050 acumula errores, pero aun ni o he observado ni nada