Skip to content

Commit

Permalink
Submission sim
Browse files Browse the repository at this point in the history
  • Loading branch information
natolambert committed Feb 24, 2018
1 parent a4874ee commit bd4378d
Show file tree
Hide file tree
Showing 7 changed files with 123 additions and 51 deletions.
9 changes: 6 additions & 3 deletions .gitignore
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,9 @@
*.autosave
/slprj
/results
.mat
.gitignore
.avi
parameters.mat
*.gitignore
*.avi
*.eps
*.fig
*.png
2 changes: 1 addition & 1 deletion PolePlacement_DesiredState.m
100755 → 100644
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,7 @@
equil = [state_equil; input_equil];

% Hard codes desired states to try
state_des = [0; 0; .07 ; 0; .00; .00; 0; 0; 0; 0; 0; 0]; %x_eq
state_des = [0; 0; .015; 0; .00; .00; 0; 0; 0; 0; 0; 0]; %x_eq
% NOTE: The system will not converge with a nonzero roll or pitch desired
% state. Have not tested angular or linear velocities. For now, desired
% state sould be a vector of the form [X,Y,Z,YAW] with 0's appended as need
Expand Down
Binary file added final_sim.mat
View file
Binary file not shown.
Binary file modified iros2018_control.slx
View file
Binary file not shown.
106 changes: 82 additions & 24 deletions iros2018plotting.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,28 +20,27 @@

subplot(4,4,[1 2 5 6 9 10 13 14])

x_lim = [-10 30]; %[cm], x axis in 3D quiver plot
y_lim = [-30 10]; %[cm], y axis in 3D quiver plot
z_lim = [-5 5]; %[cm], z axis in 3D quiver plot
view_1 = 15; %viewing angle for 3D quiver plot
x_lim = [-20 5]; %[cm], x axis in 3D quiver plot
y_lim = [-20 5]; %[cm], y axis in 3D quiver plot
z_lim = [-1 1]; %[cm], z axis in 3D quiver plot

set(gca,'XLim',x_lim,'YLim',y_lim,'ZLim',z_lim);

view_1 = 45; %viewing angle for 3D quiver plot, first parameter (like yaw rotation of plot)
view_1 = 45-180; %viewing angle for 3D quiver plot, first parameter (like yaw rotation of plot)
view_2 = 15; %viewing angle for 3D quiver plot, second parameter (roll of plot)

view(view_1,view_2);
hold on;
grid on;
title('Simulated Flight (Takeoff)')
title('Simulated Square Flight')
xlabel('X [cm]');
ylabel('Y [cm]');
zlabel('Z [cm]');

% plot X on xy plane
quiver3([0 0 0 0],[0 0 0 0],[0 0 0 0],[100 0 -100 0],[0 100 0 -100],[0 0 0 0],'ShowArrowHead','Off','Color','k');
% quiver3([0 0 0 0],[0 0 0 0],[0 0 0 0],[100 0 -100 0],[0 100 0 -100],[0 0 0 0],'ShowArrowHead','Off','Color','k');

scale_fact = 19500;
scale_fact = 32500;
numpts = floor(length(X)/scale_fact);

% Add's trajectory lines to plot
Expand Down Expand Up @@ -95,7 +94,7 @@

end
text(X(20:scale_fact:end),Y(1:scale_fact:end),Z(1:scale_fact:end),...
strcat(" t =",num2str(state_vector.Time(1:scale_fact:end))," s"),'fontsize',18)
strcat(" t =",num2str(state_vector.Time(1:scale_fact:end))," s"),'fontsize',16)



Expand Down Expand Up @@ -174,16 +173,29 @@

%%%%%%%%%%%%%%% Force Curves %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

t_lim = 1; % limit for force and attitude plots
t_lim = 4; % limit for force and attitude plots

subplot(4,4,[11 12 15 16])
% yyaxis right
% ylabel('Controller Voltage (kV)')
% plot(force_inputs.Time,sin(force_inputs.Data(:,1).^2));
% yyaxis left
plot(force_inputs.Time,force_inputs.Data(:,1) * N_to_uN);
ylabel('Thruster Force [uN]');
title('F4');
set(gca, 'ColorOrder', [0.5 0.5 0.5; .9 .4 .2; .8 .8 0; .1 .1 .6], 'NextPlot', 'replacechildren');
forces = force_inputs.Data(:,1:4)*1000;
% % volts = ForceToVolt(forces)
% [m,n] = size(forces)
% for i = 1:m
% for j = 1:n
% volts(i,j) = ForceToVolt(forces(i,j));
% end
% end
volts = (4.*forces+3.03835229e+00)./1.84117198e-03;
plot(force_inputs.Time,volts);
% plot(force_inputs.Time,force_inputs.Data(:,1:4) * N_to_uN);
ForceToVolt(.4)
ylabel('Control Input [V]');
title('Input Voltages');
legend('Thruster 1', 'Thruster 2', 'Thruster 3', 'Thruster 4')
xlabel(xlabel_Time);
xlim([0 t_lim])
%
Expand Down Expand Up @@ -222,44 +234,65 @@

