% Wil Selby
% November 10, 2010
% 2.152 Term Project

% Sliding Control of a quadrotor

function [T,Y,XD] = sliding_control()

clear all; close all; clc;

global Kp Kd m d J Kfa Kft Jr B0 B1 B2 b V g a1 a2 a3 a4 a5 a6 a7 a8 a9 a10 a11 b1 b2 b3 motor_mat XD ctrl time sigma_bar aviobj index uplot;

Kp = 0.5;                           % lift coefficient (N*m/rad/s)
Kd = 0.01;                          % drag coefficient (N*m/rad/s)
m = 0.5;                            % mass (kg)
d = 0.25;                           % arm length (m)
J = diag([.005,.005,.010]);           % moment of inertia (kg*m^2)
Kfa = diag([5.56,5.56,5.56])*10^-4;   % aerodynamic friction coefficient
Kft = diag([.3729,.3712,.37]);   % translational drag coefficient
Jr = 2.8*10^-5;                     % mrotor inertia
B0 = 189.63;
B1 = 6.0612;
B2 = 0.012;
b = 280.19;
V = 12;                             % motor input (12V?)
g = 9.8;                            % gravity

motor_mat = [ Kp   Kp  Kp   Kp;
    -Kp   0   Kp   0;
    0   -Kp  0    Kp;
    Kd  -Kd  Kd  -Kd];

% Model parameters
a1 = (J(2,2) - J(3,3))/J(1,1);
a2 = -Kfa(1,1)/J(1,1);
a3 = -Jr/J(1,1);
b1 = d/J(1,1);

a4 = (J(3,3) - J(1,1))/J(2,2);
a5 = -Kfa(2,2)/J(2,2);
a6 = Jr/J(2,2);
b2 = d/J(2,2);

a7 = (J(1,1) - J(2,2))/J(3,3);
a8 = -Kfa(3,3)/J(3,3);
b3 = 1/J(3,3);

a9 = -Kft(1,1)/m;
a10 = -Kft(2,2)/m;
a11 = -Kft(3,3)/m;


sigma_bar = 0;

% State vector
% X = [roll,roll_d,pitch,pitch_d,yaw,yaw_d,x,x_d,y,y_d,z,z_d]
x_0 = [0; 0; 0; 0; .2; 0; 0; 0; 0; 0; 1.5; 0];


% Simulation parameters
t_0 = 0;
t_f = 60.0;
step = 1e-1;

perc = t_f/10;

r_des = 0;
p_des = 0;

index = 1;
uplot = [0 0 0];

% Set ODE step Size
options = odeset('MaxStep',step);

% Simulation
[T Y] = ode45(@odefun, [t_0 t_f], x_0, options);

%% X,Y,Z and yaw positions
figure(1);
subplot(2,2,1)
plot(T,Y(:,7));
hold on;
plot(time,XD(7,:),'r--');
title('X pos');
ylabel('Distance (m)');
xlabel('Time (sec)');
legend('Actual','Desired');

subplot(2,2,2);
plot(T,Y(:,9));
hold on;
plot(time,XD(9,:),'r--');
title('Y pos');
ylabel('Distance (m)');
xlabel('Time (sec)');
legend('Actual','Desired');

subplot(2,2,3);
plot(T,Y(:,5));
hold on;
plot(time,XD(5,:),'r--');
title('Yaw pos');
ylabel('Yaw Angle (rad)');
xlabel('Time (sec)');
legend('Actual','Desired');

subplot(2,2,4);
plot(T,Y(:,11));
hold on;
plot(time,XD(11,:),'r--');
title('Z pos');
ylabel('Distance (m)');
xlabel('Time (sec)');
legend('Actual','Desired');

%% Actual/Desired Roll and Pitch angles
% figure(2);
% subplot(2,1,1);
% plot(T,Y(:,1)*180/pi);
% hold on;
% plot(time,XD(1,:)*180/pi,'r--');
% title('Roll angle');
% ylabel('Angle (deg)');
% xlabel('Time (sec)');
% legend('Actual','Desired');
% 
% subplot(2,1,2);
% plot(T,Y(:,3)*180/pi);
% hold on;
% plot(time,XD(3,:)*180/pi,'r--');
% title('Pitch angle');
% ylabel('Angle (deg)');
% xlabel('Time (sec)');
% legend('Actual','Desired');

