% EN40 2016 HW 3 solutions
%
function HW3

   close all
%% Problem 1 - aircraft dynamics

    g = 9.81;     % Gravitational accel, m/s^2
    A = 24;       % Wing area, m
    kL = 0.1;     % Lift coeft (degrees^-1)
    kDp = 0.03;    % Parasitic drag coefft
    kDI = 0.001;  % Induced drag coefft, degrees^-2
    m = 1134;      % mass, kg
    rho = 1.225;    % Air density, kg/m^3
    thetaa = 2;    % Angle of attack, degrees
    P=1881.2;
    initial_solution = [0;0;61.34;0];
   
    [times,sols] = ode45(@airplane,[0,240],initial_solution);    
 
    figure1 = figure;
    axes1 = axes('Parent',figure1,'FontSize',16);
    box(axes1,'on');
    hold(axes1,'on');
    xlabel('Time (sec)');
    ylabel('Velocity m/s');
    title('Aircraft velocity P=1881, 2 degree AOA');
    plot1 = plot(times,sols(:,3:4));
    set(plot1(1),'DisplayName','v_x');
    set(plot1(2),'DisplayName','v_y');
    legend1 = legend(axes1,'show');
    set(legend1,...
        'Position',[0.759297079762572 0.694519932018728 0.129097432658637 0.186956516556118],...
        'FontSize',18);

    
    thetaa = 5;    % Angle of attack, degrees
    [times,sols] = ode45(@airplane,[0,240],initial_solution);  

    figure2 = figure;
    axes2 = axes('Parent',figure2,'FontSize',16);
    box(axes2,'on');
    hold(axes2,'on');
    xlabel('Time (sec)');
    ylabel('Velocity m/s');
    title('Aircraft velocity P=1881, 5 degree AOA');
    plot2 = plot(times,sols(:,3:4));
    set(plot2(1),'DisplayName','v_x');
    set(plot2(2),'DisplayName','v_y');
    legend2 = legend(axes2,'show');
    set(legend2,...
        'Position',[0.759297079762572 0.694519932018728 0.129097432658637 0.186956516556118],...
        'FontSize',18);
    
     thetaa = 2;    % Angle of attack, degrees
     P=3400;
    [times,sols] = ode45(@airplane,[0,240],initial_solution);  
    
    
    figure3 = figure;
    axes3 = axes('Parent',figure3,'FontSize',16);
    box(axes3,'on');
    hold(axes3,'on');
    xlabel('Time (sec)');
    ylabel('Velocity m/s');
    title('Aircraft velocity P=3400, 2 degree AOA');
    plot3 = plot(times,sols(:,3:4));
    set(plot3(1),'DisplayName','v_x');
    set(plot3(2),'DisplayName','v_y');
    legend3 = legend(axes3,'show');
    set(legend3,...
        'Position',[0.759297079762572 0.694519932018728 0.129097432658637 0.186956516556118],...
        'FontSize',18);
    
     thetaa = 5;    % Angle of attack, degrees
     P=3400;
    [times,sols] = ode45(@airplane,[0,240],initial_solution);  
    
    
    figure4 = figure;
    axes4 = axes('Parent',figure4,'FontSize',16);
    box(axes4,'on');
    hold(axes4,'on');
    xlabel('Time (sec)');
    ylabel('Velocity m/s');
    title('Aircraft velocity P=3400, 5 degree AOA');
    plot4 = plot(times,sols(:,3:4));
    set(plot4(1),'DisplayName','v_x');
    set(plot4(2),'DisplayName','v_y');
    legend4 = legend(axes4,'show');
    set(legend4,...
        'Position',[0.759297079762572 0.694519932018728 0.129097432658637 0.186956516556118],...
        'FontSize',18);
    
    
    function dwdt = airplane(t,w)
        x=w(1); y=w(2); vx=w(3); vy=w(4);
        V = sqrt(vx^2+vy^2);
        thetac = acos(vx/V);
        B = rho*A/(2*m)*V*(kDp + kDI*thetaa^2);
        D = rho*A/(2*m)*V*(kL*thetaa);
        
        dxdt = vx; dydt = vy;
        dvxdt = P*cos(thetaa*pi/180+thetac)/m -B*vx-D*vy;
        dvydt = P*sin(thetaa*pi/180+thetac)/m -B*vy+D*vx-g;
        dwdt = [dxdt;dydt;dvxdt;dvydt];
    end
   
