Servo Motor PID Arduino

Control de un motor DC con realimentación por codificador.

En esta entrada, muestro una práctica interesante, se trata del control por lazo cerrado, de un motor de corriente continua.

Se trata de un montaje completo y simple, tiene dos posiciones fijas seleccionables desde pulsadores, lo justo y necesario para poder probar el funcionamiento y ajuste del PID por los potenciómetros.

Manos a la obra….

Objetivo:   

-Probar la plataforma Arduino para el control PID de servo sistemas.

Recursos necesarios:

-Arduino cualquier versión, yo he utilizado el (mini-pro)

-Una placa para control de motores tipo L298

-Un alimentador 24V para el motor

-Un alimentador de 5V para el control.

-Un motor de corriente continua con codificador de doble canal.

Finalidad:

-Disponer de un proyecto de ejemplo inicial para nuevos proyectos más elaborados.

Nuestro prototipo:

FUNCIONAMIENTO

El control del motor se hace a través de dos salidas del Arduino controladas por modulación por ancho de impulso PWM, estas señales junto con la de habilitación controlan un módulo de potencia basado en el circuito integrado L298, el cual se alimenta a 24V y puede controlar hasta dos motores DC a la vez.

El Arduino dispone de dos entradas para el contaje de la realimentación del encoder.

Una de esas entradas trabaja por interrupción, trabaja en X2 es decir por flanco de subida y bajada del canal A, el sentido de giro lo marca el estado del canal B cuando se produce cada interrupción.

La interrupción produce una llamada a la función (encoder_X2), esta función tiene un tiempo de ejecución máximo de 9,5 micro segundos, esto se debe tener en cuenta para calcular la velocidad máxima del motor en función del número de impulsos del encoder utilizado.

La segunda entrada de interrupción del arduino mini la he dejado reservada para un futuro uso en un control para fresadoras CNC, Mach3.

El control PID está basado en el algoritmo publicado en el enlace indicado a pie de página.

El ajuste del PID se regula a través de tres potenciómetros y tres entradas analógicas.

Dos pulsadores están cableados en dos entradas que están configuradas como entradas Pull-up.

Un pulsador da orden de girar 10 vueltas el motor en un sentido de giro, el otro pulsador retorna el motor a su posición inicial.

Disponemos del cable adaptador USB de programación, por el cual podemos abrir el terminal serie del entorno de programación Arduino y poder monitorizar los valores del ajuste del PID y de la posición teórica y real del motor:

El entorno de programación utilizado es el IDE Arduino V1.8.7.  

Programa Arduino:

// Proyecto Servo Motor
// JColl Dic.2018
// Funcionamiento con una sola entrada de interrupción

// *************************  Patillaje ****************************
const byte    DER = 11 ;  // Entrada pulsador orden girar.
const byte    IZQ = 12 ;  // Entrada pulsador orden girar.

const byte    encA = 3;   // Entrada de la señal A del encoder.
const byte    encB = 7;   // Entrada de la señal B del encoder.

const byte    Enable = 4; // Salida habilitación Potencia.
const byte    PWMA = 5;   // Salida PWM al puente en H.
const byte    PWMB = 6;   // Salida PWM al puente en H.

// ************************* Variables Globales PID *****************
double        Input    = 0.0, Setpoint   = 0.0;   
double        ITerm    = 0.0, dInput     = 0.0, lastInput = 0.0; 
double        kp       = 1.0, ki         = 0.01, kd        = 10.0; 
double        outMin   = 0.0, outMax     = 0.0;                 
double        error    = 0.0;                                   

double        poten       = 0.0;
// ************************* Otras Variables ************************
volatile long contador =  0L;             
byte          ant      =  0,    act = 0; 
byte          pwm      =  0;             
const byte    ledok    = 13;             
// ******************************************************************

void setup(void)                        
{
  Serial.begin(115200);                 

  pinMode(DER, INPUT);  
  pinMode(IZQ, INPUT);  
  pinMode(encB, INPUT);             
  digitalWrite(encB, HIGH);   // Pone el pin a 1 (pull-up)
  
  pinMode(PWMA, OUTPUT);      // Declara las salidas PWM (pin 5).
  pinMode(PWMB, OUTPUT);      //     "      "            (pin 6).
  pinMode(Enable, OUTPUT);    //     "      "     Enable (pin 4).
  digitalWrite(PWMA, LOW);    
  digitalWrite(PWMB, LOW);
  digitalWrite(Enable, HIGH); // Habilita la potencia motores

  // Configuración de la frecuencia del PWM para los pines 5 y 6.
  // Frecuencia del PWM 1 =(32KHz)
  TCCR0B = TCCR0B & B11111000 | 1;
  
  // Interrupción En cualquier flanco ascendente o descendente  
  attachInterrupt(digitalPinToInterrupt(encA), encoder_X2, CHANGE); 
  
  // Acotación máxima y mínima; corresponde a Max.: 0=0V hasta 255=5V (PWMA), 
  // y Min.: 0=0V hasta -255=5V (PWMB). 
  // El PWM se convertirá a la salida en un valor absoluto, nunca negativo.
  outMax =  255.0;        // Límite máximo del controlador PID.
  outMin = -outMax;       // Límite mínimo del controlador PID.
 
  Setpoint = (double)contador;          
}