%% X and Y positions and inputs
figure(10);
subplot(2,2,1)
plot(T,Y(:,7));
hold on;
plot(time,XD(7,:),'r--');
title('X pos');
ylabel('Distance (m)');
xlabel('Time (sec)');
legend('Actual','Desired');

subplot(2,2,2);
plot(T,Y(:,9));
hold on;
plot(time,XD(9,:),'r--');
title('Y pos');
ylabel('Distance (m)');
xlabel('Time (sec)');
legend('Actual','Desired');

subplot(2,2,4);
plot(T,Y(:,1)*180/pi);
hold on;
plot(time,XD(1,:)*180/pi,'r--');
title('Roll angle');
ylabel('Angle (deg)');
xlabel('Time (sec)');
legend('Actual','Desired');

subplot(2,2,3);
plot(T,Y(:,3)*180/pi);
hold on;
plot(time,XD(3,:)*180/pi,'r--');
title('Pitch angle');
ylabel('Angle (deg)');
xlabel('Time (sec)');
legend('Actual','Desired');


%% Plot 3d positon
figure();
plot3(XD(7,:),XD(9,:),XD(11,:),'r--')
hold on; 
plot3(Y(:,7),Y(:,9),Y(:,11))

%% Plot error values
figure()
subplot(2,1,1);
plot(sinroll);
subplot(2,1,2);
plot(sinpitch);

figure()
plot(uplot);

%% Control Inputs
% figure();
% subplot(2,2,1);
% plot(time,ctrl(5,:)*180/pi);
% title('Ux');
% 
% subplot(2,2,2);
% plot(time,ctrl(6,:)*180/pi);
% title('Uy');
% 
% subplot(2,2,3);
% plot(time,ctrl(4,:));
% title('U yaw');
% 
% subplot(2,2,4);
% plot(time,ctrl(1,:));
% title('Uz');

%% Record movie
% aviobj = avifile('quad_sliding_control.avi');
% 
% %Display
% for k=1:size(T,1)
%     draw_quad(T(k),Y(k,:));
% %     pause(0.01);
% end
% 
% aviobj = close(aviobj);


%% Functions
    function X_d = odefun(t,x)
        
        if (mod(t,5) <= 1e-6)
                disp([num2str((t/t_f)*1e2),' % done']);
        end

        X_d = zeros(12,1);

        % Assign variables
        x(1);     % roll angle
        x(2);    % roll velocity
        x(3);     % pitch angle
        x(4);    % pitch velocity
        x(5);   % yaw angle
        x(6);  % yaw velocity
        x(7);   % x position
        x(8);    % x velocity
        x(9);     % y position
        x(10);  % y velocity
        x(11);   % z position
        x(12);  % z velocity


        xdes = trajectory(t,r_des,p_des);

        % u = control_switch(t,x,xdes,sigma_bar);
%         u = control_slide(t,x,xdes,sigma_bar);
        [u,r_des,p_des] = control_slide2(t,x,xdes,sigma_bar);
     

        ctrl(:,end+1) = u;

        U1 = u(1);
        U2 = u(2);
        U3 = u(3);
        U4 = u(4);
        Ux = u(5);
        Uy = u(6);


        % Calculate/insert accelerations into X_dot
        
        % simplified rotational dynamics
        a1=0;a2=0;a3=0;a4=0;a5=0;a6=0;a7=0;a8=0;
        
        X_d(1) = x(2);
        X_d(2) = a1*x(4)*x(6) + a2*(x(2)^2) + a3*sigma_bar*x(4) + b1*U2;
        X_d(3) = x(4);
        X_d(4) = a4*x(2)*x(6) + a5*(x(4)^2) + a6*sigma_bar*x(2) + b2*U3;
        X_d(5) = x(6);
        X_d(6) = a8*(x(6)) + b3*U4;  %a7*x(2)*x(4) + 
        X_d(7) = x(8);
        X_d(8) = a9*x(8) + Ux*U1/m;
        X_d(9) = x(10);
        X_d(10) = a10*x(10) + Uy*U1/m;
        X_d(11) = x(12);
        X_d(12) = a11*x(12) + (cos(x(1))*cos(x(3))*U1)/m - g;
        
        % Using Danny's model of attitude controller
        X_d(2) = -45.83*x(2) - 414.53*x(1) + 277.32*r_des;
        X_d(4) = -42.57*x(4) - 252.15*x(3) + 183.62*p_des;
        
        % Using estimated model
