PRACTICA 9.

CONTROL LQR DIGITAL EN TIEMPO REAL DE UN SERVO-MOTOR

El objetivo de la práctica es diseñar y posteriormente realizar un controlador digital LQR en tiempo

real de un motor DC, implementado en una tarjeta Arduino Mega.

El modelo simplificado en el espacio de estados, de un motor DC, suponiendo que L=0, es:

Los parámetros del motor son:

J=0.00012; Kg.m2
R=3.8 ohm
L=3.6E-3 mH ( aprox =0 )
K=0.004; % V/(rad.seg)
b=0.0002 Nms;

1ª parte de la Práctica

Realizar un programa Matlab para diseñar y simular inicialmente un controlador discreto LQR de la
posición angular del motor (configuración servo).

• El periodo de muestreo debe ser de Ts=0.005 seg.

• La consigna será de 2*pi radianes (un giro de una vuelta del eje del motor)
• Hay que obtener valores adecuados de Kd (K discreta) para ajustar los valores Q y R del
controlador LQR de forma que:
o La respuesta de la salida debe tener un tiempo de asentamiento ts < 1 seg y un
sobredisparo mínimo.
o El voltaje de salida del controlador hacia el motor no debe sobrepasar los 8 voltios.
• Representar las señales simuladas de: ángulo del eje, velocidad angular, y Voltaje del motor.
2ª parte de la Práctica

Implementación y prueba del controlador LQR digital previamente diseñado, sobre un motor DC,
mediante el test de un programa realizado en C++ y ejecutado en una tarjeta Arduino Mega.

