clear all
close all
clc

% Sliding Mode Control Lecture - Though Experiment #1
% First Order Plant, Proportional Control with Actuator Limits
% Austin Gurley 2018

%%  Effort Limited Proportional Control 1st Order Plant

dt = 0.001; % time step
t = 0:dt:3; % time vector

v = 1;    % velocity
dv = 0;
u = 0;      % control effort

k = 4;

uMax = 1.0;
uMin = -1.0;

for i = 1:(length(t)-1)
  % plant simulation
  dv(i+1) = u(i);
   v(i+1) = v(i) + dv(i+1)*dt;
   
  % control update
  u(i+1) = -k*v(i+1);
  u(i+1) = min(uMax,max(uMin,u(i+1)));
  
end

figure

subplot(2,1,1)
plot(t,v,'b','lineWidth',2)
xlabel('time (s)')
ylabel('v')
title('First Order Plant, P Control with Effort Limits')
grid on

subplot(2,1,2)
plot(t,u,'b','lineWidth',2)
xlabel('time (s)')
ylabel('control effort')
grid on

   
%% Repeat with very high gain

v = 1;    % velocity
dv = 0;
u = 0;      % control effort

k = 400;

uMax = 1.0;
uMin = -1.0;

for i = 1:(length(t)-1)
  % plant simulation
  dv(i+1) = u(i);
   v(i+1) = v(i) + dv(i+1)*dt;
   
  % control update
  u(i+1) = -k*v(i+1);
  u(i+1) = min(uMax,max(uMin,u(i+1)));
  
end

subplot(2,1,1)
hold on
plot(t,v,'r','lineWidth',2)
legend('k=4','k=400')

subplot(2,1,2)
hold on
plot(t,u,'r','lineWidth',2)
legend('k=4','k=400')

   
%% Repeat with disturbance

v = 1;    % velocity
dv = 0;
u = 0;      % control effort

k = 400;

uMax = 1.0;
uMin = -1.0;

for i = 1:(length(t)-1)
  % plant simulation
  dv(i+1) = u(i)-0.2;
   v(i+1) = v(i) + dv(i+1)*dt;
   
  % control update
  u(i+1) = -k*v(i+1);
  u(i+1) = min(uMax,max(uMin,u(i+1)));
  
end

subplot(2,1,1)
hold on
plot(t,v,'k','lineWidth',2)
legend('k=4','k=400','k=400 w/disturbance')

subplot(2,1,2)
hold on
plot(t,u,'k','lineWidth',2)
legend('k=4','k=400','k=400 w/disturbance')