Después de haberme inspirado en los motores RYNO y otros scooters autoequilibrados de Segway, siempre quise construir algo parecido. Pensando por un tiempo, decidí construir un Robot de Equilibrio con Arduino. De esta forma, podría entender el concepto subyacente detrás de todos estos scooters y también comprender cómo funciona el algoritmo PID.

Una vez que empece a armarlo, me di cuenta de que este robot es un desafío para elaborar. Hay tantas opciones para elegir y, por lo tanto, las confusiones comienzan desde la elección de los motores y permanece hasta ajustar los valores PID. Y hay tantas cosas a tener en cuenta como el tipo de batería, la posición de la batería, el agarre de la rueda, el tipo de motor, el mantenimiento del CoG (centro de gravedad) y mucho más. Pero déjame aclarartelo, una vez que lo armes estarás de acuerdo en que no es tan complicado como parece. Así que, admitámoslo, en este proyecto documentaré mi experiencia en la construcción del robot de autoequilibrado. Podrías ser un principiante absoluto que recién está comenzando o que podría haber aterrizado aquí después de una larga frustración de no lograr que tu robot funcione. Este lugar pretende ser tu destino final. Entonces comencemos……

Seleccionar las partes para su robot

Antes de que te diga unas de las tantas opciones para construir el robot, déjame mostrarte los elementos que he usado en este proyecto

• 1 Arduino UNO
• 2 Motores DC con engranaje (amarillo) 
• 1 Módulo de controlador de motor L298N MPU6050
• 1 par de ruedas
• 1 Batería de iones de litio de 7.4V
• 1 Conexión de cables
• 1 Cuerpo impreso en 3D

Controlador: el controlador que he usado aquí es Arduino UNO, porque es simplemente
fácil de usar. También puede usar un Arduino Nano o un Arduino mini pero le
recomendaría que se quede con UNO ya que podemos programarlo directamente sin ningún
hardware externo.

Motores: la mejor opción de motor que puede usar para un robot autoequilibrante,
sin duda será el motor paso a paso. Pero para mantener las cosas simples, he usado
un motor de engranajes de CC. Sí, no es obligatorio tener un paso a paso; el robot
funciona bien con estos motores de engranajes de CC de color amarillo de bajo
costo comúnmente disponibles también.
Controlador de motor: si ha seleccionado los motores de engranajes de CC como los
míos, puede utilizar el módulo de controlador L298N como yo, o incluso un L293D
debería funcionar bien. Obtenga más información sobre el control del motor de CC
con L293D y Arduino.

Ruedas: No subestimen a estos tipos; Me costó trabajo descubrir que el problema
era con mis ruedas. Por lo tanto, asegúrese de que sus ruedas tengan un buen
agarre sobre el piso que está utilizando. Mire de cerca, su agarre nunca debe
permitir que sus ruedas toquen el piso.

Acelerómetro y giroscopio: la mejor opción de acelerómetro y giroscopio para su
robot será el MPU6050. Así que no intente construir uno con un Acelerómetro normal
como ADXL345 o algo así, simplemente no funcionará. Sabrá por qué al final de
esta publicación.

Batería: Necesitamos una batería que sea lo más liviana posible y el voltaje de
operación debe ser más de 5 V para que podamos alimentar nuestro Arduino
directamente sin un módulo de impulso. Entonces, la elección ideal será una
batería de Li-polímero de 7.4V. Aquí, como tenía una batería Li-ion de
7.4V disponible, la he usado. Pero recuerda que un Li-po es ventajoso que
el Li-ion.

El módulo de controlador del motor Arduino y L298N se alimenta directamente a través del pin Vin y el terminal de 12V, respectivamente. El regulador a bordo de la placa Arduino convertirá la entrada de 7.4V a 5V y ATmega IC y MPU6050 funcionarán con ella. Los motores de CC pueden funcionar con un voltaje de 5V a 12V. Pero conectaremos el cable positivo de 7.4V de la batería al terminal de entrada de 12V del módulo del controlador del motor. Esto hará que los motores funcionen con 7.4V. La siguiente tabla mostrará una lista de cómo el módulo del controlador del motor MPU6050 y L298N está conectado con Arduino.

El MPU6050 se comunica con Arduino a través de la interfaz I2C, por lo que usamos los pines SPI A4 y A5 de Arduino.Los motores de CC están conectados a los pines PWM D6, D9 D10 y D11, respectivamente. Necesitamos conectarlos a los pines PWM porque controlaremos la velocidad del motor de CC variando el ciclo de trabajo de las señales PWM.

