Equilibrista

Vamos ver cómo construir un pequeño robot de autoequilibrio que puede moverse evitando obstáculos. Este  robot se basa en la placa de desarrollo Arduino UNO y el módulo de acelerómetro-giroscopio MPU6050.

Veremos cómo conectar el MPU6050 con Arduino, cómo medir el ángulo de inclinación del robot, cómo usar PID para hacer que el robot se mantenga equilibrado.

Lista de componentes

• Arduino UNO.
  

  Arduino UNO

• Módulo GY-521 con MPU-6050.

• Controlador de motoresL298N.

• Baterías y soporte.
• 2 ruedas.
• 2 Motores.

Aparte de lo anterior, necesitaremos algunos cables y un interruptor de encendido / apagado.

Paso 1: Un poco de teoría.

El robot de autoequilibrio es similar a un péndulo invertido. A diferencia de un péndulo normal que sigue oscilando una vez que se le da un empujón, este péndulo invertido no puede mantenerse equilibrado por sí solo. Simplemente se caerá. Entonces, ¿cómo lo equilibramos?

Intenta equilibrar un palo de escoba con nuestro dedo índice, que es un ejemplo clásico de equilibrar un péndulo invertido. Movemos nuestro dedo en la dirección en la que cae el palo. Similar es el caso de un robot autoequilibrado, solo que el robot caerá hacia adelante o hacia atrás. Al igual que cuando equilibramos un palo en nuestro dedo, equilibramos el robot conduciendo sus ruedas en la dirección en la que cae. Lo que estamos tratando de hacer aquí es mantener el centro de gravedad del robot exactamente por encima del punto de pivote.

Para impulsar los motores necesitamos cierta información sobre el estado del robot. Necesitamos saber la dirección en la que cae el robot, cuánto se ha inclinado el robot y la velocidad con la que cae.

Toda esta información se puede deducir de las lecturas obtenidas del MPU6050. Combinamos todas estas entradas y generamos una señal que impulsa los motores y mantiene el robot equilibrado.

¿Cómo funciona el equilibrio?

Para mantener el robot equilibrado, los motores deben contrarrestar la caída del robot. Esta acción requiere retroalimentación y elementos de corrección. El elemento de retroalimentación es el giroscopio MPU6050 + acelerómetro, que proporciona aceleración y rotación en los tres ejes. El Arduino usa esto para conocer la orientación actual del robot. El elemento corrector es la combinación de motor y rueda.

Paso 2: Construir el robot.

El robot está construido sobre tres capas de metacrilato que están separados por xx mm utilizando 4 bariilas roscadas.

• La capa inferior contiene los dos motores y el controlador del motor.
• La capa intermedia tiene la batería, un interruptor de encendido / apagado.
• La capa  superior tiene  el MPU6050.

Paso 3:Manejo del MPU6050.

En este punto, vamos a controlar por ejemplo un servomotor usando el sensor MPU6050 (acelerómetro y giroscopio). Conectaremos el servomotor y el sensor MPU6050 con el Arduino de tal forma que siempre que movamos el MPU6050, el servomotor se moverá de acuerdo a él.

¿Como funciona?

Al mover el sensor hacia arriba o hacia abajo, nos da valores de -17000 a 17000. Los mapearemos de 0 a 180 para mover el servomotor. Ahora, cuando movemos el sensor hacia arriba, la salida del sensor será 180. Cuando movemos el sensor hacia abajo, la salida del sensor será 0.

Código Arduino

#include <Wire.h>
#include <MPU6050.h>
#include <Servo.h>   

Servo sg90;          
int servo_pin = 2;
MPU6050 sensor ;
int16_t ax, ay, az ;
int16_t gx, gy, gz ;

void setup(){ 
   sg90.attach(servo_pin);
   Wire.begin();
   Serial.begin(9600); 
   Serial.println("Initializando el sensor"); 
   sensor.initialize(); 
   Serial.println(sensor.testConnection() ? "Conectado con Éxito" : "Falló el conexión"); 
   delay (1000); 
   Serial.println ( "Tamando valores del sensor");
   delay (1000);
}

void loop(){ 
sensor.getMotion6 (&ax, &ay, &az, &gx, &gy, &gz);
ax = map (ax, -17000, 17000, 0, 180) ;
Serial.println (ax);
sg90.write (ax); 
delay (200);
}

Usamos estas librería escrita por Jeff Rowberg para leer los datos de MPU6050.

• I2Cdev
• MPU6050

El siguiente código muestra cómo varía el ángulo de inclinación.

#include «Wire.h»
#include «I2Cdev.h»
#include «MPU6050.h»

