Java ME 8 + Raspberry Pi + Acelerómetro + PWM + Driver de Motor = Robot JBalancePI (Parte 1)

Por José Cruz
Publicado en Septiembre 2016

Aprenda como construir un robot de dos ruedas que se auto balancea.

En mis últimos artículos, expliqué cómo conectar sensores o dispositivos electrónicos a un Raspberry Pi 2 Modelo B utilizando varios tipos de interfaces: En la Parte 1 mostré como utilizar el bus de entrada/salida de propósito general (GPIO), en la Parte 2 utilicé el bus tipo I2C, en la Parte 3 utilicé una interfaz de circuito receptor y transmisor asíncrono (UART)  y en la Parte 4 utilicé una interfaz de periféricos serial (SPI).

Este artículo se enfoca en el uso de dos de los tipos de interfaces ya comentadas, GPIO and I2C, para crear un prototipo de robot de dos ruedas que se auto balancea utilizando los siguientes módulos:

  • Sensor MPU-6050, el cual contiene un giroscopio y acelerómetro de tres ejes. El acelerómetro mide la aceleración lineal y el vector de gravedad terrestre y el giroscopio mide la velocidad angular. El sensor utiliza una interfaz tipo I2C que emplea la dirección 0x68h en la cual se puede leer un valor compuesto con data del acelerómetro y el giroscopio en una sola lectura.
  • Adafruit PCA9685, es un modulador de ancho de pulsos PWM para servos, con 16 canales de 12-bit que produce un pulso a una determinada frecuencia controlando así la velocidad de los motores. Utiliza una interfaz I2C con la dirección 0x41h y de esta manera libera al Raspberry Pi de quedar esclavizado produciendo pulsos PWM vía software forzando a un bajo desempeño del CPU.
  • L298N driver para motores DC tipo H-bridge, el cual controla la velocidad y la dirección de los dos motores del robot. Se utiliza la interfaz GPIO para producir una señal de dirección y activar el módulo PWM para activar y controlar la velocidad de cada motor.

Nota: Los ejemplos de código están en un proyecto NetBeans IDE 8.1 y pueden ser descargados aquí.

El módulo MPU-6050, junto con el PCA9685, y el módulo L298N crean un ciclo cerrado de retroalimentación como lo muestra la Figura 1, el cual balancea el robot y automáticamente corrige su posición a pesar del ruido o disturbios en su equilibrio.

El robot evita caer produciendo una aceleración en sus dos ruedas de acuerdo a su inclinación en relación al eje vertical. Si el robot inclina su cuerpo en determinado ángulo, su centro de masa experimentará, una pseudo fuerza aplicada por un torque en la dirección opuesta a la dirección de su inclinación.


Figura 1. Pasos del ciclo de control retroalimentado para balancear el robot.

Circuitos que crearemos

Basado en el diagrama de bloques de la Figura 2, los componentes electrónicos que se muestran en la Figura 3, las partes mecánicas mostradas en la Figura 4, crearemos los circuitos mostrados en la Figura 5 y el robot de dos ruedas que se auto balancea y que se muestra como prototipo en la Figura 6.


Figura 2. Diagrama de bloques de los circuitos que crearemos.

  
  
  

Figura 3. Componentes electrónicos que utilizaremos.


Listado de componentes electrónicos:

  • 1 × Raspberry Pi 2 Model B
  • 1 × MPU-6050 modulo sensor giroscopio y acelerómetro de tres ejes
  • 1 × L298N controlador para motores DC tipo H-bridge,
  • 1 x Adafruit PCA9685 16-canales 12-bit PWM controlador para servos
  • 1 × Convertidor DC/DC LM2596
  • 1 x Adafruit Perma-Proto Pi HAT
  • 1 × 7,4V 6A Batería LiPo recargable
  • 2 x 0.1 uF condensadores
  • 1 x ThePiHut Wi-Fi dongle 802.11N

Nota: Para obtener buenos resultados, se sugiere utilizar una batería LiPo de mínimo 7.4v con un mínimo de 2.200 mAh conectada al convertidor DC–DC configurado para generar 5v como voltaje final.
  
 

Figura 4. Partes mecánicas que utilizaremos.

Lista de las partes mecánicas:

  • 2 × ruedas
  • 2 × abrazaderas para las ruedas
  • 2 × motores
  • 4 × pernos de acero medida M4x40
  • 4 x pernos de acero medida M4x30
  • 8 x pernos de acero medida M4x10
  • 2 × soportes para motores
  • 3 × planchas acrílicas
  • 20 x tornillos M4
  • 14 x tuercas M4