Programación del robot de equilibrio automático

Ahora tenemos que programar nuestra placa Arduino UNO para equilibrar el robot. Aquí es donde sucede toda la magia; el concepto detrás de esto es simple. Tenemos que comprobar si el robot se inclina hacia delante o hacia atrás usando el MPU6050 y luego si está inclinado hacia adelante tenemos que girar las ruedas hacia adelante y si está inclinado hacia atrás tenemos que girar las ruedas en la dirección inversa.

Al mismo tiempo , también tenemos que controlar la velocidad a la que giran las ruedas , si el robot está ligeramente desorientado desde la posición central, las ruedas giran lentamente y la velocidad aumenta a medida que se aleja más de la posición central. Para lograr esta lógica, utilizamos el algoritmo PID, que tiene la posición central como punto de referencia y el nivel de desorientación como salida.

Para conocer la posición actual del robot utilizamos el MPU6050 , que es un sensor de acelerómetro y giroscopio de 6 ejes combinados. Para obtener un valor confiable de posición del sensor, necesitamos usar el valor del acelerómetro y del giroscopio, porque los valores del acelerómetro tienen problemas de ruido y los valores del giroscopio tienden a desplazarse con el tiempo. Así que tenemos que combinar ambos y obtener el valor de cabeceo y balanceo de nuestro robot, del cual usaremos solo el valor de la guiñada.

Suena un poco de cabeza tambaleándose ¿no? Pero no se preocupe, gracias a la comunidad Arduino tenemos bibliotecas disponibles que pueden realizar el cálculo PID y también obtener el valor de la orientación desde el MPU6050. La biblioteca está desarrollada por br3ttb y jrowberg respectivamente. Antes de proceder, descargue sus bibliotecas desde el siguiente enlace y agréguelos a su directorio lib de Arduino.

https://github.com/br3ttb/Arduino-PID-Library/blob/master/PID_v1.h

https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050

Ahora que tenemos las bibliotecas agregadas a nuestro Arduino IDE. Comencemos la programación de nuestro Robot de auto equilibrado. Como siempre, el código completo del Proyecto se encuentra al final de esta página, aquí solo estoy explicando los fragmentos más importantes en el código. A dicho anteriormente, el código se basa en el código de ejemplo de MPU6050 . Simplemente optimizaremos el código para nuestro propósito y agregaremos el PID y la técnica de control para nuestro robot de autoequilibrado.

Primero, incluimos las bibliotecas que se requieren para que este programa funcione. Incluyen la biblioteca I2C incorporada, la Biblioteca PID y la Biblioteca MPU6050 que acabamos de descargar.

//libreria utilizadas
#include <I2Cdev.h>
 #include <PID_v1.h> 
 #include <MPU6050_6Axis_MotionApps20.h>

Luego declaramos las variables que se requieren para obtener los datos del sensor MPU6050 . Leemos el vector de gravedad y los valores de cuaternión y luego calculamos el valor de cabeceo y balanceo del robot. La matriz flotante ypr [3] mantendrá el resultado final.

// MPU control / estado variables
 bool dmpReady = falso;  // establece verdadero si el inicio de DMP fue exitoso
 uint8_t mpuIntStatus;  // contiene 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 = éxito,! 0 = error)
 uint16_t packetSize;  // tamaño de paquete DMP esperado (el valor predeterminado es 42 bytes)
 uint16_t fifoCount;  // cuenta de todos los bytes actualmente en FIFO
 uint8_t fifoBuffer [64];  // búfer de almacenamiento FIFO

 // orientación / movimiento vars
 Quaternion q;  // [w, x, y, z] contenedor quaternion
 VectorFloat Gravity;  // [x, y, z] vector de gravedad
 float ypr [3];  // [yaw, pitch, roll] contenedor de yaw / pitch / roll y vector de gravedad

Luego viene el segmento muy importante del código, y aquí es donde pasará un largo tiempo ajustando el conjunto correcto de valores . Si tu robot está construido con un muy buen centro de gravedad y los componentes están dispuestos simétricamente (lo que en la mayoría de los casos no es), entonces el valor de tu punto de ajuste será 180. De lo contrario, conecta tu robot al monitor serie Arduino y inclínalo hasta encuentra una buena posición de equilibrio, lea el valor que se muestra en el monitor en serie y este es su valor de punto de ajuste. El valor de Kp, Kd y Ki debe ajustarse de acuerdo con tu robot. No hay dos robots idénticos que tengan los mismos valores de Kp, Kd y Ki por lo que no hay escapatoria.