%%%%%%%%%%%%%%% Control Ability %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

t = linspace(0,t_lim,200);
x_d = rad_to_deg*square(t);

z_accel_d = state_des(3)*ones(200);

subplot(4,4,3)
hold on
plot(state_vector.Time,estimates.Data(:,3), 'color',[0.9100 0.4100 0.1700]);
plot(state_vector.Time,accel_z.Data(:,1), 'b');
plot(state_vector.Time,estimates.Data(:,3), 'color',[0.5 0.5 0.5]); %[0.9100 0.4100 0.1700]);
plot(state_vector.Time,accel_z.Data(:,1), 'b','LineWidth',2);
plot(t,z_accel_d,'--r','LineWidth',2)
ylabel('Z Accel [m/s^2]');
title('Z Accel');
legend('Noisy Data', 'Ground Truth')
title('Z Accel - Body');
xlim([0 t_lim])
ylim([-.8 .8])
hold off

subplot(4,4,4)
plot(state_vector.Time,state_vector.Data(:,3) * m_to_cm);
plot(state_vector.Time,state_vector.Data(:,3) * m_to_cm, 'b','LineWidth',4);
ylabel('Z [cm]');
title('Z');
title('Z Position - Global');
xlim([0 t_lim])
hold off

subplot(4,4,7)
hold on
plot(state_vector.Time,estimates.Data(:,5) * rad_to_deg, 'color',[0.9100 0.4100 0.1700]);
plot(state_vector.Time,state_vector.Data(:,5) * rad_to_deg, 'b');
plot(state_vector.Time,estimates.Data(:,5) * rad_to_deg, 'color',[0.5 0.5 0.5]);
plot(state_vector.Time,state_vector.Data(:,5) * rad_to_deg, 'b','LineWidth',4);
plot(t,x_d(:,1), 'r','LineWidth',1.5);
ylabel('Pitch [deg]');
title('Pitch');
xlim([0 t_lim])
ylim([-6,6])
hold off

subplot(4,4,8)
hold on
plot(state_vector.Time,estimates.Data(:,6) * rad_to_deg, 'color',[0.9100 0.4100 0.1700]);
plot(state_vector.Time,state_vector.Data(:,6) * rad_to_deg, 'b');
plot(state_vector.Time,estimates.Data(:,6) * rad_to_deg, 'color',[0.5 0.5 0.5]);
plot(state_vector.Time,state_vector.Data(:,6) * rad_to_deg, 'b','LineWidth',4);
plot(t,x_d(:,2), 'r', 'LineWidth',1.5);
ylabel('Roll [deg]');
title('Roll');
ylim([-6,6])
xlim([0 t_lim])
legend('Sensor Data', 'Ground Truth', 'Desired State')

hold off



%%
function V = ForceToVolt(force)
% the force = w(1)v^2 + w(2)v + w(3)
w = [7.22076994e-07, -8.70949206e-04, -5.13566754e-01];
V = max(roots([w(1), w(2), w(3)-force]));
% w_linear = [ 1.84117198e-03 -3.03835229e+00]

end


function out = b2g(vect, att)
% Define Rotation matrices of given index
% vect = [x,y,z], att = [yaw,pitch,roll]
Expand All @@ -286,4 +319,29 @@

Body2Global = Ryaw*Rpitch*Rroll;
out = Body2Global*vect;
end
end

function x_d = square(t)
s = 1;
angle_max_deg = 5;
angle_max = angle_max_deg*pi/180;
circ_period = 2;
square_period = 1;
n = length(t);
for i = 1:n
if t(i) < square_period
x_d(i,1) = 0;
x_d(i,2) = 1*angle_max*cos(t(i)*2*pi/circ_period);
elseif t(i) < 2*square_period
x_d(i,1) = 1*angle_max*cos(t(i)*2*pi/circ_period);
x_d(i,2) = 0;
elseif t(i) < 3*square_period
x_d(i,1) = 0;
x_d(i,2) = -1*angle_max*cos(t(i)*2*pi/circ_period);
else
x_d(i,1) = -1*angle_max*cos(t(i)*2*pi/circ_period);
x_d(i,2) = 0;
end

end
end
25 changes: 18 additions & 7 deletions parameters.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@