Figura 5. Esquemático de los circuitos que construiremos.

Figura 6. Prototipo del robot de dos ruedas que se auto balancea que crearemos.

Un poco de Matemáticas


Problema del pendulum invertido

Robots que se balancean representan el clásico problema de un pendulum invertido, en los cuales una larga masa es colocada al final de un poste. El poste es libre de girar alrededor de la base, y la base es libre de moverse en el plano perpendicular a la vertical. El objetivo es mantener el poste vertical moviendo la base en respuesta a cambios en el ángulo.

Filtro Complementario

La Figura 7 muestra un robot de dos ruedas con un acelerómetro de tres ejes para medir la inclinación, ángulo θa y un giroscopio de un solo eje para medir el ángulo θg de inclinación dinámica. Ambos ángulos deben ser fusionados mediante el uso de un filtro para obtener un θ último ángulo de inclinación. En nuestro caso, utilizaremos un filtro complementario, como se muestra en la Figura 8, que controla la velocidad de los motores en base a la información de la velocidad angular.

Figura 7. Robot con acelerómetro de tres ejes y un giroscopio de eje simple.

 


Figura 8. Diagrama de bloques de un filtro Complementario (fuente: Mouser)


Obtenemos el ángulo θ leyendo el  acelerómetro en un lapso de tiempo continuo mediante el uso de un filtro de paso bajo, y sumamos los resultados con los valores giroscopio aplicando un filtro de paso alto.

Construimos ese filtro dentro del método Filter en la clase JBalancePI, como se muestra en el Listado 1. La variable angle_filtered es el ángulo θ:

/**
* Leer la data del acelerómetro y giroscopio y 
* aplicar un filtro complementario
*/
private void Filter() {
//Data del MPU-6050 Acelerómetro y Giroscopio
data = accel.getMotion6();
//Calcular el angulo y convertir de radianes a grados 
float angle_raw = (float) (Math.atan2(data.AccelY, data.AccelZ) * 180.00 / pi + accel_offset);
float omega = (float) (data.GyroX / gyro_gain + gyro_offset);
// Filtrar la data para obtener el valor real
long now = System.currentTimeMillis();
float dt = (float) ((now - preTime) / 1000.00);
preTime = now;
//Calcular el error utilizando un filtro complementario 
float K = 0.8F;
float A = K / (K + dt);
angle_filtered = A * (angle_filtered + omega * dt) + (1 - A) * angle_raw;
}

Listado 1. Método Filter que implementa un filtro complementario

Para más detalles de las fórmulas matemáticas por favor vea esta información

Es muy importante que la orientación Y del sensor MPU- 6050 esté ubicado en paralelo a las ruedas del robot, como se muestra en la Figura 9

Figura 9. Sensor Y del MPU-6050 (dentro del círculo rojo), montado paralelo a las ruedas. 

Sistema de control Proporcional, Integral, y Derivativo (PID)

El algoritmo de control que vamos a utilizar para mantener el equilibrio de nuestro robot está basado en un controlador PID. El controlador PID es conocido como un controlador de tres términos; se trata de un algoritmo de control increíblemente potente. Una señal de salida u puede ser generada mediante la suma de tres componentes, como se muestra en la Figura 10: 


Figura 10. u(t) señal de control.


La entrada al controlador es el error del sistema. Los términos Kp, Ki y Kd se conocen como las constantes proporcional, integral y derivativo (los tres términos se multiplican por estas constantes respectivamente), y e(t) es el error de la salida deseada en el tiempo t.

El sistema de control en bucle cerrado también se conoce como un sistema de retroalimentación negativo. La idea básica de un sistema de retroalimentación negativo es que mida la salida del proceso a partir de un sensor. La salida del proceso medida se resta del valor del punto de ajuste de referencia para producir un error. El error se introduce en el controlador PID, en el que el error se logró de tres maneras.

El error será utilizado en el controlador PID para ejecutar el término proporcional, el término integral para la reducción de errores de estado estacionario, y el término derivado para manejar rebasamiento. Después de que el algoritmo PID procesa el error, el controlador produce una señal de control u. La señal de control PID a continuación, se introduce en el proceso bajo control.