/ ********* Sintoniza estos 4 valores para tu ROBOT ********* /
 punto de consigna doble = 176;  // establece el valor cuando el robot está perpendicular a la tierra usando un monitor serial.
 doble Kp = 21;  // Establecer esto primero
 doble Kd = 0.8;  // Establecer este segundo
 doble Ki = 140;  // Finalmente configuré esto
 / ****** Ajuste de fin de valores ********* /

En la siguiente línea inicializamos el algoritmo PID pasando las variables de entrada de entrada, salida, punto de ajuste, Kp, Ki y Kd . De estos, ya hemos establecido los valores del punto de ajuste Kp, Ki y Kd en el fragmento de código anterior. El valor de entrada será el valor actual de la guiñada que se lee desde el sensor MPU6050 y el valor de salida será el valor que se calcule mediante el algoritmo PID. Entonces, básicamente, el algoritmo PID nos dará un valor de salida que se debe usar para corregir el valor de entrada para que esté cerca del punto de ajuste.

PID pid (& input, & output, & setpoint, Kp, Ki, Kd, ​​DIRECT);

Dentro de la función de configuración de vacío inicializamos el MPU6050 configurando el DMP (procesador de movimiento digital). Esto nos ayudará a combinar los datos del acelerómetro con los datos de giroscopio y proporcionar un valor confiable de Yaw, Pitch and Roll. No profundizaremos mucho en esto, ya que irá más allá del tema. De todos modos, un segmento de código que debe buscarse en la función de configuración son los valores de desplazamiento del giro. Cada sensor MPU6050 tiene sus propios valores de desviaciones. Puede utilizar este Sketch  Arduino  para calcular el valor de compensación de su sensor y actualizar las siguientes líneas según corresponda en su programa.

// proporciona tus propias compensaciones de giroscopio aquí, escaladas para sensibilidad mínima
 mpu.setXGyroOffset (220);
 mpu.setYGyroOffset (76);
 mpu.setZGyroOffset (-85);
 mpu.setZAccelOffset (1688);

También tenemos que inicializar los pines digitales PWM que estamos utilizando para conectar nuestros motores.En nuestro caso, es D6, D9, D10 y D11. Entonces inicializamos estos pines ya que los pines de salida los hacen LOW  por defecto.

// Inicializa los pines de salida del motor
 pinMode (6, OUTPUT);
 pinMode (9, OUTPUT);
 pinMode (10, OUTPUT);
 pinMode (11, OUTPUT);

 // Por defecto apaga ambos motores
 analogWrite (6, LOW);
 analogWrite (9, LOW);
 analogWrite (10, LOW);
 analogWrite (11, LOW);

Dentro de la función de bucle principal verificamos si los datos del MPU6050 están listos para ser leídos . Si es así, entonces lo usamos para calcular el valor de PID y luego mostramos el valor de entrada y salida de PID en el monitor de serie solo para verificar cómo está respondiendo el PID. Luego, en función del valor de la producción, decidimos si el robot debe moverse hacia adelante o hacia atrás o quedarse quieto.

Dado que suponemos que el MPU6050 devolverá 180 cuando el robot está en posición vertical. Obtendremos valores de corrección positivos cuando el robot esté cayendo hacia adelante y obtendremos valores en negativo si el robot cae hacia atrás . Así que buscamos esta condición y llamamos a las funciones apropiadas para mover el robot hacia delante o hacia atrás.

while (! mpuInterrupt && fifoCount <packetSize)
 {
 // no hay datos de mpu - realizando cálculos PID y salida a motores
 pid.Compute ();
       
 // Imprime el valor de Entrada y Salida en el monitor serie para verificar cómo está funcionando.
 Serial.print (input);  Serial.print ("=>");  Serial.println (output);
              
 if (input> 150 && input <200) {// Si el robot está cayendo
         
 if (output> 0) // Caída hacia adelante
 forward();  // Gire las ruedas hacia adelante
 else if (output <0) // Caída hacia atrás
 Reverse();  // Gire las ruedas hacia atrás
 }
 else // Si robot no cae
 Stop();  // Mantenga las ruedas inmóviles
 }

