Commit 3228cb5c authored by yean's avatar yean
Browse files

Working, but not perfect simulation code of the RC Pilot Controller. Code...

Working, but not perfect simulation code of the RC Pilot Controller. Code works for the square trajectory.
parent eb6de156
......@@ -3,63 +3,63 @@ function [vehicle_params] = setPIDgains(vehicle_params)
%% Position -> Velocity
% x
vehicle_params.PIDctrl.x.kP = 1;
vehicle_params.PIDctrl.x.kP = 4;
% y
vehicle_params.PIDctrl.y.kP = 1;
vehicle_params.PIDctrl.y.kP = 4;
% z
vehicle_params.PIDctrl.z.kP = 13;
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 = 1.5;
vehicle_params.PIDctrl.x_dot.kP = 10;
vehicle_params.PIDctrl.x_dot.kI = 0;
vehicle_params.PIDctrl.x_dot.kD = 1;
vehicle_params.PIDctrl.x_dot.kD = 0;
% y-dot
vehicle_params.PIDctrl.y_dot.kP = 1.5;
vehicle_params.PIDctrl.y_dot.kP = 10;
vehicle_params.PIDctrl.y_dot.kI = 0;
vehicle_params.PIDctrl.y_dot.kD = 1;
vehicle_params.PIDctrl.y_dot.kD = 0;
% z-dot
vehicle_params.PIDctrl.z_dot.kP = 15;
vehicle_params.PIDctrl.z_dot.kP = 10;
vehicle_params.PIDctrl.z_dot.kI = 0;
vehicle_params.PIDctrl.z_dot.kD = 5;
vehicle_params.PIDctrl.z_dot.kD = 0;
%% Attitude -> Attitude Rate
% Roll
vehicle_params.PIDctrl.roll.kP = 1;
vehicle_params.PIDctrl.roll.kP = 10;
vehicle_params.PIDctrl.roll.kI = 0;
vehicle_params.PIDctrl.roll.kD = 0;
% Pitch
vehicle_params.PIDctrl.pitch.kP = 1;
vehicle_params.PIDctrl.pitch.kP = 10;
vehicle_params.PIDctrl.pitch.kI = 0;
vehicle_params.PIDctrl.pitch.kD = 0;
% Yaw
vehicle_params.PIDctrl.yaw.kP = 1;
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 = 1;
vehicle_params.PIDctrl.roll_rate.kI = 0.25;
vehicle_params.PIDctrl.roll_rate.kD = 0.25;
vehicle_params.PIDctrl.roll_rate.kP = 2;
vehicle_params.PIDctrl.roll_rate.kI = 0;
vehicle_params.PIDctrl.roll_rate.kD = 0.3;
% Pitch Rate
vehicle_params.PIDctrl.pitch_rate.kP = 1;
vehicle_params.PIDctrl.pitch_rate.kI = 0.25;
vehicle_params.PIDctrl.pitch_rate.kD = 0.25;
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.25;
vehicle_params.PIDctrl.yaw_rate.kD = 0.25;
vehicle_params.PIDctrl.yaw_rate.kP = 0.4;
vehicle_params.PIDctrl.yaw_rate.kI = 0;
vehicle_params.PIDctrl.yaw_rate.kD = 0;
end
......@@ -10,7 +10,7 @@ function [params] = setUpParameters_OneQuad(traj_num)
params.save_figs_bool = true;
%% Debugging Parameters (true / false)
params.disable_linear_vehicle_motion = true;
params.disable_linear_vehicle_motion = false;
params.disable_angular_vehicle_motion = false;
%% Global Parameters
......@@ -57,7 +57,7 @@ function [params] = setUpParameters_OneQuad(traj_num)
%% 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 = 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]
......@@ -73,10 +73,12 @@ function [params] = setUpParameters_OneQuad(traj_num)
% 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];
%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];
%% Offset in orientation
params.multirotors{1}.x0(7:9) = params.multirotors{1}.x0(7:9) + [0.2;0.0;0.0];
%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
......
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(X(:, 7)).*cos(X(:, 8))), 'r--');
hold off;
ylabel('f_{thrust} adjustment (N)');
xlabel('Time (s)');
title('Attitude Thrust adjustment');
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
function [] = plot_force_torque_des(T,X,Xd, simData)
figure();
subplot(4,1,1);
hold on;
plot(T,simData.q1_force_torque_des(:, 1));
hold off;
ylabel('f_{thrust} (N)');
xlabel('Time (s)');
title('Force Torque Desired');
subplot(4,1,2);
hold on;
plot(T,simData.q1_force_torque_des(:, 2), 'c');
hold off;
ylabel('T_{x} (Nm)');
xlabel('Time (s)');
subplot(4,1,3);
hold on;
plot(T,simData.q1_force_torque_des(:, 3), 'r');
hold off;
ylabel('T_{y} (Nm)');
xlabel('Time (s)');
subplot(4,1,4);
hold on;
plot(T,simData.q1_force_torque_des(:, 4), 'g');
hold off;
ylabel('T_{z} (Nm)');
xlabel('Time (s)');
title('Force Torque Desired');
end
function [] = plot_orientation(T,X,Xd)
function [] = plot_orientation(T,X,Xd, simData)
figure();
subplot(3,1,1);
plot(T,X(:,7));
hold on;
plot(T,Xd(:,7),'r--');
plot(T,simData.q1_phi_des,'r--');
hold off;
ylabel('\phi (rad)');
xlabel('Time (s)');
title('Attitude');
legend('Actual','Desired');
ylim([-pi/2 pi/2]);
% ylim([-pi/2 pi/2]);
% subplot(4,1,2);
% %plot(T,X(:,7));
% hold on;
% plot(T,simData.q1_phi_err);
% hold off;
% ylabel('\phi (rad)');
% xlabel('Time (s)');
% title('Attitude');
% legend('Actual','Desired');
% ylim([-pi/2 pi/2]);
subplot(3,1,2);
plot(T,X(:,8));
hold on;
plot(T,Xd(:,8),'r--');
plot(T,simData.q1_theta_des,'r--');
hold off;
ylabel('\theta (rad)');
xlabel('Time (s)');
ylim([-pi/2 pi/2]);
% ylim([-pi/2 pi/2]);
legend('Actual','Desired');
subplot(3,1,3);
......@@ -30,7 +41,7 @@ function [] = plot_orientation(T,X,Xd)
hold off;
ylabel('\psi (rad)');
xlabel('Time (s)');
ylim([-pi/2 pi/2]);
% ylim([-pi/2 pi/2]);
legend('Actual','Desired');
end
......
function [] = plot_orientation_rates(T,X,Xd)
function [] = plot_orientation_rates(T,X,Xd, simData)
figure();
subplot(3,1,1);
plot(T,X(:,10));
figure();
subplot(3,1,1);
plot(T,X(:,10));
hold on;
plot(T,Xd(:,10),'r--');
plot(T,simData.q1_phi_dot_des,'r--');
hold off;
ylabel('p (rad/s)');
xlabel('Time (s)');
title('Attitude Rates');
legend('Actual','Desired');
legend('Actual','Desired');
subplot(3,1,2);
plot(T,X(:,11));
hold on;
plot(T,Xd(:,11),'r--');
plot(T,simData.q1_theta_dot_des,'r--');
hold off;
ylabel('q (rad/s)');
xlabel('Time (s)');
......@@ -24,7 +24,7 @@ function [] = plot_orientation_rates(T,X,Xd)
subplot(3,1,3);
plot(T,X(:,12));
hold on;
plot(T,Xd(:,12),'r--');
plot(T,simData.q1_psi_dot_des,'r--');
hold off;
ylabel('r (rad/s)');
xlabel('Time (s)');
......
function [] = plot_velocity(T,X,Xd)
function [] = plot_velocity(T,X,Xd, simData)
figure();
subplot(3,1,1);
plot(T,X(:,4));
hold on;
plot(T,Xd(:,4),'r--');
plot(T,simData.q1_x_dot_des,'r--');
hold off;
ylabel('x_{dot} (m/s)');
title('Velocity');
......@@ -14,7 +14,7 @@ function [] = plot_velocity(T,X,Xd)
subplot(3,1,2);
plot(T,X(:,5));
hold on;
plot(T,Xd(:,5),'r--');
plot(T,simData.q1_y_dot_des,'r--');
hold off;
ylabel('y_{dot} (m/s)');
legend('Actual','Desired');
......@@ -22,7 +22,7 @@ function [] = plot_velocity(T,X,Xd)
subplot(3,1,3);
plot(T,X(:,6));
hold on;
plot(T,Xd(:,6),'r--');
plot(T,simData.q1_z_dot_des,'r--');
hold off;
ylabel('z_{dot} (m/s)');
xlabel('Time (s)');
......
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
# coop_payload_sim
## Author
Matthew Romano (mmroma@umich.edu)
4/23/2020
## Overview
This is a simulation written in MATLAB for the cooperative payload transportation project. A system model including multiple quadrotors, a payload, aerodynamic drag, and user input is considered.
Note: The PhysicsModel/ folder is mostly unused. This was used previously when MATLAB-based simulation was being done, but now that I've switched over to using Simulink only a few helper functions from that are being used, however, which is a good enough reason to keep it for now.
## Running the Code
Run the simulator by calling the main function:
`coopPayloadSim`
Add-ons to install:
-C ontrol Systems Toolbox
-Symbolic Math Toolbox
\ No newline at end of file
......@@ -8,7 +8,7 @@
addpath(genpath(pwd));
%% Set up Parameters
traj_num = 1;
traj_num = 3;
params = setUpParameters_OneQuad(traj_num);
params.num_vehicles = 1;
......@@ -24,14 +24,15 @@ simData = sim('singlequadrotor','StartTime','0','StopTime',num2str(params.tFinal
% TODO plot quadrotor stuff, not load stuff
% 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_orientation_rates(simData.tout, simData.q1_X, simData.q1_Xd);
% plot_velocity(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_adjustment_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_simulink_data(simData, params, false);
animate_simulink_data(simData, params, false);
......
%% Cooperative Payload Simulator
% Author: Anne Ye (yean@umich.edu)
% Date:
% TODO: Add description
%% Add Necessary Folders to MATLAB Path (add everything)
addpath(genpath(pwd));
%% Set up Parameters
traj_num = 2;
params = setUpParameters_OneQuad(traj_num);
params.num_vehicles = 1;
%% Load Top Level Simulink Model (helps Simulink load ahead of time)
open('singlequadrotor');
%% Call Simulink and Run the Simulation
simData = sim('singlequadrotor','StartTime','0','StopTime',num2str(params.tFinal));
%simData = sim('singlequadrotor','StartTime','0','StopTime','10');
%% Plot
% TODO plot quadrotor stuff, not load stuff
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, simData);
plot_orientation_rates(simData.tout, simData.q1_X, simData.q1_Xd, simData);
%plot_force_torque_des(simData.tout, simData.q1_X, simData.q1_Xd, simData);
%% Animate
%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