% explicación de la necesidad de calcular la respuesta en frecuencia a
% través del ejemplo del control de velocidad de un coche. Introducimos
% cambios en la referencia al acelerador con una entrada de forma
% sinusoidal a una determinada frecuencia. A frecuencias bajas, la salida
% puede sigue a la consigna, alcanzando el régimen permanente, segun vamos
% aumentando la frecuencia, a la salida no le da tiempo a alcanzar a la
% señal, llegando un punto en el cual la salida no varia su valor.


k = 1;                                                  % ganancia estática
bw = 1;                                                 % ancho banda (Hz)
T = 1/(bw*2*pi);                                        % cte de tiempo


primer_order = 0;
if(primer_order == 1)
    % sistema 1er orden
    num=[k];
    den=[T 1];
    ymax = 1.2;
else
    % sistema 2o orden
    wn = bw*2*pi;
    chi = cos(85*pi/180);
    num=[k*wn^2];
    den=[1 2*chi*wn wn^2];
    ymax = k*wn;
end

% entrada
t_end=10;
t_points=1000;
t = linspace(0,t_end,t_points+1);
t = t(1:end-1);
A = 1;                                                          % amplitud
f0 = 0;                                          % frecuencia inicial (Hz.)
f1 = 0.5;                                          % frecuencia final (Hz.) 
t1 = 1;                                                    % tiempo para f1

var_freq=1
if(var_freq==1)
    % señal chirp con variación lineal en la frecuencia
    u = A*chirp(t,f0,t1,f1,'linear',-90);
    fmax=f1*t_end/t1;
    fchirp=linspace(0,fmax,t_points+1);
    fchirp=fchirp(1:end-1);
    fref=fchirp;
else
    u = A*cos(2*pi*f0*t);
    fref=f0*ones(1,t_points);
    fmax=f0*1.1;
    if(fmax == 0)
        fmax = 0.1;
    end
end


% salida

y = lsim(num,den,u,t);

fig1=figure(1);
subplot(211);
u_h=plot(-1,u(1));grid on;
axis([0 t_end -ymax ymax]);
ylabel('(m/s)');
title('ref. velocidad');
xlabel('tiempo (s)');
subplot(212);
y_h=plot(-1,y(1));grid on;
axis([0 t_end -ymax ymax]);
xlabel('tiempo (s)');
ylabel('(m/s)');
title('velocidad');
set(fig1,'Color',[1 1 1],'Position',[151   295   672   504]);
% frecuencia de la señal de entrada
fig2=figure(2);
f_h=plot(-1,fref(1));grid on;
axis([0 t_end 0 fmax]);
set(fig2,'Color',[1 1 1],'Position',[782   405   405   240]);
xlabel('tiempo (s)');
ylabel('(Hz)');
title('frecuencia ref. velocidad');
for cc=1:t_points
    set(u_h,'XData',t(1:cc),'YData',u(1:cc),'Marker','.');
    set(y_h,'XData',t(1:cc),'YData',y(1:cc),'Marker','.');
    set(f_h,'XData',t(1:cc),'YData',fref(1:cc),'Marker','.');
    drawnow;
end

figure(3);
bode(num,den);grid on;

[y,ti] = impulse(tf(num,den));

sound(y,1/(ti(2)-ti(1)));



