Commit 5c83087c authored by yean's avatar yean
Browse files

Attitude looks fine on hover test

parent 8c996226
...@@ -15,28 +15,28 @@ function [vehicle_params] = setPIDgains(vehicle_params) ...@@ -15,28 +15,28 @@ function [vehicle_params] = setPIDgains(vehicle_params)
%% Velocity -> Acceleration %% Velocity -> Acceleration
% x-dot % x-dot
vehicle_params.PIDctrl.x_dot.kP = 10; vehicle_params.PIDctrl.x_dot.kP = 2.5;
vehicle_params.PIDctrl.x_dot.kI = 0; vehicle_params.PIDctrl.x_dot.kI = 0;
vehicle_params.PIDctrl.x_dot.kD = 0; vehicle_params.PIDctrl.x_dot.kD = 0;
% y-dot % y-dot
vehicle_params.PIDctrl.y_dot.kP = 10; vehicle_params.PIDctrl.y_dot.kP = 2.5;
vehicle_params.PIDctrl.y_dot.kI = 0; vehicle_params.PIDctrl.y_dot.kI = 0;
vehicle_params.PIDctrl.y_dot.kD = 0; vehicle_params.PIDctrl.y_dot.kD = 0;
% z-dot % z-dot
vehicle_params.PIDctrl.z_dot.kP = 10; vehicle_params.PIDctrl.z_dot.kP = 2.8;
vehicle_params.PIDctrl.z_dot.kI = 0; vehicle_params.PIDctrl.z_dot.kI = 0;
vehicle_params.PIDctrl.z_dot.kD = 0; vehicle_params.PIDctrl.z_dot.kD = 0;
%% Attitude -> Attitude Rate %% Attitude -> Attitude Rate
% Roll % Roll
vehicle_params.PIDctrl.roll.kP = 10; vehicle_params.PIDctrl.roll.kP = 8;
vehicle_params.PIDctrl.roll.kI = 0; vehicle_params.PIDctrl.roll.kI = 0;
vehicle_params.PIDctrl.roll.kD = 0; vehicle_params.PIDctrl.roll.kD = 0;
% Pitch % Pitch
vehicle_params.PIDctrl.pitch.kP = 10; vehicle_params.PIDctrl.pitch.kP = 8;
vehicle_params.PIDctrl.pitch.kI = 0; vehicle_params.PIDctrl.pitch.kI = 0;
vehicle_params.PIDctrl.pitch.kD = 0; vehicle_params.PIDctrl.pitch.kD = 0;
...@@ -47,19 +47,31 @@ function [vehicle_params] = setPIDgains(vehicle_params) ...@@ -47,19 +47,31 @@ function [vehicle_params] = setPIDgains(vehicle_params)
%% Attitude Rate -> Torques %% Attitude Rate -> Torques
% Roll Rate % Roll Rate
vehicle_params.PIDctrl.roll_rate.kP = 2; vehicle_params.PIDctrl.roll_rate.kP = 5;
vehicle_params.PIDctrl.roll_rate.kI = 0; vehicle_params.PIDctrl.roll_rate.kI = 0.1;
vehicle_params.PIDctrl.roll_rate.kD = 0.3; vehicle_params.PIDctrl.roll_rate.kD = 0;
% Pitch Rate % Pitch Rate
vehicle_params.PIDctrl.pitch_rate.kP = 1; vehicle_params.PIDctrl.pitch_rate.kP = 5;
vehicle_params.PIDctrl.pitch_rate.kI = 0; vehicle_params.PIDctrl.pitch_rate.kI = 0.1;
vehicle_params.PIDctrl.pitch_rate.kD = 0; vehicle_params.PIDctrl.pitch_rate.kD = 0;
% Yaw Rate % Yaw Rate
vehicle_params.PIDctrl.yaw_rate.kP = 0.4; vehicle_params.PIDctrl.yaw_rate.kP = 3;
vehicle_params.PIDctrl.yaw_rate.kI = 0; vehicle_params.PIDctrl.yaw_rate.kI = 0.1;
vehicle_params.PIDctrl.yaw_rate.kD = 0; vehicle_params.PIDctrl.yaw_rate.kD = 0;
%% Saturation Values (symmetric upper/lower)
vehicle_params.max_pitch = deg2rad(45);
vehicle_params.max_roll = deg2rad(45);
vehicle_params.max_pitch_rate = deg2rad(90);
vehicle_params.max_roll_rate = deg2rad(90);
vehicle_params.max_yaw_rate = deg2rad(90);
vehicle_params.max_x_vel = 5;
vehicle_params.max_y_vel = 5;
vehicle_params.max_z_vel = 5;
end end
function [vehicle_params] = setPIDgains(vehicle_params)
%SETPIDGAINS
%% Position -> Velocity
% x
vehicle_params.PIDctrl.x.kP = 4;
% y
vehicle_params.PIDctrl.y.kP = 4;
% z
vehicle_params.PIDctrl.z.kP = 10;
vehicle_params.PIDctrl.z.kI = 0;
vehicle_params.PIDctrl.z.kD = 0;
%% Velocity -> Acceleration
% x-dot
vehicle_params.PIDctrl.x_dot.kP = 2.5;
vehicle_params.PIDctrl.x_dot.kI = 0;
vehicle_params.PIDctrl.x_dot.kD = 0;
% y-dot
vehicle_params.PIDctrl.y_dot.kP = 2.5;
vehicle_params.PIDctrl.y_dot.kI = 0;
vehicle_params.PIDctrl.y_dot.kD = 0;
% z-dot
vehicle_params.PIDctrl.z_dot.kP = 2.8;
vehicle_params.PIDctrl.z_dot.kI = 0;
vehicle_params.PIDctrl.z_dot.kD = 0;
%% Attitude -> Attitude Rate
% Roll
vehicle_params.PIDctrl.roll.kP = 11;
vehicle_params.PIDctrl.roll.kI = 0;
vehicle_params.PIDctrl.roll.kD = 0;
% Pitch
vehicle_params.PIDctrl.pitch.kP = 11;
vehicle_params.PIDctrl.pitch.kI = 0;
vehicle_params.PIDctrl.pitch.kD = 0;
% Yaw
vehicle_params.PIDctrl.yaw.kP = 16;
vehicle_params.PIDctrl.yaw.kI = 0;
vehicle_params.PIDctrl.yaw.kD = 0;
%% Attitude Rate -> Torques
% Roll Rate
vehicle_params.PIDctrl.roll_rate.kP = 3;
vehicle_params.PIDctrl.roll_rate.kI = 0;
vehicle_params.PIDctrl.roll_rate.kD = 0;
% Pitch Rate
vehicle_params.PIDctrl.pitch_rate.kP = 3;
vehicle_params.PIDctrl.pitch_rate.kI = 0;
vehicle_params.PIDctrl.pitch_rate.kD = 0;
% Yaw Rate
vehicle_params.PIDctrl.yaw_rate.kP = 3;
vehicle_params.PIDctrl.yaw_rate.kI = 0;
vehicle_params.PIDctrl.yaw_rate.kD = 0;
%% Saturation Values (symmetric upper/lower)
vehicle_params.max_pitch = deg2rad(45);
vehicle_params.max_roll = deg2rad(45);
vehicle_params.max_pitch_rate = deg2rad(90);
vehicle_params.max_roll_rate = deg2rad(90);
vehicle_params.max_yaw_rate = deg2rad(90);
vehicle_params.max_x_vel = 5;
vehicle_params.max_y_vel = 5;
vehicle_params.max_z_vel = 5;
end
...@@ -3,10 +3,10 @@ function [coarse_WPs] = WPs_set19_spiral_traj() %takes a while to run ...@@ -3,10 +3,10 @@ function [coarse_WPs] = WPs_set19_spiral_traj() %takes a while to run
%WPs_set19 %WPs_set19
a = 0.1; a = 0.1;
coarse_WPs = []; coarse_WPs = [];
for time = 0:.25:24 z_pos = 0;
for time = 0:.1:24
x_pos = a*(cos(time)+time*sin(time)); x_pos = a*(cos(time)+time*sin(time));
y_pos = a*(sin(time)-time*cos(time)); y_pos = a*(sin(time)-time*cos(time));
z_pos = 0;
B = [x_pos, y_pos, z_pos, time]; B = [x_pos, y_pos, z_pos, time];
coarse_WPs = [coarse_WPs; B]; coarse_WPs = [coarse_WPs; B];
end end
......
...@@ -43,7 +43,8 @@ function [WPs] = splineTogether(coarseWPs, dt) ...@@ -43,7 +43,8 @@ function [WPs] = splineTogether(coarseWPs, dt)
end end
%% 3) Combine Data into a single WPs variable %% 3) Combine Data into a single WPs variable
WPs = [x_all', y_all', z_all', vx_all', vy_all', vz_all']; %WPs = [x_all', y_all', z_all', vx_all', vy_all', vz_all'];
WPs = [x_all', y_all', z_all', zeros(size(x_all', 1), 1), zeros(size(x_all', 1), 1), zeros(size(x_all', 1), 1)];
end end
...@@ -57,7 +57,9 @@ function [params] = setUpParameters_OneQuad(traj_num) ...@@ -57,7 +57,9 @@ function [params] = setUpParameters_OneQuad(traj_num)
%% Load Quad Default Parameters %% Load Quad Default Parameters
params.multirotors{1} = parameters_Quad(params); params.multirotors{1} = parameters_Quad(params);
[params.multirotors{1}.xd, params.tFinal] = gen_payload_traj(params); [params.multirotors{1}.xd, params.tFinal] = gen_payload_traj(params);
% params.multirotors{1}.x0 = params.multirotors{1}.xd(:,1); params.multirotors{1}.x0 = params.multirotors{1}.xd(:,1);
%%
% params.multirotors{1}.x0(1:3) = params.multirotors{1}.x0(1:3) + [0.01;0.01;0.01]; % params.multirotors{1}.x0(1:3) = params.multirotors{1}.x0(1:3) + [0.01;0.01;0.01];
% params.multirotors{1}.xd = [0;... % x position [m] % params.multirotors{1}.xd = [0;... % x position [m]
% 0;... % y position [m] % 0;... % y position [m]
...@@ -74,9 +76,22 @@ function [params] = setUpParameters_OneQuad(traj_num) ...@@ -74,9 +76,22 @@ function [params] = setUpParameters_OneQuad(traj_num)
%% Offset in position %% Offset in position
%params.multirotors{1}.x0(1:3) = params.multirotors{1}.x0(1:3) + [0.1;0.1;0.1]; %params.multirotors{1}.x0(1:3) = params.multirotors{1}.x0(1:3) + [0.1;0.1;0.1];
%params.multirotors{1}.x0(4:6) = params.multirotors{1}.x0(4:6) + [0.01;0.01;0.01]; %params.multirotors{1}.x0(1:3) = params.multirotors{1}.x0(1:3) + [0.1; 0.1; 0.1]; %10 cm offset
params.multirotors{1}.x0(1:3) = params.multirotors{1}.x0(1:3) + [0.2;0.2; 0.2]; %20 cm offset
%params.multirotors{1}.x0(1:3) = params.multirotors{1}.x0(1:3) + [0.22;0.22; 0.22]; %22 cm offset
%%Simulation starts breaking at an offset of 23 cm
%params.multirotors{1}.x0(1:3) = params.multirotors{1}.x0(1:3) + [0.23;0.23; 0.23]; %23 cm offset
%% Offset in orientation %% Offset in orientation
%params.multirotors{1}.x0(7:9) = params.multirotors{1}.x0(7:9) + [0.17;0.17;0.17]; %10 degree offset
%params.multirotors{1}.x0(7:9) = params.multirotors{1}.x0(7:9) + [0.35;0.35;0.35]; %20 degree offset
%params.multirotors{1}.x0(7:9) = params.multirotors{1}.x0(7:9) + [0.44;0.44;0.44]; %25 degrees
%%%Simulation starts breaking at an offset of 26 degrees
%params.multirotors{1}.x0(7:9) = params.multirotors{1}.x0(7:9) + [0.46;0.46;0.46]; %26 degrees
%params.multirotors{1}.x0(7:9) = params.multirotors{1}.x0(7:9) + [0.2;0.2;0.2]; %params.multirotors{1}.x0(7:9) = params.multirotors{1}.x0(7:9) + [0.2;0.2;0.2];
%params.multirotors{1}.x0(10:12) = params.multirotors{1}.x0(10:12) + [0.2;0.2;0.2]; %params.multirotors{1}.x0(10:12) = params.multirotors{1}.x0(10:12) + [0.2;0.2;0.2];
......
function [params] = setUpParameters_OneQuad(traj_num)
%setUpParameters
% Calls parameters_Quad() and parameters_Payload() with default
% settings and then adjusts from there. All edits should be made to
% this file.
%%
params.traj_num = traj_num;
params.save_figs_bool = true;
%% Debugging Parameters (true / false)
params.disable_linear_vehicle_motion = false;
params.disable_angular_vehicle_motion = false;
%% Global Parameters
params.g = 9.81;
params.curr_run = 1;
%% Time Step Parameters
params.ctlDt = 0.01; % Controller Step Size (s)
params.tStep = params.ctlDt / 10; % Simulation Step Size (s)
%% Disturbance Forces & Torques
% quads_dist_F_power = 1e-3;
% quads_dist_M_power = 0;
%
% %% Sensor Noise
% quad_noise_power = [ones(3,1).* 0;
% ones(3,1).* 0;
% ones(3,1).* 0;
% ones(3,1).* 0];
%% Load Multirotors
params.num_vehicles = 1;
%% Set 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.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);
%% Load Quad Default Parameters
params.multirotors{1} = parameters_Quad(params);
[params.multirotors{1}.xd, params.tFinal] = gen_payload_traj(params);
% params.multirotors{1}.x0 = params.multirotors{1}.xd(:,1);
% params.multirotors{1}.x0(1:3) = params.multirotors{1}.x0(1:3) + [0.01;0.01;0.01];
% params.multirotors{1}.xd = [0;... % x position [m]
% 0;... % y position [m]
% 0;... % z position [m]
% 0;... % u (m/s)
% 0;... % v (m/s)
% 0;... % w (m/s)
% 0;... % phi (roll) (rad)
% 0;... % theta (pitch) (rad)
% 0;... % psi (yaw) (rad)
% 0;... % p (rad/s)
% 0;... % q (rad/s)
% 0]; % r (rad/s)
%% Offset in position
%params.multirotors{1}.x0(1:3) = params.multirotors{1}.x0(1:3) + [0.1;0.1;0.1];
%params.multirotors{1}.x0(1:3) = params.multirotors{1}.x0(1:3) + [0.1; 0.1; 0.1]; %10 cm offset
params.multirotors{1}.x0(1:3) = params.multirotors{1}.x0(1:3) + [0.2;0.2; 0.2]; %20 cm offset
%params.multirotors{1}.x0(1:3) = params.multirotors{1}.x0(1:3) + [0.22;0.22; 0.22]; %22 cm offset
%%Simulation starts breaking at an offset of 23 cm
%params.multirotors{1}.x0(1:3) = params.multirotors{1}.x0(1:3) + [0.23;0.23; 0.23]; %23 cm offset
%% Offset in orientation
%params.multirotors{1}.x0(7:9) = params.multirotors{1}.x0(7:9) + [0.17;0.17;0.17]; %10 degree offset
%params.multirotors{1}.x0(7:9) = params.multirotors{1}.x0(7:9) + [0.35;0.35;0.35]; %20 degree offset
%params.multirotors{1}.x0(7:9) = params.multirotors{1}.x0(7:9) + [0.44;0.44;0.44]; %25 degrees
%%%Simulation starts breaking at an offset of 26 degrees
%params.multirotors{1}.x0(7:9) = params.multirotors{1}.x0(7:9) + [0.46;0.46;0.46]; %26 degrees
%params.multirotors{1}.x0(7:9) = params.multirotors{1}.x0(7:9) + [0.2;0.2;0.2];
%params.multirotors{1}.x0(10:12) = params.multirotors{1}.x0(10:12) + [0.2;0.2;0.2];
%% Choose Additional Parameters
params.anim_FPS = 30; % Animation FPS
%%
params.multirotors{1}.dist_F_power = 0 .* [1; 1; 1]; % 0-mean Band-limited, White Gaussian Dist (Ext Force)
params.multirotors{1}.dist_F_seed = [1 2 3];
params.multirotors{1}.dist_M_power= 0 .* [1; 1; 1]; % 0-mean Band-limited, White Gaussian Dist (Ext Force)
params.multirotors{1}.dist_M_seed = [4 5 6];
%% Noise
params.multirotors{1}.noise_power = 0.0 .* ones(12,1);
params.multirotors{1}.noise_seed = 101:112;
%% Specify PID controller gains
params.multirotors{1} = setPIDgains(params.multirotors{1});
end
function [] = plot_FF(T,X,Xd, simData)
figure();
subplot(3,1,1);
hold on;
plot(T,simData.q1_x_dot_FF,'r--');
ylabel('x_dot (m/s)');
xlabel('Time (s)');
title('FeedForward Terms Position');
subplot(3,1,2);
hold on;
plot(T,simData.q1_y_dot_FF,'r--');
ylabel('y_dot (m/s)');
xlabel('Time (s)');
subplot(3,1,3);
hold on;
plot(T,simData.q1_z_dot_FF,'r--');
ylabel('z_dot (m/s)');
xlabel('Time (s)');
end
function [] = plot_FF_2(T,X,Xd, simData)
figure();
subplot(3,1,1);
hold on;
plot(T,simData.q1_phi_FF,'r--');
ylabel('phi (rad/s)');
xlabel('Time (s)');
title('FeedForward Terms Attitude');
subplot(3,1,2);
hold on;
plot(T,simData.q1_theta_FF,'r--');
ylabel('theta (rad/s)');
xlabel('Time (s)');
subplot(3,1,3);
hold on;
plot(T,simData.q1_psi_FF,'r--');
ylabel('psi (rad/s)');
xlabel('Time (s)');
end
function [] = plot_acceleration(T,X,Xd, simData)
figure();
subplot(3,1,1);
hold on;
plot(T,simData.q1_x_ddot_des,'r--');
hold off;
ylabel('x_{acc} (m/s^2)');
title('Acceleration');
subplot(3,1,2);
hold on;
plot(T,simData.q1_y_ddot_des,'r--');
hold off;
ylabel('y_{acc} (m/s^2)');
subplot(3,1,3);
hold on;
plot(T,simData.q1_z_ddot_des,'r--');
hold off;
ylabel('z_{acc} (m/s^2)');
xlabel('Time (s)');
end
function [] = plot_adjustment_des(T,X,Xd, simData)
figure();
subplot(1,1,1);
hold on;
plot(T,simData.q1_attitude_thrust_adjustment);
plot(T,1/(cos()));
hold off;
ylabel('f_{thrust} adjustment (N)');
xlabel('Time (s)');
title('Attitude Thrust adjustment');
end
...@@ -8,7 +8,7 @@ ...@@ -8,7 +8,7 @@
addpath(genpath(pwd)); addpath(genpath(pwd));
%% Set up Parameters %% Set up Parameters
traj_num = 2; traj_num = 1;
params = setUpParameters_OneQuad(traj_num); params = setUpParameters_OneQuad(traj_num);
params.num_vehicles = 1; params.num_vehicles = 1;
...@@ -23,16 +23,20 @@ simData = sim('singlequadrotor','StartTime','0','StopTime',num2str(params.tFinal ...@@ -23,16 +23,20 @@ simData = sim('singlequadrotor','StartTime','0','StopTime',num2str(params.tFinal
%% Plot %% Plot
% TODO plot quadrotor stuff, not load stuff % TODO plot quadrotor stuff, not load stuff
% plot_position(simData.tout, simData.q1_X, simData.q1_Xd); plot_position(simData.tout, simData.q1_X, simData.q1_Xd);
% plot_velocity(simData.tout, simData.q1_X, simData.q1_Xd, simData); plot_velocity(simData.tout, simData.q1_X, simData.q1_Xd, simData);
% plot_orientation(simData.tout, simData.q1_X, simData.q1_Xd, simData); plot_orientation(simData.tout, simData.q1_X, simData.q1_Xd, simData);
% plot_orientation_rates(simData.tout, simData.q1_X, simData.q1_Xd, simData); plot_orientation_rates(simData.tout, simData.q1_X, simData.q1_Xd, simData);
%plot_FF(simData.tout, simData.q1_X, simData.q1_Xd, simData);
%plot_FF_2(simData.tout, simData.q1_X, simData.q1_Xd, simData);
%plot_acceleration(simData.tout, simData.q1_X, simData.q1_Xd, simData);
% plot_adjustment_des(simData.tout, simData.q1_X, simData.q1_Xd, simData); % plot_adjustment_des(simData.tout, simData.q1_X, simData.q1_Xd, simData);
% plot_force_torque_des(simData.tout, simData.q1_X, simData.q1_Xd, simData); % plot_force_torque_des(simData.tout, simData.q1_X, simData.q1_Xd, simData);
%% Animate %% Animate
animate_simulink_data(simData, params, false); %animate_simulink_data(simData, params, false);
......
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