%         X_d(8) = .00264*Ux*U1 -.3729*x(2) +.1642;
%         X_d(10) = .00256*Uy*U1 -.1812*x(2) -.2785;

        time(end+1) = t;

    end

    function xd = trajectory(t,r_des,p_des)

        xd(1,:) = 0;
        xd(2,:) = 0;
        xd(3,:) = 0;
        xd(4,:) = 0;
        xd(5,:) = 0;
        xd(6,:) = 0;
        
        xd(8,:) = 0;
        
        xd(10,:) = 0;
        
        xd(12,:) = 0;
        
        if(t<=15)
            xd(7,:) = 0;
        end
        if( t>15 && t <= 30)
            xd(7,:) = .5;
        end
        if( t>30 && t <= 45)
            xd(7,:) = .8;
        end
        if(t>45)
            xd(7,:) = .6;
        end
        
        if(t<=30)
            xd(9,:) = 0;
        end
        if(t>30)
            xd(9,:) = .5;
        end
        
        if(t<=45)
            xd(11,:) = 1.5;
        end
        if(t>45)
            xd(11,:) = 1.25;
        end
        
        % Vicon trajectory
%         if( t < 10)
%             xd(7,:) = 0;
%             xd(9,:) = 0;
%             xd(11,:) = 1.5;
%         else
%             xd(7,:) = 0.5*cos(2*pi*.1*(t-30)) - 0.5;
%             xd(9,:) = 0.5*sin(2*pi*.1*(t-30)) - 0.0;
%             xd(11,:) = 1.5;
%         end
        
        
%         xd(7,:) = 0; %sin(2*pi*t);
%         xd(9,:) = 0;    %sin(3*pi*t);
%         xd(11,:)= 1.5;

        XD(:,end+1) = xd;
    end

    function u = control_switch(t,x,xd,sigma_bar)

        q = 10*[1 1 1 1 1 1]';
        k = 0*[1 1 1 1 1 1]';
        lambda = 5*[1 1 1 1 1 1 1 1 1 1 1 1]';

        for i=1:2:size(x,1)-1

            S(i) = x(i+1)-xd(i+1) + lambda(i)*(x(i)-xd(i));

        end

        % note hack desired accelerations = desired velocities = 0;

        u(1) = (m/(cos(x(1))*cos(x(3))))*(-q(6)*sign(S(11)) - k(6)*S(11) - a11*x(12) + xd(12) + lambda(11)*(xd(12)-x(12)) + g );

        u(2) = (1/b1)*(-q(1)*sign(S(1)) - k(1)*S(1) - a1*x(4)*x(6) - a2*(x(2)^2) - a3*sigma_bar*x(4) + xd(2) + lambda(1)*(xd(2)-x(2)));

        u(3) = (1/b2)*(-q(2)*sign(S(3)) - k(2)*S(3) - a4*x(2)*x(6) - a5*(x(4)^2) - a6*sigma_bar*x(2) + xd(4) + lambda(3)*(xd(4)-x(4)));

        u(4) = (1/b3)*(-q(3)*sign(S(5)) - k(3)*S(5) - a7*x(2)*x(4) - a8*(x(6)^2) + xd(6) + lambda(5)*(xd(6)-x(6)));

        if(u(1) ~= 0)

            u(5) = (m/u(1))*(-q(4)*sign(S(7)) - k(4)*S(7) - a9*x(8) + xd(8) + lambda(7)*(xd(8)-x(8)));

            u(6) = (m/u(1))*(-q(5)*sign(S(9)) - k(5)*S(9) - a10*x(10) + xd(10) + lambda(10)*(xd(10)-x(10)));

        else

            u(5) = 0;
            u(6) = 0;

        end

    end

    function [u,r,p] = control_slide2(t,x,xd,sigma_bar)

        e = 0.5;
        meu = 0*[1 1 1 2.5 2.5 1]';
        sigma = 1*[1 1 1 1.5 1.5 1]';
        lambda = 1*[1 1 1 1 1 1 2 2 2 2 10 10]';

