%% LANDER SIM PROTOTYPE close all; clear all; clc; %% User Defined Values % Initial Conditions Kp = -6.8699; Ki = 0; Kd = -0.775; v0 = 0; % Initial Velocity (z) [m/s] M0 = 1.2; % Initial Mass [kg] yaw0 = 75; % Initial Yaw [deg] pitch0 = 30; % Initial Pitch [deg] roll0 = 0; % Initial Roll [deg] p0 = 0; % Initial Angular Velocity (x) [deg/s] q0 = 0; % Initial Angular Velocity (y) [deg/s] r0 = 0; % Initial Angular Velocity (z) [deg/s] Tcurve = readmatrix('F15_thrustCurve.txt'); % Thrust Curve from .txt [t, N] % Constants g = -9.81; % Gravitational Acceleration [m/s2] Mp = 0.06; % Propellant Mass [kg] Mb = M0 - Mp; % Burnout Mass [kg] tb = Tcurve(end, 1) - Tcurve(1, 1); % Burn Time [s] mdot = Mp / tb; % Mass Flow Rate [kg/s] D = 0; % Drag [N] stepSize = 1/1000; % Simulation Step Size [s] maxServo = 7; % Max Servo Rotation [deg] % Moment of Inertia / Mass I11 = (1/12) * 0.5318^2 + 0.25 * 0.05105^2; % (1/12) * h^2 + 0.25 * r^2 I22 = (1/12) * 0.5318^2 + 0.25 * 0.05105^2; % (1/12) * h^2 + 0.25 * r^2 I33 = 0.5 * 0.05105^2; % 0.5 * r^2 I = [I11 0 0; 0 I22 0; 0 0 I33]; % I divided by Mass... this is taken care of in Simulink since our mass isn't constant %% Pre-Sim Calcs %K = calcLQR(I*M0) % LQR Gain Calcs [h0, vb, burnStartTime] = burnStartTimeCalc(Tcurve, tb, M0, mdot, Mb, v0) % Burn Start Time Calc h0 = 21; simTime = burnStartTime + tb % Simulation Time [s] yaw0 = yaw0 * pi / 180; % Initial Yaw [rad] pitch0 = pitch0 * pi / 180; % Initial Pitch [rad] roll0 = roll0 * pi / 180; % Initial Roll [rad] p0 = p0 * pi / 180; % Initial Angular Velocity (x) [rad/s] q0 = q0 * pi / 180; % Initial Angular Velocity (y) [rad/s] r0 = r0 * pi / 180; % Initial Angular Velocity (z) [rad/s] %% Simulink tic model = 'simProtoype'; load_system(model); simOut = sim(model); toc %% Write to CSV t = simOut.v.Time; pos = simOut.h.Data; vel = simOut.v.Data; accel = simOut.a.Data; ypr = simOut.YPR.Data; yprDot = simOut.YPRdot.Data; servo1 = [zeros(1163,1);simOut.servo1.Data]; servo2 = [zeros(1163,1);simOut.servo2.Data]; PIDx = simOut.PIDx.Data; PIDy = simOut.PIDy.Data; M = [t, pos, vel, accel, ypr, yprDot, servo1, servo2, PIDx, PIDy]; writematrix(M, 'out.xls'); %% Outputs % figure(1) % % % Acceleration % subplot(3, 1, 1) % plot(simOut.a.Data(:, 3)) % title('Acceleration vs Time') % xlabel('Time (s)') % ylabel('Acceleration (g''s)') % % % Velocity % subplot(3, 1, 2) % plot(simOut.v.Data(:, 3)) % title('Velocity vs Time') % xlabel('Time (s)') % ylabel('Velocity (m/s)') % % % Altitude % subplot(3, 1, 3) % plot(simOut.h.Time, simOut.h.Data(:,3)) % title('Altitude vs Time') % xlabel('Time (s)') % ylabel('Altitude (m)') % ylim([0 h0+5]) % saveas(gcf,'outputs/Accel-Vel-Alt vs Time.png') % % figure(2) % % % Euler Angles % subplot(2, 1, 1) % plot(simOut.YPR.Data) % title('Euler Angles vs Time') % xlabel('Time (ms)') % ylabel('Euler Angles (deg)') % legend('Yaw', 'Pitch', 'Roll') % % % Angular Velocity % subplot(2, 1, 2) % plot(simOut.YPRdot.Data) % title('Angular Velocity vs Time') % xlabel('Time (ms)') % ylabel('Angular Velocity (deg/s)') % legend('X', 'Y', 'Z') % saveas(gcf,'outputs/Euler Angles vs Time.png') % % figure(3) % % % Servo 1 Position % subplot(2, 1, 1) % plot(simOut.servo1.Data) % title('Servo 1 Position vs Time') % xlabel('Time (ms)') % ylabel('Servo 1 Position (rad)') % % % Servo 2 Position % subplot(2, 1, 2) % plot(simOut.servo2.Data) % title('Servo 2 Position vs Time') % xlabel('Time (ms)') % ylabel('Servo 2 Position (rad)') % saveas(gcf,'outputs/Servo Position vs Time.png') % Animation % h = figure(4); % K = animatedline('Marker', 'o'); % axis([-10, 10, 0, h0]) % xlabel('X-Position (m)') % ylabel('Altitude (m)') % title('Altitude') % grid on % for i = 1 : length(simOut.h.Data) % clearpoints(K); % addpoints(K, simOut.h.Data(i, 1), simOut.h.Data(i, 3)); % title(sprintf('Altitude at T = %f', simOut.h.Time(i))) % drawnow limitrate % % % Write Animation to gif, set to zero when testing since its slow to render. % outframes = 0; % 50 is a nice default % if outframes % % Write to the GIF File % if i == 1 % % % Capture the plot as an image % frame = getframe(h); % im = frame2im(frame); % [imind,cm] = rgb2ind(im,256); % % %initalize plot % imwrite(imind,cm,'outputs/Altitude.gif','gif', 'Loopcount',inf); % % elseif mod(i,floor(length(simOut.h.Data)/outframes)) == 0 % % Capture the plot as an image % frame = getframe(h); % im = frame2im(frame); % [imind,cm] = rgb2ind(im,256); % % % Append to plot % imwrite(imind,cm,'outputs/Altitude.gif','gif','WriteMode','append', 'DelayTime', .2); % end % end % end