MPU6050 mpu;

int16_t accY, accZ;
float accAngle;

void setup(){
mpu.initialize();
Serial.begin(9600);
}

void loop(){
accZ = mpu.getAccelerationZ();
accY = mpu.getAccelerationY();

accAngle = atan2(accY,accZ)*RAD_TO_DEG;

if(!isnan(accAngle))
Serial.println(accAngle);
}

Mueve el robot hacia adelante y hacia atrás mientras se mantiene inclinado en un ángulo fijo. Observará que el ángulo que se muestra en su monitor de serie cambia repentinamente. Esto se debe a que la componente horizontal de la aceleración interfiere con los valores de aceleración de los ejes y y z.

El robot de autoequilibrio es esencialmente un péndulo invertido. Puede equilibrarse mejor si el centro de masa es más alto en relación con los ejes de las ruedas. Un centro de masa más alto significa un momento de inercia de masa más alto, que corresponde a una aceleración angular más baja (caída más lenta). Por eso he colocado la batería en la parte superior.

Paso 4: Más teoría del autoequilibrio.

En la teoría del control, mantener estable alguna variable (en este caso, la posición del robot) necesita un controlador especial llamado PID (derivada integral proporcional). Cada uno de estos parámetros tiene «ganancias», normalmente llamadas Kp, Ki y Kd.

PID proporciona corrección entre el valor deseado (o entrada) y el valor real (o salida). La diferencia entre la entrada y la salida se denomina «error». El controlador PID reduce el error al valor más pequeño posible ajustando continuamente la salida. En nuestro robot de autoequilibrio Arduino, la entrada (que es la inclinación deseada, en grados) se establece mediante software. El MPU6050 lee la inclinación actual del robot y la alimenta al algoritmo PID, que realiza cálculos para controlar el motor y mantener el robot en posición vertical.

PID requiere que los valores de las ganancias Kp, Ki y Kd estén «sintonizados» a valores óptimos. Los ingenieros utilizan software como MATLAB para calcular estos valores automáticamente. Desafortunadamente, no podemos usar MATLAB en nuestro caso porque complicaría aún más el proyecto. En su lugar, ajustaremos los valores PID manualmente. He aquí cómo hacer esto:

1. Haz que Kp, Ki y Kd sean iguales a cero.
2. Ajusta Kp. Demasiado poco Kp hará que el robot se caiga, porque no hay suficiente corrección. Demasiado Kp hará que el robot vaya y venga salvajemente. Un Kp suficientemente bueno hará que el robot se mueva ligeramente hacia adelante y hacia atrás (u oscile un poco).
3. Una vez establecido el Kp, ajusta el Kd. Un buen valor de Kd reducirá las oscilaciones hasta que el robot esté casi estable. Además, la cantidad correcta de Kd mantendrá al robot en pie, incluso si se lo empuja.
4. Por último, configure el Ki. El robot oscilará cuando se encienda, incluso si se configuran Kp y Kd, pero se estabilizará a tiempo. El valor de Ki correcto acortará el tiempo que tarda el robot en estabilizarse.

Tambien tendremos que calibrar nuestro dispositivo y modificar 4 variables en nuestro programa, concretamente:

// escribir las compensaciones de giroscopio aquí, escaladas para sensibilidad mínima
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788);

Puedes calcular estos números con el siguiente programa.

Paso 5:  Código de robot de autoequilibrio Arduino.

Necesitamos cuatro bibliotecas externas para que este robot de autoequilibrio Arduino funcionara.

1. La biblioteca PID facilita el cálculo de los valores P, I y D.
2. La biblioteca LMotorController se utiliza para impulsar los dos motores con el módulo L298N.
3. La biblioteca I2Cdev.
4. La biblioteca MPU6050_6_Axis_MotionApps20 son para leer datos del MPU6050. Puede descargar el código, incluidas las bibliotecas en este arduino-self-balancing-robot-master.

#include <PID_v1.h>
#include <LMotorController.h>
#include <I2Cdev.h>
#include <MPU6050_6Axis_MotionApps20.h>

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include <Wire.h>
#endif

#define MIN_ABS_SPEED 20

MPU6050 mpu;

// MPU control y variables
bool dmpReady = false; // Establecer true si DMP iniciado con éxito
uint8_t mpuIntStatus; // Mantiene el byte de estado de interrupción real de MPU
uint8_t devStatus; // Devuelve el estado después de cada operación del dispositivo (0 = success, !0 = error)
uint16_t packetSize; // Tamaño de paquete DMP (el valor predeterminado 42 bytes)
uint16_t fifoCount; // Contador de bytes actualmente en FIFO
uint8_t fifoBuffer[64]; // Búfer de almacenamiento FIFO