El proceso bajo control PID es nuestro robot de dos ruedas, como se muestra en la Figura 11. La señal de control PID manejará el proceso con el valor del punto de ajuste de referencia deseado. En el caso de nuestro, el valor de referencia deseado es la posición vertical de cero grados.


Figura 11. Controlador PID y filtro complementario trabajando juntos. 

Ajuste de los valores del controlador PID

Algunos pasos de cómo obtener rápidamente los valores Kp, Ki, y Kd:

  • Establecer los términos de Ki y Kd a 0, y ajustar Kp para que el robot comience a oscilar (avanzar y retroceder) cerca de la posición de equilibrio. Kp debe ser lo suficientemente grande para que el robot se mueva pero no demasiado grande; de lo contrario, el movimiento no será suave.
  • Con Kp establecido, aumentar Ki para que el robot acelere más rápido cuando está fuera de equilibrio. Con Kp y Ki sintonizados correctamente, el robot debe ser capaz de auto - equilibrio durante al menos unos segundos.
  • Por último, aumentar la Kd para que el robot se mueva en torno a su posición de equilibrio con más suavidad. No debe haber ningún rebasamiento significativo. 

Construiremos este controlador PID dentro del método PID en la clase JBalancePI como muestra el Listado 2.

//Establecer valores iniciales del controlador PID 
private final float Kp = 17F;
private final float Kd = 840F;
private final float Ki = 0.1F;

/*
* Control Proporcional, Integral y Derivativo
*/
private void PID() {
long now = System.currentTimeMillis();
int timeChange = (int) (now - lastTime);
lastTime = now;
float error = angle_filtered;  // Proportion
errSum += error * timeChange;  // Integration
float dErr = (error - lastErr) / timeChange;  // Differentiation
float output = Kp * error + Ki * errSum + Kd * dErr;
lastErr = error;
LOutput = output - Turn_Speed + Run_Speed;
ROutput = output + Turn_Speed + Run_Speed;
}

Listado 2. Controlador PID.


Resumen del código que escribiremos


Controlador de pulsos PWM para los motores

El controlador PID produce la data necesaria para crear los pulsos PWM para mover los motores hacia adelante, hacia atrás o detenerlos. Para hacer esto, crearemos una clase llamada L298Device que utiliza la clase GPIOPin y la clase PCA9685Device de mi artículo anterior. Ver Listado 3.

/**
* Interfaz para el driver de motores L298N utilizando el bus GPIO 
*
* @author Jose Cruz
*/
public class L298Device {

//Enable motor left
private GPIOPin IN1 = null;
private GPIOPin IN2 = null;
//Enable motor right
private GPIOPin IN3 = null;
private GPIOPin IN4 = null;

    //PWM port 0 for motor left
private byte ENA = 0;
//PWM port 1 for motor right
private byte ENB = 1;

    //Define PWM motor object
PCA9685Device pwm;

Listado 3. Creando la clase L298Device.

A seguir, definimos un constructor L298Device que crea un dispositivo definiendo los pines GPIO y el objeto PWM, como se muestra en el Listado 4.

    /**
* Inicializar GPIO para las señales IN1 a IN4 del control de motores
*
* @param in1
* @param in2
* @param in3
* @param in4
*/
public L298Device(int in1, int in2, int in3, int in4) {
try {
// define device for IN1 pin
IN1 = (GPIOPin) DeviceManager.open(
new GPIOPinConfig.Builder()
.setControllerNumber(0)
.setPinNumber(in1)
.setDirection(GPIOPinConfig.DIR_OUTPUT_ONLY)
.setDriveMode(GPIOPinConfig.MODE_OUTPUT_OPEN_DRAIN)
.setInitValue(false)
.build());

   // define device for IN2 pin
IN2 = (GPIOPin) DeviceManager.open(new GPIOPinConfig.Builder()
.setControllerNumber(0)
.setPinNumber(in2)
.setDirection(GPIOPinConfig.DIR_OUTPUT_ONLY)
.setDriveMode(GPIOPinConfig.MODE_OUTPUT_OPEN_DRAIN)
.setInitValue(false)
.build());
// define device for IN3 pin
IN3 = (GPIOPin) DeviceManager.open(new GPIOPinConfig.Builder()
.setControllerNumber(0)
.setPinNumber(in3)
.setDirection(GPIOPinConfig.DIR_OUTPUT_ONLY)
.setDriveMode(GPIOPinConfig.MODE_OUTPUT_OPEN_DRAIN)
.setInitValue(false)
.build());
// define device for IN4 pin
IN4 = (GPIOPin) DeviceManager.open(new GPIOPinConfig.Builder()
.setControllerNumber(0)
.setPinNumber(in4)
.setDirection(GPIOPinConfig.DIR_OUTPUT_ONLY)
.setDriveMode(GPIOPinConfig.MODE_OUTPUT_OPEN_DRAIN)
.setInitValue(false)
.build());

            pwm = new PCA9685Device();
pwm.setPWMFreq(1000);
} catch (IOException ex) {
Logger.getGlobal().log(Level.WARNING, ex.getMessage());
}
}

Listado 4. Inicializando los pines GPIO y creando el objeto pwm.

Ahora creamos los métodos para mover adelante (moveL_Forward, moveR_Forward), mover hacia atrás (moveL_Back, moveR_Back), y detener ambos motores  (stopL, stopR), como se muestra en el Listado 5.

