Robot de equilibrio de una bola con PID
Introducción
Un robot de equilibrio de una bola consiste en una balanza que se puede inclinar en dos direcciones y una bola que se mueve sobre ella. El principio básico es controlar la posición de la bola cambiando el ángulo de la balanza mediante un servomotor. Esto se suele hacer utilizando un controlador PID y un sensor de distancia para medir la posición de la bola. El objetivo suele ser mantener la bola en el centro de la plataforma. Sin embargo, en este proyecto se añade al sistema un panel de control para que el usuario pueda controlar manualmente la posición final de la bola, así como las ganancias del controlador y .
Lista de piezas y diseño 3D
Como se ve en la foto de portada, la balanza consta de dos piezas de madera pegadas entre sí y tiene una regla impresa para medir la posición de la bola. La bola es una simple pelota de ping-pong limitada en ambos extremos por dos piezas impresas en 3D. La balanza está unida a un soporte central que le permite inclinarse en ambas direcciones. El servomotor está fijado a un soporte impreso en 3D y está conectado a la balanza mediante un mecanismo que controla el ángulo de inclinación. Aunque algunas piezas impresas en 3D proceden del diseño original, otras se diseñaron para adaptarse a las necesidades del proyecto.
Las piezas sin modificar son:
- La bisagra central: pieza impresa en 3D que sujeta la balanza.
- La base cónica: pieza impresa en 3D que sujeta la bisagra central. Ambas piezas están pegadas a una varilla de madera de 8 mm.
- La bisagra del servo: pieza impresa en 3D que sujeta la balanza y está conectada al servomotor para controlar el ángulo de inclinación.
- El disco del servo: pieza impresa en 3D que conecta el servomotor con la bisagra del servo.
Las piezas nuevas son:
- El soporte del sensor IR: pieza impresa en 3D que sujeta el sensor IR en su sitio mediante 2 tornillos.
- Tope de proximidad: pieza impresa en 3D que impide que la bola se acerque al sensor IR a menos de una determinada distancia. Como se verá más adelante, esto evita que la bola se acerque a menos de 6 cm y permite medir su posición de forma unívoca.
- Soporte del servomotor: pieza impresa en 3D que sujeta el servomotor. Se rediseñó para adaptarse al nuevo servomotor utilizado en este proyecto.
- Panel de control: piezas impresas en 3D que sujetan la pantalla LCD, 4 potenciómetros, 1 botón y 2 interruptores.
Como se ve en la siguiente imagen, las piezas se diseñaron en Catia V5 utilizando Part Design y aplicando técnicas de diseño paramétrico. Los modelos 3D se exportaron como archivos STL y se laminaron mediante Cura. Finalmente, todas las piezas impresas en 3D se imprimieron en una impresora 3D Artillery X1 utilizando filamento PLA.
Pieza del panel de control en Catia V5
Extracción de datos de una imagen para el sensor IR
Para medir la posición de la bola se utilizó un sensor IR. El sensor es un ARCELI GP2Y0A21YK0F que proporciona una tensión en función de la distancia del objeto que tiene delante. El sensor tiene un rango de 10 a 80 cm, como se ve en la siguiente gráfica de la hoja de datos de Sharp. La siguiente gráfica muestra la tensión de salida del sensor en función de la distancia del objeto que tiene delante. Como se ha mencionado antes, para garantizar que el sensor pueda medir la posición de la bola de forma unívoca, se añadió un tope de proximidad para evitar que la bola se acerque a menos de 6 cm del sensor.
Tensión frente a distancia del sensor IR
Los datos del sensor se extrajeron utilizando ImageJ y un complemento llamado Figure Calibration. Una vez calibrado, el complemento permite extraer datos haciendo clic sobre la gráfica y guardarlos en un archivo CSV. Después, el archivo CSV se importa en un script de Python que interpola los datos para obtener la distancia de la bola al sensor. La siguiente imagen muestra los datos extraídos de la gráfica y su interpolación.
Extracción de datos con ImageJ
Una vez extraídos los datos, se escribió un script de Python para aplicarles un ajuste de curva. El siguiente script utiliza la función curve_fit de la biblioteca scipy.optimize para ajustar los datos a la expresión . Después, el script representa los datos y la curva resultante.
import pandas as pd
import matplotlib.pyplot as plt
import numpy as np
from scipy.optimize import curve_fit
df = pd.read_csv("GP2Y0A21YK0F.csv")
# Split the data into d and V
x = df["V"].values
y = df["d"].values
coeff, _ = curve_fit(lambda v, a, b, c: a*(v**b)*np.exp(c*v), x, y, p0=(4, 0.1, -0.01))
a2 = coeff[0]
b2 = coeff[1]
c2 = coeff[2]
# X values for the prediction
x_new = np.linspace(0.3, 4, 100)
y_new = a2*x_new**b2*np.exp(c2*x_new)
plt.scatter(x, y)
plt.plot(x_new, y_new, c="green", label=f"{round(a2, 4)} * (x^{round(b2, 4)}) * exp({round(c2, 4)} * x)")
plt.grid()
plt.xlabel("V [V]")
plt.ylabel("d [cm]")
plt.legend()
plt.show()
Regresión de tensión frente a distancia del sensor IR
Circuito de Arduino
El circuito completo integra el sensor IR, el servomotor, la pantalla LCD, 4 potenciómetros, 1 botón y 2 interruptores. La siguiente imagen muestra el esquema del circuito.
Esquema del circuito
La placa Arduino UNO se alimenta mediante un cargador USB normal conectado al conector de corriente continua. La mayoría de los componentes se alimentan mediante la salida de 5 V del Arduino. Sin embargo, se utiliza un portapilas de 4 pilas AA para alimentar el servomotor, ya que requiere más corriente de la que puede proporcionar el Arduino en total.
Empezando por la izquierda, el sensor IR está conectado al pin analógico A0 del Arduino para leer la tensión de salida del sensor. El servomotor está conectado al pin PWM 3 del Arduino y la pantalla LCD de 16x2 con interfaz I2C está conectada a los pines A4 (SDA) y A5 (SCL) del Arduino. Esto permite utilizar solo 2 pines para comunicarse con la pantalla en lugar de los 16 pines que requiere la interfaz normal.
En cuanto al panel de control del usuario, 3 potenciómetros están conectados a los pines analógicos A1, A2 y A3 del Arduino. Como el Arduino solo tiene 6 pines analógicos, no se implementó el potenciómetro de . Por otro lado, el botón y un interruptor están conectados a los pines digitales 8 y 9 del Arduino, respectivamente. Finalmente, el último interruptor se utiliza para encender la alimentación del servomotor, por lo que no está conectado al Arduino.
La siguiente imagen muestra el circuito final montado. Los componentes de entrada/salida del usuario están colocados en un panel de control impreso en 3D y todos los componentes están conectados mediante cables puente y 2 placas de pruebas. Para ello, la mayoría de los componentes se soldaron a conectores hembra para poder conectar los cables puente. Finalmente, el portapilas de 4 pilas AA se fija a la mesa mediante un imán de neodimio y se puede retirar fácilmente para sustituir las pilas.
Circuito montado
Programación
El código de Arduino se divide en 3 partes principales: setup, loop y las funciones. Primero, setup se ejecuta una vez para inicializar los componentes. Una vez completado setup, loop se ejecuta en un bucle continuo. El bucle lee los datos del sensor IR y del panel de control del usuario para controlar el servomotor. Por último, las funciones se utilizan para implementar la medición de distancia del sensor IR y algunos algoritmos de ordenación y filtrado para reducir el ruido de los datos.
En la parte superior del archivo se definen los pines de entrada/salida y se importan las bibliotecas. También se definen las constantes y variables utilizadas en el código, como los ángulos mínimo, máximo y de equilibrio del servomotor. Esto se utilizará para limitar el movimiento del servomotor y situar la balanza en la posición central. La variable period se utiliza para establecer la frecuencia de actualización del bucle principal. Las ganancias del PID también se definen en esta sección. Los valores trim son los valores seleccionados para las constantes del PID y los valores mínimos y máximos se utilizan para limitar los valores que el usuario puede establecer mediante los potenciómetros.
Setup inicializa los pines digitales y analógicos del Arduino, así como el servomotor y la pantalla LCD. También inicializa las variables utilizadas en el bucle y la variable de tiempo para controlar la frecuencia de actualización del bucle.
En cada iteración del bucle, el código primero lee el interruptor para seleccionar el modo del controlador. Si el interruptor está encendido, el controlador definido por las ganancias del usuario o las preestablecidas cambia la posición del servomotor para mantener la bola a la distancia de consigna. Si el interruptor está apagado, el servomotor se sitúa en la posición de equilibrio. Mientras está apagado, el usuario puede cambiar el modo del controlador pulsando el botón. En modo manual, el usuario puede controlar la posición final de la bola y las ganancias del PID mediante los potenciómetros, mientras que en modo automático el controlador utiliza las ganancias preestablecidas del PID y el usuario solo puede establecer la consigna de distancia.
La pantalla LCD se actualiza con los datos leídos de los potenciómetros y del sensor IR para obtener información del sistema. Cuando el interruptor está encendido, la distancia a la bola se mide mediante la función get_distance. Esta función lee el sensor IR n veces y calcula la distancia de la bola utilizando el 20 % central de los datos. Los datos se ordenan y filtran para reducir su ruido. Esto se hace mediante el algoritmo de ordenación de burbuja y el algoritmo de media central definidos en las funciones bubbleSort y centralMean. Después se calcula la diferencia entre la consigna de distancia y la distancia de la bola y se utiliza para calcular la señal de control PID. A continuación, la señal de control PID se utiliza para situar el servomotor en la nueva posición. Finalmente, la pantalla LCD se actualiza con los nuevos datos.
#include <math.h>
#include <Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
/////////////////// Inputs/outputs /////////////////////
#define SERVO_PIN 3 // Servomotor
#define SENSOR_PIN A0 // IR Distance sensor
#define POT_D A3 // Potentiometer to set distance setpoint
#define POT_kp A1 // Potentiometer to set kp
#define POT_kd A2 // Potentiometer to set kd
#define BUTTON 8 // Button input to switch
#define SWITCH 9 // Switch input
Servo myservo; // create servo object to control a servo
// CONFIG:
int min_angle = 31; // Higher position 10
int max_angle = 97; // Lower position 106
int trim_angle = 64; // Balance position 63
///////////////////////////////////////////////////////
struct sensor_data {
float distance;
float volts;
};
////////////////////// Variables //////////////////////
int Read = 0;
float distance = 0.0;
float elapsedTime, time, timePrev; // Variables for time control
float distance_previous_error, distance_error;
// CONFIG:
int period = 50; // Refresh rate period of the loop is 50ms
///////////////////////////////////////////////////////
////////////////// PID constants //////////////////////
// CONFIG:
float trim_kp=6; // Mine was 8
float trim_kd=550; // Mine was 3100, 600, 900 (cerca), 1000, 990, 1300, 1700 (casi) 400, 500 x
float trim_ki=0.2; // Mine was 0.2
float min_kp = 0.5 * trim_kp;
float max_kp = 2 * trim_kp;
float min_kd = 0.5 * trim_kd;
float max_kd = 2 * trim_kd;
float trim_setpoint = 22.5; // Should be the distance from sensor to the middle of the bar in mm
float min_setpoint = trim_setpoint - 10;
float max_setpoint = trim_setpoint + 10;
float PID_p, PID_i, PID_d, PID_total;
///////////////////////////////////////////////////////
//////////////////// LCD Config ///////////////////////
LiquidCrystal_I2C lcd(0x27,20,4); //sometimes the LCD adress is not 0x3f. Change to 0x27 if it dosn't work.
int i = 0;
///////////////////////////////////////////////////////
//// Button setup ////
int lastButtonState = LOW;
int mode = 0;
void setup() {
//// SERVO ////
Serial.begin(9600);
myservo.attach(SERVO_PIN); // attaches the servo pin to the servo object
myservo.write(min_angle);
delay(1000);
myservo.write(max_angle);
delay(1000);
myservo.write(trim_angle); // Put the servo at trim position, so the balance is in the middle
delay(1000);
//// IR Distance sensor ////
pinMode(SENSOR_PIN, INPUT);
//// POTENTIOMETERS ////
pinMode(POT_D, INPUT);
pinMode(POT_kp, INPUT);
pinMode(POT_kd, INPUT);
//// BUTTON AND SWITCH ////
pinMode(BUTTON, INPUT);
pinMode(SWITCH, INPUT_PULLUP);
//// LCD 16X2 Display ////
lcd.init(); // Init the LCD
lcd.backlight(); // Activate backlight
lcd.home();
//// Variables setup ////
time = millis();
}
void loop() {
//// Read switch ////
int switchState = digitalRead(SWITCH);
if (switchState == HIGH) {
// After each wait period
if (millis() > time + period)
{
time = millis(); // Update time
//// Read Potentiometers ////
int pot_d = analogRead(POT_D);
float distance_setpoint = map(pot_d, 0, 1023, min_setpoint, max_setpoint);
float kp;
float kd;
float ki;
if (mode == 0) {
int pot_kp = analogRead(POT_kp);
kp = map(pot_kp, 0, 1023, min_kp, max_kp);
int pot_kd = analogRead(POT_kd);
kd = map(pot_kd, 0, 1023, min_kd, max_kd);
ki = trim_ki;
}
else {
kp = trim_kp;
kd = trim_kd;
ki = trim_ki;
}
Serial.print("kp: ");
Serial.print(kp);
Serial.print(" kd: ");
Serial.print(kd);
//// Sensor ////
sensor_data data = get_distance(100);
float distance = data.distance;
float volts = data.volts;
//// Controller ////
distance_error = distance_setpoint - distance; // 22.0 - d
PID_p = kp * distance_error; // 10 x 8 = 80
float dist_diference = distance_error - distance_previous_error;
float speed = (distance_error - distance_previous_error)/period;
PID_d = kd*(speed);
// Filter to activate kD
// if (abs(speed) < 0.01)
// {
// PID_d = 0;
// }
// Filter to activate kI
if(-3 < distance_error && distance_error < 3)
{
PID_i = PID_i + (ki * distance_error);
}
else
{
PID_i = 0;
}
PID_total = PID_p + PID_i + PID_d;
Serial.print("PID: ");
Serial.print(PID_total);
PID_total = map(PID_total, -150, 150, max_angle, min_angle);
if (PID_total < min_angle) {
PID_total = min_angle;
}
if(PID_total > max_angle) {
PID_total = max_angle;
}
Serial.print(" PID_total: ");
Serial.println(PID_total);
//// Actuator ////
myservo.write(PID_total);
//// Save current values ////
distance_previous_error = distance_error;
//// LCD Display ////
// Turn off the display:
lcd.clear();
// First line: d, d_set
//String m1 = "V: " + String(volts, 1) + " d: " + String(distance, 1);
String m1_on = "d: " + String(distance, 1) + " set:" + String(distance_setpoint, 1);
// Add distance and angle of the servo to a string, rounded to 2 decimal places
// Print the message
lcd.setCursor(0, 0);
lcd.print(m1_on);
// Second line: kp, kd, ki
String m2_on = "kp:" + String(kp, 1) + " kd:" + String(kd, 0);
//String m2 = "kp:" + String(kp, 1) + " kd:" + String(kd, 0) + " ki:" + String(ki, 1);
lcd.setCursor(0, 1);
lcd.print(m2_on);
// Serial.print("D: ");
// Serial.println(distance);
// Serial.print("set:");
// Serial.println(distance_setpoint);
}
}
else {
//// Stop the servo ////
myservo.write(trim_angle);
//// Read Button ////
int buttonState = digitalRead(BUTTON);
if (buttonState == LOW && lastButtonState == HIGH) {
if (mode == 0) {
mode = 1;
} else {
mode = 0;
}
Serial.print("PUSHED Mode: ");
Serial.println(mode);
}
lastButtonState = buttonState;
lcd.clear();
String m1 = "OFF";
lcd.setCursor(0, 0);
lcd.print(m1);
String m2_off;
if (mode == 0) {
m2_off = "Mode: Manual";
}
else {
m2_off = "Mode: Preset";
}
lcd.setCursor(0, 1);
lcd.print(m2_off);
// Serial.print("Mode: ");
// Serial.println(m2_off);
}
}
sensor_data get_distance(int n) {
// Read analog pin n times and sum Analog Data
long sumAD = 0;
long ADarray[n];
for (int i=0; i<n; i++) {
long AD_read = analogRead(SENSOR_PIN);
sumAD = sumAD + AD_read; // Read (0 - 1023) <-> (0V - 3.3 V)
ADarray[i] = AD_read;
}
//float aData = sumAD / n; // Mean value of Analog Data
bubbleSort(n, ADarray); // Sort measurements
//float aData = median(n, ADarray); // Use median value
float aData = centralMean(n, ADarray, 0.5); // Use 20% of central data for average
// Calculate volts mapping analog data (0 - 1023) to read voltage (0 - 3.3 V)
//float volts = map(aData, 0, 1023, 0, 3.3); // Map from 0 - 1023 to 0 - 3.3 V
//float volts = aData * 2 / 1024;
float volts = aData * 3.3 / 1024; // Map from 0 - 1023 to 0 - 3.3 V
// Calculate distance base on a*t^b*e(c*t) regression from documentation data
float a = 32.6639;
float b = -1.0277;
float c = -0.1458;
float offset = -2.5; // To center
float distance = a * pow(volts, b) * exp(c * volts) + offset; // regression evaluation
// Serial Communication
Serial.print(volts);
Serial.println(" V ");
sensor_data data;
data.distance = distance;
data.volts = volts;
return data;
}
void printArray(int N, long arr[])
{
for (int i=0; i < N; i++)
{
Serial.print(arr[i]);
Serial.print(" ");
}
Serial.print("\n");
}
void bubbleSort(int N, long arr[])
{
for (int i=0; i < N; i++)
{
int changes = 0;
for (int j=0; j < N-i-1; j++)
{
if (arr[j] > arr[j+1])
{
long val1 = arr[j];
long val2 = arr[j+1];
arr[j] = val2;
arr[j+1] = val1;
changes = changes + 1;
}
}
if (changes == 0)
{
break;
}
}
}
long median(int N, long arr[])
{
if (N % 2 == 0)
{
int idx = N/2;
return (arr[idx - 1] + arr[idx])/2;
}
else
{
int idx = (N - 1) / 2;
return arr[idx];
}
}
float centralMean(int N, long arr[], float r)
{
long centralSum = 0;
int nC = (int) N * r;
if (N % 2 == 0)
{
if (nC % 2 != 0)
{
nC = nC - 1;
}
// x|x|o|x
for (int i = (N/2 - nC/2); i < (N/2 + nC/2); i++)
{
centralSum += arr[i];
// Serial.println(arr[i]);
}
}
else
{
if (nC % 2 == 0)
{
nC = nC - 1;
}
// x|o|x
for (int i = (N/2 - (nC-1)/2); i < ((N-1)/2 + (nC-1)/2 + 1); i++)
{
centralSum += arr[i];
Serial.println(arr[i]);
}
}
// Serial.print("n: ");
// Serial.println(nC);
return (float) centralSum / (float) nC;
}