Anson/Lander-Simulink
1
0
Fork 0
mirror of https://gitlab.com/lander-team/lander-sim.git synced 2025-08-03 11:51:27 +00:00
Code Releases Activity

PID implementation

Browse Source
This commit is contained in:
Brendan McGeeney
2021-09-19 02:59:22 +00:00
parent 6d4cc2bbdb
commit c24f6d0fd7
1 changed files with 106 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files

208
simPrototypeSTART.m
View File

@@ -3,10 +3,14 @@ close all; clear all; clc;
%% User Defined Values %% User Defined Values
% Initial Conditions % Initial Conditions
Kp = -2^3;
Ki = -2^-2;
Kd = -2^0.75;
v0 = 0; % Initial Velocity (z) [m/s] v0 = 0; % Initial Velocity (z) [m/s]
M0 = 1.2; % Initial Mass [kg] M0 = 1.2; % Initial Mass [kg]
yaw0 = 5; % Initial Yaw [deg] yaw0 = 75; % Initial Yaw [deg]
pitch0 = 15; % Initial Pitch [deg] pitch0 = 50; % Initial Pitch [deg]
roll0 = 0; % Initial Roll [deg] roll0 = 0; % Initial Roll [deg]
p0 = 0; % Initial Angular Velocity (x) [deg/s] p0 = 0; % Initial Angular Velocity (x) [deg/s]
q0 = 0; % Initial Angular Velocity (y) [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 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 %% Pre-Sim Calcs
K = calcLQR(I*M0) % LQR Gain Calcs %K = calcLQR(I*M0) % LQR Gain Calcs
[h0, vb, burnStartTime] = burnStartTimeCalc(Tcurve, tb, M0, mdot, Mb, v0) % Burn Start Time Calc [h0, vb, burnStartTime] = burnStartTimeCalc(Tcurve, tb, M0, mdot, Mb, v0); % Burn Start Time Calc
h0 = 21; h0 = 21;
simTime = burnStartTime + tb; % Simulation Time [s] simTime = burnStartTime + tb; % Simulation Time [s]
@@ -52,104 +56,104 @@ simOut = sim(model);
toc toc
%% Outputs %% Outputs
figure(1) % figure(1)
%
% Acceleration % % Acceleration
subplot(3, 1, 1) % subplot(3, 1, 1)
plot(simOut.a.Data(:, 3)) % plot(simOut.a.Data(:, 3))
title('Acceleration vs Time') % title('Acceleration vs Time')
xlabel('Time (s)') % xlabel('Time (s)')
ylabel('Acceleration (g''s)') % ylabel('Acceleration (g''s)')
%
% Velocity % % Velocity
subplot(3, 1, 2) % subplot(3, 1, 2)
plot(simOut.v.Data(:, 3)) % plot(simOut.v.Data(:, 3))
title('Velocity vs Time') % title('Velocity vs Time')
xlabel('Time (s)') % xlabel('Time (s)')
ylabel('Velocity (m/s)') % ylabel('Velocity (m/s)')
%
% Altitude % % Altitude
subplot(3, 1, 3) % subplot(3, 1, 3)
plot(simOut.h.Time, simOut.h.Data(:,3)) % plot(simOut.h.Time, simOut.h.Data(:,3))
title('Altitude vs Time') % title('Altitude vs Time')
xlabel('Time (s)') % xlabel('Time (s)')
ylabel('Altitude (m)') % ylabel('Altitude (m)')
ylim([0 h0+5]) % ylim([0 h0+5])
saveas(gcf,'outputs/Accel-Vel-Alt vs Time.png') % saveas(gcf,'outputs/Accel-Vel-Alt vs Time.png')
%
figure(2) % figure(2)
%
% Euler Angles % % Euler Angles
subplot(2, 1, 1) % subplot(2, 1, 1)
plot(simOut.YPR.Data) % plot(simOut.YPR.Data)
title('Euler Angles vs Time') % title('Euler Angles vs Time')
xlabel('Time (ms)') % xlabel('Time (ms)')
ylabel('Euler Angles (deg)') % ylabel('Euler Angles (deg)')
legend('Yaw', 'Pitch', 'Roll') % legend('Yaw', 'Pitch', 'Roll')
%
% Angular Velocity % % Angular Velocity
subplot(2, 1, 2) % subplot(2, 1, 2)
plot(simOut.YPRdot.Data) % plot(simOut.YPRdot.Data)
title('Angular Velocity vs Time') % title('Angular Velocity vs Time')
xlabel('Time (ms)') % xlabel('Time (ms)')
ylabel('Angular Velocity (deg/s)') % ylabel('Angular Velocity (deg/s)')
legend('X', 'Y', 'Z') % legend('X', 'Y', 'Z')
saveas(gcf,'outputs/Euler Angles vs Time.png') % saveas(gcf,'outputs/Euler Angles vs Time.png')
%
figure(3) % figure(3)
%
% Servo 1 Position % % Servo 1 Position
subplot(2, 1, 1) % subplot(2, 1, 1)
plot(simOut.servo1.Data) % plot(simOut.servo1.Data)
title('Servo 1 Position vs Time') % title('Servo 1 Position vs Time')
xlabel('Time (ms)') % xlabel('Time (ms)')
ylabel('Servo 1 Position (rad)') % ylabel('Servo 1 Position (rad)')
%
% Servo 2 Position % % Servo 2 Position
subplot(2, 1, 2) % subplot(2, 1, 2)
plot(simOut.servo2.Data) % plot(simOut.servo2.Data)
title('Servo 2 Position vs Time') % title('Servo 2 Position vs Time')
xlabel('Time (ms)') % xlabel('Time (ms)')
ylabel('Servo 2 Position (rad)') % ylabel('Servo 2 Position (rad)')
saveas(gcf,'outputs/Servo Position vs Time.png') % saveas(gcf,'outputs/Servo Position vs Time.png')
% Animation % Animation
h = figure(4); % h = figure(4);
K = animatedline('Marker', 'o'); % K = animatedline('Marker', 'o');
axis([-10, 10, 0, h0]) % axis([-10, 10, 0, h0])
xlabel('X-Position (m)') % xlabel('X-Position (m)')
ylabel('Altitude (m)') % ylabel('Altitude (m)')
title('Altitude') % title('Altitude')
grid on % grid on
for i = 1 : length(simOut.h.Data) % for i = 1 : length(simOut.h.Data)
clearpoints(K); % clearpoints(K);
addpoints(K, simOut.h.Data(i, 1), simOut.h.Data(i, 3)); % addpoints(K, simOut.h.Data(i, 1), simOut.h.Data(i, 3));
title(sprintf('Altitude at T = %f', simOut.h.Time(i))) % title(sprintf('Altitude at T = %f', simOut.h.Time(i)))
drawnow limitrate % drawnow limitrate
%
% Write Animation to gif, set to zero when testing since its slow to render. % % Write Animation to gif, set to zero when testing since its slow to render.
outframes = 0; % 50 is a nice default % outframes = 0; % 50 is a nice default
if outframes % if outframes
% Write to the GIF File % % Write to the GIF File
if i == 1 % if i == 1
%
% Capture the plot as an image % % Capture the plot as an image
frame = getframe(h); % frame = getframe(h);
im = frame2im(frame); % im = frame2im(frame);
[imind,cm] = rgb2ind(im,256); % [imind,cm] = rgb2ind(im,256);
%
%initalize plot % %initalize plot
imwrite(imind,cm,'outputs/Altitude.gif','gif', 'Loopcount',inf); % imwrite(imind,cm,'outputs/Altitude.gif','gif', 'Loopcount',inf);
%
elseif mod(i,floor(length(simOut.h.Data)/outframes)) == 0 % elseif mod(i,floor(length(simOut.h.Data)/outframes)) == 0
% Capture the plot as an image % % Capture the plot as an image
frame = getframe(h); % frame = getframe(h);
im = frame2im(frame); % im = frame2im(frame);
[imind,cm] = rgb2ind(im,256); % [imind,cm] = rgb2ind(im,256);
%
% Append to plot % % Append to plot
imwrite(imind,cm,'outputs/Altitude.gif','gif','WriteMode','append', 'DelayTime', .2); % imwrite(imind,cm,'outputs/Altitude.gif','gif','WriteMode','append', 'DelayTime', .2);
end % end
end % end
end % end