// variables de orientación / movimiento
Quaternion q; // [w, x, y, z]
VectorFloat gravity; // [x, y, z] gravedad
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll velocidad de giro

//PID
double originalSetpoint = 173;
double setpoint = originalSetpoint;
double movingAngleOffset = 0.1;
double input, output;
int moveState = 0; //0 = equilibrio; 1 = atrás; 2 = adelante
double Kp = 50;
double Kd = 1.4;
double Ki = 60;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

//Control de motores
int ENA = 9;
int IN1 = 5;
int IN2 = 4;
int IN3 = 6;
int IN4 = 7;
int ENB = 10;
double motorSpeedFactorLeft = 0.6;
double motorSpeedFactorRight = 0.5;
LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, motorSpeedFactorLeft, motorSpeedFactorRight);

//timers
long time1Hz = 0;
long time5Hz = 0;

volatile bool mpuInterrupt = false; // indica si el pin de interrupción de MPU se ha elevado

void dmpDataReady() {
mpuInterrupt = true;
}

void setup() {
// Unirse al bus I2C (la biblioteca I2Cdev no hace esto automáticamente)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif

// inicializar la comunicación en serie
Serial.begin(115200);
while (!Serial); // Solo para Leonardo Arduino

// inicializar dispositivo MPU
Serial.println(F(«Initializando I2C – MPU6050…»));
mpu.initialize();

// verificar conexión
Serial.println(F(«Test de comunicación…»));
Serial.println(mpu.testConnection() ? F(«MPU6050 conexión correcta») : F(«MPU6050 falló la conexión»));

// cargamos y configuramos DMP
Serial.println(F(«Initializando DMP…»));
devStatus = mpu.dmpInitialize();

// escribir las compensaciones de giroscopio aquí, escaladas para sensibilidad mínima
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

// si está todo correcto
if (devStatus == 0) {
// turn on the DMP, now that it’s ready
Serial.println(F(«Enabling DMP…»));
mpu.setDMPEnabled(true);

// habilitar interrupciones de Arduino
Serial.println(F(«Enabling interrupt detection (Arduino external interrupt 0)…»));
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();

// set our DMP Ready flag so the main loop() function knows it’s    okay to use it
Serial.println(F(«DMP ready! Waiting for first interrupt…»));
dmpReady = true;

// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();

//setup PID

pid.SetMode(AUTOMATIC);
pid.SetSampleTime(10);
pid.SetOutputLimits(-255, 255);
}
else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it’s going to break, usually the code will be 1)
Serial.print(F(«DMP Initialization failed (code «));
Serial.print(devStatus);

}
}

void loop() {
if (dmpReady) {
// Esperamos interrupción para MPU y datos
while (!mpuInterrupt && fifoCount < packetSize) {
// Realización de cálculos PID y salida a motores
pid.Compute();
motorController.move(output, MIN_ABS_SPEED);
}

// reset interrupt y cogemos INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();

// cogemos longitud de la cola FIFO
fifoCount = mpu.getFIFOCount();

// verifica el desbordamiento (esto nunca debería suceder)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
// reiniciar para que podamos continuar limpiamente
mpu.resetFIFO();
Serial.println(F(«FIFO overflow!»));
// de lo contrario, verifique la interrupción de datos DMP listos
}
else if (mpuIntStatus & 0x02) {
// esperar la longitud correcta de los datos disponibles, debe ser una espera MUY corta
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

// leer los datos de FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);

// rastrear el conteo FIFO, en caso de que sea > 1 paquete disponible
// (esto nos permite leer más rápido sin esperar una interrupción)
fifoCount -= packetSize;

mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
#if LOG_INPUT
Serial.print(«ypr\t»);
Serial.print(ypr[0] * 180 / M_PI);
Serial.print(«\t»);
Serial.print(ypr[1] * 180 / M_PI);
Serial.print(«\t»);
Serial.println(ypr[2] * 180 / M_PI);
#endif
input = ypr[1] * 180 / M_PI + 180;
}
}
}

Mis valores de Kp, Ki, Kd pueden funcionar o no. Si no es así, siga los pasos descritos anteriormente. Observe que la inclinación de entrada en mi código está configurada en 173 grados. Puede cambiar este valor si lo desea, pero tenga en cuenta que este es el ángulo de inclinación al que debe mantenerse el robot. Además, si sus motores son demasiado rápidos, puede ajustar los valores motorSpeedFactorLeft y motorSpeedFactorRight.

Video
¡Aquí está el robot autoequilibrado en acción!

Autor: Samuel Arce Belmonte XD