From d8cb278a4a71695df25f069a345c10a2e5076f6b Mon Sep 17 00:00:00 2001 From: Brendan McGeeney Date: Thu, 12 Aug 2021 00:31:00 +0000 Subject: [PATCH] LQR implementation and general improvements --- simPrototypeSTART.m | 116 ++++++++++++++++++++++++++++++++------------ 1 file changed, 84 insertions(+), 32 deletions(-) diff --git a/simPrototypeSTART.m b/simPrototypeSTART.m index f3c877a..4c7b136 100644 --- a/simPrototypeSTART.m +++ b/simPrototypeSTART.m @@ -3,77 +3,130 @@ close all; clear all; clc; %% User Defined Values % Initial Conditions -v_0 = 0; % Initial Velocity [m/s] +v0 = 0; % Initial Velocity (z) [m/s] +M0 = 1.2; % Initial Mass [kg] +yaw0 = 0; % Initial Yaw [deg] +pitch0 = 5; % 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] -% Thrust Curve Data from Text File [t, N] -Tcurve = readmatrix('F15_thrustCurve.txt'); +Tcurve = readmatrix('F15_thrustCurve.txt'); % Thrust Curve from .txt [t, N] % Constants -g = -9.81; % Gravitational Acceleration [m/s2] -M0 = 0.8; % Initial Mass [kg] -Mp = 0.06; % Propellant Mass [kg] -Mb = M0 - Mp; % Burnout Mass [kg] -tb = Tcurve(end, 1); % Burn Time [s] -mdot = Mp / tb; % Mass Flow Rate [kg/s] -D = 0; % Drag [N] -stepSize = 0.001; % Simulation Step Size [s] +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 = 0.001; % Simulation Step Size [s] +maxServo = 15; % Max Servo Rotation [deg] -[h_0, vb, burnStartTime] = burnStartTimeCalc(Tcurve, tb, M0, mdot, Mb) +% 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 +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 -simTime = burnStartTime + tb; % Simulation Time [s] model = 'simProtoype'; load_system(model); simOut = sim(model); toc -%% Output -% Acceleration +%% Outputs figure(1) -plot(simOut.a) + +% Acceleration +subplot(3, 1, 1) +plot(simOut.a.Data(:, 3)) title('Acceleration vs Time') xlabel('Time (s)') ylabel('Acceleration (g''s)') -legend('X','Y') -saveas(gcf,'outputs/Acceleration vs Time.png') % Velocity -figure(2) -plot(simOut.v) +subplot(3, 1, 2) +plot(simOut.v.Data(:, 3)) title('Velocity vs Time') xlabel('Time (s)') ylabel('Velocity (m/s)') -legend('X','Y') -saveas(gcf,'outputs/Velocity vs Time.png') % Altitude -figure(3)%k) -plot(sqrt(simOut.h.Data(:,2).^2 + simOut.h.Data(:,1).^2), simOut.h.Time) +subplot(3, 1, 3) +plot(simOut.h.Time, simOut.h.Data(:,3)) title('Altitude vs Time') -% title(['burnStart = ',num2str(burnStart),' s']) xlabel('Time (s)') ylabel('Altitude (m)') -saveas(gcf,'outputs/Altitude vs Time.png') +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([0, 2, 0, h_0]) +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, 1, simOut.h.Data(i, 2)); + 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 = 50; % 50 is a nice default if outframes @@ -98,5 +151,4 @@ for i = 1 : length(simOut.h.Data) imwrite(imind,cm,'outputs/Altitude.gif','gif','WriteMode','append', 'DelayTime', .2); end end -end - +end \ No newline at end of file