setUpParameters.m 4.53 KB
Newer Older
1
function [params] = setUpParameters(traj_num)
2
%setUpParameters
mmroma's avatar
mmroma committed
3
    % Calls parameters_Quad() and parameters_Payload() with default
mmroma's avatar
mmroma committed
4
5
    % settings and then adjusts from there. All edits should be made to
    % this file.
mmroma's avatar
mmroma committed
6
    
7
8
9
    %%
    params.save_figs_bool = true;
    
mmroma's avatar
mmroma committed
10
11
12
13
14
    %% Debugging Parameters (true / false)
    params.disable_linear_vehicle_motion  = false;
    params.disable_angular_vehicle_motion = false;
    params.disable_linear_payload_motion  = false;
    params.disable_angular_payload_motion = false;    
15
    
16
17
    %% Global Parameters
    params.g = 9.81;
18
    params.curr_run = 1;
19
    
mmroma's avatar
mmroma committed
20
    %% Time Step Parameters
mmroma's avatar
Clean    
mmroma committed
21
    params.ctlDt = 0.01;                      % Controller Step Size (s)
mmroma's avatar
mmroma committed
22
23
    params.tStep = params.ctlDt / 10;                % Simulation Step Size (s)
    
24
25
26
27
28
    %% Disturbance Forces & Torques 
    quads_dist_F_power = 1e-3;
    quads_dist_M_power = 0;
    load_dist_F_power = 1e-3;
    load_dist_M_power = 0;
29
    
30
31
32
33
34
35
36
37
38
39
40
    %% 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_noise_power = [ones(3,1).* 1e-8;
                        ones(3,1).* 1e-6;
                        ones(3,1).* 1e-6;
                        ones(3,1).* 1e-4];                    
                         
mmroma's avatar
mmroma committed
41
    %% Load Payload Parameters
42
    params.payload = parameters_Payload(params);
43
%     params.payload.distSTD = load_dist_STD;
44
    %params.payload.m = 0.5;
45
46
47
    params.payload.dist_F_power = load_dist_F_power;
    params.payload.dist_M_power = load_dist_M_power;
    params.payload.noise_power = load_noise_power;
48
    params.payload.m = 0.5;
49
    r = params.payload.diameter / 2;
mmroma's avatar
mmroma committed
50

mmroma's avatar
mmroma committed
51
    %% Adjust Payload Controller Parameters
52
53
54
55
    int_limit = 1;
    params.e_load_pos_Kp = 1;
    params.e_load_pos_Ki = 0.1;
    params.e_load_pos_Kd = 0.1;
mmroma's avatar
mmroma committed
56
57
58
    params.e_load_pos_int_LL = [-int_limit;-int_limit;-int_limit];
    params.e_load_pos_int_UL = [int_limit;int_limit;int_limit];
    params.e_load_pos_int = zeros(3,1);
59
60
61
62
    
    %% Load Multirotors
    params.num_vehicles = 5;
    L_all = 1.5;
mmroma's avatar
mmroma committed
63
    azi = [ pi/4, 3*pi/4,-3*pi/4, -pi/4, 0];
64
65
66
67
    ele = [ pi/4, pi/4,   pi/4,    pi/4, pi/2];
    params.azi = azi;
    params.ele = ele;
    
68
    for i=1:params.num_vehicles
69
        params.multirotors{i} = parameters_Quad(params);
70
71
72
73
74
        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);
75
%         params.multirotors{i}.distSTD = quad_dist_STD;
76
        params.l_i_last{i} = L_all;
mmroma's avatar
mmroma committed
77
        params.multirotors{i}.ctlDt = params.ctlDt;
78
79
80
81
82
83
84
85
86
        
        params.multirotors{i}.dist_F_seed = i*10 + [1 2 3];
        params.multirotors{i}.dist_F_power = quads_dist_F_power .* [1 1 1];
        
        params.multirotors{i}.dist_M_seed = i*10 + [4 5 6];
        params.multirotors{i}.dist_M_power = quads_dist_M_power .* [1 1 1];
   
        params.multirotors{i}.noise_power = quad_noise_power;
        params.multirotors{i}.noise_seed = i*100 + [1:12];
87
88
    end
   
mmroma's avatar
mmroma committed
89
    %% Set Wind Parameters
90
    params.wind.enable = 0;
91
92
93
94
95
96
    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
mmroma's avatar
mmroma committed
97
    
98
99
    params.wind.C_D = @(Re) C_D_sphere(Re);   % Sphere Drag Model
%     params.wind.C_D = @(Re) C_D_cube(Re);   % Cube Drag Model
mmroma's avatar
mmroma committed
100

101
102
103
104
105
106
107
108
109
110
111
112
    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.anim_FPS = 30;                % Animation FPS

mmroma's avatar
mmroma committed
113
    %% Set Guidamce
114
    params.traj_num = traj_num;
115
    [params.payload.xd, params.tFinal] = gen_payload_traj(params);
116
    params.payload.x0 = params.payload.xd(:,1);
mmroma's avatar
mmroma committed
117
    params = compute_quad_guidance_2(params);
118

mmroma's avatar
mmroma committed
119
    %% Adjust Quad Control Based on Payload Trajectory
mmroma's avatar
mmroma committed
120
    params = compute_quad_eq_state_and_controller(params);
mmroma's avatar
mmroma committed
121
    
122
    %% User Force Input
123
124
    params.user_force_method = 4;
    params.user_force_amplitude = 3; % Newton's
125
126
    params.user_force = user_force_input(params);
    
127
128
end