% cruisepi_lin_nl_Ch3.m - PI control with antiwindup for linear and models
% kja 060726

aminit;

cruise_conpar;
cruise_lin;
theta_d = 4;				% slope of hill

%
% PI controller simulation of linear and nonlinear models
%

[t, x] = ode45('cruise_clsysode', [0 30], [v_e u_e v_e u_e]);
vd = vref * ones(length(t));
vert = [18; 22];                    % vertical line

subplot(421);
% Add reference lines (underneath)
hold on;
pl = plot(t,vd,'k-',[5, 5],vert,'k--');
set(pl, 'LineWidth', AM_ref_linewidth);

pl=plot(t,x(:,1),'b-');
set(gca,'Ytick',[19 20]);
set(pl,'Linewidth', AM_data_linewidth)

% Set up axis limits, etc
amaxis([0 30 18.8 20.5]);
grid off; box on;
xlabel('Time {\itt} [s]');
ylabel('Velocity {\itv} [m/s]');

% Compute control signal for plotting
e = vref -x(:,1);                   % speed error
u = kp * e + x(:,2);                % throttle
us=max(u,0);us=min(us,1);           % saturate throttle  
ul=kp*(vref-x(:,3))+x(:,4);         % throttle for linear model 
usl=max(ul,0);usl=min(usl,1);

subplot(423)
pl=plot(t,us,'b-');
set(gca,'Ytick',[0 1]);
set(pl,'Linewidth', AM_data_linewidth);

hold on; pl = plot([5, 5], [0, 1], 'k--');
set(pl,'Linewidth', AM_ref_linewidth);
grid off; box on;

amaxis([0 30 0 1.0]);
xlabel('Time {\itt} [s]');
ylabel('Throttle {\itu}');

amprint('cruisepi-lin-nl-ch3.eps');