%         meu = 1*[1 1 1 1 1 1]';
%         sigma = 1*[1 1 1 1 1 1]';
%         lambda = 1*[1 1 1 1 1 1 1 1 1 1 10 1]';

        S(7) = x(8)-xd(8) + lambda(7)*(x(7)-xd(7));
        S(9) = x(10)-xd(10) + lambda(9)*(x(9)-xd(9));
        S(11) = x(12)-xd(12) + lambda(11)*(x(11)-xd(11));
        S(5) = x(6) - xd(6) + lambda(5)*(x(5)-xd(5));

        % Position Control
        u(1) =(m/(cos(x(1))*cos(x(3))))*(-sigma(6)*(S(11)/(abs(S(11))+e)) - meu(6)*S(11) - a11*x(12) + xd(12) - lambda(6)*(x(12)-xd(12)) + g );

        % desired roll with y axis error
        %xd(1) = -atan((-sigma(5)*(S(9)/(abs(S(9))+e)) - meu(5)*S(9) - a10*x(10) + xd(10) - lambda(10)*(x(10)-xd(10)))*cos(x(1))/                  (-sigma(1)*(S(11)/(abs(S(11))+e)) - meu(1)*S(11) - a11*x(12) + xd(12) - lambda(1)*(x(12)-xd(12)) + g));

        % desired pitch with x axis eror
        %xd(3) = atan((-sigma(4)*(S(7)/(abs(S(7))+e)) - meu(4)*S(7) - a9*x(8) + xd(8) - lambda(7)*(x(8)-xd(8))) / (-sigma(1)*(S(11)/(abs(S(11))+e)) - meu(1)*S(11) - a11*x(12) + xd(12) - lambda(1)*(x(12)-xd(12)) + g));


        Ux = (m/u(1))*(-sigma(4)*(S(7)/(abs(S(7))+e)) - meu(4)*S(7) - a9*x(8) + xd(8) - lambda(7)*(x(8)-xd(8)));

        Uy = (m/u(1))*(-sigma(5)*(S(9)/(abs(S(9))+e)) - meu(5)*S(9) - a10*x(10) + xd(10) - lambda(10)*(x(10)-xd(10)));

        yaw = x(5);
        
        uplot(index,:) = [u(1),Ux,Uy];

        sinroll(index) = (Ux*sin(x(5))-Uy*cos(x(5)))/(cos(x(5))+sin(x(5))^2);
        xd(1) = asin(sinroll(index));

        sinpitch(index) = (Uy+sin(xd(1))*cos(x(5)))/(cos(xd(1))*sin(x(5)));
        xd(3) = asin(sinpitch(index));
        
        index = index+1;

        r = xd(1);
        p = xd(3);

        % Save for logging/plotting purposes
        XD(1,end) = xd(1);
        XD(3,end) = xd(3);

        % uses current values
        u(5) = cos(x(1))*sin(x(3))*cos(x(5)) + sin(x(1))*sin(x(5));
        u(6) = cos(x(1))*sin(x(3))*sin(x(5)) - sin(x(1))*cos(x(5));
        

        u(4) = (1/b3)*(-sigma(3)*(S(5)/(abs(S(5))+e)) - meu(3)*S(5) - a8*x(6)  + xd(6) - lambda(5)*(x(6)-xd(6)));  %- a7*x(2)*x(4) 
       

    end

    function u = control_slide(t,x,xd,sigma_bar)

        q = 10*[1 1 1 1 1 1]';
        k = 2*[1 1 1 1 1 1]';
        lambda = 5*[1 1 1 1 1 1 1 1 1 1 1 1]';

        e = .05;

        for i=1:2:size(x,1)-1

            S(i) = x(i+1)-xd(i+1) + lambda(i)*(x(i)-xd(i));

        end

        % note hack desired accelerations = desired velocities = 0;

        u(1) = (m/(cos(x(1))*cos(x(3))))*(-q(6)*(S(11)/(abs(S(11))+e)) - k(6)*S(11) - a11*x(12) + xd(12) - lambda(11)*(x(12)-xd(12)) + g );

        u(2) = (1/b1)*(-q(1)*(S(1)/(abs(S(1))+e)) - k(1)*S(1) - a1*x(4)*x(6) - a2*(x(2)^2) - a3*sigma_bar*x(4) + xd(2) - lambda(1)*(x(2)-xd(2)));

        u(3) = (1/b2)*(-q(2)*(S(3)/(abs(S(3))+e)) - k(2)*S(3) - a4*x(2)*x(6) - a5*(x(4)^2) - a6*sigma_bar*x(2) + xd(4) - lambda(3)*(x(4)-xd(4)));

        u(4) = (1/b3)*(-q(3)*(S(5)/(abs(S(5))+e)) - k(3)*S(5) - a7*x(2)*x(4) - a8*(x(6)^2) + xd(6) - lambda(5)*(x(6)-xd(6)));

        % true inputs