%% Basic and Physical Paramters
g = 9.8; % [m/s^2], acceleration of gravity
m = 60e-6; % [kg], body mass Changed from 10e-6 9/5/2017 5-e-6 is with IMU + flexboard
m = 67e-6; % [kg], body mass Changed from 10e-6 9/5/2017 5-e-6 is with IMU + flexboard
lx = 1e-2; % [m], x distance to body center of mass
ly = 1e-2; % [m], y distance to body center of mass
lz = 20e-6; % [m], z distance to body center of mass
Expand Down Expand Up @@ -193,6 +193,8 @@
ly*cos(angle) -ly*cos(angle) -ly*cos(angle) ly*cos(angle);]; % Taux


FtoV = [ 7.22076994e-07 -8.70949206e-04 -5.13566754e-01]

%% Matrices for PID contorllers translating PID output to force inputs
M_z = [1,1,1,1]'; % Translates PID of z direction uniformly across 4 thrusters
M_roll = [1,-1,-1,1]';
Expand All @@ -206,15 +208,24 @@
% view_1 = 15; %viewing angle for 3D quiver plot, first parameter
% view_2 = 46; %viewing angle for 3D quiver plot, second parameter

x_lim = [-10 30]; %[cm], x axis in 3D quiver plot
y_lim = [-30 10]; %[cm], y axis in 3D quiver plot
z_lim = [-5 5]; %[cm], z axis in 3D quiver plot
x_lim = [-10 10]; %[cm], x axis in 3D quiver plot
y_lim = [-10 10]; %[cm], y axis in 3D quiver plot
z_lim = [-3 3]; %[cm], z axis in 3D quiver plot
view_1 = 15; %viewing angle for 3D quiver plot, first parameter
view_2 = 46; %viewing angle for 3D quiver plot, second parameter

x_lim = [-20 5]; %[cm], x axis in 3D quiver plot
y_lim = [-20 5]; %[cm], y axis in 3D quiver plot
z_lim = [-3 3]; %[cm], z axis in 3D quiver plot

% set(gca,'XLim',x_lim,'YLim',y_lim,'ZLim',z_lim);

view_1 = 45-180; %viewing angle for 3D quiver plot, first parameter (like yaw rotation of plot)
view_2 = 15; %viewing angle for 3D quiver plot, second parameter (roll of plot)

video_frame_frequency = 330; % how many frame jumps per frame that is recorded in video 66 = 60fps
sim_time = 1; %[s], how long the simulation runs
% view(view_1,view_2);
video_frame_frequency = 1000; % how many frame jumps per frame that is recorded in video 66 = 60fps
sim_time = 2; %[s], how long the simulation runs
video_flag = 0; % set to 1 to record video, set to 0 otherwise

save('parameters.mat');
% save('parameters.mat');
32 changes: 16 additions & 16 deletions plot_simulation.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,8 +72,8 @@
zlabel('Z [cm]');

%plot X on xy plane
quiver3(-1000,-1000,0,5000,5000,0,'ShowArrowHead','Off','Color','k');
quiver3(-1000,1000,0,5000,-5000,0,'ShowArrowHead','Off','Color','k');
% quiver3(-1000,-1000,0,5000,5000,0,'ShowArrowHead','Off','Color','k');
% quiver3(-1000,1000,0,5000,-5000,0,'ShowArrowHead','Off','Color','k');


for i=1:video_frame_frequency:length(X)
Expand Down Expand Up @@ -157,20 +157,20 @@
delete(y_vec_global_neg_offset_1_head);
delete(y_vec_global_neg_offset_2_head);

delete(x_vec_global_pos_xy_head);
delete(x_vec_global_neg_xy_head);
delete(y_vec_global_pos_xy_head);
delete(y_vec_global_neg_xy_head);

delete(x_vec_global_pos_xy_offset_1_head);
delete(x_vec_global_pos_xy_offset_2_head);
delete(x_vec_global_neg_xy_offset_1_head);
delete(x_vec_global_neg_xy_offset_2_head);

delete(y_vec_global_pos_xy_offset_1_head);
delete(y_vec_global_pos_xy_offset_2_head);
delete(y_vec_global_neg_xy_offset_1_head);
delete(y_vec_global_neg_xy_offset_2_head);
% delete(x_vec_global_pos_xy_head);
% delete(x_vec_global_neg_xy_head);
% delete(y_vec_global_pos_xy_head);
% delete(y_vec_global_neg_xy_head);
%
% delete(x_vec_global_pos_xy_offset_1_head);
% delete(x_vec_global_pos_xy_offset_2_head);
% delete(x_vec_global_neg_xy_offset_1_head);
% delete(x_vec_global_neg_xy_offset_2_head);
%
% delete(y_vec_global_pos_xy_offset_1_head);
% delete(y_vec_global_pos_xy_offset_2_head);
% delete(y_vec_global_neg_xy_offset_1_head);
% delete(y_vec_global_neg_xy_offset_2_head);


end
Expand Down

0 comments on commit bd4378d

Please sign in to comment.