Commit 1fc38457 authored by mmroma's avatar mmroma
Browse files

Prepped code before I sent to Ella

parent 2b9af726
function [params] = payload_controller(X_curr, params)
% payload_controller
% Adds additional terms to desired state for quadrotors
%% Stupid Simple Controller
load_x = X(params.num_vehicles*12 + 1);
load_y = X(params.num_vehicles*12 + 2);
load_z = X(params.num_vehicles*12 + 3);
% xd = params.xd(params.tstep_count,:)';
load_xd = params.payload.xd(params.tstep_count,:)';
%%
e_load_
end
function [coarse_WPs] = WPs_set1_hover()
%WPs_set1
coarse_WPs = [0, 0, -1.5, 0;
0, 0, -1.5, 10];
h = 1.5;
coarse_WPs = [0, 0, h, 0;
0, 0, h, 10];
end
......@@ -2,8 +2,8 @@ function [Xd, T] = gen_payload_traj(params)
%gen_payload_traj
%% 1) Design Coarse Waypoints [x, y, z, t] in meters and sec
% coarse_WPs = WPs_set1_hover();
coarse_WPs = WPs_set2_square();
coarse_WPs = WPs_set1_hover();
% coarse_WPs = WPs_set2_square();
% coarse_WPs = WPs_set3_rise_and_fall();
%% 2) Extract Relevant Params
......
......@@ -65,9 +65,9 @@ function [params] = parameters_Quad()
%% 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.NegTension = 0; % Allow "Negative Tensions"? 0 = No, 1 = Yes
params.k = 1; % Spring Constant
params.b = 0; % Damping Coefficient
params.NegTension = 1; % 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;
......
function [params] = setUpParameters_sphere_4()
%setUpParameters
% Wind Added and a 5th quad (cube)
%% 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
% 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;
params.payload.m = 0.5;
r = params.payload.diameter / 2;
% r = 0;
%% Load Multirotors
params.num_vehicles = 5;
L_all = 1.5;
azi = [ 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;
params.l_i_last{i} = L_all;
end
%% 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
% 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);
%% Choose Additional Parameters
params.tFinal = 10; % Simulation Length (s)
params.tStep = 0.005; % Simulation Step Size (s)
params.anim_FPS = 30; % Animation FPS
%% 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,:)';
params = compute_quad_guidance_2(params);
%% Control (new stuff)
% params = compute_quad_eq_state_and_controller(params);
%% Sanity check guidance
% plot_guidance(params);
end
......@@ -91,6 +91,9 @@ function [params] = setUpParameters_sphere_4()
%% Sanity check guidance
% plot_guidance(params);
%% Perturb Load Off Equilibrium
params.payload.x0(1:2) = [0.02; 0];
end
function [attRate_d] = attRate_der(Jx,Jy,Jz,p,q,r,Tao)
%att_rate_der_term1
C1 = [(Jy-Jz)/Jx*q*r;
(Jz-Jx)/Jy*p*r;
(Jx-Jy)/Jz*p*q];
C1 = [((Jy-Jz)/Jx)*q*r;
((Jz-Jx)/Jy)*p*r;
((Jx-Jy)/Jz)*p*q];
C2 = [Tao(1)/Jx;
Tao(2)/Jy;
Tao(3)/Jz];
......
......@@ -179,16 +179,16 @@ function [xdot,F_nets,Tao_nets,F_tethers,F_drags,Tether_states,l_i_last] = syste
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
......
......@@ -9,9 +9,9 @@ function [] = plotData(simData,params)
end
%% Plot!
params.fig_num = plotData_quadrotor(simData,params);
% 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_tethers(simData,params);
% params.fig_num = plotData_drag(simData,params);
end
......
......@@ -11,14 +11,29 @@ function [fig_num] = plotData_tethers(simData,params)
F_mags(:,i) = vecnorm(simData.F_tethers(:,i*3-2:i*3)')';
end
offset = 1;
figure(fig_num);
legend_entries = cell(1,params.num_vehicles);
plot(simData.T, F_mags(:,1));
plot(simData.T, F_mags(:,1),'g');
legend_entries{1} = "1";
hold on;
% plot(simData.T, F_mags(:,2),'r');
% legend_entries{2} = string(2);
%
% plot(simData.T, F_mags(:,3),'b');
% legend_entries{3} = string(3);
%
% plot(simData.T, F_mags(:,2),'k--');
% legend_entries{4} = string(4);
%
% plot(simData.T, F_mags(:,5),'y--');
% legend_entries{5} = string(5);
for i=2:params.num_vehicles
plot(simData.T, F_mags(:,i));
legend_entries{i} = string(i);
offset = offset + 1;
end
hold off;
ylabel('Force (N)');
......
......@@ -12,8 +12,4 @@ This is a simulation written in MATLAB for the cooperative payload transportatio
Run the simulator by calling the main function:
`coopPayloadSim`
For testing and tuning, adjust parameters in parameters_APM_Quad.m or other parameter files selected by setUpParameters.m. There you can change physics parameters, controller gains, and more.
If you are interested in developing and testing a controller, use one of the existing controllers as a reference to make a new file making sure to select it within the parameters file when testing.
`coopPayloadSim`
\ No newline at end of file
......@@ -19,7 +19,7 @@ simData = runSimulation(params);
plotData(simData,params);
%% Animate
% animate(simData,params);
animate(simData,params);
%% Save the Data
% saveData(simData,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