Skip to content
GitLab
Commit b9e2e50c authored by yean's avatar yean
Browse files

Merge branch 'anne_single_vehicle_sim' of... 

Merge branch 'anne_single_vehicle_sim' of https://gitlab.eecs.umich.edu/mmroma/coop_payload_sim into anne_single_vehicle_sim
parents a760ca77 6754371f
Hide whitespace changes
Inline Side-by-side
Controllers/setPIDgains.m 0 → 100644
View file @ b9e2e50c
function [vehicle_params] = setPIDgains(vehicle_params)
%SETPIDGAINS
%% Position -> Velocity
% x
vehicle_params.PIDctrl.x.kP = 1;
% y
vehicle_params.PIDctrl.y.kP = 1;
% z
vehicle_params.PIDctrl.z.kP = 1;
vehicle_params.PIDctrl.z.kI = 0;
vehicle_params.PIDctrl.z.kD = 0;
%% Velocity -> Acceleration
% x-dot
vehicle_params.PIDctrl.x_dot.kP = 1;
vehicle_params.PIDctrl.x_dot.kI = 0;
vehicle_params.PIDctrl.x_dot.kD = 0;
% y-dot
vehicle_params.PIDctrl.y_dot.kP = 1;
vehicle_params.PIDctrl.y_dot.kI = 0;
vehicle_params.PIDctrl.y_dot.kD = 0;
% z-dot
vehicle_params.PIDctrl.z_dot.kP = 1;
vehicle_params.PIDctrl.z_dot.kI = 0;
vehicle_params.PIDctrl.z_dot.kD = 0;
%% Attitude -> Attitude Rate
% Roll
vehicle_params.PIDctrl.roll.kP = 1;
vehicle_params.PIDctrl.roll.kI = 0;
vehicle_params.PIDctrl.roll.kD = 0;
% Pitch
vehicle_params.PIDctrl.pitch.kP = 1;
vehicle_params.PIDctrl.pitch.kI = 0;
vehicle_params.PIDctrl.pitch.kD = 0;
% Yaw
vehicle_params.PIDctrl.yaw.kP = 1;
vehicle_params.PIDctrl.yaw.kI = 0;
vehicle_params.PIDctrl.yaw.kD = 0;
%% Attitude Rate -> Torques
% Roll Rate
vehicle_params.PIDctrl.roll_rate.kP = 1;
vehicle_params.PIDctrl.roll_rate.kI = 0;
vehicle_params.PIDctrl.roll_rate.kD = 0;
% Pitch Rate
vehicle_params.PIDctrl.pitch_rate.kP = 1;
vehicle_params.PIDctrl.pitch_rate.kI = 0;
vehicle_params.PIDctrl.pitch_rate.kD = 0;
% Yaw Rate
vehicle_params.PIDctrl.yaw_rate.kP = 1;
vehicle_params.PIDctrl.yaw_rate.kI = 0;
vehicle_params.PIDctrl.yaw_rate.kD = 0;
end
Parameters/setUpParameters_OneQuad.m
View file @ b9e2e50c
......@@ -10,7 +10,7 @@ function [params] = setUpParameters_OneQuad(traj_num)
params.save_figs_bool = true;
%% Debugging Parameters (true / false)
params.disable_linear_vehicle_motion = false;
params.disable_linear_vehicle_motion = true;
params.disable_angular_vehicle_motion = false;
%% Global Parameters
......@@ -30,20 +30,7 @@ function [params] = setUpParameters_OneQuad(traj_num)
% ones(3,1).* 0;
% ones(3,1).* 0;
% ones(3,1).* 0];
%%
params.dist_F_power = 0 .* [1; 1; 1]; % 0-mean Band-limited, White Gaussian Dist (Ext Force)
params.dist_F_seed = [1 2 3];
params.dist_M_power = 0 .* [1; 1; 1]; % 0-mean Band-limited, White Gaussian Dist (Ext Force)
params.dist_M_seed = [4 5 6];
params.multirotors{1}.dist_F_power = 0;
params.multirotors{1}.dist_M_power = 0;
params.multirotors{1}.noise_power = 0;
%% Noise
params.noise_power = 0.1 .* ones(12,1);
params.noise_seed = 101:112;
%% Load Multirotors
params.num_vehicles = 1;
......@@ -85,12 +72,29 @@ function [params] = setUpParameters_OneQuad(traj_num)
% 0;... % q (rad/s)
% 0]; % r (rad/s)
%% Offset in position
% params.multirotors{1}.x0(1:3) = params.multirotors{1}.x0(1:3) + [0.01;0.01;0.01];
%% Offset in orientation
params.multirotors{1}.x0(7:9) = params.multirotors{1}.x0(7:9) + [0.2;0.0;0.0];
%% Choose Additional Parameters
params.anim_FPS = 30; % Animation FPS
%% Set Guidamce
params.traj_num = traj_num;
%%
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
Plotting/helper_functions/plot_orientation.m
View file @ b9e2e50c
......@@ -11,7 +11,7 @@ function [] = plot_orientation(T,X,Xd)
xlabel('Time (s)');
title('Attitude');
legend('Actual','Desired');
ylim([-pi pi]);
ylim([-pi/2 pi/2]);
subplot(3,1,2);
plot(T,X(:,8));
......@@ -20,7 +20,7 @@ function [] = plot_orientation(T,X,Xd)
hold off;
ylabel('\theta (rad)');
xlabel('Time (s)');
ylim([-pi pi]);
ylim([-pi/2 pi/2]);
legend('Actual','Desired');
subplot(3,1,3);
......@@ -30,7 +30,7 @@ function [] = plot_orientation(T,X,Xd)
hold off;
ylabel('\psi (rad)');
xlabel('Time (s)');
ylim([-pi pi]);
ylim([-pi/2 pi/2]);
legend('Actual','Desired');
end
......
Plotting/helper_functions/plot_orientation_rates.m 0 → 100644
View file @ b9e2e50c
function [] = plot_orientation_rates(T,X,Xd)
figure();
subplot(3,1,1);
plot(T,X(:,10));
hold on;
plot(T,Xd(:,10),'r--');
hold off;
ylabel('p (rad/s)');
xlabel('Time (s)');
title('Attitude Rates');
legend('Actual','Desired');
subplot(3,1,2);
plot(T,X(:,11));
hold on;
plot(T,Xd(:,11),'r--');
hold off;
ylabel('q (rad/s)');
xlabel('Time (s)');
legend('Actual','Desired');
subplot(3,1,3);
plot(T,X(:,12));
hold on;
plot(T,Xd(:,12),'r--');
hold off;
ylabel('r (rad/s)');
xlabel('Time (s)');
legend('Actual','Desired');
end
singleVehicleSim.m
View file @ b9e2e50c
......@@ -8,17 +8,13 @@
addpath(genpath(pwd));
%% Set up Parameters
traj_num = 2;
traj_num = 1;
params = setUpParameters_OneQuad(traj_num);
params.num_vehicles = 1;
%% Load Top Level Simulink Model (helps Simulink load ahead of time)
open('singlequadrotor');
params.multirotors{1}.dist_F_power = 0;
params.multirotors{1}.dist_M_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
simData = sim('singlequadrotor','StartTime','0','StopTime',num2str(params.tFinal));
%simData = sim('singlequadrotor','StartTime','0','StopTime','10');
......@@ -27,13 +23,15 @@ simData = sim('singlequadrotor','StartTime','0','StopTime',num2str(params.tFinal
%% Plot
% 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);
plot_orientation(simData.tout, simData.q1_X, simData.q1_Xd);
plot_velocity(simData.tout, simData.q1_X, simData.q1_Xd);
plot_orientation_rates(simData.tout, simData.q1_X, simData.q1_Xd);
%% 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