Commit 0e0eab2d authored by yean's avatar yean
Browse files

Somewhat working code for rc_pilot controller

parent 7d29036f
...@@ -2,6 +2,6 @@ function [coarse_WPs] = WPs_set1_hover() ...@@ -2,6 +2,6 @@ function [coarse_WPs] = WPs_set1_hover()
%WPs_set1 %WPs_set1
h = 1.5; h = 1.5;
coarse_WPs = [0, 0, h, 0; coarse_WPs = [0, 0, h, 0;
0, 0, h, 10]; 0, 0, h, 20];
end end
...@@ -71,6 +71,7 @@ function [params] = setUpParameters_OneQuad(traj_num) ...@@ -71,6 +71,7 @@ function [params] = setUpParameters_OneQuad(traj_num)
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}.xd = [0;... % x position [m] % params.multirotors{1}.xd = [0;... % x position [m]
% 0;... % y position [m] % 0;... % y position [m]
% 0;... % z position [m] % 0;... % z position [m]
......
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.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).* 1e-8;
ones(3,1).* 1e-6;
ones(3,1).* 1e-6;
ones(3,1).* 1e-4];
%% 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.payload.xd, params.tFinal] = gen_payload_traj(params);
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)
%% Choose Additional Parameters
params.tFinal = 10; % Simulation Length (s)
params.anim_FPS = 30; % Animation FPS
%% Set Guidamce
params.traj_num = traj_num;
end
function [] = plot_velocity(T,X,Xd)
figure();
subplot(3,1,1);
plot(T,X(:,4));
hold on;
plot(T,Xd(:,4),'r--');
hold off;
ylabel('x_{dot} (m/s)');
title('Velocity');
legend('Actual','Desired');
subplot(3,1,2);
plot(T,X(:,5));
hold on;
plot(T,Xd(:,5),'r--');
hold off;
ylabel('y_{dot} (m/s)');
legend('Actual','Desired');
subplot(3,1,3);
plot(T,X(:,6));
hold on;
plot(T,Xd(:,6),'r--');
hold off;
ylabel('z_{dot} (m/s)');
xlabel('Time (s)');
legend('Actual','Desired');
end
function [] = plot_velocity(T,X,Xd)
figure();
subplot(3,1,1);
plot(T,X(:,1));
hold on;
plot(T,Xd(:,4),'r--');
hold off;
ylabel('x_dot (m/s)');
title('Velocity');
legend('Actual','Desired');
subplot(3,1,2);
plot(T,X(:,2));
hold on;
plot(T,Xd(:,5),'r--');
hold off;
ylabel('y_dot (m/s)');
legend('Actual','Desired');
subplot(3,1,3);
plot(T,X(:,3));
hold on;
plot(T,Xd(:,6),'r--');
hold off;
ylabel('z_dot (m/s)');
xlabel('Time (s)');
legend('Actual','Desired');
end
File added
...@@ -18,8 +18,10 @@ open('singlequadrotor'); ...@@ -18,8 +18,10 @@ open('singlequadrotor');
params.multirotors{1}.dist_F_power = 0; params.multirotors{1}.dist_F_power = 0;
params.multirotors{1}.dist_M_power = 0; params.multirotors{1}.dist_M_power = 0;
params.multirotors{1}.noise_power = 0; params.multirotors{1}.noise_power = 0;
params.multirotors{1}.x0(1:3) = params.multirotors{1}.x0(1:3) + [0.01;0.01;0.01];
%% Call Simulink and Run the Simulation %% Call Simulink and Run the Simulation
simData = sim('singlequadrotor','StartTime','0','StopTime',num2str(params.tFinal)); simData = sim('singlequadrotor','StartTime','0','StopTime',num2str(params.tFinal));
%simData = sim('singlequadrotor','StartTime','0','StopTime','10');
%% Plot %% Plot
...@@ -27,6 +29,8 @@ simData = sim('singlequadrotor','StartTime','0','StopTime',num2str(params.tFinal ...@@ -27,6 +29,8 @@ simData = sim('singlequadrotor','StartTime','0','StopTime',num2str(params.tFinal
plot_position(simData.tout, simData.q1_X, simData.q1_Xd); plot_position(simData.tout, simData.q1_X, simData.q1_Xd);
plot_orientation(simData.tout, simData.q1_X, simData.q1_Xd); plot_orientation(simData.tout, simData.q1_X, simData.q1_Xd);
plot_velocity(simData.tout, simData.q1_X, simData.q1_Xd);
%% 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