La variable de salida PID también decide qué tan rápido debe girarse el motor . Si el robot está a punto de caer, hacemos una corrección menor girando la rueda lentamente. Si estas correcciones menores no funcionan, y si el robot se cae, aumentamos la velocidad del motor. El valor del movimiento de las ruedas se decidirá mediante el algoritmo PI. Tenga en cuenta que para la función Reversa hemos multiplicado el valor de salida por -1 para que podamos convertir el valor negativo en positivo.

código para el movimiento de la ruedas hacia adelante

void Forward() 

{

analogWrite(6,output);

analogWrite(9,0);

analogWrite(10,output);

analogWrite(11,0);

Serial.print(“F”); //informacion para depurar

}

código para el movimiento de la ruedas hacia atras

void Reverse() 

{

analogWrite(6,0);

analogWrite(9,output*-1);

analogWrite(10,0);

analogWrite(11,output*-1);

Serial.print(“R”);

}

código para frenar ambas ruedas

void Stop() 

{

analogWrite(6,0);

analogWrite(9,0);

analogWrite(10,0);

analogWrite(11,0);

Serial.print(“S”);

}

Una vez que esté listo con el hardware, puede cargar el código en su placa Arduino. Asegúrese de que las conexiones sean las adecuadas, ya que estamos utilizando una batería de ion de litio. Se debe tener mucho cuidado. Por lo tanto, verifique si hay cortocircuitos y asegúrese de que los terminales no entren en contacto, incluso si su robot experimenta algunos pequeños impactos. Encienda su módulo y abra su monitor serie, si su Arduino pudo comunicarse con MPU6050 con éxito y si todo funciona como se espera, debería ver la siguiente pantalla.

/ * Robot de autoequilibrio Arduino
* Código por: B. Aswinth Raj
* Construido encima de Lib: https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050
*/
#include <I2Cdev.h> 

#include <PID_v1.h> 

#include <MPU6050_6Axis_MotionApps20.h>

MPU6050 mpu;

// MPU control / estado variables 

bool dmpReady = falso;         // establece verdadero si el inicio de DMP fue exitoso 

uint8_t mpuIntStatus;         // contiene 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 = éxito,! 0 = error) 

uint16_t packetSize;            // tamaño de paquete DMP esperado (el valor predeterminado es 42 bytes) 

uint16_t fifoCount;               // cuenta de todos los bytes actualmente en FIFO 

uint8_t fifoBuffer [64];      // búfer de almacenamiento FIFO

// orientación / movimiento vars 

Quaternion q;        // [w, x, y, z] contenedor quaternion 

VectorFloat gravity;      // [x, y, z] vector de gravedad 

float ypr [3];        // [yaw, pitch, roll] contenedor de yaw / pitch / roll y vector de gravedad

/ ********* Sintoniza estos 4 valores para tu ROBOT ********* / 

 double setpoint= 176;     // establece el valor cuando el robot está perpendicular a la tierra usando un monitor serial. 

doble Kp = 21;      // Establecer esto primero 

doble Kd = 0.8;   // Establecer este segundo 

doble Ki = 140;   // Finalmente configuré esto 

/ ****** Ajuste de fin de valores ********* /

doble input, output; 

PID pid (& input, & output, & setpoint, Kp, Ki, Kd, ​​DIRECT);

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

void dmpDataReady () 

{ 

mpuInterrupt = true; 

}

void setup () { 

Serial.begin (115200);

// inicializa el dispositivo 

Serial.println (F (“Inicializando dispositivos I2C …”)); 

mpu.initialize ();

// verificar la conexión 

Serial.println (F (“Conexiones de dispositivo de prueba …”)); 

Serial.println (mpu.testConnection ()? F (“conexión MPU6050 exitosa”): F (“conexión MPU6050 fallida”));

// cargar y configurar el DMP 

devStatus = mpu.dmpInitialize ();

// proporciona tus propias compensaciones de giroscopio aquí, escaladas para sensibilidad mínima 

mpu.setXGyroOffset (220); 

mpu.setYGyroOffset (76); 

mpu.setZGyroOffset (-85); 

mpu.setZAccelOffset (1688);

// asegúrese de que funcionó (devuelve 0 si es así) 

if (devStatus == 0) 

{ 

// enciende el DMP, ahora que está listo 

Serial.println (F (“Habilitando DMP …”)); 

mpu.setDMPEnabled (true);

// habilitar la detección de interrupciones Arduino 

Serial.println (F (“Habilitación de la detección de interrupción (interrupción externa Arduino 0) …”)); 

attachInterrupt (0, dmpDataReady, RISING); 

mpuIntStatus = mpu.getIntStatus ();

// configuramos nuestra bandera de DMP Ready para que la función principal de bucle () sepa que está bien usarla 

Serial.println (F (“DMP listo! Esperando la primera interrupción …”)); 

dmpReady =  true;

// obtener el tamaño esperado del paquete DMP para una comparación posterior 

packetSize = mpu.dmpGetFIFOPacketSize (); 

// configuración PID 

pid.SetMode (AUTOMATIC); 

pid.SetSampleTime (10); 

pid.SetOutputLimits (-255, 255); 

} 

else

{ 

// ¡ERROR! 

// 1 = carga de memoria inicial fallida 

// 2 = Error en las actualizaciones de configuración DMP 

// (si se va a romper, generalmente el código será 1) 

Serial.print (F (“Inicialización de DMP fallida (code)); 

Serial.print (devStatus); 

Serial.println (F (“)”)); 

}

// Inicializa los pines de salida del motor 

pinMode (6, OUTPUT); 

pinMode (9,  OUTPUT); 

pinMode (10, OUTPUT); 

pinMode (11,  OUTPUT);

// Por defecto apaga ambos motores 

analogWrite (6, LOW); 

analogWrite (9, LOW); 

analogWrite (10, LOW); 

analogWrite (11, LOW); 

}

