// I2Cdev and MPU6050 must be installed as libraries  
   #include "I2Cdev.h"#  
 include "MPU6050.h"#  
 include "Wire.h"  
 ///////////////////////////////////  CONFIGURATION  /////////////////////////////  
 //Change this 3 variables if you want to fine tune the skecth to your needs.  
 int buffersize = 1000; //Amount of readings used to average, make it higher to get more precision but sketch will be slower (default:1000)  
 int acel_deadzone = 8; //Acelerometer error allowed, make it lower to get more precision, but sketch may not converge (default:8)  
 int giro_deadzone = 1; //Giro error allowed, make it lower to get more precision, but sketch may not converge (default:1)  
 // default I2C address is 0x68  
 // specific I2C addresses may be passed as a parameter here  
 // AD0 low = 0x68 (default for InvenSense evaluation board)  
 // AD0 high = 0x69  
 //MPU6050 accelgyro;  
 MPU6050 accelgyro(0x68); // <-- use for AD0 high  
 int16_t ax, ay, az, gx, gy, gz;  
 int mean_ax, mean_ay, mean_az, mean_gx, mean_gy, mean_gz, state = 0;  
 int ax_offset, ay_offset, az_offset, gx_offset, gy_offset, gz_offset;  
 ///////////////////////////////////  SETUP  ////////////////////////////////////  
 void setup() {  
   // join I2C bus (I2Cdev library doesn't do this automatically)  
   Wire.begin();  
   // COMMENT NEXT LINE IF YOU ARE USING ARDUINO DUE  
   TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz). Leonardo measured 250kHz.  
   // initialize serial communication  
   Serial.begin(115200);  
   // initialize device  
   accelgyro.initialize();  
   // wait for ready  
   while (Serial.available() && Serial.read()); // empty buffer  
   while (!Serial.available()) {  
     Serial.println(F("Send any character to start sketch.n"));  
     delay(1500);  
   }  
   while (Serial.available() && Serial.read()); // empty buffer again  
   // start message  
   Serial.println("nMPU6050 Calibration Sketch");  
   delay(2000);  
   Serial.println("nYour MPU6050 should be placed in horizontal position, with package letters facing up. nDon't touch it until you see a finish message.n");  
   delay(3000);  
   // verify connection  
   Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");  
   delay(1000);  
   // reset offsets  
   accelgyro.setXAccelOffset(0);  
   accelgyro.setYAccelOffset(0);  
   accelgyro.setZAccelOffset(0);  
   accelgyro.setXGyroOffset(0);  
   accelgyro.setYGyroOffset(0);  
   accelgyro.setZGyroOffset(0);  
 }  
 ///////////////////////////////////  LOOP  ////////////////////////////////////  
 void loop() {  
   if (state == 0) {  
     Serial.println("nReading sensors for first time...");  
     meansensors();  
     state++;  
     delay(1000);  
   }  
   if (state == 1) {  
     Serial.println("nCalculating offsets...");  
     calibration();  
     state++;  
     delay(1000);  
   }  
   if (state == 2) {  
     meansensors();  
     Serial.println("nFINISHED!");  
     Serial.print("nSensor readings with offsets:t");  
     Serial.print(mean_ax);  
     Serial.print("t");  
     Serial.print(mean_ay);  
     Serial.print("t");  
     Serial.print(mean_az);  
     Serial.print("t");  
     Serial.print(mean_gx);  
     Serial.print("t");  
     Serial.print(mean_gy);  
     Serial.print("t");  
     Serial.println(mean_gz);  
     Serial.print("Your offsets:t");  
     Serial.print(ax_offset);  
     Serial.print("t");  
     Serial.print(ay_offset);  
     Serial.print("t");  
     Serial.print(az_offset);  
     Serial.print("t");  
     Serial.print(gx_offset);  
     Serial.print("t");  
     Serial.print(gy_offset);  
     Serial.print("t");  
     Serial.println(gz_offset);  
     Serial.println("nData is printed as: acelX acelY acelZ giroX giroY giroZ");  
     Serial.println("Check that your sensor readings are close to 0 0 16384 0 0 0");  
     Serial.println("If calibration was succesful write down your offsets so you can set them in your projects using something similar to mpu.setXAccelOffset(youroffset)");  
     while (1);  
   }  
 }  
 ///////////////////////////////////  FUNCTIONS  ////////////////////////////////////  
 void meansensors() {  
   long i = 0, buff_ax = 0, buff_ay = 0, buff_az = 0, buff_gx = 0, buff_gy = 0, buff_gz = 0;  
   while (i < (buffersize + 101)) {  
     // read raw accel/gyro measurements from device  
     accelgyro.getMotion6( & ax, & ay, & az, & gx, & gy, & gz);  
     if (i > 100 && i <= (buffersize + 100)) { //First 100 measures are discarded  
       buff_ax = buff_ax + ax;  
       buff_ay = buff_ay + ay;  
       buff_az = buff_az + az;  
       buff_gx = buff_gx + gx;  
       buff_gy = buff_gy + gy;  
       buff_gz = buff_gz + gz;  
     }  
     if (i == (buffersize + 100)) {  
       mean_ax = buff_ax / buffersize;  
       mean_ay = buff_ay / buffersize;  
       mean_az = buff_az / buffersize;  
       mean_gx = buff_gx / buffersize;  
       mean_gy = buff_gy / buffersize;  
       mean_gz = buff_gz / buffersize;  
     }  
     i++;  
     delay(2); //Needed so we don't get repeated measures  
   }  
 }  
 void calibration() {  
   ax_offset = -mean_ax / 8;  
   ay_offset = -mean_ay / 8;  
   az_offset = (16384 - mean_az) / 8;  
   gx_offset = -mean_gx / 4;  
   gy_offset = -mean_gy / 4;  
   gz_offset = -mean_gz / 4;  
   while (1) {  
     int ready = 0;  
     accelgyro.setXAccelOffset(ax_offset);  
     accelgyro.setYAccelOffset(ay_offset);  
     accelgyro.setZAccelOffset(az_offset);  
     accelgyro.setXGyroOffset(gx_offset);  
     accelgyro.setYGyroOffset(gy_offset);  
     accelgyro.setZGyroOffset(gz_offset);  
     meansensors();  
     Serial.println("...");  
     if (abs(mean_ax) <= acel_deadzone) ready++;  
     else ax_offset = ax_offset - mean_ax / acel_deadzone;  
     if (abs(mean_ay) <= acel_deadzone) ready++;  
     else ay_offset = ay_offset - mean_ay / acel_deadzone;  
     if (abs(16384 - mean_az) <= acel_deadzone) ready++;  
     else az_offset = az_offset + (16384 - mean_az) / acel_deadzone;  
     if (abs(mean_gx) <= giro_deadzone) ready++;  
     else gx_offset = gx_offset - mean_gx / (giro_deadzone + 1);  
     if (abs(mean_gy) <= giro_deadzone) ready++;  
     else gy_offset = gy_offset - mean_gy / (giro_deadzone + 1);  
     if (abs(mean_gz) <= giro_deadzone) ready++;  
     else gz_offset = gz_offset - mean_gz / (giro_deadzone + 1);  
     if (ready == 6) break;  
   }  

 #include <PID_v1.h> // Libreria para calcular los valores de la posicion del robot: P (proportional), I (integral) y D (derivative).  
 #include <LMotorController.h> // Libreria para el uso de los dos motores con el modulo LN298N.  
 #include "I2Cdev.h" // Libreria para leer datos de la inclinación del robot con el MPU6050.  
 #include "MPU6050_6Axis_MotionApps20.h" // Libreria para leer datos de la inclinación del robot con el MPU6050.  
 #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE  
  #include "Wire.h"  
 #endif  
 #define MIN_ABS_SPEED 30 // Output a los motores.  
 MPU6050 mpu; // Variable del MPU.  
 // MPU6050 - variables de control y estado.  
 bool dmpReady = false; // Se inia a true si la inicializacion del DMP fue correcta.  
 uint8_t mpuIntStatus; // Byte de estado del MPU.  
 uint8_t devStatus; // Estado de retorno de cada operacion del robot (0 = correcto, !0 = error).  
 uint16_t packetSize; // Tamanio del paquete DMP (por defecto 42 bytes).  
 uint16_t fifoCount; // Conteo de los bytes actuales de la cola.  
 uint8_t fifoBuffer[64]; // Almacenamiento de buffer de cola.  
 // Variables de orientacion y movimiento.  
 Quaternion q; // [w, x, y, z] Contenedor cuaternio.  
 VectorFloat gravity; // [x, y, z] vector gravedad.  
 float ypr[3]; // [yaw, pitch, roll] Contenedor yaw/pitch/roll y vector de gravedad.  
 /* PID - Proporciona la correcion entre el valor deseado (o entrada) y el valor real (salida).  
  * La diferencia entre estas dos es el margen de error.  
  * El controlador del PID reduce el error a lo minimo posible, ajustando constantemente el valor de salida.  
  */  
 double originalSetpoint = 172.50; // Configuracion del punto de ajuste.  
 double setpoint = originalSetpoint;  
 double movingAngleOffset = 0.1; // Configuracion del angulo.  
 double input, output;  
 /* KP, KD y KI  
  * El input es la inclinacion deseada, en grados.  
  * El MPU6050 lee la inclinacion actual del robot y pasa esa informacion al PID, el cual  
  * hace calculos para controlar el motor y mantener al robot en equilibrio.  
 */  
 double Kp = 38; // < kp = poca correcion (el robot se caera). > kp = robot "baile" (hacia delante y hacia atras) .   
 double Kd = 1.95; // Un buen kd disminuye las oscilaciones hasta estabilizarlo.  
 double Ki = 270; // El valor ki correcto acortará el tiempo necesario para que el robot se estabilice.  
 PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);  
 // VELOCIDAD MOTOR - Valores de velocidad de los dos motores.  
 double motorSpeedFactorLeft = 0.6; //0.555  
 double motorSpeedFactorRight = 0.49; //0.45  
 // L298N - Control de pines.  
 int ENA = 5;  
 int IN1 = 6;  
 int IN2 = 7;  
 int IN3 = 9;  
 int IN4 = 8;  
 int ENB = 10;  
 LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, motorSpeedFactorLeft, motorSpeedFactorRight);  
 /* Indica si el pin de interrupción de la MPU se ha elevado.  
  * Variable "volatile" para cambios en tiempo de ejecucion.  
 */  
 volatile bool mpuInterrupt = false;   
 void dmpDataReady(){ // Inicializacion en el setup de la variable mpuInterrupt a true.  
  mpuInterrupt = true;  
 }  
 void setup(){  
  // Union de I2C bus (la libreria I2Cdev no lo hace automaticamente).  
  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE  
  Wire.begin();  
  TWBR = 24; // 400kHz I2C de reloj (200kHz si la CPU es de 8MHz).  
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE  
  Fastwire::setup(400, true);  
  #endif  
  mpu.initialize(); // Inicializacion del MPU6050.  
  devStatus = mpu.dmpInitialize(); // Inicializacion del DMP.  
  // OFFSETS - Mis offsets en el test de calibracion son: 1327 -940 1316 82 36 -25, pero no tienen por que ser esos.  
  mpu.setXGyroOffset(82);  
  mpu.setYGyroOffset(304);  
  mpu.setZGyroOffset(-25);  
  mpu.setZAccelOffset(1316);  
  // DMP - Devuleve 0 si esta listo, en caso contrario control de errores.  
  if (devStatus == 0){  
   mpu.setDMPEnabled(true); // Habilitamos el DMP.  
   attachInterrupt(0, dmpDataReady, RISING); // Habilitar la detecion de interrupciones del arduino.  
   mpuIntStatus = mpu.getIntStatus(); // Estado del MPU.  
   dmpReady = true; // Control de errores mediante booleano en la funcion loop.  
   packetSize = mpu.dmpGetFIFOPacketSize(); // Tamanio de paquete para una comparacion posterior en el loop.  
   // Inicializacion de PID.  
   pid.SetMode(AUTOMATIC);  
   pid.SetSampleTime(10);  
   pid.SetOutputLimits(-255, 255);   
  } else {  
   /* ERROR!   
   * 1 = Carga de memoria inicial fallo.  
   * 2 = Las actualizaciones de configuración de DMP fallaron.  
   */  
   Serial.print(F("DMP Inicializacion fallida (codigo "));  
   Serial.print(devStatus);  
   Serial.println(F(")"));  
   }  
 }  
 void loop()  
 {  
  // Si el codigo falla, no hace nada.  
  if (!dmpReady) return;  
  // A la espera de interrupcion de MPU o paquete extra disponible.  
  while (!mpuInterrupt && fifoCount < packetSize){ // Si hay interrupcion o si la cola es mayor, no entro al bucle.  
  pid.Compute(); // Calculos del algoritmo del PID.  
  motorController.move(output, MIN_ABS_SPEED); // Output a los motores.  
  }  
  mpuInterrupt = false; // Reseteo de la varibale de interrupcion.  
  mpuIntStatus = mpu.getIntStatus(); // Seteo del estado del MPU.  
  fifoCount = mpu.getFIFOCount(); // Contado de la cola.  
  /* Interrupcion de datos DMP -> Lectura de paquete de la cola -> Seteo de las variables de orientacion y movimiento.  
  * En caso contrario, control de errores. Checkeo del desbordamiento (esto nunca debería ocurrir a menos   
  * que nuestro código sea demasiado ineficiente).  
  */  
  if ((mpuIntStatus & 0x10) || fifoCount == 1024){ // Seleccionamos el bit de interes y enmascaramos el resto.  
  mpu.resetFIFO(); // Reseteamos la cola.  
  Serial.println(F("FIFO overflow!")); // Retroalimentacion.  
  } else if (mpuIntStatus & 0x02){ // Por el contrario, checkeamos la interrupción de datos DMP.  
  while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); // Esperamos a un tamanio correcto de cola.  
  mpu.getFIFOBytes(fifoBuffer, packetSize); // Leemos paquete de la cola.  
  fifoCount -= packetSize; // Seguimiento del conteo de la cola si > 1 paquete disponible.  
  // Seteamos las variables de orientacion y movimiento globales pasadas por referencia.  
  mpu.dmpGetQuaternion(&q, fifoBuffer);  
  mpu.dmpGetGravity(&gravity, &q);  
  mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);  
  input = ypr[1] * 180/M_PI + 180;  
  }  
 }