% draw graphics 
% angular_rates_controller_draw_graphics
% Carrilho - 21 Jan 2015

t = simout.Time;

p_r = simout.Data(:,1);
q_r = simout.Data(:,2);
r_r = simout.Data(:,3);

p_I_n = simout.Data(:,4);
q_I_n = simout.Data(:,5);
r_I_n = simout.Data(:,6);
tau_phi_n = simout.Data(:,7);
tau_theta_n = simout.Data(:,8);
tau_psi_n = simout.Data(:,9);
p_n = simout.Data(:,10);
q_n = simout.Data(:,11);
r_n = simout.Data(:,12);

p_I_l = simout.Data(:,13);
q_I_l = simout.Data(:,14);
r_I_l = simout.Data(:,15);
tau_phi_l = simout.Data(:,16);
tau_theta_l = simout.Data(:,17);
tau_psi_l = simout.Data(:,18);
p_l = simout.Data(:,19);
q_l = simout.Data(:,20);
r_l = simout.Data(:,21);


figure(1)
plot(t,p_n,t,p_l,t,p_r,'LineWidth',2)
grid
title('p')
xlabel('Time [s]')
ylabel('[rad/s]')
legend('Nonlinear','Linear','Reference')

figure(2)
plot(t,q_n,t,q_l,t,q_r,'LineWidth',2)
grid
title('q')
xlabel('Time [s]')
ylabel('[rad/s]')
legend('Nonlinear','Linear','Reference')

figure(3)
plot(t,r_n,t,r_l,t,r_r,'LineWidth',2)
grid
title('r')
xlabel('Time [s]')
ylabel('[rad/s]')
legend('Nonlinear','Linear','Reference')

% figure(4)
% plot(t,p_I_n,t,p_I_l,'LineWidth',2)
% grid
% title('p_I')
% xlabel('Time [s]')
% ylabel('[rad]')
% legend('Nonlinear','Linear')
% 
% figure(5)
% plot(t,q_I_n,t,q_I_l,'LineWidth',2)
% grid
% title('q_I')
% xlabel('Time [s]')
% ylabel('[rad]')
% legend('Nonlinear','Linear')
% 
% figure(6)
% plot(t,r_I_n,t,r_I_l,'LineWidth',2)
% grid
% title('r_I')
% xlabel('Time [s]')
% ylabel('[rad]')
% legend('Nonlinear','Linear')

figure(7)
plot(t,tau_phi_n,t,tau_phi_l,'LineWidth',2)
grid
title('\tau_\phi')
xlabel('Time [s]')
ylabel('[N.m]')
legend('Nonlinear','Linear')

figure(8)
plot(t,tau_theta_n,t,tau_theta_l,'LineWidth',2)
grid
title('\tau_\theta')
xlabel('Time [s]')
ylabel('[N.m]')
legend('Nonlinear','Linear')

figure(9)
plot(t,tau_psi_n,t,tau_psi_l,'LineWidth',2)
grid
title('\tau_\psi')
xlabel('Time [s]')
ylabel('[N.m]')
legend('Nonlinear','Linear')

% save figures 
print -f1 -dpng -r200 simulation_rotation_dynamics_p.png
print -f2 -dpng -r200 simulation_rotation_dynamics_q.png
print -f3 -dpng -r200 simulation_rotation_dynamics_r.png
print -f7 -dpng -r200 simulation_rotation_dynamics_tau_phi.png
print -f8 -dpng -r200 simulation_rotation_dynamics_tau_theta.png
print -f9 -dpng -r200 simulation_rotation_dynamics_tau_psi.png