void loop () { 

// si falla la programación, no intentes hacer nada 

if (! dmpReady) return;

// espera la interrupción de MPU o paquete (s) esten disponible (s) 

while (! mpuInterrupt && fifoCount <packetSize) 

{ 

// no hay datos de mpu – realizando cálculos PID y salida a motores 

pid.Compute (); 

// Imprime el valor de Entrada y Salida en el monitor serie para verificar cómo está funcionando. 

Serial.print (input); Serial.print (“=>”); Serial.println (output); 

if (input> 150 && input <200) {// Si el robot está cayendo 

if (output> 0) // Caída hacia adelante 

Forward(); // Gire las ruedas hacia adelante 

else if (output <0) // Caída hacia atrás 

Reverse(); // Gire las ruedas hacia atrás 

} 

else // Si robot no cae 

Stop(); // Mantenga las ruedas inmóviles 

}

// restablecer el indicador de interrupción y obtener INT_STATUS byte 

mpuInterrupt = false; 

mpuIntStatus = mpu.getIntStatus ();

// obtener el conteo actual de FIFO 

fifoCount = mpu.getFIFOCount ();

// verifica el desbordamiento (esto nunca debería ocurrir a menos que nuestro código sea demasiado ineficiente) 

if ((mpuIntStatus & 0x10) || fifoCount == 1024) 

{ 

// restablecer para que podamos continuar limpiamente 

mpu.resetFIFO (); 

Serial.println (F (“FIFO overflow!”));

// de lo contrario, compruebe la interrupción de datos listos para DMP (esto debería suceder con frecuencia) 

} 

else if (mpuIntStatus & 0x02) 

{ 

// esperar la longitud correcta de los datos disponibles, debe ser una espera MUY corta 

while (fifoCount <packetSize) fifoCount = mpu.getFIFOCount ();

// lee un paquete de FIFO 

mpu.getFIFOBytes (fifoBuffer, packetSize); 

// rastrea el conteo FIFO aquí en caso de que haya> 1 paquete disponible 

// (esto nos permite leer más sin esperar una interrupción) 

fifoCount – = packetSize;

mpu.dmpGetQuaternion (& q, fifoBuffer); // obtener valor para q 

mpu.dmpGetGravity (& gravity, & q); // obtener valor para la gravedad 

mpu.dmpGetYawPitchRoll (ypr, & q, & gravity); // obtener valor para ypr

input = ypr [1] * 180 / M_PI + 180;

} 

}

void Forward () // Código para girar la rueda hacia adelante 

{ 

analogWrite (6, output); 

analogWrite (9,0); 

analogWrite (10, output); 

analogWrite (11,0); 

Serial.print (“F”); // información de depuración 

}

void Reverse () // Código para girar la rueda hacia atrás 

{ 

analogWrite (6,0); 

analogWrite (9, output * -1); 

analogWrite (10,0); 

analogWrite (11, output * -1); 

Serial.print (“R”); 

}

void Stop () // Código para detener ambas ruedas 

{ 

analogWrite (6,0); 

analogWrite (9,0); 

analogWrite (10,0); 

analogWrite (11,0); 

Serial.print (“S”); 

}

 

Autor Aswinth Raj

Espero le guste el proyecto y nos vemos en la próxima. Saludos

1479