SSY047 Simulering Matlab Kod

matlabkod.m

% Denna fil innehåller en dynamisk modell av det styrda gardinsystemet och även en simulering som bygger på Forward Euler metoden.
clear variables;

%% Definiera konstanter och parametrar
g = 9.82;                   % [m/s^2] gravitationskonstant
m_1 = 1;                    % [kg] massa hos "främre" gardin
m_2 = 1;                    % [kg] massa hos "bortre" gardin
u = 0.4;                    % [1] friktion hos gardinskena
R_a = 8.71;                 % [ohm] motorns ankarresistans
k_t = 14.5*10^(-3);         % [Nm/A] motorns momentkonstant (lambda)
L_a = 200*10^(-6);          % [H] motorns induktans
J_tot = 8.76*10^(-7);       % [kg*m^2] totala tröghetsmomentet
k_utv = 134;                % [1] växellådans utväxling
r_drev = 0.03;              % [m] drevets radie
Emodul = 5.75*10^9;         % [N/m^2] Kulsnörets E-modul
r_snre = 0.001;             % [m] kulsnörets radie
n_drev = 0.8;               % [1] drevets verkningsgrad
k_f_vinkel = 0.06*10^(-3);  % [1] förlustkonstant i vinkelväxel
T_0_vinkel = 52.8*10^(-3);  % [Nm] konstant förlustmoment i vinkelväxel
n_koppling = 0.9;           % [1] kopplingens verkningsgrad
k_f_vaxel = 0.2*10^(-3);    % [1] förlustkonstant i växellåda
T_0_vaxel = 220*10^(-3);    % [Nm] konstant förlustmoment i växellåda

L_fnstr = 1.2;              % [m] avstånd mellan dreven (cc)
L_0 = L_fnstr / 2;          % [m] kulsnörets start-avstånd från drev till främre gardin
L_1 = L_fnstr + (2 * r_drev * pi / 2); % [m] kulsnörets längd mellan främre och bakre gardin

c_ks = 12;                  % [1] dämpningskonstant hos kulsnöret
c_minlangd = 0.01;          % [m] minsta tillåtna längd för kulsnöret

% Simulationskonfiguration
t_tot = 4;                  % [s] hur många sekunder som ska simuleras
dt = 0.00001;               % [s] tidssteget i sekunder

% Hjälp-parametrar
N = round(t_tot / dt);      % antal steg att simulera avrundat till heltal

A_snre = r_snre ^ 2 * pi;   % [m^2] kulsnörets tvärsnittsarea
funk_k_snre = @(L)(Emodul * A_snre / L); % Funktion för uträkning av fjäderkonstant för given längd

funk_radtorpm = @(rad)(60 * rad / (2 * pi));   % Funktion för rad/s -> RPM

%% Val av tillstånd, och tilldelning av deras startvärden


% Definera vektorer att spara tillstånden i
x_1 = zeros(1,N);       % främre gardinens tillryggalagda sträcka
x = zeros(1,N);         % drevets tillryggalagda sträcka
x_2 = zeros(1,N);       % bortre gardinens tillryggalagda sträcka
ddt_x_1 = zeros(1,N);   % främre gardinens hastighet
ddt_x = zeros(1,N);     % drevets hastighet
ddt_x_2 = zeros(1,N);   % bortre gardinens hastighet
delta_L_1 = zeros(1,N); % förlängning av kulsnörets främre del
delta_L_2 = zeros(1,N); % förlängning av kulsnörets bakre del
i_a = zeros(1,N);       % motorströmmen
w_motor = zeros(1,N);   % motorns varvtal
w_drev = zeros(1,N);    % drevets varvtal
T_dev = zeros(1,N);     % motorns utvecklade moment
T_last = zeros(1,N);    % momentet som drivlinan ger på motorn
F_1 = zeros(1,N);       % kraft i främre kulsnöret
F_2 = zeros(1,N);       % kraft i bakre kulsnöret
k_1 = zeros(1,N);       % främre kulsnörets fjäderkonstant
k_2 = zeros(1,N);       % bakre kulsnörets fjäderkonstant

abs_x_1 = zeros(1,N);   % absoluta positionen (avstånd till kontrollenheten)
abs_x_2 = zeros(1,N);   %


%% Definera insignalen

% Konfiguration
U = 9.6;                     % [V] vald inspänning

t_u_pa = 0.2;                % [s] tiden då spänningen slås på
t_u_av = 2;                  % [s] tiden då spänningen slås av
t_u_a_inc = 0.3;             % [t] Hur lång tid spänningen ska öka över
t_u_a_dec = 0;               % [t] Hur lång tid spänningen ska sänkas över