El motor dispone de un “encoder” incremental que suministra 1040 pulsos por cada vuelta de eje
principal del motor. (Para obtener el ángulo del eje en radianes, se multiplica el nº de pulsos leídos
por: (2*pi/1040).

El “encoder” del motor permite la lectura del ángulo de giro (y posteriormente de la velocidad
angular, realizando una derivada discreta mediante el método “Backward-Euler”).

Se puede detectar también el sentido de giro con las señales suministradas por el encoder. La
detección de flacos ascendentes o descendentes se realiza mediante interrupciones originadas por
cambios en las señales de los pines de entrada 2 y 3.
El motor necesita un “driver” de potencia implementado con un puente H, mediante el circuito
integrado Toshiba TB6612FNG, que puede controlar hasta 2 motores en forma independiente.

La velocidad del motor se regula mediante una señal PWM, y el sentido de giro se controla con dos
señales (AIN1 y AIN2):

Tareas a realizar:

• El alumno deberá disponer del IDE de Arduino y de la librería TimerOne.h, y se conectará a

través del puerto serie USB con la tarjeta Arduino Mega suministrada, que dispone además
del driver del motor y de las conexiones con el encoder.
• Se suministra un programa en C++ casi completo para realizar el control de posición angular
con realimentación en el espacio de estados. Después de estudiar el programa y comprender
su estructura, el alumno deberá completar el programa, añadiendo:
o Los valores de Kd (K discreta) calculados en la primera parte de la práctica, que
cumplan las condiciones de diseño recomendadas.
o El cálculo de la derivada discreta, para obtener la velocidad angular (w) a partir del
valor del ángulo para la muestra actual (teta[0]) y del valor correspondiente a la
muestra anterior (teta[1]).
o El cálculo del voltaje que se envía al motor.
• Una vez terminado el programa, se transferirá a la tarjeta Arduino y se visualizará:
o El giro del motor para consignas de
 Pi, 2pi y 4 pi radianes sucesivamente
 La evolución dinámica del ángulo del motor (mediante la herramienta “serial
plotter” del IDE de Arduino, comparándola con la señal simulada obtenida en
la primera parte de la práctica

A continuación se suministra el programa en C++ que deberá terminarse y compilarse para Arduino
Mega. La comunicación serie para la transferencia a la tarjeta Arduino se realizará a través del puerto
serie adecuado. Se deberán completar las instrucciones marcadas en amarillo.
//Control_LQR_servo-motor
// Variables de estado: angulo (teta) y velocidad angular(w)
#include <TimerOne.h>
#include <Arduino.h>
//Direcciones pines
#define CHA 2 //Canal A del encoder del motor
#define CHB 3 //Canal B del encoder del motor
#define PWM_Motor 9 //Salida PWM para controlar la velocidad del motor
#define AIN1 11 //Enable AIN1 del puente H del motor
#define AIN2 10 //Enable AIN2 del puente H del motor
//Parametros
const float Ts = 0.005; // Periodo de muestreo = 5 miliseg
const float Tm = 5000; // 5000 micro seg = 0.005 seg (para interrupciones periodicas)
int teta_pulsos = 0; // nº cuentas del encoder
double teta[2] = {0,0}; // angulo actual (rad) motor(teta[0]) y valor anterior(teta[1])
double w=0; // velocidad de giro del motor (rad/seg)
float r=6.28318; // consigna del ángulo del servo: una vuelta completa=2.pi
double V=0; // Voltaje enviado al motor (volts)
byte valorPWM8 = 0; //ValorPWM para tension de salida en valor absoluto

float kd[2] = {X,X}; // Incluir constante Kd diseñada

void setup() {
pinMode(2, INPUT_PULLUP);
pinMode(3, INPUT_PULLUP);
pinMode(AIN1,OUTPUT);
pinMode(AIN2,OUTPUT);
pinMode(PWM_Motor,OUTPUT);
//Comenzamos con el motor parado
digitalWrite(AIN1,HIGH);
digitalWrite(AIN2,HIGH);
//La lectura de los encoder se realizara por interrupciones, una por cada canal
attachInterrupt(0, Encoder_CHA, CHANGE); // INTERRUP MEGA PIN 2
attachInterrupt(1, Encoder_CHB, CHANGE); // INTERRUP MEGA PIN 3
//Configuramos el temporizador para saltar la rutina de servicio cada 5ms
Timer1.initialize (Tm);
Timer1.attachInterrupt(control);// La rutina control se ejecuta cada Tm miliseg
Serial.begin(115200);// % baudios puerto serie
teta_pulsos = 0;
}

void loop()
{
}
void control() {
// valores del angulo del eje
teta[1] = teta[0];// actualización del valor anterior
// teta[0] es el valor de teta para la muestra actual
teta[0] = float(teta_pulsos * 0.0060415); // (2*pi/1040)=0.0060415
w= // derivada con método backward Euler

//cálculo del voltaje (salida del controlador)

V=
 //Acotamos la salida de tension hacia el motor
// Voltaje máximo fuente de alimentación = 8 volts
if (V > 8) { V= 8; }
if (V< -8){ V= -8;}

valorPWM8 = byte(abs(V)*31.875); // 31.875=255/8volts

analogWrite(PWM_Motor, valorPWM8);
if (V > 0) {
digitalWrite(AIN1, LOW); // giro en un sentido
digitalWrite(AIN2, HIGH); }
if (V < 0) {
digitalWrite(AIN1, HIGH); // giro en sentido contrario
digitalWrite(AIN2, LOW);}
// Se envía a continuación angulo actual por puerto serie
// para monitorizar la grafica con "Serial Plotter"
Serial.println(teta[0]);
}
//___________________________________________________________
// Rutinas para lectura de los pulsos del encoder
// ___________________________________________________________

//Lectura del canal A del encoder del motor en cada transicion

void Encoder_CHA() {
if (digitalRead(CHA) == digitalRead(CHB)) {teta_pulsos--;}
else {teta_pulsos++; }
}
//Lectura del canal B del encoder del motor en cada transicion
void Encoder_CHB() {
if (digitalRead(CHA) != digitalRead(CHB)) {teta_pulsos--;}
else {teta_pulsos++;}
}