MATLAB

% Geometrianlys.m
% Koden löser de geometriska tvångsekvationerna för mekanismen med
% Newton-Raphsons metod genom att anropa funktionen NRsolver.m som måste
% finnas i samma mapp som huvudprogrammet.
% Anders Söderberg, KTH Maskinkonstruktion, 2011-02-04

%% Städar arbetsminnet och prompten
clc
clear all
close all
%% Sätter värden på givna parameterar och variabler (Skriv in era värden)
% Parametrar
L1  = 0.657;     % Avstånd mellan infästningspunkter på styrarm [m]
L2  = 0.05;      % Avstånd mellan infästningspunkter på tak [m]
L3  = 0.67;      % Avstånd mellan infästningspunkter på bakruta [m]
L   = 0.5;       % Takets längd [m]
L4x = 0.035;     % Avstånd i xled mellan infästningspunkter i kaross [m]
L4y = 0.044;     % Avstånd i yled mellan infästningspunkter i kaross [m]

%% Beräknar vinklarna beta och gamma som funktion av alpha
beta  = [];     % Vinkel på bakrutan [rad]
gamma = [];     % Vinkel på tak [rad]
% Löser tvångsekvationerna med avseende på beta och gamma för alla alpha
for i = 1:length(alpha)
    if i == 1
        gamma0 = pi/9;                              
        beta0  = pi/180;          
        [beta(i),gamma(i)] = NRsolver(alpha(i),beta0,gamma0,L1,L2,L3,L4x,L4y);
    else
        [beta(i),gamma(i)] = NRsolver(alpha(i),beta(i-1),gamma(i-1),L1,L2,L3,L4x,L4y);
    end  
end
% Kontroll att lösningen uppfyller tvångsekvationer
eq1 = L1*cos(alpha)+L2*cos(gamma)-L3*cos(beta)-L4x;
eq2 = L1*sin(alpha)-L2*sin(gamma)-L3*sin(beta)+L4y;
% Plotta felet i tvångsekvationerna
figure(1)
grid on
hold on
plot(alpha*180/pi,eq1*1e6,'b')
plot(alpha*180/pi,eq2*1e6,'r')
title('Resulterande fel vid lösning av tvångsekvationer')
xlabel('\alpha [\circ]')
ylabel('Nummeriskt fel  [\mum]')
legend('Loop i x-led','Loop i y-led')
%% Animering av rörelsen
% Koordinater till viktiga punkter
xO  = zeros(size(alpha));       % Vektor med x-koordinater för origo
xA =  L1*cos(alpha);            % Vektor med x-koordinater för led A
yA =  yO + L1*sin(alpha);       % Vektor med y-koordinater för led A
xB =  xA + L2*cos(gamma);       % Vektor med x-koordinater för led B
yB =  yA - L2*sin(gamma);       % Vektor med y-koordinater för led B
xP =  xB - (L+L2)*cos(gamma);   % Vektor med x-koordinater för punkt P
yP =  yB + (L+L2)*sin(gamma);   % Vektor med y-koordinater för punkt P
% Plottar mekanimens position för samtliga vinklar
figure(2)                    
for i=1:length(alpha)
    clf
    hold on
    plot([xO(i) xA(i)],[yO(i) yA(i)],'b')       % Plottar styrarm (linje mellan O och A)
    plot([xB(i) xC(i)],[yB(i) yC(i)],'g')       % Plottar bakruta (linje mellan B och C)
    plot([xB(i) xP(i)],[yB(i) yP(i)],'r')       % Plottar tak (linje mellan B och P)
    grid on                                     % Aktiverar rutnät
    axis equal                                  % Sätt eregenskaper hos axlar
    title('Mekanismens rörelse i xy-planet')    % Sätter figur titel
    xlabel('x [m]')                             % Sätter text på x-axel
    ylabel('y [m]')                             % Sätter text på x-axel
    xlim([-1.5 1.0])                            % Sätter gränsvärden på x-axel
    ylim([-1.0])                            % Sätter gränsvärden på y-axel
    pause(0.01)                                 % Pausar programmet i 0.1 sekund
