% part 5 of the mini project: 
% Prabir Barooah, March 20, 2009

clear all
%conversion: 
mph2mpersec = 0.44704; %m/sec
mpersec2mph = 1/mph2mpersec;

m = 1300; %kg
b = 42926.2; %N/volt
a = 150.91; %30.17; % N/m^2 s^2 

v_0 = 60*mph2mpersec; %m/s
theta_0 = a*v_0^2/b; %volt

%more constants
p= 2*a*v_0/m;
k = b/m;

%PI controller - previously designed
kp = 0.01;
ki = 0.4341;

% the closed loop transfer function from r to y 
P = tf(k,[1 p]);
C = tf([kp ki],[1 0]);
H_ry = feedback(series(P,C),1);
% since we need to plot u's, also construct 
% the closed loop transfer function from r to u:
H_ru = feedback(C,P);

step_sizes = [0.2 0.5 0.8 1];
time = [0:0.01:10]';

fig_y  =  figure;
fig_u = figure;
fig_theta = figure;

ph = [];
ph_u = [];
ph_theta = [];
for ii=1:length(step_sizes)
    step_command = step_sizes(ii)*ones(length(time),1);
    
    step_resp_ii = lsim(H_ry,step_command,time);
    
    u = lsim(H_ru,step_command,time);
    
    figure(fig_y);
    hold on;
    ph(ii) = plot(time,step_resp_ii);
    
    figure(fig_u);
    hold on;
    ph_u(ii) = plot(time,u);
    
    figure(fig_theta);
    hold on;
    ph_theta(ii) = plot(time,u+theta_0);
    
end;
figure(fig_y);
set(ph(1),'color','b','linestyle','-');
set(ph(2),'color','b','linestyle','--');
set(ph(3),'color','r','linestyle','-.');
set(ph(4),'color','m','linestyle',':','linewidth',2);
legend(ph,'0.2','0.5','0.8','1');
set(gca,'fontsize',18);
xlabel('t (sec)');
ylabel('y (m/s)');
title('step ');


figure(fig_u);
set(ph_u(1),'color','b','linestyle','-');
set(ph_u(2),'color','b','linestyle','--');
set(ph_u(3),'color','r','linestyle','-.');
set(ph_u(4),'color','m','linestyle',':','linewidth',2);
legend(ph_u,'0.2','0.5','0.8','1');
set(gca,'fontsize',18);
xlabel('t (sec)');
ylabel('u (Volt)');

figure(fig_theta);
set(ph_theta(1),'color','b','linestyle','-');
set(ph_theta(2),'color','b','linestyle','--');
set(ph_theta(3),'color','r','linestyle','-.');
set(ph_theta(4),'color','m','linestyle',':','linewidth',2);
legend(ph_theta,'0.2','0.5','0.8','1');
set(gca,'fontsize',18);
xlabel('t (sec)');
ylabel('\theta (Volt)');