Commit 7d29036f authored by yean's avatar yean
Browse files

This is the working code for one quadrotor

parent 32706b42
function [params] = compute_single_quad_guidance_2(params)
%compute_quad_guidance
% Hard coded to have last quad be on top
% and also that remaining quads are evenly distributed
% Assume its a sphere
% Assume payload desired orientation is 0
%% 7) Compute Trajectory in Position and Velocity and Attitude
r_d_P_G = params.payload.xd(1:3,:);
r_dot_d_P_G = params.payload.xd(4:6,:);
for i=1:n
r_d_Vi_G = r_d_P_G + s_i_P{i};
r_dot_d_Vi_G = r_dot_d_P_G;
rpy = zeros(6,size(r_dot_d_Vi_G,2));
rpy(1:2,:) = [phi_i{i}; theta_i{i}] * ones(1,size(r_dot_d_Vi_G,2));
quads_xd{i} = [r_d_Vi_G; r_dot_d_Vi_G; rpy];
end
%% Copy everything back to params
for i=1:length(quads_xd)
params.multirotors{i}.xd = quads_xd{i};
params.multirotors{i}.x0 = params.multirotors{i}.xd(:,1);
end
params.F_Tether_i_P = F_Tether_i_P;
params.s_hat_i_P = s_hat_i_P;
params.l_d_i = l_d_i;
params.s_i_P = s_i_P;
params.f_d_i = f_d_i;
params.f_thrust_i = f_thrust_i;
params.phi_i = phi_i;
params.theta_i = theta_i;
end
function [params] = setUpParameters_OneQuad(traj_num)
%setUpParameters
% Calls parameters_Quad() and parameters_Payload() with default
% settings and then adjusts from there. All edits should be made to
% this file.
%%
params.traj_num = traj_num;
params.save_figs_bool = true;
%% Debugging Parameters (true / false)
params.disable_linear_vehicle_motion = false;
params.disable_angular_vehicle_motion = false;
%% Global Parameters
params.g = 9.81;
params.curr_run = 1;
%% Time Step Parameters
params.ctlDt = 0.01; % Controller Step Size (s)
params.tStep = params.ctlDt / 10; % Simulation Step Size (s)
%% Disturbance Forces & Torques
% quads_dist_F_power = 1e-3;
% quads_dist_M_power = 0;
%
% %% Sensor Noise
% quad_noise_power = [ones(3,1).* 0;
% ones(3,1).* 0;
% ones(3,1).* 0;
% ones(3,1).* 0];
%%
params.dist_F_power = 0 .* [1; 1; 1]; % 0-mean Band-limited, White Gaussian Dist (Ext Force)
params.dist_F_seed = [1 2 3];
params.dist_M_power = 0 .* [1; 1; 1]; % 0-mean Band-limited, White Gaussian Dist (Ext Force)
params.dist_M_seed = [4 5 6];
params.multirotors{1}.dist_F_power = 0;
params.multirotors{1}.dist_M_power = 0;
params.multirotors{1}.noise_power = 0;
%% Noise
params.noise_power = 0.1 .* ones(12,1);
params.noise_seed = 101:112;
%% Load Multirotors
params.num_vehicles = 1;
%% Set Wind Parameters
params.wind.enable = 0;
params.wind.speed = 0; % (m/s)
params.wind.yaw = 0; % (rad). 0 = +x, pi/2 = +y
params.wind.rho_f = 1.225; % (kg/m^3)
params.wind.mu = 1.802e-5; % (kg/ms)
params.wind.C_D = @(Re) C_D_sphere(Re); % Sphere Drag Model
% params.wind.C_D = @(Re) C_D_cube(Re); % Cube Drag Model
c_bar_d = 0.04; % Quad Lumped Drag
params.wind.C_bar_d = [c_bar_d, 0, 0;
0, c_bar_d, 0;
0, 0, 0];
params.wind.R_W_G = makehgtform('zrotate',params.wind.yaw);
params.wind.R_W_G = params.wind.R_W_G(1:3,1:3);
%% Load Quad Default Parameters
params.multirotors{1} = parameters_Quad(params);
[params.multirotors{1}.xd, params.tFinal] = gen_payload_traj(params);
params.multirotors{1}.x0 = params.multirotors{1}.xd(:,1);
% params.multirotors{1}.xd = [0;... % x position [m]
% 0;... % y position [m]
% 0;... % z position [m]
% 0;... % u (m/s)
% 0;... % v (m/s)
% 0;... % w (m/s)
% 0;... % phi (roll) (rad)
% 0;... % theta (pitch) (rad)
% 0;... % psi (yaw) (rad)
% 0;... % p (rad/s)
% 0;... % q (rad/s)
% 0]; % r (rad/s)
%% Choose Additional Parameters
params.anim_FPS = 30; % Animation FPS
%% Set Guidamce
params.traj_num = traj_num;
end
function [params] = setUpParameters_OneQuad(traj_num)
%setUpParameters
% Calls parameters_Quad() and parameters_Payload() with default
% settings and then adjusts from there. All edits should be made to
% this file.
%%
params.save_figs_bool = true;
%% Debugging Parameters (true / false)
params.disable_linear_vehicle_motion = false;
params.disable_angular_vehicle_motion = false;
%% Global Parameters
params.g = 9.81;
params.curr_run = 1;
%% Time Step Parameters
params.ctlDt = 0.01; % Controller Step Size (s)
params.tStep = params.ctlDt / 10; % Simulation Step Size (s)
%% Disturbance Forces & Torques
quads_dist_F_power = 1e-3;
quads_dist_M_power = 0;
%% Sensor Noise
quad_noise_power = [ones(3,1).* 1e-8;
ones(3,1).* 1e-6;
ones(3,1).* 1e-6;
ones(3,1).* 1e-4];
%% Load Multirotors
params.num_vehicles = 1;
%% Set Wind Parameters
params.wind.enable = 0;
params.wind.speed = 0; % (m/s)
params.wind.yaw = 0; % (rad). 0 = +x, pi/2 = +y
params.wind.rho_f = 1.225; % (kg/m^3)
params.wind.mu = 1.802e-5; % (kg/ms)
params.wind.C_D = @(Re) C_D_sphere(Re); % Sphere Drag Model
% params.wind.C_D = @(Re) C_D_cube(Re); % Cube Drag Model
c_bar_d = 0.04; % Quad Lumped Drag
params.wind.C_bar_d = [c_bar_d, 0, 0;
0, c_bar_d, 0;
0, 0, 0];
params.wind.R_W_G = makehgtform('zrotate',params.wind.yaw);
params.wind.R_W_G = params.wind.R_W_G(1:3,1:3);
%% Load Quad Default Parameters
params.multirotors{1} = parameters_Quad(params);
params.multirotors{1}.xd =
[params.payload.xd, params.tFinal] = gen_payload_traj(params);
params.multirotors{1}.xd = [0;... % x position [m]
0;... % y position [m]
0;... % z position [m]
0;... % u (m/s)
0;... % v (m/s)
0;... % w (m/s)
0;... % phi (roll) (rad)
0;... % theta (pitch) (rad)
0;... % psi (yaw) (rad)
0;... % p (rad/s)
0;... % q (rad/s)
0]; % r (rad/s)
%% Choose Additional Parameters
params.tFinal = 10; % Simulation Length (s)
params.anim_FPS = 30; % Animation FPS
%% Set Guidamce
params.traj_num = traj_num;
end
......@@ -8,20 +8,25 @@
addpath(genpath(pwd));
%% Set up Parameters
traj_num = 1;
params = setUpParameters(traj_num);
traj_num = 2;
params = setUpParameters_OneQuad(traj_num);
params.num_vehicles = 1;
%% Load Top Level Simulink Model (helps Simulink load ahead of time)
open('singlequadrotor');
params.multirotors{1}.dist_F_power = 0;
params.multirotors{1}.dist_M_power = 0;
params.multirotors{1}.noise_power = 0;
%% Call Simulink and Run the Simulation
simData = sim('singlequadrotor','StartTime','0','StopTime',num2str(params.tFinal));
%% Plot
plot_vec3(simData.tout, simData.Xd_load,'Xd Load');
plot_vec3(simData.tout, simData.Xd_load_control,'Xd Load Control');
% TODO plot quadrotor stuff, not load stuff
plot_position(simData.tout, simData.q1_X, simData.q1_Xd);
plot_orientation(simData.tout, simData.q1_X, simData.q1_Xd);
%% Animate
animate_simulink_data(simData, params, false);
......
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