% This program controls a nonlinear system using slding mode control.

% Fundamentals of Spacecraft Attitude Determination and Control by Markley and Crassidis
% Example E.3

% Written by John L. Crassidis 9/13

% Other Required Routines: sat1.m, slide_fun.m

% Time
dt=0.01;tf=50;t=[0:dt:tf]';mt=length(t);

% Initial Conditions
xd=zeros(mt,2);xd(1,:)=[1 0];
x=zeros(mt,2);x(1,:)=[0 0];
xs=zeros(mt,2);xs(1,:)=[0 0];
u=zeros(mt,1);us=zeros(mt,1);

% True and Assumed Model Parameters
m=1;c=2;k1=1;k2=0.5;
ma=1;ca=3;ka1=1.5;ka2=0.75;

% Sliding Mode Parameters
lam=3;eps=1;k=1;

% Main Loop
for i = 1:mt-1
    
% Desired Trajectories
f1=dt*slide_fun(xd(i,:),m,c,k1,k2,0);
f2=dt*slide_fun(xd(i,:)+0.5*f1',m,c,k1,k2,0);
f3=dt*slide_fun(xd(i,:)+0.5*f2',m,c,k1,k2,0);
f4=dt*slide_fun(xd(i,:)+f3',m,c,k1,k2,0);
xd(i+1,:)=xd(i,:)+1/6*(f1'+2*f2'+2*f3'+f4');
 
% Sliding Model Controller (signum)
x_ddc=-k1/m*xd(i,1)-k2/m*xd(i,1)^3-c/m*xd(i,2)*abs(xd(i,2));
f=-ka1/ma*x(i,1)-ka2/ma*x(i,1)^3-ca/ma*x(i,2)*abs(x(i,2));
s=(x(i,2)-xd(i,2))+lam*(x(i,1)-xd(i,1));
u_bar=-f+x_ddc-lam*(x(i,2)-xd(i,2));
u(i)=u_bar-k*sign(s);

% Actual Trajectories 
f1=dt*slide_fun(x(i,:),ma,ca,ka1,ka2,u(i));
f2=dt*slide_fun(x(i,:)+0.5*f1',ma,ca,ka1,ka2,u(i));
f3=dt*slide_fun(x(i,:)+0.5*f2',ma,ca,ka1,ka2,u(i));
f4=dt*slide_fun(x(i,:)+f3',ma,ca,ka1,ka2,u(i));
x(i+1,:)=x(i,:)+1/6*(f1'+2*f2'+2*f3'+f4');

% Sliding Model Controller (sat)
x_ddc=-k1/m*xd(i,1)-k2/m*xd(i,1)^3-c/m*xd(i,2)*abs(xd(i,2));
f=-ka1/ma*xs(i,1)-ka2/ma*xs(i,1)^3-ca/ma*xs(i,2)*abs(xs(i,2));
s=(xs(i,2)-xd(i,2))+lam*(xs(i,1)-xd(i,1));
u_bar=-f+x_ddc-lam*(xs(i,2)-xd(i,2));
us(i)=u_bar-k*sat1(s,eps);

% Actual Trajectories 
f1=dt*slide_fun(xs(i,:),ma,ca,ka1,ka2,us(i));
f2=dt*slide_fun(xs(i,:)+0.5*f1',ma,ca,ka1,ka2,us(i));
f3=dt*slide_fun(xs(i,:)+0.5*f2',ma,ca,ka1,ka2,us(i));
f4=dt*slide_fun(xs(i,:)+f3',ma,ca,ka1,ka2,us(i));
xs(i+1,:)=xs(i,:)+1/6*(f1'+2*f2'+2*f3'+f4');

end

% Sliding Model Controller (last time step)
x_ddc=-k1/m*xd(i+1,1)-k2/m*xd(i+1,1)^3-c/m*xd(i+1,2)*abs(xd(i+1,2));
f=-ka1/ma*x(i+1,1)-ka2/ma*x(i+1,1)^3-ca/ma*x(i+1,2)*abs(x(i+1,2));
s=(x(i+1,2)-xd(i+1,2))+lam*(x(i+1,1)-xd(i+1,1));
u_bar=-f+x_ddc-lam*(x(i+1,2)-xd(i+1,2));
u(i+1)=u_bar-k*sign(s);

f=-ka1/ma*xs(i+1,1)-ka2/ma*xs(i+1,1)^3-ca/ma*xs(i+1,2)*abs(xs(i+1,2));
s=(xs(i+1,2)-xd(i+1,2))+lam*(xs(i+1,1)-xd(i+1,1));
u_bar=-f+x_ddc-lam*(xs(i+1,2)-xd(i+1,2));
us(i+1)=u_bar-k*sat1(s,eps);

% Plot Results
clf
plot(t,xd(:,1)-x(:,1))
set(gca,'fontsize',12)
axis([0 50 -0.2 1])
set(gca,'ytick',[-0.2 0 0.2 0.4 0.6 0.8 1])
ylabel('Position Error','Interpreter','LaTex','fontsize',14)
xlabel('Time (Sec)','Interpreter','LaTex','fontsize',14)

pause

plot(t,u)
set(gca,'fontsize',12)
xlabel('Time (Sec)','Interpreter','LaTex','fontsize',14)
ylabel('Control Input','Interpreter','LaTex','fontsize',14)

pause

plot(t,xd(:,1)-xs(:,1))
set(gca,'fontsize',12)
axis([0 50 -0.2 1])
set(gca,'ytick',[-0.2 0 0.2 0.4 0.6 0.8 1])
ylabel('Position Error','Interpreter','LaTex','fontsize',14)
xlabel('Time (Sec)','Interpreter','LaTex','fontsize',14)

pause

plot(t,us)
set(gca,'fontsize',12)
xlabel('Time (Sec)','Interpreter','LaTex','fontsize',14)
ylabel('Control Input','Interpreter','LaTex','fontsize',14)

pause

clf
dx1=x(:,1)-xd(:,1);dx2=x(:,2)-xd(:,2);
plot(dx1,dx2)
set(gca,'fontsize',12)
axis([-1.2 0.2 -0.05 0.35])
set(gca,'xtick',[-1.2 -1 -0.8 -0.6 -0.4 -0.2 0 0.2])
set(gca,'ytick',[-0.05 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35])
set(gcf,'Units','normalized')
set(gca,'Units','normalized')
ax=axis;
ap=get(gca,'Position');
p1=21;
xo1=[dx1(p1)+0.15,dx1(p1)];
yo1=[dx2(p1),dx2(p1)];
xp1=(xo1-ax(1))/(ax(2)-ax(1))*ap(3)+ap(1);
yp1=(yo1-ax(3))/(ax(4)-ax(3))*ap(4)+ap(2);
ah1=annotation('arrow',xp1,yp1);
p2=310;
xo2=[dx1(p2)-0.15,dx1(p2)];
yo2=[dx2(p2),dx2(p2)];
xp2=(xo2-ax(1))/(ax(2)-ax(1))*ap(3)+ap(1);
yp2=(yo2-ax(3))/(ax(4)-ax(3))*ap(4)+ap(2);
ah2=annotation('arrow',xp2,yp2);
text(dx1(p1)+0.16,dx2(p1),'Reaching Phase')
text(dx1(p2)-0.43,dx2(p2),'Sliding Phase')
xlabel('$\Delta x$','Interpreter','LaTex','fontsize',14)
ylabel('$\Delta \dot{x}$','Interpreter','LaTex','fontsize',14)
