clear all
close all
clc

% Sliding Mode Control Lecture - Thought Experiment #2
% 2nd Order Plant, Proportional Control Trick
% Austin Gurley 2018
%
% Example adapted from 'Sliding Mode Control: Theory and Applications'


%% Proportional Control of Double Integrator
% > 'marginally stable' i.e. useless, locus is on Imaginary axis
% > control gain determines shape of phase plane: k<1 = wide, k>1 = tall

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

x1 = 1;     % position
x2 = -2;    % velocity
dx1 = x2;
dx2 = 0;
y1 = x1;    % position measurement
y2 = x2;    % velocity measurement
u = 0;      % control effort

%k = 0.2; % wider (higher displacement)
k = 1.0; % circular (equal velocity and displacement)
%k = 2.0; % narrow (higher velocity)

for i = 1:(length(t)-1)
  % plant simulation
  dx2(i+1) = u(i);
  dx1(i+1) = x2(i);
   x2(i+1) = x2(i) + dx2(i+1)*dt;
   x1(i+1) = x1(i) + dx1(i+1)*dt;
   
  % control update
  u(i+1) = -k*x1(i+1);
  
end

figure

subplot(2,1,1)
plot(t,x1,'b','lineWidth',2)
hold on
plot(t,x2,'b--','lineWidth',2)
legend('x_1','x_2')
xlabel('time (s)')
title('P Control - Linear')
grid on

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

subplot(2,2,4)
plot(x1,x2,'b','lineWidth',2)
xlabel('x_1')
ylabel('x_2')
axis equal
grid on

%% Proportional Control with Varying Gain
% > variable structure control with clever choice of switching conditions
% > shows, with special considerations, P effort can stabilize the system

x1 = 1;     % position
x2 = -2;    % velocity
dx1 = x2;
dx2 = 0;
y1 = x1;    % position measurement
y2 = x2;    % velocity measurement
u = 0;      % control effort

k1 = 0.2;
k2 = 2.0;

for i = 1:(length(t)-1)
  % plant simulation
  dx2(i+1) = u(i);
  dx1(i+1) = x2(i);
   x2(i+1) = x2(i) + dx2(i+1)*dt;
   x1(i+1) = x1(i) + dx1(i+1)*dt;
   
  % control update
  if (x1(i+1)*x2(i+1)>0)
    u(i+1) = -k2*x1(i+1);
  else 
    u(i+1) = -k1*x1(i+1);
  end
  
end

figure

subplot(2,1,1)
plot(t,x1,'b','lineWidth',2)
hold on
plot(t,x2,'b--','lineWidth',2)
legend('x_1','x_2')
xlabel('time (s)')
title('P Control - Varying Structure')
grid on

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

subplot(2,2,4)
plot(x1,x2,'b','lineWidth',2)
hold on
plot([-1.5 1],[0 0],'r--','lineWidth',2)
plot([0 0],[-2.25 1],'r--','lineWidth',2)
xlabel('x_1')
ylabel('x_2')
grid on