   /**

* Motor izq mover adelante
*/
public void moveL_Forward() {
try {
IN1.setValue(false);
IN2.setValue(true);
} catch (IOException ex) {
Logger.getGlobal().log(Level.WARNING, ex.getMessage());
}
}

   /**
* Motor izq mover atras
*/
public void moveL_Back() {
try {
IN1.setValue(true);
IN2.setValue(false);
} catch (IOException ex) {
Logger.getGlobal().log(Level.WARNING, ex.getMessage());
}
}

    /**
* Motor derecho mover adelante
*/
public void moveR_Forward() {
try {
IN3.setValue(false);
IN4.setValue(true);
} catch (IOException ex) {
Logger.getGlobal().log(Level.WARNING, ex.getMessage());
}
}

    /**
* Motor derecho mover atras
*/
public void moveR_Back() {
try {
IN3.setValue(true);
IN4.setValue(false);
} catch (IOException ex) {
Logger.getGlobal().log(Level.WARNING, ex.getMessage());
}
}

    /**
* Detener motor izq
*/
public void stopL() {
try {
IN1.setValue(true);
IN2.setValue(true);
motorL_PWM((short) 0);
} catch (IOException ex) {
Logger.getGlobal().log(Level.WARNING, ex.getMessage());
}
}

    /**
* Detener motor derecho
*/
public void stopR() {
try {
IN3.setValue(true);
IN4.setValue(true);
motorR_PWM((short) 0);
} catch (IOException ex) {
Logger.getGlobal().log(Level.WARNING, ex.getMessage());
}
}

Listado 5. Métodos para mover y detener ambos motores.

Para comenzar a mover los motores necesitamos establecer un pulso PWM a una determinada frecuencia utilizando para ello el objeto pwm como se muestra en el Listado 6.

   /**
* Establece el pulso PWM para la velocidad del motor izq
* @param _pwm
*/
public void motorL_PWM(short _pwm) {
pwm.setPWM(ENA, (short) 0, _pwm);
}