void loop(void)
{
  Serial.print("KP=");     Serial.print(kp);
  Serial.print(" KI=");    Serial.print(ki);
  Serial.print(" KD=");    Serial.println(kd);

  Serial.print("SetPoint:");
  Serial.print((long)Setpoint);

  double Out = Compute();               

  //Lee analógica Potenciómetro
  poten = (double)analogRead(A0)/400;     // read the input pin
  kp  =poten;

  //Lee analógica Potenciómetro
  poten = (double)analogRead(A1)/4000;     // read the input pin
  ki  =poten;

  //Lee analógica Potenciómetro
  poten = (double)analogRead(A2)/200;     // read the input pin
  kd  =poten;

  Serial.print("Contador ");
  Serial.println((double)contador);
 
int   boton1= digitalRead(DER);
int   boton2= digitalRead(IZQ);

  if (boton1==LOW) { Setpoint = 18000.0;} //Girar 10 vueltas
  if (boton2==LOW) { Setpoint = 0.0;} //Girar a posición 0 absoluto


  // *********************** Control del Motor *************************
  if (error == 0.0)           // Cuando está en el punto designado, parar el motor.
  {
    digitalWrite(PWMA, LOW);  // Pone a 0 los dos pines del puente en H.
    digitalWrite(PWMB, LOW);
    digitalWrite(ledok, HIGH);// Se enciende el led (pin 13) 
  }
  else                        
  {
    pwm = abs(Out);           // Transfiere a la variable pwm el valor absoluto de Out.
    
    if (Out > 0.0)            
    {
      digitalWrite(PWMB, LOW);// Pone a 0 el segundo pin del puente en H.
      analogWrite(PWMA, pwm); // Por el primer pin sale la señal PWM.
    }
    else                      // Gira el motor en sentido contrario.
    {
      digitalWrite(PWMA, LOW);// Pone a 0 el primer pin del puente en H.
      analogWrite(PWMB, pwm); // Por el segundo pin sale la señal PWM.
    }
  }
  
}
// Cálculo PID.
double Compute(void)
{//Esta rutina tarda del orden de 52uS hasta 150uS.
  Input  = (double)contador;                   // Lee el valor del encoder óptico. 
     
  error  = (Setpoint - Input)  * kp;           // Calcula el error proporcional.
  dInput = (Input - lastInput) * kd;           // Calcula el error derivativo.
  if ((dInput == 0.0) || (error == 0.0)) ITerm += (error * ki); else ITerm -= (error * ki);
  if (ITerm > outMax) ITerm = outMax; else if (ITerm < outMin) ITerm = outMin;
     
  double Output = error + ITerm - dInput;      // Salida del control PID.
  if (Output > outMax) Output = outMax; else if (Output < outMin) Output = outMin; 
     
  lastInput = Input;          // Se guarda la posición para convertirla en pasado.
  return Output;              // Devuelve el valor de salida PID.
}



void encoder_X2(void)   //Tiempo ejecución 9,2 a 9,5 uS.
{

  ant=act;        // Guardamos el valor 'act' en 'ant' para convertirlo en pasado.
  // Guardamos en 'act' el valor que hay en ese instante en el encoder y hacemos un
  // enmascaramiento para aislar los dos únicos bits que utilizamos para esta finalidad.
  // Mascara bit D3 y D7, donde D3 está como interrupción cambio de estado.
  act=PIND & 136;

   
  switch (ant) {
  case 0:
    if(act==136)  contador--;
    if(act==8)    contador++;
    break;
  case 8:
    if(act==0)    contador--;
    if(act==128)  contador++;
    break;
  case 128:
    if(act==136)  contador--;
    if(act==8)    contador++;
    break;
  case 136:
    if(act==0)    contador--;
    if(act==128)  contador++;
    break;
  }
}

Referencias y agradecimientos:

Enlace algoritmo PID

Enlace proyecto Control PID para Arduino mejorado.

Fin del procedimiento….