diff --git a/simPrototypeSTART.m b/simPrototypeSTART.m index f9b7b38..cba9664 100644 --- a/simPrototypeSTART.m +++ b/simPrototypeSTART.m @@ -3,10 +3,14 @@ close all; clear all; clc; %% User Defined Values % Initial Conditions +Kp = -2^3; +Ki = -2^-2; +Kd = -2^0.75; + v0 = 0; % Initial Velocity (z) [m/s] M0 = 1.2; % Initial Mass [kg] -yaw0 = 5; % Initial Yaw [deg] -pitch0 = 15; % Initial Pitch [deg] +yaw0 = 75; % Initial Yaw [deg] +pitch0 = 50; % Initial Pitch [deg] roll0 = 0; % Initial Roll [deg] p0 = 0; % Initial Angular Velocity (x) [deg/s] q0 = 0; % Initial Angular Velocity (y) [deg/s] @@ -31,8 +35,8 @@ I33 = 0.5 * 0.05105^2; % 0. 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 +%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] @@ -52,104 +56,104 @@ simOut = sim(model); toc %% 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') +% 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 +% 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 \ No newline at end of file +% 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 \ No newline at end of file