Arduino PID Control

En el post anterior, realizamos una manera de analizar la señal para un sensor óptico con un encoder en una rueda.

El objetivo de este sistema ahora es elaborar un control de una rueda mediante un método robusto y estable y analizar este control.

Hay que tener mucho cuidado con esta fase, porque cualquier solución que podamos encontrar en Internet es susceptible de no ser la adecuada para nuestros objetivos,o incluso puede ser costosa en su elaboración.

El caso de estudio es el siguiente:

 

Disponemos de un robot que venía con un chasis muy barato; pero venía con dos ruedas ranuradas; y como no encontraba información de como realizar el control, pues aquí estamos…

El caso es, que los motores que tiene son de corriente continua (DC Motors) y una placa Arduino no puede proporcionar la suficiente potencia para controlar estos motores directamente y aún menos de realizar un control. Es por ello, que se ha introducido un driver L298N para derivar la potencia de unas pilas a los motores.

Aún así el control es un poco complejo si lo que queremos es hacer mover las ruedas de una manera determinada. El esquema de conexiones es el siguiente.

El siguiente código se corresponde con un movimiento estándar del motor,  y cuando dá un número de vueltas definido (360/40 = 4 vueltas); se mostrarán en pantalla.

#define INTERRUPTION 3

#define _M11 5  //Motor 1 - Pin 1
#define _M12 6  //Motor 1 - Pin 2
#define _M21 9  //Motor 2 - Pin 1
#define _M22 10 //Motor 2 - Pin 2
volatile int wheel = 0;

const int sampleRate = 1;// 1 millisecond

double Setpoint, Input, Output;
float Kp = 2;
float Ki = 1;
float Kd = 1;

//Temporal Variables
const long SerialPing =500;
unsigned long now;
unsigned long lastMessage=0;

bool SerialDebug = true;

void setup() {
  Serial.begin(9600); 
  attachInterrupt(digitalPinToInterrupt(INTERRUPTION), counterwheel, CHANGE);

}

void loop() {
  
  //mover(0, 0, 0, 0);
  mover(255, 0, 0, 0);
  Serial.write( 0xff );                                                         
  Serial.write( (wheel >> 8) & 0xff );                                            
  Serial.write( wheel & 0xff );
  now = millis();

  if((now-lastMessage < SerialPing) && SerialDebug){
    Serial.print("Wheel= ");
    Serial.println(wheel);
  }

  if (wheel >= 360){
    wheel=0;
  }
}

void counterwheel(){
    wheel++;
}

void mover (int M11, int M12, int M21, int M22) {
  analogWrite(_M11,M11);
  analogWrite(_M12,M12);
  analogWrite(_M21,M21);
  analogWrite(_M22,M22);
}

void avanzar () {
  mover(255, 0, 255, 0);
}

void detener () {
  mover(0, 0, 0, 0);
}

Las funciones de movimiento se basan en una función “mover” en el que se establecen en los 4 pines elegidos para los motores (5,6,9,10) para activar en mayor o menor medida con valores entre 0 (no mover) y 255 (Máxima potencia en el motor).

En el siguiente Github se puede descargar la librería para el control de estos motores y en el post se explica su utilización.

De la misma manera que vimos en el post anterior, haremos que la respuesta se dibuje con Processing para poder evaluar el resultado.

import processing.serial.*;

Serial port;  // Create object from Serial class
int val;      // Data received from the serial port
int[] values;
float zoom;

void setup() 
{
  size(680, 480);
  // Open the port that the board is connected to and use the same speed (9600 bps)
  port = new Serial(this, Serial.list()[0], 9600);
  values = new int[width];
  zoom = 1.0f;
  smooth();
}

int getY(int val) {
  return (int)(height - val / 500.0f * (height - 1))-20;
}

int getValue() {
  int value = -1;
  while (port.available() >= 3) {
    if (port.read() == 0xff) {
      value = (port.read() << 8) | (port.read());
    }
  }
  return value;
}

void pushValue(int value) {
  for (int i=0; i<width-1; i++)
    values[i] = values[i+1];
  values[width-1] = value;
}

void drawLines() {
  stroke(255);
  
  int displayWidth = (int) (width / zoom);
  
  int k = values.length - displayWidth;
  
  int x0 = 0;
  int y0 = getY(values[k]);
  for (int i=1; i<displayWidth; i++) {
    k++;
    int x1 = (int) (i * (width-1) / (displayWidth-1));
    int y1 = getY(values[k]);
    line(x0, y0, x1, y1);
    x0 = x1;
    y0 = y1;
  }
}

void drawGrid() {
  stroke(255, 0, 0);
  line(0, height/2, width, height/2);
}

void keyReleased() {
  switch (key) {
    case '+':
      zoom *= 2.0f;
      println(zoom);
      if ( (int) (width / zoom) <= 1 )
        zoom /= 2.0f;
      break;
    case '-':
      zoom /= 2.0f;
      if (zoom < 1.0f)
        zoom *= 2.0f;
      break;
  }
}

