function HW3

close all

%% Problem 4

% Solve the ODE (see function rocket)
[times,sols] = ode45(@(t,w) rocket(t,w),[0,700],[0,0]);
plot(times,sols(:,2),'DisplayName','Prediction')
hold on
% Read and plot the experimetal data
data = csvread('CRS-8_Telemetry.csv',1,0);
plot(data(:,1),data(:,2)/3.6,'DisplayName','Measurement')
axes1 = gca;
grid on
xlabel('Time (s)')
ylabel('Speed (m/s)')
title('Falcon 9 velocity')
set(axes1,'FontSize',12);
% Create legend
legend1 = legend(axes1,'show');
set(legend1,...
    'Position',[0.262401723154243 0.761083476019941 0.154724815243377 0.0966735940713149],...
    'FontSize',12);



%% Problem 5


    lambda = -1.0;
    mu = 0;
    Omega = 2;
    initial_w = [1.;0.;0.;1];
    [time,sols] = ode45(@(t,w) trap(t,w,lambda,mu,Omega),[0:0.1:50],initial_w);
    figure
    polarplot(sols(:,2),sols(:,1),'Displayname','(Case a)','Color',[1,0,0]);
    hold on
    initial_w = [0.8;0.;0.;0.4];
    [time,sols] = ode45(@(t,w) trap(t,w,lambda,mu,Omega),[0,50],initial_w);
     polarplot(sols(:,2),sols(:,1),'Displayname','(Case b)','Color',[0,1,0]);
    
    mu = 8;
    initial_w = [2.5;0.;0.;0.25];
    [time,sols] = ode45(@(t,w) trap(t,w,lambda,mu,Omega),[0,50],initial_w);
    polarplot(sols(:,2),sols(:,1),'Displayname','(Case c)','Color',[0,0,1]) ;
    axes1 = gca;
    legend1 = legend(axes1,'show');
     
    figure
     initial_w = [1.5;0.;0.;0.25];
     [time,sols] = ode45(@(t,w) trap(t,w,lambda,mu,Omega),[0,100],initial_w);
     polarplot(sols(:,2),sols(:,1),'Color',[1,0,0],'LineWidth',2) ;
   

%% Problem 6
g = 9.81;
m = 80;

D = 400;    % Distance from start-end
H = 30;     % Vertical drop
L0 = sqrt(D^2+H^2)+5.5;   % Cable length
k = 1000000;   % Cable stiffness
x0 = 0;       % Initial position
y0 = (L0^2 -D^2 - H^2 )/(2*(L0-H)) %Initial position
initial_w = [x0,y0,0,0];  % Starts at x0,y0 and has zero velocity

options = odeset('Event',@(t,w) event(t,w,D));

% Solve for x,y and plot
[times,sols] = ode45(@(t,w) dragline(t,w,L0,k,D,H,m,g),[0,30],initial_w,options);
% Plot the trajectory
figure
plot(sols(:,1),-sols(:,2))
axis equal
grid on
xlabel('x (m)')
ylabel('y (m)')
title('Trajectory')
% Calculate and plot the speed
figure
for i =1:length(times)
   V(i) = sqrt(sols(i,3)^2 + sols(i,4)^2); 
end
plot(times,V)
grid on
xlabel('Time (s)')
ylabel('Speed (m/s)')
title('Speed')

end
%% Differential equation function for problem 4
function dwdt = rocket(t,w)
   x = w(1); v = w(2);
   
   rho = 1.22*exp(-x/8.5e03);  % Air density, kg/m^3
   g = 9.81; % gravitational accel
   if (t<165)   %Stage 1 burn
      m = 550000-2500*t;   % Rocket mass
      dvdt = 7.6e06/m - g - rho*50*v^2/m; % Acceleration
   elseif (t<584)  % Stage 2 burn
       m = 137500-263*(t-165);  % Rocket mass
       dvdt = 1.6e06/m - g - rho*50*v^2/m; %Acceleration
   else  % free fall
       dvdt = -g;
   end
   dxdt = v;
   
   dwdt = [dxdt;dvdt];
   
end

%% Differential equation function for problem 6
function dwdt = trap(t,w,lambda,mu,Omega)
 r = w(1); q = w(2); v = w(3); o = w(4);
 
  drdt = v;
  dqdt = o;
  dvdt = r*o^2 + (lambda+mu*cos(Omega*t))/r;
  dodt = -2*v*o/r;

   
   dwdt = [drdt;dqdt;dvdt;dodt];

end

%% Differential equation function for problem 6
function dwdt = dragline(t,w,L0,k,D,H,m,g)
 x = w(1); y = w(2); vx = w(3); vy = w(4);
 
   d1 = sqrt(x^2+y^2);
   d2 = sqrt((D-x)^2 + (y-H)^2);
   L = d1 + d2;
   T = k*(L-L0);
   V = sqrt(vx^2 + vy^2);
   c = 0.2*1*1.22/2;
   
   dvxdt = (-T*x/d1 + T*(D-x)/d2 - c*V*vx )/m; 
   dvydt = g - (T*y/d1 + T*(y-H)/d2 + c*V*vy )/m;
   
   dxdt = vx;
   dydt = vy;
   
   dwdt = [dxdt;dydt;dvxdt;dvydt];

end
% Event function for problem 6
function [ev,stop,dir] = event(t,w,D)
 x = w(1); 
 
 ev = D-x;
 dir = -1;
 stop = 1;
   

end

