Commit 5b34c4d5 authored by mmroma's avatar mmroma
Browse files

In the process of reorganizing code, with a focus on systemModel.m

parent 81c983d6
function [params] = parameters_Payload()
function [params] = parameters_Payload(global_params)
%parameters_pointMass
% Parameters file for payload
%%
params.g = 9.81; % Acc. Due to Gravity (m/s^2)
params.g = global_params.g;% Acc. Due to Gravity (m/s^2)
params.m = 1; % Point Mass (kg)
params.diameter = .246; % Diameter of sphere
......
function [params] = parameters_Quad()
function [params] = parameters_Quad(global_params)
%parameters_APM_Quad
% Parameters file for a Quadrotor using APM's cascaded PID controller
......@@ -7,7 +7,7 @@ function [params] = parameters_Quad()
params.cross = true; % Cross Configuration (as opposed to plus)
%% Multirotor Physics Parameters
params.g = 9.81; % Acc. Due to Gravity (m/s^2)
params.g = global_params.g; % Acc. Due to Gravity (m/s^2)
params.m = 1.05; % Mass of Quadrotor (kg)
params.c1 = 5; % Motor Force Constant
params.c2 = 0.04; % Motor Torque Constant
......
function [params] = setUpParameters_sphere_4()
function [params] = setUpParameters()
%setUpParameters
% Wind Added and a 5th quad (cube)
%% Global Parameters
params.g = 9.81;
%% Noise and Distrubance Forces
quad_dist_STD = 0; %0.1; % 0-mean White Gaussian Quad Disturbance
load_dist_STD = 0; %0.1; % 0-mean White Gaussian Load Disturbance
......@@ -15,7 +18,7 @@ function [params] = setUpParameters_sphere_4()
0;0;0];
%% Load Payload (sphere in this case)
params.payload = parameters_Payload();
params.payload = parameters_Payload(params);
params.payload.distSTD = load_dist_STD;
params.payload.m = 0.5;
r = params.payload.diameter / 2;
......@@ -37,7 +40,7 @@ function [params] = setUpParameters_sphere_4()
params.ele = ele;
for i=1:params.num_vehicles
params.multirotors{i} = parameters_Quad();
params.multirotors{i} = parameters_Quad(params);
params.multirotors{i}.L = L_all;
r_Pi_P = makehgtform('zrotate',azi(i)) *...
makehgtform('yrotate',ele(i)) *...
......
function [params] = setUpParameters_sphere_1()
%setUpParameters
% Stationary Test
%% Load Payload (sphere in this case)
params.payload = parameters_Payload();
% params.payload.x0(1:3) = [0;0;sqrt(3)/2];
params.payload.x0(1:3) = [0;0;0];
r = params.payload.diameter / 2;
%% Load Multirotors
params.num_vehicles = 4;
L_all = 1.5;
height_all = -1.5;
% Q1
i = 1;
params.multirotors{i} = parameters_Quad();
params.multirotors{i}.x0(1:3) = [0.5;-0.5; height_all];
params.multirotors{i}.xd(1:3) = [0.5;-0.5; height_all];
params.multirotors{i}.L = L_all;
r_Pi_P = makehgtform('zrotate',-pi/4) *...
makehgtform('yrotate',pi/4) *...
[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}.x0(1:3) = [0.5;0.5;height_all];
params.multirotors{i}.xd(1:3) = [0.5;0.5;height_all];
params.multirotors{i}.L = L_all;
r_Pi_P = makehgtform('zrotate',pi/4) *...
makehgtform('yrotate',pi/4) *...
[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}.x0(1:3) = [-0.5;-0.5;height_all];
params.multirotors{i}.xd(1:3) = [-0.5;-0.5;height_all];
params.multirotors{i}.L = L_all;
r_Pi_P = makehgtform('zrotate',-3*pi/4) *...
makehgtform('yrotate',pi/4) *...
[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}.x0(1:3) = [-0.5;0.5;height_all];
params.multirotors{i}.xd(1:3) = [-0.5;0.5;height_all];
params.multirotors{i}.L = L_all;
r_Pi_P = makehgtform('zrotate',3*pi/4) *...
makehgtform('yrotate',pi/4) *...
[r;0;0;1];
params.multirotors{i}.r_Pi_P = r_Pi_P(1:3);
%% Choose Additional Parameters
params.tFinal = 10; % Simulation Length (s)
params.tStep = 0.0025; % Simulation Step Size (s)
params.anim_FPS = 60; % Animation FPS
%% Sensor Noise
params.noiseSTD = 0; %0.1; % 0-mean White Gaussian Noise (Sensor Noise)
end
function [params] = setUpParameters_sphere_2()
%setUpParameters
% Stationary Test with Wind Added
%% Load Payload (sphere in this case)
params.payload = parameters_Payload();
% params.payload.x0(1:3) = [0;0;sqrt(3)/2];
params.payload.x0(1:3) = [0;0;0];
r = params.payload.diameter / 2;
% r = 0;
%% Load Multirotors
params.num_vehicles = 4;
L_all = 1.5;
height_all = -1.5;
azi = [-pi/4, pi/4, 3*pi/4, -3*pi/4];
ele = [ pi/4, pi/4, pi/4, pi/4];
params.azi = azi;
params.ele = ele;
% Q1
i = 1;
params.multirotors{i} = parameters_Quad();
params.multirotors{i}.x0(1:3) = [0.5;-0.5; height_all];
params.multirotors{i}.xd(1:3) = [0.5;-0.5; height_all];
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}.x0(1:3) = [0.5;0.5;height_all];
params.multirotors{i}.xd(1:3) = [0.5;0.5;height_all];
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}.x0(1:3) = [-0.5;-0.5;height_all];
params.multirotors{i}.xd(1:3) = [-0.5;-0.5;height_all];
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}.x0(1:3) = [-0.5;0.5;height_all];
params.multirotors{i}.xd(1:3) = [-0.5;0.5;height_all];
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;
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.load_d = params.payload.diameter; % m
params.wind.C_D = @(Re) C_D_sphere(Re); % Sphere 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);
%% Choose Additional Parameters
params.tFinal = 10; % Simulation Length (s)
params.tStep = 0.005; % Simulation Step Size (s)
params.anim_FPS = 30; % Animation FPS
%% Sensor Noise
params.noiseSTD = 0; %0.1; % 0-mean White Gaussian Noise (Sensor Noise)
%% Guidance
[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
%% Sanity check guidance
% plot_guidance(params);
end
function [params] = setUpParameters_sphere_3()
%setUpParameters
% Wind Added and a 5th quad
%% Noise and Distrubance Forces
quad_dist_STD = 0.1; %0.1; % 0-mean White Gaussian Quad Disturbance
load_dist_STD = 0.1; %0.1; % 0-mean White Gaussian Load Disturbance
% 0-mean White Gaussian Noise (Sensor Noise) on each vehicle's state
% std = 0.001;
std = 0;
params.noiseSTD = [std;std;std;
0;0;0;
0;0;0;
0;0;0];
%% Load Payload (sphere in this case)
params.payload = parameters_Payload();
params.payload.distSTD = load_dist_STD;
r = params.payload.diameter / 2;
%% Load Multirotors
params.num_vehicles = 5;
L_all = 1.5;
azi = [-pi/4, pi/4, 3*pi/4, -3*pi/4, 0];
ele = [ pi/4, pi/4, pi/4, pi/4, pi/2];
params.azi = azi;
params.ele = ele;
for i=1: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;
end
% % 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 = 1;
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.load_d = params.payload.diameter; % m
params.wind.C_D = @(Re) C_D_sphere(Re); % Sphere 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);
%% Choose Additional Parameters
params.tFinal = 10; % Simulation Length (s)
params.tStep = 0.005; % Simulation Step Size (s)
params.anim_FPS = 30; % Animation FPS
%% Guidance
[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
%% Sanity check guidance
% plot_guidance(params);
end
function [] = calc_drag_force_payload(X_payload, params_global)
%calc_drag_force_payload
R_W_P = R_P_G' * R_W_G;
R_P_W = R_W_P';
V_W_P_rel_W = [v_W; 0; 0] - R_P_W * [u; v; w];
v_W_rm = norm(V_W_P_rel_W);
Re = reynolds_num(v_W_rm,params);
C_D = params.wind.C_D(Re);
f_drag = (1/8)*C_D*pi* params.wind.load_d^2 *...
v_W_rm^2 * params.wind.rho_f;
F_drag_W = f_drag * V_W_P_rel_W / v_W_rm;
F_drag_P = R_P_G' * R_W_G * F_drag_W;
if params.wind.enable == 0
F_drag_P = [0; 0; 0];
end
end
function [] = calc_drag_force_vehicle(X_vehicle)
%calc_drag_force_vehicle
%% Drag Force on Quadrotor
V_W_Vi_rel_Vi = R_V_G' * R_W_G * [v_W; 0; 0] - [u; v; w];
F_drag_V = - params.wind.C_bar_d * F * V_W_Vi_rel_Vi;
if params.wind.enable == 0
F_drag_V = [0; 0; 0];
end
end
function [] = calc_gravity_force(X_vehicle, params_vehicle)
%calc_gravity_force
F_gravity_V = R_V_G' * [0; 0; m*g];
end
function [Thrust_Force, Thrust_Torque] = calc_propeller_force_torque(U, params)
%calc_propeller_force_torque
F_and_Taos = params.M * U;
Thrust_Force = F_and_Taos(1);
Thrust_Torque = F_and_Taos(2:4);
end
function [] = calc_tether_force(X_vehicle, params_vehicle, X_payload, params_payload)
%calc_tether_force
% Preliminary Calcs
r_Vi_G = [x;y;z];
r_P_G = [load_x; load_y; load_z];
r_Pi_G = R_P_G * r_Pi_P + r_P_G;
t_Pi_G = r_Pi_G - r_Vi_G;
% tether length and direction calcs
l_i = norm(t_Pi_G);
t_hat_Pi_G = t_Pi_G ./ l_i;
%
% l_dot_i method #2 (explicit formula)
t_dot_Pi_G = cross(load_omega, R_P_G * r_Pi_P) + load_pos_d - pos_d;
l_dot_i = dot(t_dot_Pi_G, t_hat_Pi_G);
% l_dot_i = 0;
% exist('i_last','var') == 1
% tether force calc (with neg tension check)
F_tether_mag = (k.*(l_i - L) + b.*l_dot_i);
if (params.multirotors{i}.NegTension == 0)
F_tether_mag = max(F_tether_mag,0);
end
% Constrain Max tension for debugging
F_tether_mag = min(F_tether_mag, params.multirotors{i}.MaxTension);
F_Tether_i = F_tether_mag .* -1 .* t_hat_Pi_G;
% Add up tether forces
F_Tether_total = F_Tether_total + F_Tether_i;
% Add up torques as well
F_Tether_i_P = R_P_G' * F_Tether_i;
% Tao_tether_i_P = R_P_G' * cross(R_P_G * r_Pi_P, F_Tether_i);
Tao_tether_i_P = cross(r_Pi_P, F_Tether_i_P);
Tao_tether_total_P = Tao_tether_total_P + Tao_tether_i_P;
end
function [X_struct] = X_struct_readable(X)
%X_struct_readable
X_struct.x = X(1);
X_struct.y = X(2);
X_struct.z = X(3);
X_struct.u = X(4);
X_struct.v = X(5);
X_struct.w = X(6);
X_struct.phi = X(7);
X_struct.theta = X(8);
X_struct.psi = X(9);
X_struct.p = X(10);
X_struct.q = X(11);
X_struct.r = X(12);
end
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