%%  Problem 2: car   
   
    Rovermug = [1/0.001,1/0.01,1/0.1,1];  

    options = odeset('Event',@stop_car);

    figure5 = figure;
    axes5 = axes('Parent',figure5,'FontSize',16);
    box(axes5,'on');
    hold(axes5,'on');
    xlabel('Angle (degrees)');
    ylabel('\omega (rad/s)');
    title('Race-car velocity');
    hold on
    for count=1:4
        mugoverR = 1/Rovermug(count);
        initial_theta = 0;
        initial_omega = 0;
        initial_w = [initial_theta;initial_omega];

        [times,sols] = ode45(@car,[0,800],initial_w,options);
        plot5 = plot(sols(:,1)*180/pi,sols(:,2));
        laptime(count) = times(end);

    end
    
    legend5 = legend(axes5,'show');
    legend5.String = {'\mu g/R = 0.001s^-^2','\mu g/R=0.01s^-^2','\mu g/R=0.1s^-^2','\mu g/R=1s^-^2'};
    set(legend5,...
        'Position',[0.759297079762572 0.694519932018728 0.129097432658637 0.186956516556118],...
        'FontSize',18 ...
        );

    figure6 = figure;
    axes6 = axes('Parent',figure6,'FontSize',16);
    box(axes6,'on');
    hold(axes6,'on');
    plot(sqrt(Rovermug),laptime);
    xlabel('\surd (R/\mu g) (s)');
    ylabel('t (s)');
    title('Time to complete a lap');    
    
    function dwdt = car(t,w)
       theta = w(1); omega = w(2);
       
       dthetadt = omega;
       if (mugoverR>omega^2)
          domegadt = sqrt(mugoverR^2 - omega^4);
       else
          domegadt = 0;
       end
       dwdt = [dthetadt;domegadt];
        
    end

    function [ev,stop,dir] = stop_car(t,w)
        theta = w(1); omega = w(2);
        ev = 2*pi-theta;
        dir = -1;
        stop = 1;
    end


   %% Problem 3 Energy Harvester

    k = 75;
    c = 0.5;
    m = 0.1;
    Omega =24;
    omega = 12;
    A = 0.01;
    d0 = 0.1;
    beta = 0.1;
    L0 = 1.1*d0;
    initial_sol = [0;0];
    [times,sols] = ode45(@energy_harvester,[0,10],initial_sol);
    Pgen1 = c*sols(:,2).^2;
    
    figure7 = figure;
    axes7 = axes('Parent',figure7,'FontSize',16);
    box(axes7,'on');
    hold(axes7,'on');
    plot(times,sols(:,1));
    
    beta=0;
    [times2,sols] = ode45(@energy_harvester,[0,10],initial_sol);
    Pgen2 = c*sols(:,2).^2;
    
    
    plot(times2,sols(:,1));
    xlabel('Time (s)');
    ylabel('Displacement (m)');
    title('Energy Harvester');   
 
    legend7 = legend(axes7,'show');
    legend7.String = {'\beta=0.1','\beta=0'};
    set(legend7,...
        'Position',[0.759297079762572 0.694519932018728 0.129097432658637 0.186956516556118],...
        'FontSize',18 ...
        );
    


    figure8 = figure;   
    axes8 = axes('Parent',figure8,'FontSize',16);
    box(axes8,'on');
    hold(axes8,'on');
    
    plot(times,Pgen1);
    plot(times2,Pgen2);
    
    xlabel('Time (s)');
    ylabel('Power (W)');
    title('Energy Harvester Power');   
 
    legend8 = legend(axes8,'show');
    legend8.String = {'\beta=0.1','\beta=0'};
    set(legend8,...
        'Position',[0.759297079762572 0.694519932018728 0.129097432658637 0.186956516556118],...
        'FontSize',18 ...
        );
    beta = 0.1;
    [times1,sols1] = ode45(@energy_harvester,[0,80],initial_sol);
    Pgen1 = c*sols1(:,2).^2;
    beta = 0;
    [times2,sols2] = ode45(@energy_harvester,[0,80],initial_sol);
    Pgen2 = c*sols2(:,2).^2;
    
    fprintf(' Power generated with actuators %g \n',trapz(times1,Pgen1)/times1(end)); 
    fprintf(' Power generated without actuators %g \n',trapz(times2,Pgen2)/times2(end)); 
    

%   Actuator power calculations
    beta = 0.1;
    for i =1:length(times1)
      dd = d0*(1+beta*sin(Omega*times1(i)));
      vd = d0*Omega*beta*cos(Omega*times1(i));
      Fhor = 2*k*(sqrt(dd^2+sols1(i,1)^2)-L0)*dd/sqrt(dd^2+sols1(i,1)^2);
      Pd1(i) = Fhor*vd;
     end   
    Pact1 = trapz(times1,Pd1)/times1(end);
   
    fprintf(' Power expended by actuators %g \n',Pact1);  
     fprintf(' Useful power with actuators %g \n',trapz(times1,Pgen1)/times1(end)-Pact1);     
  
    function dwdt = energy_harvester(t,w)
        x = w(1); v = w(2);
        d = d0*(1+beta*sin(Omega*t));
        L = sqrt(d^2 + x^2);
        dxdt = v;
        dvdt = -2*(k/m)*x-(c/m)*v+2*L0*k*x/(m*L)+A*omega^2*sin(omega*t);
        dwdt = [dxdt;dvdt];
    end


end
