Commit ee43c580 authored by mmroma's avatar mmroma
Browse files

Things are working again... ?

parent d941e5fa
......@@ -63,9 +63,10 @@ function [params] = parameters_Quad()
0]; % r (rad/s)
%% Tether Parameters
params.l_i_last = 0; % used for simple numerical integration
params.L = 0.5; % Unstretched Length (m)
params.k = 20; % Spring Constant
params.b = 5; % Damping Coefficient
params.b = 30; % Damping Coefficient
params.NegTension = 0; % Allow "Negative Tensions"? 0 = No, 1 = Yes
params.r_Pi_P = [0;0;0]; % Tether Mounting Point in Payload Frame
params.MaxTension = 100*params.m*params.g;
......
......@@ -4,7 +4,7 @@ function [params] = setUpParameters_cube_3()
%% 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
load_dist_STD = 0; %0.1; % 0-mean White Gaussian Load Disturbance
% 0-mean White Gaussian Noise (Sensor Noise) on each vehicle's state
% std = 0.001;
......@@ -17,9 +17,9 @@ function [params] = setUpParameters_cube_3()
%% Load Payload (sphere in this case)
params.payload = parameters_Payload();
params.payload.distSTD = load_dist_STD;
params.payload.m = 0.1;
r = params.payload.diameter / 2;
params.payload.m = 0.5;
% r = params.payload.diameter / 2;
r = 0;
%% Load Multirotors
params.num_vehicles = 5;
......@@ -29,7 +29,7 @@ function [params] = setUpParameters_cube_3()
params.azi = azi;
params.ele = ele;
for i=1:params.num_vehicles
for i=1:params.num_vehicles-1
params.multirotors{i} = parameters_Quad();
params.multirotors{i}.L = L_all;
r_Pi_P = makehgtform('zrotate',azi(i)) *...
......@@ -39,6 +39,16 @@ function [params] = setUpParameters_cube_3()
params.multirotors{i}.distSTD = quad_dist_STD;
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
%
%
......@@ -79,7 +89,7 @@ function [params] = setUpParameters_cube_3()
% params.multirotors{i}.r_Pi_P = r_Pi_P(1:3);
%% Wind Parameters
params.wind.enable = 1;
params.wind.enable = 0;
params.wind.speed = 0; % (m/s)
params.wind.yaw = 0; % (rad). 0 = +x, pi/2 = +y
......@@ -109,6 +119,8 @@ function [params] = setUpParameters_cube_3()
params.multirotors{i}.x0 = params.multirotors{i}.xd(1,:)';
end
params.multirotors{5}.L = L_all - 0.6;
%% Sanity check guidance
% plot_guidance(params);
......
......@@ -35,7 +35,8 @@ function [simData] = runSimulation(params)
tstep_count = 1;
for t_curr = 0:params.multirotors{1}.ctlDt:params.tFinal
%% Add sensor noise before control input
X_curr_w_noise = X_curr + dot(noiseSTDvec, randn(size(X_curr)));
% X_curr_w_noise = X_curr + dot(noiseSTDvec, randn(size(X_curr)));
X_curr_w_noise = X_curr;
% Compute Control Input
U_curr = [];
......
......@@ -111,8 +111,14 @@ function [xdot,F_nets,Tao_nets,F_tethers,F_drags,Tether_states] = systemModel(X,
% tether length and direction calcs
l_i = norm(t_Pi_G,2);
t_hat_Pi_G = t_Pi_G ./ l_i;
% try different ways to calc this...
t_dot_Pi_G = cross(load_omega, R_P_G * r_Pi_P) + load_pos_d - pos_d;
t_dot_Pi_G = 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);
......@@ -171,16 +177,16 @@ function [xdot,F_nets,Tao_nets,F_tethers,F_drags,Tether_states] = systemModel(X,
l_dot_i];
%% 2.8) Combine and append to xdot vector
xdot = [xdot;
pos_d;
vel_d;
att_d;
attRate_d];
% xdot = [xdot;
% 0;0;0;
% 0;0;0;
% 0;0;0;
% 0;0;0];
% pos_d;
% vel_d;
% att_d;
% attRate_d];
xdot = [xdot;
0;0;0;
0;0;0;
0;0;0;
0;0;0];
end
%% 3) Calculate State Derivative for the Payload
......@@ -189,22 +195,24 @@ function [xdot,F_nets,Tao_nets,F_tethers,F_drags,Tether_states] = systemModel(X,
%% 3.2) (16-18) Slung Load Velocity
% Drag Force on 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
% 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
F_drag_P = [0; 0; 0];
% Calculate Forces
F_gravity_P = R_P_G' * [0; 0; m_load*g];
......@@ -214,8 +222,13 @@ function [xdot,F_nets,Tao_nets,F_tethers,F_drags,Tether_states] = systemModel(X,
load_vel = [load_u; load_v; load_w];
load_vel_d = -cross(load_omega,load_vel) + F_net_P./m_load;
% load_vel_d = -cross(load_omega,load_vel) + F_net_P./m_load;
% C1 = [load_r*load_v - load_q*load_w;
% load_p*load_w - load_r*load_u;
% load_q*load_u - load_p*load_v];
C2 = -cross(load_omega,load_vel);
load_vel_d = C2 + F_net_P./m_load;
%% Slung Load Attitude
load_att_d = att_der_matrix(load_phi, load_theta) * load_omega;
......
......@@ -10,8 +10,8 @@ function [] = plotData(simData,params)
%% Plot!
% params.fig_num = plotData_quadrotor(simData,params);
% params.fig_num = plotData_payload(simData,params);
% params.fig_num = plotData_tethers(simData,params);
params.fig_num = plotData_payload(simData,params);
params.fig_num = plotData_tethers(simData,params);
% params.fig_num = plotData_drag(simData,params);
end
......
......@@ -10,7 +10,7 @@
addpath(genpath(pwd))
%% Set Up Parameters
params = setUpParameters_sphere_3();
params = setUpParameters_cube_3();
%% 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