% cruise_pzcancel.m
% Simulation of PI cruise control with cancellation of process pole
% kja 070709

aminit;
cruise_conpar;           %Get controller parameters
cruise_lin;               %Get parameters of linear model

%Simulation parameters
tmax=40;dt=tmax/1000;
t=0:dt:tmax;
%Generate input
u=max(0,t-5);u=min(u,1);
dd=u*4*pi/180;

%Dynamics with P control
Ap=[a-b*kp 0;-1 0];
Bp=[-bg;0];
Cp=[1 0;-kp 0];
Dp=[0;0];

%Dynamics with PI control
A=[a-b*kp b*ki;-1 0];
B=[-bg;0];
C=[1 0;-kp ki];
D=[0;0];

%Dynamics with PI control with cancellation
ki=-kp*a;
Ac=[a-b*kp b*ki;-1 0];
Bc=[-bg;0];
Cc=[1 0;-kp ki];
Dc=[0;0];

%Simulate systems with P and PI control
yp=lsim(Ap,Bp,Cp,Dp,dd,t);
ypi=lsim(A,B,C,D,dd,t);
ypic=lsim(Ac,Bc,Cc,Dc,dd,t);

%Plot
figure(1); clf; subplot(321)
pl=plot(t,ypic(:,1) + vref, '-', t,ypi(:,1) + vref,'--');
set(pl,'Linewidth', AM_data_linewidth);
ylabel('velocity');xlabel('time');
amaxis([0 40 -2+vref 0.2+vref]);
amprint('cruise-pzcancelv.eps');

figure(2); clf; subplot(321)
pl=plot(t,ypic(:,2),'-', t,ypi(:,2),'--');
set(pl,'Linewidth', AM_data_linewidth);
ylabel('throttle');xlabel('time');
amaxis([0 40 0 0.7]);

lgh = legend('ki = 0.0051 ', 'ki = 0.5 ', 'Location', 'southeast');
legend(lgh, 'boxoff');

amprint('cruise-pzcancelu.eps');
