%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This file specifies the F in y'=F(t,y) for the stable indirect adaptive controller
function dzdt=sdirwingrockc(t,z)

% Define variables to be global from associated program
global a1 a2 a3 a4 a5 b tau beta0 beta1
global pu R
global c
global sigma
global k1 k0 gamma
global Wu
global Am
global etau

% Define the various subvectors of the overall closed-loop system state vector

x=z(1:3);
ym=z(4:7);

thetau=z(length(x)+length(ym)+1:length(x)+length(ym)+pu);

% Controller

% Define the phi vector for the alpha approximator

% Next, use only x(1) an x(2) as inputs to the premise

for i=1:R
	mu(i,1)=exp(-0.5*((x(1)-c(1,i))/sigma)^2)*exp(-0.5*((x(2)-c(2,i))/sigma)^2);
end
	
denominator=sum(mu(i,1)); 

xi=(1/denominator)*mu(:,1);

% To use all of the state of the plant to the consequents:

phi=[ xi', x(1)*xi', x(2)*xi', x(3)*xi']';
%phi=[x(1)*xi', x(2)*xi', x(3)*xi']';

% Next, specify the estimates of alpha and beta

uhat=thetau'*phi;

% Next, compute the nu signal

% The following line provides an estimate of yddot
yddot=a1*x(1)+a2*x(2)+a3*(x(2)^3)+a4*(x(1)^2)*x(2)+a5*x(1)*(x(2)^2)+b*x(3);

es=(ym(3)-yddot) + k1*(ym(2)-x(2)) + k0*(ym(1)-x(1));

% Sliding mode term

usd=Wu*sgn(es);

% Form the control input to the plant

u=uhat+usd;  

%u=0; % Use to see how the wing will rock (oscillate) if you use no controller

% Plant calculations

dxdt=x; % Just to force it to be a column

dxdt(1)=x(2);
dxdt(2)=a1*x(1)+a2*x(2)+a3*(x(2)^3)+a4*(x(1)^2)*x(2)+a5*x(1)*(x(2)^2)+b*x(3);
dxdt(3)=-(1/tau)*x(3)+(1/tau)*u;

% Reference model calculations

dymdt=Am*ym;

% Parameter update for adaptation

dthetaudt=etau*phi*es;       % Simply to get it to be a column vector

% Form the output vector

dzdt=[dxdt; dymdt; dthetaudt];

%-----------------
function value=sgn(x)

if x>0, value=1; end
if x==0, value=0; end
if x<0, value=-1; end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