end
% Plottar alpha mot beta och gamma
figure(3)
hold on
plot(alpha*180/pi,beta*180/pi,'r')                  % Plottar alpha mot beta
plot(alpha*180/pi,gamma*180/pi,'b')                 % Plottar alpha mot gamma
grid on                                             % Aktiverar rutnät
axis equal                                          % Egenskaper hos axlar
title('Graf över \beta(\alpha) och \gamma(\alpha)') % Figurens titel
xlabel('Vinkel på \alpha i grader')                 % Text på x-axeln
ylabel('Vilkel på \beta och \gamma i grader')       % text på y-axeln
xlim([0 180])                                       % Gränsvärden på x-axeln
ylim([0 180])                                       % Gränsvärden på y-axeln
legend('\beta','\gamma')                            % Visar vilken linje som tillhör vilken vinkel
% Bestämmer länkarmarnas vinkelhastigheter
alphaT = 15*pi/180;                                                     % Vinkelhastighet hos styrarm [rad/s]
betaT = (L1*(sin(gamma).*cos(alpha)+cos(gamma).*sin(alpha))*alphaT)./...
    (L3*(cos(gamma).*sin(beta)+sin(gamma).*cos(beta)));                 % Räknar ut vinkelhastighet betaT för vinkeln beta
gammaT = (L1*alphaT*cos(alpha)-L3*betaT.*cos(beta))./(L2*cos(gamma));   % Räknar ut vinkelhastighet gammaT för vinkeln gamma
% Skriver ut vinkelhastigheter för betaT och gammaT för olika vinklar på alpha
disp(' ');
disp(['Alpha = num2str(betaT(41)) ' rad/s, GammaT = ' num2str(gammaT(41)) ' rad/s.']);
  
% Plottar vinkelhastigheter som funktion av alpha
figure(4)
hold on
plot(alpha,betaT,'r')                                                               % Plottar betaT som en funktion av alpha
plot(alpha,gammaT,'b')                                                              % Plottar gammaT som en funktion av alpha
grid on                                                                             % Aktivera rutnät
title('Graf över vinkelhastigheterna betaT och gammaT som funktion av \alpha')      % Figurens titel
xlabel('\alpha [rad]')                                                              % Text på x-axeln
ylabel('betaT och gammaT [rad/s]')                                                  % text på y-axeln
legend('betaT','gammaT')                                                            % Visar vilken linje som tillhör vilken vinkelhastighet
% Bestämmer länkarmarnas vinkelaccelerationer
% Räknar ut vinkelaccelerationen betaTT för vinkeln beta
betaTT = (cos(gamma).*(alphaT.^2.*cos(alpha)+L2*cos(gamma).*gammaT.^2-...
    L3*betaT.^2.*cos(beta))+sin(gamma).*(L2*sin(gamma).*gammaT.^2-...
    L1*alphaT.^2.*sin(alpha)+L3.*betaT.^2.*sin(beta)))./...
    (L3*(cos(gamma).*sin(beta)+sin(gamma).*cos(beta)));
% Räknar ut vinkelaccelerationen gammaTT för vinkeln gamma
gammaTT = (-L1*alphaT.^2.*sin(alpha)+L3*betaT.^2.*sin(beta)-...
    L3*betaTT.*cos(beta)+L2*gammaT.^2.*sin(gamma))./(L2*cos(gamma));            
% Skriver ut vinkelaccelerationer för betaTT och gammaTT för olika vinklar på alpha
disp(' ');
disp(['Alpha = ' num2str(alpha(41)*180/pi) ' grader, BetaTT = ' num2str(betaTT(41)) ' rad/s^2, GammaTT = ' num2str(gammaTT(41)) ' rad/s^2.']);

figure(5)
hold on
plot(alpha,betaTT,'r')                                                                  % Plottar betaTT som en funktion av alpha
plot(alpha,gammaTT,'b')                                                                 % Plottar gammaTT som en funktion av alpha
grid on                                                                                 % Aktivera rutnät
title('Graf över vinkelaccelerationerna betaTT och gammaTT som funktion av \alpha')     % Figurens titels
xlabel('\alpha [rad]')                                                                  % Text på x-axeln
ylabel('betaTT och gammaTT [rad/s^2]')                                                  % text på y-axeln
legend('betaTT','gammaTT')                                                              % Visar vilken linje som tillhör vilken vinkelacc
%% Simulering av av punkten T:s rörelse
xT = L1*cos(alpha)-L*cos(gamma)+(t2/2)*sin(gamma);     % Vektor med x-koordinater för punkten T
yT = L1*sin(alpha)+L*sin(gamma)+(t2/2)*cos(gamma);     % Vektor med y-koordinater för punkten T
% Plottar punkten T:s rörelse i xy-planet
figure(6)
hold on
plot(x,yT,'b')                             % Plottar punkten T:s rörelse
grid on                                     % Aktiverar rutnät
axis equal                                  % Egenskaper hos axlar
title('Punktens T:s rörelse i xy-planet')   % Sätt figur titel
xlabel('x [m]')                             % Text på y-axel
ylabel('y [m]')                             % Text på x-axel
xlim([-1.5 1.0])                            % Gränsvärden på x-axel
ylim([-0.5 1.0])                            % Gränsvärden på y-axel

% Loop som bestämmer position i vektorn för maxvärde på yT
yTmax = max(yT);
for n=1:length(yT)
    if yTmax == yT(n)
        yTmaxpos = n;
        break;
    end
end
yTmaxalpha = alpha(yTmaxpos);               % Bestämmer för vilken vinkel på alpha yTmax inträffar
% Skriver ut värden på yT för olika vinklar alpha
disp(' ');
disp(['Alpha = ' num2str(alpha(41)*180/pi) ' grader, yT = ' num2str(yT(41)) ' m, xT = ' num2str(xT(41)) ' m.']);

% Skriver ut för vilken vinkel yT har son maxposition
disp(' ');
disp(['Punkten T har sin högsta punkt y = ' num2str(yTmax) ' m vid vinkeln alpha = ' num2str(yTmaxalpha*180/pi) ' grader.']);
% Plottar yT som funktion av alpha och maxvärdet för yT
figure(7)
hold on
plot(alpha/pi,yT,'b')                                                       % Plottar punkten T:s y-koordinat som funktion av alpha
plot(yTmaxalpha*180/pi,yTmax,'r*')                                              % Plottar positionen för yTmax

xTp = gammaT.*((t2/2)*cos(gamma)+L*sin(gamma))-L1*alphaT.*sin(alpha);           % Hastighet i x-led för punkten T
yTp = gammaT.*(L*cos(gamma)-(t2/2)*sin(gamma))+L1*alphaT.*cos(alpha);           % Hastighet i y-led för punkten T
% Acceleration i x-led för punkten T
xTpp = (t2/2)*gammaTT.*cos(gamma)-(t2/2)*gammaT.^2.*sin(gamma)+...
    L*gammaTT.*sin(gamma)+L*gammaT.^2.*cos(gamma)-L1*alphaT^2*cos(alpha);
% Acceleration i y-led för punkten T
yTpp = L1*alphaT.^2.*(-sin(alpha))+L*gammaTT.*cos(gamma)+L*gammaT.^2.*(-sin(gamma))+...
    (t2/2)*gammaTT.*(-sin(gamma))+(t2/2)*gammaT.^2.*(-cos(gamma));
rTp = sqrt((xTp).^2 + (yTp).^2);                                                                % Resultanthastighet för punkten T
rTpp = sqrt((xTpp).^2 + (yTpp).^2);                                                             % Resultantacceleration för punkten T

disp(' ');
disp(['Alpha = ' num2str(alpha(41)*180/pi) ' grader, rTp =num2str(rTpp(41)) ' m/s^2.']);

% Plottar T:s hastigheter och accelerationer som funktion av alpha
figure(8)
subplot(3,2,1)
plot(alpha,xTp,'b')                                                 % Plottar T:s hastighet i x-led som funktion av alph                                                        
title('Punktens T:s hastighet i x-led som funktion av \alpha')      % Figurens titel
ylabel('Hastighet [m/s]')                                           % Text på y-axel                                                      
xlabel('\alpha [rad]')                                              % Text på x-axel
%% SLUT

Såhär kan en dag på KTH se ut...