%         u(5) = cos(x(1))*sin(x(3))*cos(x(5)) + sin(x(1))*sin(x(5));
%         u(6) = cos(x(1))*sin(x(3))*sin(x(5)) - sin(x(1))*cos(x(5));
        
        if(u(1) ~= 0)

            u(5) = (m/u(1))*(-q(4)*(S(7)/(abs(S(7))+e)) - k(4)*S(7) - a9*x(8) + xd(8) - lambda(7)*(x(8)-xd(8)));

            u(6) = (m/u(1))*(-q(5)*(S(9)/(abs(S(9))+e)) - k(5)*S(9) - a10*x(10) + xd(10) - lambda(10)*(x(10)-xd(10)));

        else

            u(5) = 0;
            u(6) = 0;

        end
        
        % saturation limits

    end

% ===============================================================
% This is the draw function
%================================================================
    function draw_quad(T,Y)
        global count x_plot z_plot x_final;

        persistent hFig;

        x = Y(7);
        z = Y(11);
        pitch = -Y(3);  % Matches system dynamics

        t = T;

        f_fig_bound = 3.5;
        r_fig_bound = -.5;
        t_fig_bound = 2.5;
        b_fig_bound = -0.5;


        base = .5;
        rotor_height = 0.05;
        blade_radius = .1;
        alpha = atan(rotor_height/base); % angle from cm to rotor
        h = base/cos(alpha);             % distace from cm to rotor

        f_base = [x + base/2*cos(pitch), z + base/2*sin(pitch)];
        r_base = [x - base/2*cos(pitch), z - base/2*sin(pitch)];

        f_rot = [f_base(1) - rotor_height*cos(pi/2-pitch), f_base(2) + rotor_height*sin(pi/2-pitch)];
        r_rot = [r_base(1) - rotor_height*cos(pi/2-pitch), r_base(2) + rotor_height*sin(pi/2-pitch)];

        f_blade_f = [ f_rot(1) + blade_radius*cos(pitch), f_rot(2) + blade_radius*sin(pitch)];
        f_blade_r = [ f_rot(1) - blade_radius*cos(pitch), f_rot(2) - blade_radius*sin(pitch)];

        r_blade_f = [ r_rot(1) + blade_radius*cos(pitch), r_rot(2) + blade_radius*sin(pitch)];
        r_blade_r = [ r_rot(1) - blade_radius*cos(pitch), r_rot(2) - blade_radius*sin(pitch)];


        if (isempty(hFig))
            hFig = figure(25);
            set(hFig,'DoubleBuffer','on');
        end

        figure(hFig);
        clf;

        % draw base
        line([r_base(1) f_base(1)],[r_base(2) f_base(2)]);
        hold on;
        % plot(f_base(1), f_base(2), 'ro');
        % plot(r_base(1), r_base(2), 'ro');

        % draw front rotor and blade
        line([f_base(1) f_rot(1)],[f_base(2) f_rot(2)]);
        line([f_blade_r(1) f_blade_f(1)],[f_blade_r(2) f_blade_f(2)]);

        % draw rear rotor and blade
        line([r_base(1) r_rot(1)],[r_base(2) r_rot(2)]);
        line([r_blade_r(1) r_blade_f(1)],[r_blade_r(2) r_blade_f(2)]);

        % draw ground
        line([r_fig_bound f_fig_bound],[0 0], 'Color', [.3 .5 .1],'LineWidth', 5);

        % draw wall
%         line([x_final x_final], [0 20],'Color',[.75 .6 .5]); %,'LineWidth', 2

        % draw trajectory
        x_plot(1,count) = x;
        z_plot(1,count) = z;
        plot(x_plot(1:end),z_plot(1:end),'r+','MarkerSize',3);
        count = count + 1;


        axis equal;
        axis([r_fig_bound f_fig_bound b_fig_bound t_fig_bound]);
        xlabel('Distance (m)');
        ylabel('Distance (m)');
        title(['time = ', num2str(t), ',  \phi = ', num2str(-pitch*180/pi), ' deg.']);

        drawnow;

        F = getframe(hFig);
        aviobj = addframe(aviobj,F);

    end



end