% Variabler
u_a = ones(1,N) * U;         % [V] huvud-signal arrayen
inc_step_start = round(t_u_pa / dt);
dec_step_start = round(t_u_av / dt);

% Noll spänning till först
u_a(1:inc_step_start) = 0;

% Linjär spänningsökning
inc_steps = round(t_u_a_inc / dt);          % Antal steg som behövs
inc_linearstep = linspace(0, U, inc_steps); % Linjär stegning från 0 -> U volt
inc_step_end = inc_step_start + inc_steps - 1;

u_a(inc_step_start:inc_step_end) = inc_linearstep; % "Klistra in" den linjära stegningen i spänningsarrayen

% Linjär spänningssänkning
dec_steps = round(t_u_a_dec / dt);
dec_linearstep = linspace(U, 0, dec_steps);
dec_step_end = dec_step_start + dec_steps - 1;

u_a(dec_step_start:dec_step_end) = dec_linearstep;

% Noll spänning i slutet
u_a((dec_step_end+1):N) = 0;


%% Simulera den dynamiska modellen med "forward Euler"-metoden

for t = 1:N-1
    % Absoluta positioner
    abs_x_1(t) = L_0 - x_1(t);
    abs_x_2(t) = L_0 + L_1 - x_2(t); %används ej

    % Bestämma strömmen orsakad av u_a
    ddt_i_a = (u_a(t) - R_a * i_a(t) - k_t * w_motor(t))/L_a;

    % Snörenas förlänging
    delta_L_1(t) = x(t) - x_1(t);
    delta_L_2(t) = x_1(t) - x_2(t);

    % Snörenas riktiga längd
    k_1_L = abs(abs_x_1(t));
    k_1_L = max(k_1_L, c_minlangd); % Måste begränsa denna annars går k -> oändligheten
    k_1(t) = funk_k_snre(k_1_L);

    k_2_L = L_1;      % Snöret mellan gardinerna har konstant längd
    k_2(t) = funk_k_snre(k_2_L);

    % Snörenas fjäderkraft
    F_1(t) = k_1(t) * delta_L_1(t);
    F_2(t) = k_2(t) * delta_L_2(t);

    %
    % Momenten från drev till motorns last
    %
    % Uträkning av T last
    T_drev       = (F_1(t) * r_drev)/n_drev;

    %b_T_0_vinkel = min(T_drev, T_0_vinkel);
    b_T_0_vinkel = T_0_vinkel * sign(w_motor(t));
    T_vinkel     = (1+k_f_vinkel)*T_drev + b_T_0_vinkel;

    T_koppling   = T_vinkel/n_koppling;

    %b_T_0_vaxel  = min(T_koppling, T_0_vaxel);
    b_T_0_vaxel  = T_0_vaxel * sign(w_motor(t));
    T_vaxel      = ((1+k_f_vaxel) * T_koppling + b_T_0_vaxel)/k_utv;

    T_last(t)    = T_vaxel;

    % Uträkning av motorns utvecklade moment
    T_dev(t) = k_t * i_a(t);

    % Uppdatera motorns utvecklade moment
    ddt_w_motor = (T_dev(t) - T_last(t))/J_tot;

    % Bestämma drevets tillryggalagda sträcka
    w_drev(t) = w_motor(t) / k_utv;
    ddt_x(t) = w_drev(t) * r_drev;

    % Friktionen från gardinskenan
    % Sign(hastighet) används för att få rätt rikning på kraften
    F_u1 = m_1 * g * u * sign(ddt_x_1(t));
    F_u2 = m_2 * g * u * sign(ddt_x_2(t));

    % Gardinernas acceleration
    d2dt2_x_1 = (F_1(t) - F_2(t) - F_u1)/m_1 - c_ks*ddt_x_1(t);
    d2dt2_x_2 = (F_2(t) - F_u2)/m_2 - c_ks*ddt_x_2(t);

    % Beräkna tillstånden i nästa tidssteg med hjälp av derivatorna
    x(t+1)       = x(t)       + ddt_x(t)    * dt;
    i_a(t+1)     = i_a(t)     + ddt_i_a     * dt;
    w_motor(t+1) = w_motor(t) + ddt_w_motor * dt;
    ddt_x_1(t+1) = ddt_x_1(t) + d2dt2_x_1   * dt;
    ddt_x_2(t+1) = ddt_x_2(t) + d2dt2_x_2   * dt;
    x_1(t+1)     = x_1(t)     + ddt_x_1(t)  * dt;
    x_2(t+1)     = x_2(t)     + ddt_x_2(t)  * dt;
