Commit 13f167b3 authored by mmroma's avatar mmroma
Browse files

Basic 4 plots and animation are working. Now onto rc_pilot testing!

parent 22691df2
function [] = animate_simulink_data_single_vehicle(simData, params)
%animate Animates the multirotor
% Inputs:
% simData
% Outputs:
% Animation Plot
%% Some hardcoding before we smooth things out
% params.num_vehicles = 1;
%using_load = true;
%% Process Input into simpler names
T0 = simData.tout;
X0 = [simData.q1_X];
anim_FPS = params.anim_FPS;
%% Interpolate to get requested display rate
deltaT = 1 / anim_FPS;
t = T0(1) : deltaT : T0(end);
load_ind = params.num_vehicles + 1;
% Get Data for Each Vehicle
for quad = 1:params.num_vehicles
%% Obtain X,Y,Z from state
X{quad} = interp1(T0,X0(:,(quad-1)*12 + 1),t);
Y{quad} = interp1(T0,X0(:,(quad-1)*12 + 2),t);
Z{quad} = interp1(T0,X0(:,(quad-1)*12 + 3),t);
%% Obtain Roll, Pitch, Yaw from state
Phi{quad} = interp1(T0,X0(:,(quad-1)*12 + 7),t);
Theta{quad} = interp1(T0,X0(:,(quad-1)*12 + 8),t);
Psi{quad} = interp1(T0,X0(:,(quad-1)*12 + 9),t);
%% Process mounting points
r_Pi_P{quad} = params.multirotors{quad}.r_Pi_P;
end
%% Set Graph Limits
cubeSideLength = 1.5;
% X Graph Limits
xUlim = cubeSideLength;
xLlim = -cubeSideLength;
% Y Graph Limtis
yUlim = cubeSideLength;
yLlim = -cubeSideLength;
% Z Graph Limits
zUlim = cubeSideLength;
zLlim = -cubeSideLength;
% zUlim = -1;
% zLlim = -2;
%% Set up the graph
figure(100); clf;
axis([xLlim xUlim yLlim yUlim zLlim zUlim]);
ax = gca;
xlabel('X');
ylabel('Y');
zlabel('Z');
% Flip Z Axis and Y Axis
set(gca, 'Zdir', 'reverse');
set(gca, 'Ydir', 'reverse');
grid on;
movegui(gcf,'north');
%% Draw Multirotor
% Quadrotor in Cross Configuration
qw = 0.25; %Quad Width
qh = 0.04; %Quad Height
quadFrontColor = [1 0 0];
quadBackColor = [0 0 0];
multiRotor_T = eye(4);
for quad = 1 : params.num_vehicles
mr{quad} = drawQuad(ax,qw,qh,quadFrontColor,quadBackColor);
set(mr{quad},'Matrix',multiRotor_T);
end
%% Change Viewpoint
view(-75,50);
%% Animate
tic
for i=1:length(t)
title(['t = ' num2str( t(i),'%2.1f') ' s']);
for quad = 1 : params.num_vehicles
% Update Multirotor
multiRotor_T = makehgtform('zrotate',Psi{quad}(i)) *...
makehgtform('yrotate',Theta{quad}(i)) *...
makehgtform('xrotate',Phi{quad}(i));
multiRotor_T(1:3,4) = [X{quad}(i); Y{quad}(i); Z{quad}(i)];
set(mr{quad},'Matrix',multiRotor_T);
end
%Draws a point tracing the quad's path
plot3(X{1}(i), Y{1}(i), Z{1}(i), 'c.')
% Display the Animation and sleep to maintain the display FPS
drawnow;
sleepTime = deltaT - toc;
if (sleepTime > 0)
java.lang.Thread.sleep((sleepTime)*1000);
end
tic;
% %recording
% if strcmp(name, 'none') ~= 1
% frame = getframe;
% writeVideo(writerObj,frame);
% end
end
%close video object
% if strcmp(name, 'none') ~= 1
% close(writerObj);
% end
end
function [] = plot_orientation(T,X,Xd,title_string,location)
function [] = plot_orientation(T,X,Xd,title_string,location,lw,fs)
figure();
......
function [] = plot_orientation_rates(T,X,Xd, simData)
function [] = plot_orientation_rates(T,X,Xd,title_string,location,lw,fs)
figure();
subplot(3,1,1);
plot(T,X(:,10));
subplot(3,1,1);
plot(T,X(:,10));
hold on;
plot(T,simData.q1_phi_dot_des,'r--');
plot(T,Xd(:,10),'r--');
hold off;
ylabel('p (rad/s)');
xlabel('Time (s)');
......@@ -15,7 +15,7 @@ function [] = plot_orientation_rates(T,X,Xd, simData)
subplot(3,1,2);
plot(T,X(:,11));
hold on;
plot(T,simData.q1_theta_dot_des,'r--');
plot(T,Xd(:,11),'r--');
hold off;
ylabel('q (rad/s)');
xlabel('Time (s)');
......@@ -24,11 +24,43 @@ function [] = plot_orientation_rates(T,X,Xd, simData)
subplot(3,1,3);
plot(T,X(:,12));
hold on;
plot(T,simData.q1_psi_dot_des,'r--');
plot(T,Xd(:,12),'r--');
hold off;
ylabel('r (rad/s)');
xlabel('Time (s)');
legend('Actual','Desired');
% figure();
%
% subplot(3,1,1);
% plot(T,X(:,10));
% hold on;
% plot(T,simData.q1_phi_dot_des,'r--');
% hold off;
% ylabel('p (rad/s)');
% xlabel('Time (s)');
% title('Attitude Rates');
% legend('Actual','Desired');
%
% subplot(3,1,2);
% plot(T,X(:,11));
% hold on;
% plot(T,simData.q1_theta_dot_des,'r--');
% hold off;
% ylabel('q (rad/s)');
% xlabel('Time (s)');
% legend('Actual','Desired');
%
% subplot(3,1,3);
% plot(T,X(:,12));
% hold on;
% plot(T,simData.q1_psi_dot_des,'r--');
% hold off;
% ylabel('r (rad/s)');
% xlabel('Time (s)');
% legend('Actual','Desired');
movegui(location);
end
......@@ -7,9 +7,14 @@
%% Add Necessary Folders to MATLAB Path (add everything)
addpath(genpath(pwd));
%% Set up Parameters
%% Set up Parameters (comment/uncomment tests below)
% Test 1: Square Trajectory
traj_num = 2;
params = setUpParameters_OneQuad(traj_num);
params.num_vehicles = 1;
%% Load Top Level Simulink Model (helps Simulink load ahead of time)
......@@ -21,12 +26,15 @@ simData = sim('singlequadrotor','StartTime','0','StopTime',num2str(params.tFinal
%% Plot
% TODO plot quadrotor stuff, not load stuff
lw = 1;
fs = 12;
plot_position(simData.tout, simData.q1_X, simData.q1_Xd,'Position','northwest',lw,fs);
plot_velocity(simData.tout, simData.q1_X, simData.q1_Xd,'Velocity','northeast',lw,fs);
plot_orientation(simData.tout, simData.q1_X, simData.q1_Xd, 'Orientation','southwest',lw,fs);
plot_orientation_rates(simData.tout, simData.q1_X, simData.q1_Xd, 'Orientation Rate','southeast',lw,fs);
plot_position(simData.tout, simData.q1_X, simData.q1_Xd);
plot_velocity(simData.tout, simData.q1_X, simData.q1_Xd, simData);
plot_orientation(simData.tout, simData.q1_X, simData.q1_Xd, simData);
plot_orientation_rates(simData.tout, simData.q1_X, simData.q1_Xd, simData);
%plot_FF(simData.tout, simData.q1_X, simData.q1_Xd, simData);
%plot_FF_2(simData.tout, simData.q1_X, simData.q1_Xd, simData);
%plot_acceleration(simData.tout, simData.q1_X, simData.q1_Xd, simData);
......@@ -36,7 +44,7 @@ plot_orientation_rates(simData.tout, simData.q1_X, simData.q1_Xd, simData);
% plot_force_torque_des(simData.tout, simData.q1_X, simData.q1_Xd, simData);
%% Animate
animate_simulink_data(simData, params, false);
animate_simulink_data_single_vehicle(simData, params);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment