Commit b75cfb7c authored by mmroma's avatar mmroma
Browse files

Intialized the transfer!

parents
% Misc_Aero_Calcs
%% Parameters
p_f = 1.225; % kg / m^3
mu = 1.802e-5; % kg / m s
d = 0.246; % m
v_max = 10; % m / s
R_max = 2e5; %
%% Reynolds Number given speed
% R = p_f * v_max * d / mu;
%% Speed given reynolds number
% v = mu * R_max / p_f / d;
%% Plot everything against velocity
v = 0.5:0.01:12; % 0.01 up to 12.0 m/s
R = p_f .* v .* d ./ mu;
C_D = (24 ./ R) .* (1 + 0.15 .* R .^ 0.681) + 0.407 ./ (1 + 8710./R);
C_D_2 = (8./R) .* (1./ sqrt(phi_par))...
+ (16./R) .* (1./ sqrt(phi))...
+ (3./sqrt(R)) .* (1./ phi.^(3./4))...
+ 0.421.^(0.4 .* (- log(phi) ).^2 )...
.* (1 ./ phi_perp);
F_D = (1/8) .* C_D .* pi .* d.^2 .* p_f .* v.^2;
F_D_2 = (1/8) .* C_D_2 .* pi .* d.^2 .* p_f .* v.^2;
figure(1);
plot(v, R);
xlabel('Relative Wind Speed (m/s)');
ylabel('Reynolds Number');
% title('Reynolds Number');
figure(2);
plot(v, C_D);
xlabel('Relative Wind Speed (m/s)');
ylabel('Coefficient of Drag');
% title('Coefficient of Drag');
figure(3);
plot(v, F_D);
xlabel('Relative Wind Speed (m/s)');
ylabel('Drag Force (N)');
% title('Drag Force');
figure(4);
plot(v, C_D_2);
xlabel('Relative Wind Speed (m/s)');
ylabel('Coefficient of Drag');
% title('Coefficient of Drag');
figure(5);
plot(v, F_D_2);
xlabel('Relative Wind Speed (m/s)');
ylabel('Drag Force (N)');
% Paper 4 - New simple correlation formula for the drag coefficient of non-spherical particles
%% phi values for cubes
phi = (pi/6)^(1/3);
phi_perp = (9*pi/16)^(1/3);
phi_par = ((9*pi/16)^(1/3) ) / (3 - (4/pi));
%% C_D
p_f = 1.225; % kg / m^3
mu = 1.802e-5; % kg / m s
d = 0.246; % m
v_max = 10; % m / s
R_max = 2e5; %
v = 0.5:0.01:12; % 0.01 up to 12.0 m/s
R = p_f .* v .* d ./ mu;
C_D = (8./R) .* (1./ sqrt(phi_par))...
+ (16./R) .* (1./ sqrt(phi))...
+ (3./sqrt(R)) .* (1./ phi.^(3./4))...
+ 0.421.^(0.4 .* (- log(phi) ).^2 )...
.* (1 ./ phi_perp);
%% Calc 4 phi_parallel
% Let's say we have a cube being rotated and we want to find the average
% parallel cross section.
%
% s = 1;
% theta = 0:0.01:2*pi;
%
%
% x1 = calc_x(theta + pi/4);
% % y1 = calc_y(theta);
% x2 = calc_x(theta + pi/2 + pi/4);
% % y2 = calc_y(theta + pi/2);
% x3 = calc_x(theta + pi + pi/4);
% % y3 = calc_y(theta + pi);
% x4 = calc_x(theta + 3*pi/2 + pi/4);
% % y4 = calc_y(theta + 3*pi/2);
%
% max_x = max( max(x1,x2), max(x3,x4) );
% min_x = min( min(x1,x2), min(x3,x4) );
%
% %%
% % figure(1);
% % plot(theta,x1);
% % hold on;
% % plot(theta,x2);
% % plot(theta,x3);
% % plot(theta,x4);
% % hold off;
% % legend('x1','x2','x3','x4');
% %
% % figure(2);
% % plot(theta,max_x);
% % hold on;
% % plot(theta,min_x);
% % hold off;
% % legend('max x','min x');
%
% figure(3);
% plot(theta,max_x - min_x);
% legend('length');
%
% %%
% % figure(4);
% % plot(theta, (sqrt(2)-s).*sin(2.*theta) + s);
% % hold on;
% % plot(theta, -(sqrt(2)-s).*sin(2.*theta) + s);
% % plot(theta,max_x - min_x);
% % title('Length with function');
%
% % %%
% % figure(5);
% % plot(theta, sqrt(2)./2.*sin(2.*theta) + 1);
% % hold on;
% % plot(theta, -sqrt(2)./2.*sin(2.*theta) + 1);
%
%
% %%
% % phi_par_avg = (sqrt(2)*s + (pi/2 - 1) * s * s) / (pi/2);
% %
% % length_par_avg = (sqrt(2) + (pi/2 - 1) * s) / (pi/2);
% %
% % calc_length_par_avg = mean(max_x - min_x);
%
%
% function [xs] = calc_x(theta_in)
% xs = sqrt(2) ./ 2 .* cos(theta_in);
% end
%
%
% function [ys] = calc_y(theta_in)
% ys = sqrt(2) ./ 2 .* sin(theta_in);
% end
\ No newline at end of file
% Analysis of the system from a Controls perspective (stability,
% linearization, feedback design, etc)
%% 1) Get Model
params = parameters_APM_Quad();
quadModel = @(x,u) multirotorModel(x,u,params);
n = 12; % 12 state system
m = 4; % 4 control inputs
%% 2) Linearize the model about stable hover
xeq = zeros(n,1);
ueq = params.hoverMotorCmd * ones(m,1);
[A,B] = symLin(quadModel, xeq, ueq);
C = eye(n);
display(A);
display(B);
%% 3) Check Stability, Controllability, Observability
% Stability
lambdas = eig(A); % all 0, regular system is not stable
% Controllability
C0 = ctrb(A,B);
if rank(C0) == n
display('System is Controllable');
else
display('System is NOT Controllable');
end
% Observability
Ob = obsv(A,C);
if rank(Ob) == n
display('System is Observable');
else
display('System is NOT Observable');
end
%% 4) Design a Controller (with lqr)
Q = eye(n,n);
R = eye(m,m);
[K,~,~] = lqr(A,B,Q,R);
%% 5) Check Stability of closed loop system
A_ = A - B*K;
lambdas = eig(A_);
function [A, B] = symLin(model, xeq, ueq)
%symLin
syms x1 x2 x3 x4 x5 x6 x7 x8 x9 x10 x11 x12
syms u1 u2 u3 u4
syms A_ B_
xdot = model([x1;x2;x3;x4;x5;x6;x7;x8;x9;x10;x11;x12],[u1;u2;u3;u4]);
A_ = jacobian(xdot,[x1;x2;x3;x4;x5;x6;x7;x8;x9;x10;x11;x12]);
B_ = jacobian(xdot,[u1;u2;u3;u4]);
% Evaluate at equilibrium points
x1 = xeq(1);
x2 = xeq(2);
x3 = xeq(3);
x4 = xeq(4);
x5 = xeq(5);
x6 = xeq(6);
x7 = xeq(7);
x8 = xeq(8);
x9 = xeq(9);
x10 = xeq(10);
x11 = xeq(11);
x12 = xeq(12);
u1 = ueq(1);
u2 = ueq(2);
u3 = ueq(3);
u4 = ueq(4);
% Obtain Numerical Constants
A = double(vpa(subs(A_),5));
B = double(vpa(subs(B_),5));
end
classdef PID
%PID Class trying to replicate APM's AC_PID class
% https://github.com/ArduPilot/ardupilot/blob/master/libraries/AC_PID/AC_PID.h
% Author: Matthew Romano (mmroma@umich.edu)
% Date: 1/17/2019
% TODO:
% Add back in feed-forward term
% Potentially add back in extra functions
properties
% Public (need to add it)
kp
ki
kd
imax
filt_hz = 20;
ff = 0;
filt_val = 1;
% Internal
dt
integrator = 0;
input = 0;
derivative = 0;
input_filt_change = 0;
end
methods
function obj = PID(kp_,ki_,kd_,imax_,filt_hz_,dt_,ff_)
obj.kp = kp_;
obj.ki = ki_;
obj.kd = kd_;
obj.imax = imax_;
obj.filt_hz = filt_hz_;
obj.dt = dt_;
obj.ff = ff_;
obj.filt_val = obj.get_filt_alpha();
end
function obj = set_input_filter_all(obj,input)
% set input to PID controller input is filtered before the PID
% controllers are run this should be called before any other
% calls to get_p, get_i or get_d
if (~isfinite(input))
return
end
obj.input_filt_change = obj.filt_val * (input - obj.input);
obj.input = obj.input + obj.input_filt_change;
if (obj.dt > 0)
obj.derivative = obj.input_filt_change / obj.dt;
end
end
function [obj] = set_input_filter_d(obj,input)
% set_input_filter_d - set input to PID controller
% only input to the D portion of the controller is filtered
% this should be called before any other calls to get_p, get_i or get_d
if (~isfinite(input))
return
end
if (obj.dt > 0)
der = (input - obj.input) / obj.dt;
obj.derivative = obj.derivative + obj.filt_val * ...
(der - obj.derivative);
end
obj.input = input;
end
function [result] = get_pid(obj)
% Get Results from PID controller
% P term
% I term
if( (obj.ki == 0) || (obj.dt == 0) )
% obj.integrator = 0;
else
obj.integrator = obj.integrator + ...
obj.input * obj.ki * obj.dt;
if(obj.integrator < -obj.imax)
obj.integrator = -obj.imax;
elseif(obj.integrator > obj.imax)
obj.integrator = obj.imax;
end
end
% D term
obj.derivative = obj.kd * obj.derivative;
result = obj.kp * obj.input + obj.integrator + obj.derivative;
end
% function [obj,result] = get_p(obj)
% result = obj.kp * obj.input;
% end
% function [obj,result] = get_i(obj)
% if( (obj.ki == 0) || (obj.dt == 0) )
% obj.integrator = 0;
% else
% obj.integrator = obj.integrator + ...
% obj.input * obj.ki * obj.dt;
% if(obj.integrator < -obj.imax)
% obj.integrator = -obj.imax;
% elseif(obj.integrator > obj.imax)
% obj.integrator = obj.imax;
% end
% end
%
% result = obj.integrator;
% end
% function [obj,result] = get_d(obj)
% obj.derivative = obj.kd * obj.derivative;
% result = obj.derivative;
% end
function [obj] = reset_I(obj)
% Reset the integrator
obj.integrator = 0;
end
function [result] = get_filt_alpha(obj)
if (obj.filt_hz == 0)
result = 1;
else
rc = 1 / (2*pi*obj.filt_hz);
result = obj.dt / (obj.dt + rc);
end
end
end
end
function [U,X_des] = testHoverThrottle(X,params)
%% Input
% X
% params
%% Output
% U (params.numRotors x 1) vector of control inputs normalized from 0->1
%% Compute Desired Thrust and Torques
T_d = params.hoverThrust;
Tao_phi_d = 0;
Tao_theta_d = 0;
Tao_psi_d = 0;
%% Compute Desired Motor Inputs
U = params.M \ [T_d; Tao_phi_d; Tao_theta_d; Tao_psi_d];
U = min(U,1);
U = max(U,0);
%% Grab Desired States
X_des = [zeros(12,1)];
end
function [U,X_des] = testNegativePitchTorque(X,params)
%% Input
% X
% params
%% Output
% U (params.numRotors x 1) vector of control inputs normalized from 0->1
%% Compute Desired Thrust and Torques
T_d = params.hoverThrust;
Tao_phi_d = 0;
Tao_theta_d = -0.1;
Tao_psi_d = 0;
%% Compute Desired Motor Inputs
U = params.M \ [T_d; Tao_phi_d; Tao_theta_d; Tao_psi_d];
U = min(U,1);
U = max(U,0);
%% Grab Desired States
X_des = [zeros(12,1)];
end
function [U,X_des] = testNegativeRollTorque(X,params)
%% Input
% X
% params
%% Output
% U (params.numRotors x 1) vector of control inputs normalized from 0->1
%% Compute Desired Thrust and Torques
T_d = params.hoverThrust;
Tao_phi_d = -0.1;
Tao_theta_d = 0;
Tao_psi_d = 0;
%% Compute Desired Motor Inputs
U = params.M \ [T_d; Tao_phi_d; Tao_theta_d; Tao_psi_d];
U = min(U,1);
U = max(U,0);
%% Grab Desired States
X_des = [zeros(12,1)];
end
function [U,X_des] = testNegativeYawTorque(X,params)
%% Input
% X
% params
%% Output
% U (params.numRotors x 1) vector of control inputs normalized from 0->1
%% Compute Desired Thrust and Torques
T_d = params.hoverThrust;
Tao_phi_d = 0;
Tao_theta_d = 0;
Tao_psi_d = -0.1;
%% Compute Desired Motor Inputs
U = params.M \ [T_d; Tao_phi_d; Tao_theta_d; Tao_psi_d];
U = min(U,1);
U = max(U,0);
%% Grab Desired States
X_des = [zeros(12,1)];
end
function [U,X_des] = testPositivePitchTorque(X,params)
%% Input
% X
% params
%% Output
% U (params.numRotors x 1) vector of control inputs normalized from 0->1
%% Compute Desired Thrust and Torques
T_d = params.hoverThrust;
Tao_phi_d = 0;
Tao_theta_d = 0.1;
Tao_psi_d = 0;
%% Compute Desired Motor Inputs
U = params.M \ [T_d; Tao_phi_d; Tao_theta_d; Tao_psi_d];
U = min(U,1);
U = max(U,0);
%% Grab Desired States
X_des = [zeros(12,1)];
end
function [U,X_des] = testPositiveRollTorque(X,params)
%% Input
% X
% params
%% Output
% U (params.numRotors x 1) vector of control inputs normalized from 0->1
%% Compute Desired Thrust and Torques
T_d = params.hoverThrust;
Tao_phi_d = 0.1;
Tao_theta_d = 0;
Tao_psi_d = 0;
%% Compute Desired Motor Inputs
U = params.M \ [T_d; Tao_phi_d; Tao_theta_d; Tao_psi_d];
U = min(U,1);
U = max(U,0);
%% Grab Desired States
X_des = [zeros(12,1)];
end
function [U,X_des] = testPositiveYawTorque(X,params)
%% Input
% X
% params
%% Output
% U (params.numRotors x 1) vector of control inputs normalized from 0->1
%% Compute Desired Thrust and Torques
T_d = params.hoverThrust;
Tao_phi_d = 0;
Tao_theta_d = 0;
Tao_psi_d = 0.1;
%% Compute Desired Motor Inputs
U = params.M \ [T_d; Tao_phi_d; Tao_theta_d; Tao_psi_d];
U = min(U,1);
U = max(U,0);
%% Grab Desired States
X_des = [zeros(12,1)];
end
function [U,X_des] = attitudePID(X,params)
%% attitudePID
%% Input
% X
% params
%% Output
% U (4x1) vector of control inputs normalized from 0->1
% X_des (12x1) vector of desired states
%% Desired Attitude
rollDesired = params.xd(7);
pitchDesired = params.xd(8);
yawDesired = params.xd(9);
rollRateDesired = params.xd(10);
pitchRateDesired = params.xd(11);
yawRateDesired = params.xd(12);
%% Actual Attitude
roll = X(7);
pitch = X(8);
yaw = X(9);
rollRate = X(10);
pitchRate = X(11);
yawRate = X(12);
%% Update Attitude PID loops
params.rollPID = params.rollPID.set_input_filter_all(rollDesired - roll);
params.pitchPID = params.pitchPID.set_input_filter_all(pitchDesired - pitch);
params.yawPID = params.yawPID.set_input_filter_all(yawDesired - yaw);
rollRateDesired = rollRateDesired + params.rollPID.get_pid();