% CONTROL MANUAL Y AUTOMÁTICO DE PÉNDULO INVERTIDO
%
% Este script ejecuta la simulación de un péndulo que puede ser controlado manualmente (accionando
% una barra deslizante o "slider").  
%
% La simulación está basada en la suposición de que el intervalo de simulación (tiempo de ejecución
% de cada bucle) es aproximadamente constante e igual a Tm. Esto puede variar en función de la CPU y
% de los procesos que estén corriendo en la máquina que lo ejecute. El objetivo no es una simulación
% perfecta, sino el aprendizaje de los conceptos de teoría de sistemas y control a través del
% fomento de la creatividad del alumno. 
%
% Este ejemplo ha sido probado en MATLAB Version 7.0.4.365 (R14) sobre operativo Windows XP v.5.1.
%
% El script puede servir de base para que el alumno ensaye otros tipos de sistemas los pruebe 
% creando a partir de éste otros ejemplos interactivos mediante el procedimiento de "cortar y pegar".
%
%
% Fecha: 2006-11-02
% Autor: Ignacio Díaz
% Area de Ingeniería de Sistemas y Automática
% Universidad de Oviedo


clear;
close all;
clc;

disp('Instrucciones:');
disp('El objetivo es mantener el péndulo en posición vertical');
disp('mediante el movimiento de la base, manteniéndolo además');
disp('cerca de la posición de origen (x=0).');
disp(' ');
disp('- Pulsar ''+'' para aumentar  la longitud del péndulo (control más fácil)');
disp('- Pulsar ''-'' para disminuir la longitud del péndulo (control más difícil)');
disp(' ');
disp('Control manual: actuar sobre la barra deslizante para modificar la base del péndulo.');
pause(1);    


% PARÁMETROS DEL PÉNDULO
l = 2;          % Longitud del péndulo
m = 1;          % Masa del péndulo
J = m*l^2;      % Momento de inercia referido al eje
B = 1;        % Coeficiente de fricción
g = 10;         % Aceleración de la gravedad


% ESTADO INICIAL DEL PÉNDULO
x = [pi-0.1;0];     % Para que se vea el efecto del control, empezamos 
                    % con el péndulo casi vertical (theta = pi +/- "algo")


% DEFINICIÓN DE UN "SLIDER"  PARA CONTROLAR MANUALMENTE EL PÉNDULO
f = figure(1);
set(f,'pos',[100,100,700 700],'windowstyle','modal');
h = uicontrol('style','slider','pos',[20 20 680 20],'min',-4,'max',4);


Tm = 0.01;              % Período de muestreo



x0 = [0;0];             % Condiciones iniciales del péndulo
a0 = [0;0];

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






pos = 0;    % Valor inicial de la posición de la base del péndulo
k = 2;      % Empezamos en k=2 para tener acceso al menos a dos muestras anteriores
while 1,
    k = k + 1;


    % Obtenemos el valor actual de la posición de la base consultando 
    % el valor del objeto 'slider'
    pos = get(h,'value');


    % CAMBIO ON-LINE DE PARÁMETROS DE LA SIMULACIÓN (ej. longitud del péndulo)
    tecla = get(f,'currentchar');
    switch tecla
        case '+'
            l = l+.5;
            J = m*l^2;
            set(f,'currentchar','0');
        case '-'
            l = l-.5;
            J = m*l^2;
            set(f,'currentchar','0');
    end



    % Suavizado de la aceleración (muy conveniente, porque el movimiento del
    % objeto "slider" con un ratón se produce a saltos, dando lugar a segundas
    % derivadas muy 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;




    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % ECUACIONES EN ESPACIO DE ESTADOS (NO LINEALES) DEL PÉNDULO
    u = -a;         % Asignamos la entrada

    % Ecuación 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))));
    % Ecuación de salida
    y    = x(1);

    th = y;         % Asignamos la salida
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%




    % REPRESENTACIÓN GRÁFICA DE LA SIMULACIÓN
    figure(1);
    plot(X(k),0,'.',0,0,'+','markersize',20,'linewidth',2);
    hold on;
    flecha(X(k),X(k)+l*exp(j*(th-pi/2)),0.3);
    hold off;
    % Sugerencia: pueden dibujarse también otras flechas indicando en tiempo real las fuerzas reales
    % o de inercia que actúan en cada elemento del sistema

    % Centrado automático de la perspectiva sobre el objeto de control
    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]);

    % Refresco de la imagen
    drawnow;
    
    % Theta(:,k) = th;
    % xvec(:,k) = x;

end






