Skip to content

Instantly share code, notes, and snippets.

@samtx

samtx/..meen431_project.md

Last active March 26, 2018 00:01
Show Gist options
  • Save samtx/578c6f02a1de678b4e388b3c611e82e0 to your computer and use it in GitHub Desktop.
Save samtx/578c6f02a1de678b4e388b3c611e82e0 to your computer and use it in GitHub Desktop.
Download ZIP
Texas A&M, MEEN 431, Dynamic Systems and Controls, Project
Raw
..meen431_project.md

Texas A&M, MEEN 431, Dynamic Systems and Controls, Project Sam Friedman, Nilesh Pore

Raw
.gitignore
# ignore matlab working files
*.m~
*.asv
# ignore data files
*.mat
Raw
meen431_project.m
% Dynamics Project, MEEN 431
% Sam Friedman, Nilesh Pore
% given values in rpm and rpm^2
function varargout = meen431_project()
do_part2 = true;
do_part3 = 1;
do_part4 = 1;
do_part5 = 1;
do_part6 = 0;
w10 = 15 * pi/30;
w1dt0 = 5 * 2*pi/3600;
w20 = 10 * pi/30;
w30 = 5 * pi/30;
w1 = linspace(2,30,15); % psi_dt, 15 steps
w2 = linspace(2,10,5); % phi_dt, 5 steps
w3 = [5, 10, 15]; % theta_dt
w1dt = [3, 5, 10]; % psi_ddt
%% Part 2
if do_part2
% find acceleration in g's
psidt = 15 * pi/30;
phidt = 10 * pi/30;
thetadt = 5 * pi/30;
psiddt = 5 * 2*pi/3600;
phiddt = 0;
thetaddt = 0;
a = getAccel(psidt, phidt, thetadt, psiddt);
fprintf('Q2 Accel (body-fixed): %.3f g\n',a);
aground = getGsGround(getAccelGround(psidt,phidt,thetadt,psiddt,phiddt,thetaddt));
fprintf('Q2 Accel (ground-fixed): %.3f g\n',aground);
end
%% Part 3
if do_part3
a3 = zeros(length(w1),length(w2),length(w3),length(w1dt));
% get acceleration values
for i = 1:length(w1)
for j = 1:length(w2)
for k = 1:length(w3)
for m = 1:length(w1dt)
a3(i,j,k,m) = getAccel(w1(i)*pi/30, w2(j)*pi/30, w3(k)*pi/30, w1dt(m)*2*pi/3600);
end
end
end
end
[W1,W2] = meshgrid(w1,w2);
fontsize = 16; % plot settings
fig = figure(1);
p = uipanel('Parent',fig,'BorderType','none','BackgroundColor','white');
k = 0;
for i = 1:length(w3)
for j = 1:length(w1dt)
k = k + 1;
% fprintf('k=%d\n',k);
subplot(3,3,k,'Parent',p)
surf(W1,W2,a3(:,:,i,j)');
xlabel('$\omega_1$ [rpm]','Interpreter', 'Latex','FontSize',fontsize);
ylabel('$\omega_2$ [rpm]','Interpreter', 'Latex','FontSize',fontsize);
zlim([0, 20]);
zlabel('$a/g$','Interpreter', 'Latex','FontSize',fontsize);
title({' Head acceleration with';['$\omega_3 = ',num2str(w3(i)),' $ rpm, $\dot{\omega}_1=',num2str(w1dt(j)),' $ rpm\textsuperscript{2}']},'Interpreter','Latex','FontSize',16);
end
end
end
%% Part 4
if do_part4
fontsize = 16; % plot settings
% make figure of plane through 3d surface at a=1.0g, 1.5g
w1 = linspace(2,10,40)'; % psi_dt, 15 steps
w2 = linspace(0,30,40)'; % phi_dt, 5 steps
w3 = [5,10,15]'; % theta_dt
w1dt = [3,5,10]'; % psi_ddt
% w3 = linspace(0,20,30)'; % theta_dt
% w1dt = linspace(0,10,50)'; % psi_ddt
a4 = zeros(length(w1),length(w2));
w3_0 = 5; % rpm
w1dt_0 = 5; % rpm^2
g = [1.0, 1.5];
W1 = zeros(length(w2),length(g));
% fprintf('!!! size(a4)=(%d, %d) \n',size(a4));
% fprintf('!!! length(w2)= %d \n',length(w2));
% fprintf('!!! length(w1)= %d \n',length(w1));
for i = 1:length(w2)
for k = 1:length(w1)
% fprintf('i=%d, k=%d, size(a4)=(%d, %d) \n',[i,k,size(a4)]);
a4(k,i) = getAccel(w1(k)*pi/30, w2(i)*pi/30, w3_0*pi/30, w1dt_0*2*pi/3600);
end
for j = 1:length(g)
W1(i,j) = getW1(w2(i),g(j));
end
end
% W2 = zeros(length(w1),2);
% for i = 1:length(w1)
% W2(i,1) = getW2(w1(i),1.0,pi/30,2*pi/3600);
% W2(i,2) = getW2(w1(i),1.5);
% end
% for i = 1:length(w2)
% for k = 1:length(w3)
% for m = 1:length(w1dt)
% for j = 1:length(g)
% W1(i,k,m,j) = getW1(w2(i),g(j),w3(k)*pi/30,w1dt(m)*2*pi/3600);
% end
% end
% end
% end
[ww1, ww2] = meshgrid(w1,w2);
w2tmp = [linspace(0,11.4,100)';linspace(11.4,11.5,100)';linspace(11.5,30,100)'];
W1tmp = zeros(length(w2tmp),2);
for i = 1:length(w2tmp)
for j = 1:length(g)
W1tmp(i,j) = getW1(w2tmp(i),g(j));
end
end
W2tmp = [w2tmp, w2tmp];
G = repmat(g,size(W1tmp,1),1);
% remove data points where w1 < 2,
idx_1 = find(W1tmp(:,1)>=2);
idx_15 = find(W1tmp(:,2)>=2);
W1_1 = W1tmp(idx_1,1);
W2_1 = W2tmp(idx_1,1);
G_1 = G(idx_1,1);
W1_15 = W1tmp(idx_15,2);
W2_15 = W2tmp(idx_15,2);
G_15 = G(idx_15,2);
save('data4.mat');
figure(2);
hold on
surf(ww1,ww2,a4');
X = [2, 10, 10, 2 ];
Y = [0, 0, 30, 30];
Z1 = [1, 1, 1, 1];
Z15 = [1.5,1.5,1.5,1.5];
patch(X,Y,Z1,'g','FaceAlpha',0.25);
patch(X,Y,Z15,'r','FaceAlpha',0.25);
plot3(W1_1,W2_1,G_1,'-g','MarkerSize',20,'LineWidth',3);
plot3(W1_15,W2_15,G_15,'-r','MarkerSize',20,'LineWidth',3);
grid on
hold off
xlabel('$\omega_1$ [rpm]','Interpreter', 'Latex','FontSize',fontsize);
ylabel('$\omega_2$ [rpm]','Interpreter', 'Latex','FontSize',fontsize);
% zlim([0, 20]);
zlabel('$a/g$','Interpreter', 'Latex','FontSize',fontsize);
% title({' Head acceleration with';['$\omega_3 = ',num2str(w3(i)),' $ rpm, $\dot{\omega}_1=',num2str(w1dt(j)),' $ rpm\textsuperscript{2}']},'Interpreter','Latex','FontSize',12);
% [XX, YY] = meshgrid(w2, w3);
% surface(XX,YY,W1(:,:,1,1));
% xlabel('$\omega_2$ [rpm]','Interpreter', 'Latex','FontSize',fontsize);
% ylabel('$\omega_3$ [rpm]','Interpreter', 'Latex','FontSize',fontsize);
% zlabel('$\omega_1$ [rpm]','Interpreter', 'Latex','FontSize',fontsize);
% legend({'1.0g w1','1.5g w1','1.0g w2','1.5g w2'});
% g = 1.5;
% x0 = [0,0];
% F = @(x) getAccel(x(1),x(2),w3(i)*pi/30,w1dt*2*pi/3600)-g;
% x = fsolve(F,x0);
% re
return
% % get acceleration values
for i = 1:length(w1)
for j = 1:length(w2)
for k = 1:length(w3)
for m = 1:length(w1dt)
a4(i,j) = getAccel(w1(i)*pi/30, w2(j)*pi/30, w3(k)*pi/30, w1dt(m)*2*pi/3600);
end
end
end
end
fig = figure(3);
% uipanel('Parent',fig,'BorderType','none','BackgroundColor','white');
hold on
plot(W2, [w1,w1]);
plot([w2,w2], W1);
hold off
xlabel('$\omega_2$ [rpm]','Interpreter', 'Latex','FontSize',fontsize);
ylabel('$\omega_1$ [rpm]','Interpreter', 'Latex','FontSize',fontsize);
legend({'1.0g w1','1.5g w1','1.0g w2','1.5g w2'});
grid on
end
%% Parts 5 and 6
psidt = 2; % rad/s
phidt = 3;
thetadt = 4;
psiddt = 3;
phiddt = -2;
thetaddt = 4;
% psidt = 2 * pi/30; % rpms
% phidt = 3 * pi/30;
% thetadt = 4 * pi/30;
% psiddt = 3 * 2*pi/3600;
% phiddt = -2 * 2*pi/3600;
% thetaddt = 4 * 2*pi/3600;
if do_part5
dz = 0/3.2808; % m
m = 800/9.81; % kg
kp = [0.6,0.5,0.15]; % m, radius of gyration
Ipp = m*kp.^2;
I = [Ipp(1)+m*dz^2, Ipp(2)+m*dz^2, Ipp(3)];
W = getW(psidt,phidt,thetadt);
Wdt = getWdt(psidt,phidt,thetadt,psiddt,phiddt,thetaddt);
M = zeros(3,1);
M(1) = I(1)*Wdt(1) + (I(3)-I(2))*W(2)*W(3); % Mx
M(2) = I(2)*Wdt(2) + (I(1)-I(3))*W(1)*W(3); % My
M(3) = I(3)*Wdt(3) + (I(2)-I(1))*W(1)*W(2); % Mz
M2(1) = I(1)*thetaddt + (I(3)-I(2))*phidt*psidt; % Mx
M2(2) = I(2)*phiddt + (I(1)-I(3))*psidt*thetadt; % My
M2(3) = I(3)*psiddt + (I(2)-I(1))*thetadt*phidt; % Mz
M3(1) = I(1)*thetaddt + (-I(1)-I(2))*phidt*psidt; % Mx
M3(2) = I(2)*phiddt + (I(1)+I(2))*psidt*thetadt; % My
M3(3) = I(3)*psiddt + (-I(3))*thetadt*phidt; % Mz
M4(1) = I(1)*thetaddt + (-I(1)-I(2)+I(3))*phidt*psidt; % Mx
M4(2) = I(2)*phiddt + (I(1)+I(2))*psidt*thetadt; % My
M4(3) = I(3)*psiddt + (-I(3)-I(1))*thetadt*phidt; % Mz
M5(1) = I(1)*(thetaddt-psidt*phidt); % Mx
M5(2) = I(2)*(phiddt+psidt*thetadt); % My
M5(3) = I(3)*(psiddt-thetadt*phidt); % Mz
% get magnitude of torque
tau = sqrt(sum(M.^2));
tau2 = sqrt(sum(M2.^2));
tau3 = sqrt(sum(M3.^2));
tau4 = sqrt(sum(M4.^2));
tau5 = sqrt(sum(M5.^2));
fprintf('Q5: torque1 = %.2f N m \n',tau);
fprintf('Q5: torque2 = %.2f N m \n',tau2);
fprintf('Q5: torque3 = %.2f N m \n',tau3);
fprintf('Q5: torque4 = %.2f N m \n',tau4);
fprintf('Q5: torque5 = %.2f N m \n',tau5);
end
if do_part6
dy = 40/3.2808; % m
m = 800/9.81; % kg
kp = [0.6,0.5,0.15]; % m, radius of gyration
Ipp = m*kp.^2;
I = [Ipp(1)+m*dy^2, Ipp(2), Ipp(3)+m*dy^2];
W = getW(psidt,phidt,thetadt);
Wdt = getWdt(psidt,phidt,thetadt,psiddt,phiddt,thetaddt);
M = zeros(3,1);
M(1) = I(1)*Wdt(1) + (I(3)-I(2))*W(2)*W(3); % Mx
M(2) = I(2)*Wdt(2) + (I(1)-I(3))*W(1)*W(3); % My
M(3) = I(3)*Wdt(3) + (I(2)-I(1))*W(1)*W(2); % Mz
M2(1) = I(1)*thetaddt + (I(3)-I(2))*phidt*psidt; % Mx
M2(2) = I(2)*phiddt + (I(1)-I(3))*psidt*thetadt; % My
M2(3) = I(3)*psiddt + (I(2)-I(1))*thetadt*phidt; % Mz
% get magnitude of torque
tau = sqrt(sum(M.^2));
tau2 = sqrt(sum(M2.^2));
fprintf('\nQ6: torque1 = %.3f kN m \n',tau/1000);
fprintf('Q6: torque2 = %.3f kN m \n',tau2/1000);
end
end
function W = getW(psidt,phidt,thetadt,varargin)
% get angular velocity vector in body-fixed coordinates
if isempty(varargin)
% set angles to zero
psi = 0;
phi = 0;
theta = 0;
else
psi = varargin{1};
phi = varargin{2};
theta = varargin{3};
end
W = [thetadt-psidt*sin(phi);
psidt*cos(phi)*sin(theta)+phidt*cos(theta);
psidt*cos(phi)*cos(theta)-phidt*sin(theta)];
end
function Wdt = getWdt(psidt,phidt,thetadt,psiddt,phiddt,thetaddt,varargin)
% get angular acceleration vector in body-fixed coordinates
% assume psi = phi = theta = 0
Wdt = [thetaddt-psidt*phidt;
psidt*thetadt-phiddt;
psiddt-phidt*thetadt];
end
function a = getAccel(psidt, phidt, thetadt, psiddt)
a = sqrt(...
(46.*psidt.*thetadt-40.*psiddt+40.*thetadt.*phidt).^2+...
(6.*psidt.*phidt-40.*psidt.^2-40.*thetadt.^2).^2+...
(3.*thetadt.^2-3.*phidt.^2).^2)/32.2;
end
function a = getAccelGround(psidt, phidt, thetadt, psiddt, phiddt, thetaddt)
a = [40*psiddt+3*phiddt-44*thetadt*phidt+6*thetadt*psidt,
-3*thetaddt+40*thetadt.^2+40*psidt.^2+6*phidt*psidt,
-40*thetaddt-3*phidt.^2-3*thetadt.^2];
end
function ag = getGsGround(a)
ag = sqrt(sum(a.^2))/32.2;
end
% function w1 = getW1(w2,g)
% % w1, w2 in rpm
% w3 = 5 * pi/30;
% w1dt = 5 * 2*pi/3600;
% F = @(x) getAccel(x, w2*pi/30, w3, w1dt) - g;
% x0 = 2 * pi/30; % init point, w1 = 2 rpm
% options = optimoptions('fsolve','Display','none');
% w1 = fsolve(F, x0, options) * 30/pi; % solve and convert back to rpm
% end
function w1 = getW1(w2,g,varargin)
% w1, w2 in rpm
if length(varargin)==2
w3 = varargin{1};
w1dt = varargin{2};
elseif length(varargin)==1
w3 = varargin{1};
w1dt = 5 * 2*pi/3600;
else
w3 = 5 * pi/30;
w1dt = 5 * 2*pi/3600;
end
F = @(x) getAccel(x, w2*pi/30, w3, w1dt) - g;
x0 = 3 * pi/30; % init point, w1 = 2 rpm
options = optimoptions('fsolve','Display','none');
w1 = fsolve(F, x0, options) * 30/pi; % solve and convert back to rpm
end
function w2 = getW2(w1,g,varargin)
% w1, w2 in rpm
w3 = 5 * pi/30;
w1dt = 5 * 2*pi/3600;
F = @(x) getAccel(w1*pi/30, x, w3, w1dt) - g;
x0 = 2 * pi/30; % init point, w1 = 2 rpm
options = optimoptions('fsolve','Display','none');
w2 = fsolve(F, x0, options) * 30/pi; % solve and convert back to rpm
end
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment