%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% CONTROL MANUAL E AUTOMATICO DE PENDULO INVERTIDO

%
%
%
%
%
% Desenvolvido por:
Marco Antonio Colaneri Junior %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear;
close all;
clc;
disp('Instruoes:')
pause(1);
disp('- Digite ''0'' para desativar o sistema de controle')
pause(1);
disp('- Digite qualquer outra tecla para reativar o sistema de controle')
pause(1);
disp(' ')
disp('Controle manual: atuar sobre a barra deslizante para modificar a base do p
endulo.')
pause(1);
% PARMETROS DO PNDULO
l = input('Digite o tamanho da barra do pendulo (em metros): ');
% Tamanho
da barra do pendulo
% l = 1.5;
m = input('Digite a massa do pendulo (em Kg): ');
% Massa do pe
ndulo
% m = 1;
J = m*l^2;
% Momento de inercia
referente ao eixo
B = 1;
% Coeficiente de frica
o
g = 9.8;
% Aceleraao da gravida
de
% ESTADO INICIAL DO PENDULO
x = [pi-0.1;0];
% Para ser visto o efeito do controle, comeamos
% com o pendulo quase vertical (theta = pi +/- "algo")
% DEFINIAO DE UM SLIDER PARA CONTROLAR O PENDULO MANUALMENTE
f = figure(1);
set(f,'pos',[200,200,700 700]);
h = uicontrol('style','slider','pos',[20 20 680 20],'min',-4,'max',4);
Tm = 0.01;

% Perodo de amostragem

e0 = zeros(2,1);
ep0 = zeros(2,1);

% Condioes iniciais do controle do angulo

% Condioes iniciais do controle da posiao

x0 = [0;0];
a0 = [0;0];

% Condioes iniciais do pendulo

xmin = -2;
xmax = +2;
y = x(1);

% PARAMETROS DO CONTROLE PID DO ANGULO

Kp = 10;
Ki = 100;
Kd = .1;
s = tf('s');
Cth = c2d(Kp + Ki/s + Kd*s/(0.01*s+1),Tm,'tustin');
% PARAMETROS DO CONTROLE PID DA POSIAO DA BASE
Cpos = c2d(.04 + 0.0001/s + s*0.0001/(0.01*s+1),Tm,'tustin');
pos = 0;
% Valor inicial da posiao da base do pendulo
k = 2;
% Comeamos com k=2 para ter acesso a pelo menos duas amostragens ante
riores
while 1,
k = k + 1;
%

% BUFFER COM A POSIAO (X(3) TEM A POSIAO ATUAL)

% X(k) = get(h,'Value');
e(k) = pi-y;
% RESTRINGIMOS O VALOR DO ERRO AO INTERVALO [-pi,pi]
e(k) = mod(e(k)+pi,2*pi)-pi;
% CONTROLE DA POSIAO DA BASE DO PENDULO
ep = 0-pos;
[dpos,ep0] = filter(Cpos.num{1},Cpos.den{1},ep,ep0);
e(k) = e(k) - dpos;

% SELEAO DE CONTROLE MANUAL/AUTOMATICO

tecla = get(f,'currentchar');
switch tecla
case '0'
pos = get(h,'value');
otherwise
[pos,e0] = filter(Cth.num{1},Cth.den{1},e(k),e0);
end
% Suavizador da aceleraao (muito conveniente, porque o movimento do
% objeto slider com um tempo se movimenta em saltos, dando lugar a segundas
% derivadas muito elevadas)
[X(k),x0] = filter(.01,poly([.9 .9]),pos,x0);
[a,a0] = filter((1/Tm^2)*[1 -2 1],[1 0 0],X(k),a0);
A(k) = a;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% EQUAOES NO ESPAO DE ESTADO (NAO LINEARES) DO PENDULO
u = -a;
% Atribuimos a entrada
% Equaao de estados

x(1) = x(1) + Tm*x(2);

x(2) = x(2) + Tm*(1/J*(-B*x(2)-m*g*l*sin(x(1))+m*u*l*cos(x(1))));
% Equaao de saida
y
= x(1);
th = y;

% Atribuimos a saida

% REPRESENTAAO GRAFICA DA SIMULAAO

figure(1);
plot(X(k),0,'.');
hold on;
p1 = X(k);
p2 = X(k)+l*exp(j*(th-pi/2));
line(real([p1,p2]),imag([p1,p2]));
plot(real(p2),imag(p2),'.','markersize',40);
hold off;
% Sugestao: pode-se desenhar tambem outras flechas indicando, em tempo real,
as foras reais
% ou de inercia que atuam em cada elemento do sistema
% Localizador automatico da perspectiva sobre o objeto de controle
if X(k)>xmax-1
xmin = xmin + 0.1;
xmax = xmax + 0.1;
elseif X(k)<xmin+1
xmin = xmin - 0.1;
xmax = xmax - 0.1;
end
grid on;
axis([xmin-3 xmax+3 -5 5]);
% Atualizaao das imagens
drawnow;
end