   /**
* Establece el pulso PWM para la velocidad del motor derecho
* @param _pwm
*/
public void motorR_PWM(short _pwm) {
pwm.setPWM(ENB, (short) 0, _pwm);
}

Listado 6. Creando pulsos PWM a una determinada frecuencia para ambos motores.

Clase completa JBalancePI

Utilizando la clase L298Device, crearemos un método para control de motores PWMControl que empleara los valores calculados dentro del controlador PID métodos PID, LOutput,  y ROutput para controlar el movimiento, dirección y velocidad como se muestra en el Listado 7.

/*
* Control de motores con pulso PWM 
*/
private void PWMControl() {
if (LOutput > 0) {
motor.moveL_Forward();
} else if (LOutput < 0) {
motor.moveL_Back();
} else {
motor.stopL();
}
if (ROutput > 0) {
motor.moveR_Forward();
} else if (ROutput < 0) {
motor.moveR_Back();
} else {
motor.stopR();
}
motor.motorL_PWM((short) (Math.min(4095, Math.abs(LOutput) * 4095 / 256)));
motor.motorR_PWM((short) (Math.min(4095, Math.abs(ROutput) * 4095 / 256)));
}

Listado 7. Definiendo el tipo de movimiento y creando un pulso PWM para controlar la velocidad.

Crearemos un ciclo cerrado retroalimentado como se mostró en la Figura 1, con la clase thread  ControlLoop, como se muestra en el Listado 8.

/*
* Thread para mover y balancear el robot
*/
class ControlLoop extends Thread {
@Override
public void run() {
while (shouldRun) {
Filter();
Logger.getGlobal().log(Level.INFO, "Angle = " + angle_filtered);
// If angle > 45 or < -45 then stop the robot
if (Math.abs(angle_filtered) < 45) {
PID();
PWMControl();
} else {
motor.stopL();
motor.stopR();
// Keep reading accelerometer and gyroscope values after falling down
for (int i = 0; i < 100; i++) 
Filter();
// Empty data and restart the robot automatically
if (Math.abs(angle_filtered) < 45)                {
for (int i = 0; i <= 500; i++) // Reset the robot and delay 2 seconds
{
angle_filtered = 0;
Filter();
errSum = Run_Speed = Turn_Speed = 0;
PID();
}
}
}
}
accel.close();
motor.close();
}
} 

Listado 8. Clase thread que implementa un ciclo cerrado retroalimentado.

Completaremos la clase MIDlet con los métodos startApp y destroyApp para inicializar el filtro complementario, el controlador PID, el control de motores, el acelerómetro y el giroscopio y crear la tarea para balancear el robot con un ciclo cerrado retroalimentado como se muestra en el Listado 9.

/**
*
*/
public void startApp() {

loggerHandler.start();
Logger.getGlobal().setLevel(Level.INFO);

Logger.getGlobal().log(Level.INFO, "***** JBalancePi v1.3 Started *****");
try {
//Activate MPU-6050 with I2C bus
accel = new MPU6050Device();
//Activate motor control with GPIO bus: In1=27, In2=22, In3=24, In4=25
motor = new L298Device(27, 22, 24, 25);
// Loop 200 times to get the real values when starting
for (int i = 0; i < 200; i++) {
Filter();
}
if (Math.abs(angle_filtered) < 45) // Start the robot after cleaning data
{
angle_filtered = 0;
Filter();
errSum = Run_Speed = Turn_Speed = 0;
PID();
}
//Start move and balance thread
controlLoopTask = new ControlLoop();
controlLoopTask.start();

    } catch (IOException ex) {
Logger.getGlobal().log(Level.WARNING, ex.getMessage());
}
}

/**
*
* @param unconditional
*/
@Override
public void destroyApp(boolean unconditional) {
//Stop thread
shouldRun = false;
Logger.getGlobal().log(Level.INFO, "***** JBalancePi v1.0 Stopped *****");

    }
}

Listado 9. Clase MIDlet con los métodos startApp y destroyApp.


Estableciendo algunas configuraciones adicionales

Antes de ejecutar nuestro MIDlets utilizando NetBeans IDE 8.1 es importante establecer los permisos del API adecuados. Para hacer esto, en el IDE, seleccione el proyecto JavaMEDemos, y con click derecho seleccionar Propiedades para mostrar la ventana de propiedades. Seleccionar Descriptor de aplicaciones y seleccionar la pestaña permisos del API, allí incluir todos los permisos mostrados en la Figura 12.


Figura 12. Establecer permisos del API.

Conclusión

En este artículo, hemos visto cómo construir un robot de dos ruedas que se auto equilibra usando algunos módulos como el acelerómetro y giroscopio MPU-6050, el controlador de servos PCA9685 PWM, y el controlador de motores L298N.

Es importante establecer algunos valores iniciales para las constantes PID Kp, Ki y Kd y ajustarlos hasta que logremos equilibrar el robot. En el próximo artículo, vamos a conectar una pantalla OLED y algunos elementos para modificar los valores de forma dinámica.

Estamos abriendo una gran cantidad de posibilidades para el control remoto de este robot, tales como Bluetooth, Wi-Fi, REST, Twitter, interfaces web, y los gestos con Kinect. En los próximos artículos, vamos a ver cómo podemos implementar el control remoto.
 


José Cruz (@joseacruzp) es un ingeniero de software quien ha estado trabajando con Java desde 1998. Es el líder de desarrollo de aplicaciones Java, Java ME y Java EE en Ferretería EPA C.A. en Venezuela. Desde temprana edad su hobbie ha sido la electrónica. Esto le ha permitido combinar la computación con la electrónica y desarrollar proyectos donde Java y los dispositivos embebidos como Arduino y Raspberry Pi son protagonistas.

Este artículo ha sido revisado por el equipo de productos Oracle y se encuentra en cumplimiento de las normas y prácticas para el uso de los productos Oracle.