sravan/Quadcopter-Control
1
0
Fork 0
mirror of https://github.com/ME-561-W20-Quadcopter-Project/Quadcopter-Control.git synced 2025-08-31 21:03:13 +00:00
Code Issues Packages Projects Releases Wiki Activity

Added PD controller

Browse Source
This commit is contained in:
Lakshu Periakaruppan
2020-04-11 15:11:34 -04:00
parent d463ccf0ac
commit 0de43c3cb8
3 changed files with 81 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/.gitignore vendored Normal file
View File

@@ -0,0 +1,4 @@
*.slxc
*.slx.autosave
*.asv
slprj

128
src/PlantModel.m
View File

@@ -1,51 +1,77 @@
% Parameters source: https://sal.aalto.fi/publications/pdf-files/eluu11_public.pdf
g=9.81;
m = .468;
Ix = 4.856*10^-3;
Iy = 4.856*10^-3;
Iz = 8.801*10^-3;
% State Space Source: https://arxiv.org/ftp/arxiv/papers/1908/1908.07401.pdf
% X' = Ax+Bu
% y = Cx+Du
% Inputs:
% U1: Total Upward Force on the quad rotor along z-axis
% U2: Pitch Torque (about x-axis)
% U3: Roll Torque (about y-axis)
% U4: Yaw Torque (about z-axis)
A = [0 0 0 1 0 0 0 0 0 0 0 0;...
0 0 0 0 1 0 0 0 0 0 0 0;...
0 0 0 0 0 1 0 0 0 0 0 0;...
0 0 0 0 0 0 0 -g 0 0 0 0;...
0 0 0 0 0 0 g 0 0 0 0 0;...
0 0 0 0 0 0 0 0 0 0 0 0;...
0 0 0 0 0 0 0 0 0 1 0 0;...
0 0 0 0 0 0 0 0 0 0 1 0;...
0 0 0 0 0 0 0 0 0 0 0 1;...
0 0 0 0 0 0 0 0 0 0 0 0;...
0 0 0 0 0 0 0 0 0 0 0 0;...
0 0 0 0 0 0 0 0 0 0 0 0];
% Note: In paper, 1/m is in wrong spot
B = [0 0 0 0;...
0 0 0 0;...
0 0 0 0;...
0 0 0 0;...
0 0 0 0;...
1/m 0 0 0;...
0 0 0 0;...
0 0 0 0;...
0 0 0 0;...
0 1/Ix 0 0;...
0 0 1/Iy 0;...
0 0 0 1/Iz];
C = [1 0 0 0 0 0 0 0 0 0 0 0;...
0 1 0 0 0 0 0 0 0 0 0 0;...
0 0 1 0 0 0 0 0 0 0 0 0;...
0 0 0 0 0 0 1 0 0 0 0 0;...
0 0 0 0 0 0 0 1 0 0 0 0;...
0 0 0 0 0 0 0 0 1 0 0 0];
D = zeros(6,4);
%Parameters source: https://sal.aalto.fi/publications/pdf-files/eluu11_public.pdf
g=9.81;
m = .468;
Ix = 4.856*10^-3;
Iy = 4.856*10^-3;
Iz = 8.801*10^-3;
%State Space Source: https://arxiv.org/ftp/arxiv/papers/1908/1908.07401.pdf
%X' = Ax+Bu
%y = Cx+Du
%Inputs:
%U1: Total Upward Force on the quad rotor along z-axis
%U2: Pitch Torque (about x-axis)
%U3: Roll Torque (about y-axis)
%U4: Yaw Torque (about z-axis)
A = [0 0 0 1 0 0 0 0 0 0 0 0;...
0 0 0 0 1 0 0 0 0 0 0 0;...
0 0 0 0 0 1 0 0 0 0 0 0;...
0 0 0 0 0 0 0 -g 0 0 0 0;...
0 0 0 0 0 0 g 0 0 0 0 0;...
0 0 0 0 0 0 0 0 0 0 0 0;...
0 0 0 0 0 0 0 0 0 1 0 0;...
0 0 0 0 0 0 0 0 0 0 1 0;...
0 0 0 0 0 0 0 0 0 0 0 1;...
0 0 0 0 0 0 0 0 0 0 0 0;...
0 0 0 0 0 0 0 0 0 0 0 0;...
0 0 0 0 0 0 0 0 0 0 0 0];
%Note: In paper, 1/m is in wrong spot
B = [0 0 0 0;...
0 0 0 0;...
0 0 0 0;...
0 0 0 0;...
0 0 0 0;...
1/m 0 0 0;...
0 0 0 0;...
0 0 0 0;...
0 0 0 0;...
0 1/Ix 0 0;...
0 0 1/Iy 0;...
0 0 0 1/Iz];
C = [1 0 0 0 0 0 0 0 0 0 0 0;...
0 1 0 0 0 0 0 0 0 0 0 0;...
0 0 1 0 0 0 0 0 0 0 0 0;...
0 0 0 0 0 0 1 0 0 0 0 0;...
0 0 0 0 0 0 0 1 0 0 0 0;...
0 0 0 0 0 0 0 0 1 0 0 0]
D = zeros(6,4)
x0 = 0;
y0 = 0;
z0 = 0;
xdot0 = 0;
ydot0 = 0;
zdot0 = 0;
phi0 = 10*(pi/180);
theta0 = 10*(pi/180);
psi0 = 0;
phidot0 = 0;
thetadot0 = 0;
psidot0 = 0;
initialCondition = [x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0];
%Gains
kpp = 10;
kdp = 0.2;
kpt = 4;
kdt = 0.2;
kpps = 1;
kdps = .4;
kpz = 100;
kdz = 20;

BIN
src/PlantModelSim.slx
View File

Binary file not shown.