void draw()
{
  background(0);
  drawGrid();
  val = getValue();
  if (val != -1) {
    pushValue(val);
  }
  drawLines();
}

La rueda se mueve de manera constante, por lo que la gráfica es incremental y cuando llega a un numero de vueltas vuelve a cero.

Control PID

Uno de los problemas más grandes de este modelo es que, una vez que se programa para que las dos ruedas avancen y el coche se mueva hacia delante; de forma inevitable, una rueda gira más rápido que la otra. Por la razón que sea, uno de los motores obtiene más potencia que el otro, y usando varios modelos del mismo tipo la respuesta es diferente.

Este problema quiere decir que el control, no debe depender de la forma en la que programamos el coche, asumiendo que todos funcionan por igual; sino que debemos programarlo con un control en bucle cerrado para conocer el estado de nuestros motores en todo momento.

Para ello, vamos a utilizar un control PID; y que además se puede integrar facilmente con una librería en Arduino creada por Brett Beaugerard. PIDLibrary

El control PID se basa en la introducción de una consigna, que es el valor que nosotros queremos que se cumpla y el estado actual. Entonces el sistema actuara para alcanzzar el valor deseado realizando una medición del error, o de la diferencia entre el valor deseado y el valor acutal.

   

   

 

Una vez la instalemos podremos utilizarla de forma rápida, si definimos correctamente cuales son las variables necesarias para su ejecución. En la siguiente Guía de uso de la librería PID de Arduino se puede obtener mucha información elaborada a raiz de esta librería.

*No implementéis esos códigos, porque ya están metidos en la librería. Este documento es solo un estudio del por qué y cómo se ha programado esa librería de esa manera.

Nosotros nos centraremos en desarrollar un control para una rueda mediante control PID.

 

#include <PID_v1.h>

#define INTERRUPTION 3

#define _M11 5  //Motor 1 - Pin 1
#define _M12 6  //Motor 1 - Pin 2
#define _M21 9  //Motor 2 - Pin 1
#define _M22 10 //Motor 2 - Pin 2
volatile int wheel = 0;

const int sampleRate = 1;// 1 millisecond

double Setpoint, Input, Output;
float Kp = 2;
float Ki = 1;
float Kd = 1;

PID ControlPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

//Temporal Variables
const long SerialPing =500;
unsigned long now;
unsigned long lastMessage=0;

bool SerialDebug = false;

void setup() {
  Serial.begin(9600); 
  attachInterrupt(digitalPinToInterrupt(INTERRUPTION), counterwheel, CHANGE);
  ControlPID.SetMode(AUTOMATIC);
  ControlPID.SetSampleTime(sampleRate);

  //InitializeSetPoint
  Setpoint = 120;
  Input=0;
  Output=0;
}

void loop() {
  
  Input = wheel;
  //mover(0, 0, 0, 0);
  mover(Output, 0, 0, 0);
  ControlPID.Compute();
  Serial.write( 0xff );                                                         
  Serial.write( (wheel >> 8) & 0xff );                                            
  Serial.write( wheel & 0xff );
  now = millis();

  if((now-lastMessage > SerialPing) && SerialDebug){

    Serial.print("SetPoint= ");
    Serial.println(Setpoint);
    Serial.print("Input= ");
    Serial.println(Input);
    Serial.print("Output= ");
    Serial.println(Output);
    Serial.print("Wheel= ");
    Serial.println(wheel);
  }

    
  /*if (wheel >= 360){
    wheel=0;
  }*/
}

void counterwheel(){
    wheel++;
}

void mover (int M11, int M12, int M21, int M22) {
  analogWrite(_M11,M11);
  analogWrite(_M12,M12);
  analogWrite(_M21,M21);
  analogWrite(_M22,M22);
}

void avanzar () {
  mover(255, 0, 255, 0);
}

void detener () {
  mover(0, 0, 0, 0);
}

*Si queremos debuggear el estado de nuestras variables, solo tendremos que poner a TRUE, la variable SerialDebug.

Aún con el siguiente ejercicio, no vamos a conseguir nada útil, ya que a niveles bajos de entrada en el driver, el motor deja de moverse; entrando en un estado de bloqueo. Es decir, el error de la función va disminuyendo y el valor Output que establece el movimiento de la función mover en el PID, por debajo de un número (100 aproximadamente), los motores no responde y nos puede dar una respuesta a trompicones como esta.

Deberemos de calibrar esta salida con otros valores de kp, kd y ki para obtener una respuesta más continua. Como habréis visto hemos puesto unos valores iniciales sin mucho fundamento de Kp = 2, Kd = 1 y Ki = 1.

Tendremos que estudiar ahora la respuesta del error, para observar su comportamiento para unos nuevos valores.

Aún no hemos implementado la función para retroceder, ya que en el caso de haber sobreoscilación en el sistema, tendremos que obligar a la rueda a corregir ese estado.

En el siguiente post, elaboraremos un análisis más profundo del problema. 🙂