Texas A&M, MEEN 431, Dynamic Systems and Controls, Project

..meen431_project.md

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

.gitignore

# ignore matlab working files
*.m~
*.asv

# ignore data files
*.mat

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