Commit d8c18e12 authored by mmroma's avatar mmroma
Browse files

Coded new simple guidance in position. Controller and equilibrium state still aren't done yet

parent 11308508
function [quads_xd] = compute_quad_guidance_2(params)
%compute_quad_guidance
% Hard coded to have last quad be on top
%% Init
eps = 0.5; % Load distribution parameter
n = params.num_vehicles;
L = params.multirotors{quad_num}.L;
r = params.payload.diameter / 2;
azi = params.azi;
ele = params.ele;
quads_xd = cell(n,1);
F_Tether_i_P = cell(n,1);
s_hat_i_P = cell(n,1);
%% 1) Compute s_hat_i_P
for i=1:n
s_hat_i_P{i} = makehgtform('zrotate',azi(i)) *...
makehgtform('yrotate',ele(i)) *...
[1;0;0;1];
s_hat_i_P{i} = s_hat_i_P{i}(1:3);
end
%% 2) Compute F_Gravity_P_P
F_Gravity_P_P = [0; 0; params.payload.m * params.payload.g];
%% 3.1) Compute Tether n (5)
F_Tether_i_P{n} = - eps * F_Gravity_P_P;
%% 3.2) Compute Tether 1, ..., n-1 (1-4)
F_Tether_i_P_mag = -(1-eps) * (dot(F_Gravity_P_P,[0;0;1]))...
/ ( 4 * dot(s_hat_i_P{1},[0;0;1]));
for i=1:n-1
F_Tether_i_P{i} = F_Tether_i_P_mag
end
%% Compute F_tether_i_P_mag
%% Compute l_d_i
%%
t_d_Pi_G = makehgtform('zrotate',azi(quad_num)) *...
makehgtform('yrotate',ele(quad_num)) *...
[r+L;0;0;1];
t_d_Pi_G = t_d_Pi_G(1:3);
r_d_P_G = params.payload.xd(:,1:3);
r_dot_d_P_G = params.payload.xd(:,4:6);
%% Compute
r_d_Vi_G = r_d_P_G + t_d_Pi_G';
r_dot_d_Vi_G = r_dot_d_P_G;
%% Format Output
xd = [r_d_Vi_G, r_dot_d_Vi_G, zeros(size(r_dot_d_Vi_G,1),6)];
end
function [quads_xd] = compute_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
%% Init
eps = 0.5; % Load distribution parameter
n = params.num_vehicles;
r = params.payload.diameter / 2;
azi = params.azi;
ele = params.ele;
quads_xd = cell(n,1);
F_Tether_i_P = cell(n,1);
s_hat_i_P = cell(n,1);
l_d_i = cell(n,1);
s_i_P = cell(n,1);
%% 1) Compute s_hat_i_P
for i=1:n
s_hat_i_P{i} = makehgtform('zrotate',azi(i)) *...
makehgtform('yrotate',ele(i)) *...
[1;0;0;1];
s_hat_i_P{i} = s_hat_i_P{i}(1:3);
end
%% 2) Compute F_Gravity_P_P
F_Gravity_P_P = [0; 0; params.payload.m * params.payload.g];
%% 3) Compute F_Tether_i_P
% Compute Tether n (5)
F_Tether_i_P{n} = - eps * F_Gravity_P_P;
%Compute Tether 1, ..., n-1 (1-4)
F_Tether_i_P_mag = -(1-eps) * (dot(F_Gravity_P_P,[0;0;1]))...
/ ( 4 * dot(s_hat_i_P{1},[0;0;1]));
for i=1:n-1
F_Tether_i_P{i} = F_Tether_i_P_mag .* s_hat_i_P{i};
end
%% 4) Compute l_d_i
for i=1:n
l_d_i{i} = norm(F_Tether_i_P{i}) / params.multirotors{i}.k + params.multirotors{i}.L;
end
%% 5) Compute s_i_P
for i=1:n
s_i_P{i} = (l_d_i{i} + r) .* s_hat_i_P{i};
end
% t_d_Pi_G = makehgtform('zrotate',azi(quad_num)) *...
% makehgtform('yrotate',ele(quad_num)) *...
% [r+L;0;0;1];
% t_d_Pi_G = t_d_Pi_G(1:3);
%% 6) Compute Trajectory in Position and Velocity
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;
quads_xd{i} = [r_d_Vi_G, r_dot_d_Vi_G, zeros(size(r_dot_d_Vi_G,1),6)];
end
end
function [params] = setUpParameters_cube_3()
function [params] = setUpParameters_sphere_4()
%setUpParameters
% Wind Added and a 5th quad (cube)
......@@ -39,55 +39,6 @@ function [params] = setUpParameters_cube_3()
params.multirotors{i}.distSTD = quad_dist_STD;
params.l_i_last{i} = L_all;
end
% r = params.payload.diameter / 2;
% i=params.num_vehicles;
% params.multirotors{i} = parameters_Quad();
% params.multirotors{i}.L = L_all;
% r_Pi_P = makehgtform('zrotate',azi(i)) *...
% makehgtform('yrotate',ele(i)) *...
% [r;0;0;1];
% params.multirotors{i}.r_Pi_P = r_Pi_P(1:3);
% params.multirotors{i}.distSTD = quad_dist_STD;
% % Q1
%
%
% i = 1;
%
% params.multirotors{i} = parameters_Quad();
% params.multirotors{i}.L = L_all;
% r_Pi_P = makehgtform('zrotate',azi(i)) *...
% makehgtform('yrotate',ele(i)) *...
% [r;0;0;1];
% params.multirotors{i}.r_Pi_P = r_Pi_P(1:3);
%
% % Q2
% i = 2;
% params.multirotors{i} = parameters_Quad();
% params.multirotors{i}.L = L_all;
% r_Pi_P = makehgtform('zrotate',azi(i)) *...
% makehgtform('yrotate',ele(i)) *...
% [r;0;0;1];
% params.multirotors{i}.r_Pi_P = r_Pi_P(1:3);
%
% % Q3
% i = 3;
% params.multirotors{i} = parameters_Quad();
% params.multirotors{i}.L = L_all;
% r_Pi_P = makehgtform('zrotate',azi(i)) *...
% makehgtform('yrotate',ele(i)) *...
% [r;0;0;1];
% params.multirotors{i}.r_Pi_P = r_Pi_P(1:3);
%
% % Q4
% i = 4;
% params.multirotors{i} = parameters_Quad();
% params.multirotors{i}.L = L_all;
% r_Pi_P = makehgtform('zrotate',azi(i)) *...
% makehgtform('yrotate',ele(i)) *...
% [r;0;0;1];
% params.multirotors{i}.r_Pi_P = r_Pi_P(1:3);
%% Wind Parameters
params.wind.enable = 0;
......@@ -97,8 +48,8 @@ function [params] = setUpParameters_cube_3()
params.wind.rho_f = 1.225; % (kg/m^3)
params.wind.mu = 1.802e-5; % (kg/ms)
params.wind.load_d = params.payload.diameter; % m
% params.wind.C_D = @(Re) C_D_sphere(Re); % Sphere Drag Model
params.wind.C_D = @(Re) C_D_cube(Re); % Cube Drag Model
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;
......@@ -112,16 +63,26 @@ function [params] = setUpParameters_cube_3()
params.tStep = 0.005; % Simulation Step Size (s)
params.anim_FPS = 30; % Animation FPS
%% Guidance
%% Guidance (version 1)
% [params.payload.xd, params.tFinal] = gen_payload_traj(params);
% params.payload.x0 = params.payload.xd(1,:)';
% for i=1:params.num_vehicles
% params.multirotors{i}.xd = compute_quad_guidance(params,i);
% params.multirotors{i}.x0 = params.multirotors{i}.xd(1,:)';
% end
% params.multirotors{5}.L = L_all - 0.6;
%% Guidamce (version 2)
[params.payload.xd, params.tFinal] = gen_payload_traj(params);
params.payload.x0 = params.payload.xd(1,:)';
for i=1:params.num_vehicles
params.multirotors{i}.xd = compute_quad_guidance(params,i);
quads_xd = compute_quad_guidance_2(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.multirotors{5}.L = L_all - 0.6;
%% Sanity check guidance
% plot_guidance(params);
......
......@@ -10,7 +10,7 @@
addpath(genpath(pwd))
%% Set Up Parameters
params = setUpParameters_cube_3();
params = setUpParameters_sphere_4();
%% Simulate
simData = runSimulation(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