end


%% Presentation av resultat

% Snygga till sista iterationen (simulations-fel)
x_1(N:end)       = [];
x(N:end)         = [];
x_2(N:end)       = [];
ddt_x_1(N:end)   = [];
ddt_x(N:end)     = [];
ddt_x_2(N:end)   = [];
delta_L_1(N:end) = [];
delta_L_2(N:end) = [];
i_a(N:end)       = [];
u_a(N:end)       = [];
w_motor(N:end)   = [];
w_drev(N:end)    = [];
T_dev(N:end)     = [];
T_last(N:end)    = [];
F_1(N:end)       = [];
F_2(N:end)       = [];
k_1(N:end)       = [];
k_2(N:end)       = [];
abs_x_1(N:end)   = [];
abs_x_2(N:end)   = [];

% Definiera en tidsvektor. Används vid plottning av resultat
T = (1:N-1) * dt;

% Funktion för att plotta spänning-av/på-linjen
plot_u_av = @()line([t_u_av t_u_av t_u_pa t_u_pa], [-9999, 9999, 9999, -9999], 'Color', 'k', 'LineStyle', '--');

% Plotta strömmen
figure(1);
clf;
subplot(2,1,1);
hold on
grid on
plot(T, i_a, 'r');
plot_u_av();
title('Ström över tid');
xlabel('Tid [s]'); ylabel('Ström [A]')
legend('Motorström [A]','Spänning på/av')
axis([0 N*dt -0.8 1.2])

% Plotta spänningen
subplot(2,1,2);
hold on
grid on
plot(T, u_a, 'r');
plot_u_av();
title('Spänning över tid');
xlabel('Tid [s]'); ylabel('Spänning [V]')
legend('Motorspänning [V]','Spänning på/av')
axis([0 N*dt -1 12])

% Plotta hastigheten
figure(2);
clf;
subplot(2,1,1)
hold on
grid on
plot(T, ddt_x_1*100, 'r');
plot(T, ddt_x*100, 'k');
plot(T, ddt_x_2*100, 'b');
plot_u_av();
title('Hastighet över tid');
xlabel('Tid [s]'); ylabel('Hastighet [cm/s]')
legend('Främre gardin', 'Drev', 'Bakre gardin','Spänning på/av')
axis([0 N*dt -4 16])

% Plotta motor hastighet
subplot(2,1,2);
hold on
grid on
plot(T, funk_radtorpm(w_motor), 'r');
plot_u_av();
title('Varvtal över tid');
xlabel('Tid [s]'); ylabel('Varvtal [RPM]')
legend('Motor','Spänning på/av','Location','NorthEast')
axis([0 N*dt -500 5000])

% Plotta position
figure(3)
clf;
hold on
grid on
plot(T, x_1, 'r');
plot(T, x, 'k');
plot(T, x_2, 'b');
plot_u_av();
title('Postion hos systemet');
xlabel('Tid [s]'); ylabel('Sträcka [m]')
legend('Främre gardin', 'Drev', 'Bakre gardin','Spänning på','Location','North')
axis([0 N*dt 0 0.25])



% Plotta förlänging av kulsnöret
figure(4);
clf;
subplot(2,1,1)
hold on
grid on
plot(T, delta_L_1*1000, 'r');
plot(T, delta_L_2*1000, 'b');
plot_u_av();
title('Förlängning av kulsnöret');
xlabel('Tid [s]'); ylabel('Sträcka [mm]')
legend('Främre del av kulsnöret','Bakre del av kulsnöret','Spänning på/av')
axis([0 N*dt -0.3 0.8])

% Plotta fjäderkrafter
subplot(2,1,2)
hold on
grid on
plot(T, F_1, 'r');
plot(T, F_2, 'b');
plot_u_av();
title('Krafter i kulsnöret');
xlabel('Tid [s]'); ylabel('Kraft [N]')
legend('Främre del av kulsnöret','Bakre del av kulsnöret','Spänning på/av','Location','SouthWest')
axis([0 N*dt -5 20])

% Plotta fjäderkonstanter
figure(5);
clf;
hold on
grid on
plot(T, k_1/1000, 'r');
plot(T, k_2/1000, 'b');
plot_u_av();
title('Fjäderkonstanter i kulsnöret');
xlabel('Tid [s]'); ylabel('Fjäderkonstant [N/mm]')
legend('Främre del av kulsnöret','Bakre del av kulsnöret','Spänning på/av','Location','SouthWest')
axis([0 N*dt 0 100])