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 proyecto Control PID para Arduino mejorado.
Fin del procedimiento….