mirror of
https://gitlab.com/lander-team/lander-sim.git
synced 2025-08-02 11:21:27 +00:00
173 lines
6.6 KiB
Matlab
173 lines
6.6 KiB
Matlab
%% 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 |