daily pastebin goal
73%
SHARE
TWEET

Untitled

a guest Jan 17th, 2019 72 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. clear all
  2.  
  3. % Parametri mehanizma
  4.   m1 = 0.066; % Masa clena 1 [kg]
  5.   m2 = 0.036; % Masa clena 2 [kg]
  6.  
  7.   l1 = 0.095;  % Dolzina clena 1 [m]
  8.   l2 = 0.110;  % Dolzina clena 2 [m]
  9.   lc1 = 0.0395; % Razdalja od KS 0 do masnega centra c1 [m]
  10.   lc2 = 0.0237; % Razdalja od KS 1 do masnega centra c2 [m]
  11.  
  12.   I1 = 11.78e-5; % Vztrajnost clena 1 v masnem centru, okoli z osi (poravnan s sklepno z osjo) [kgm^2]
  13.   I2 = 4.14e-5; % Vztrajnost clena 2 v masnem centru, okoli z osi (poravnan s sklepno z osjo) [kgm^2]
  14.  
  15. % Parametri gonila
  16.   p = 3.75; % Prestava
  17.   P = p*[1 0;1 1]; % Prestavna matrika
  18.   Jg = (4.5e-7); % Vztrajnost gonila preslikana na stran motorjev [kgm^2]
  19.   iP=inv(P); %Inverz prestavne matrike
  20. % Parametri motorjev
  21.   Jm=17.6e-7 + Jg; % Vztrajnost rotorja okoli lastne osi skupaj z gonilom [kgm^2]
  22.   Bm=1.41e-5; % Viskozno dusenje v lezajih motorja [Nms]
  23.  
  24. % Parametri bremena (masni delec, brez rotacijske inercije)
  25.  
  26.   lb = 0.01; % dolzina od KS 2 do masnega centra bremena [m]
  27.  
  28.  
  29.  
  30. % Zacetni pogoji (polozaj in hitrost)
  31.   q0 = [0 0];  % [rad]
  32.   qd0 = [0 0]; % [rad/s]
  33.  
  34. %% Regulacijski parametri
  35.  
  36. % konstantni del masne vztrajnostne matrike (izvendiagonalni elementi so
  37. % zanemarjeni)
  38.   mc_11 = m1*lc1*lc1 + m2*(l1*l1+lc2*lc2) + I1 + I2;
  39.   mc_22 = m2*lc2*lc2 + I2;
  40.  
  41. % Nominalni vztrajnostni moment aktivnega mehanizma (preslikan na rotor motorja)
  42.   Jnoml1=(mc_11 + mc_22)/p^2;
  43.   Jnoml2=mc_22/p^2;
  44.  
  45. % Ojačanja osnega regulatorja 1
  46.  
  47.   J1=Jnoml1 + Jm;
  48.   % parameter opazovalnika motenj
  49.   g1=1*100; % mejna frekvenca [rad/s]
  50.  
  51. % Ojačanja osnega regulatorja 2
  52.  
  53.   J2=Jnoml2 + Jm;
  54.   % parameter opazovalnika motenj
  55.   g2=1*100; % mejna frekvenca [rad/s]
  56.  
  57.  
  58. % Referencne vrednosti za regulator (generator ref. trajektorij)
  59.   q1r = 90*pi/180;   % Zeljena kooridnata 1. sklepa
  60.   q2r = 90*pi/180;   % Zeljena kooridnata 2. sklepa
  61.  
  62.   % ...za regulacijo...
  63.   phi1r = 1*q1r*p;
  64.   phi2r = 1*(q1r + q2r)*p;
  65.  
  66.   % ...za sledenje (generator ref. trajektorije)...
  67.   dir1 = 0*sign(q1r-q0(1));
  68.   dir2 = 0*sign(q2r-q0(2));
  69.   Vmax = sqrt(q1r*q1r+q2r*q2r)/2;
  70.   Amax = Vmax/0.5;
  71.  
  72. %% Simulacija
  73.  
  74. Tstep = 0.001; % Korak racunanja [s]
  75. T = Tstep;
  76. %% Animacija
  77.  
  78. global scaraLinkWidth
  79. scaraLinkWidth = 5;
  80.  
  81. global Tanim
  82. Tanim  = Tstep*8;
  83.  
  84. global Xmin;
  85. global Xmax;
  86. global Ymin;
  87. global Ymax;
  88.  
  89. Xmin = -25; % cm
  90. Xmax = 25;  % cm
  91. Ymin = -25; % cm
  92. Ymax = 25;  % cm
  93.  
  94. %% Izris rezultatov
  95.  
  96. % Brez bremena, brez opazovalnika, horizontalno
  97. %{
  98.  mb = 0*0.03; % Masa bremena [kg]
  99.  kp1=100;
  100.  kv1=20;
  101.  %kp1=1000;
  102.  %kv1=80;
  103.  kp2=100;
  104.  kv2=20;
  105.  %kp2=1000;
  106.  %kv2=100;
  107.  en=0; % vklop/izklop opazovalnika
  108.    
  109.     % Gravitacijski pospesek
  110.   G = 0*9.81;   % [m/s2]
  111.   sim('SCARA_PDaxisctrl');
  112.  
  113.   figure();
  114.   subplot(3,1,1);
  115.   plot(data_qr.Time, data_qr.Data(:,1));
  116.   hold on;
  117.   plot(data_q_tao.Time, data_q_tao.Data(:,1));
  118.   ylabel('Sklepni polozaj [rad]');
  119.   legend('q1r','q1');
  120.  
  121.   subplot(3,1,2);
  122.   plot(data_qr.Time, data_qr.Data(:,2));
  123.   hold on;
  124.   plot(data_q_tao.Time, data_q_tao.Data(:,2));
  125.   ylabel('Sklepni polozaj [rad]');
  126.   legend('q2r','q2');
  127.  
  128.   subplot(3,1,3);
  129.   plot(data_q_tao.Time, [data_q_tao.Data(:,3), data_q_tao.Data(:,4)]);
  130.   legend('tao 1','tao 2');
  131.   ylabel('Navor [Nm]');
  132.   xlabel('cas [s]');
  133.   %}
  134.  
  135.  
  136. % Z bremenom, brez opazovalnika, horizontalno
  137. %{
  138.  
  139.  mb = 1*0.03; % Masa bremena [kg]
  140.  kp1=100;
  141.  kv1=20;
  142.  kp2=100;
  143.  kv2=20;
  144.  en=0; % vklop/izklop opazovalnika
  145.    
  146.     % Gravitacijski pospesek
  147.   G = 0*9.81;   % [m/s2]
  148.   sim('SCARA_PDaxisctrl');
  149.  
  150.   figure();
  151.   subplot(3,1,1);
  152.   plot(data_qr.Time, data_qr.Data(:,1));
  153.   hold on;
  154.   plot(data_q_tao.Time, data_q_tao.Data(:,1));
  155.   ylabel('Sklepni polozaj [rad]');
  156.   legend('q1r','q1');
  157.  
  158.   subplot(3,1,2);
  159.   plot(data_qr.Time, data_qr.Data(:,2));
  160.   hold on;
  161.   plot(data_q_tao.Time, data_q_tao.Data(:,2));
  162.   ylabel('Sklepni polozaj [rad]');
  163.   legend('q2r','q2');
  164.  
  165.   subplot(3,1,3);
  166.   plot(data_q_tao.Time, [data_q_tao.Data(:,3), data_q_tao.Data(:,4)]);
  167.   legend('tao 1','tao 2');
  168.   ylabel('Navor [Nm]');
  169.   xlabel('cas [s]');
  170.   %}
  171.  
  172.  
  173.   % Z bremenom, brez opazovalnika, horizontalno
  174.  
  175.  mb = 1*0.03; % Masa bremena [kg]
  176.  kp1=100;
  177.  kv1=20;
  178.  kp2=100;
  179.  kv2=20;
  180.  en=0; % vklop/izklop opazovalnika
  181.    
  182.     % Gravitacijski pospesek
  183.   G = 0*9.81;   % [m/s2]
  184.   sim('SCARA_PDaxisctrl');
  185.  
  186.   figure();
  187.   subplot(3,1,1);
  188.   plot(data_qr.Time, data_qr.Data(:,1));
  189.   hold on;
  190.   plot(data_q_tao.Time, data_q_tao.Data(:,1));
  191.   ylabel('Sklepni polozaj [rad]');
  192.   legend('q1r','q1');
  193.  
  194.   subplot(3,1,2);
  195.   plot(data_qr.Time, data_qr.Data(:,2));
  196.   hold on;
  197.   plot(data_q_tao.Time, data_q_tao.Data(:,2));
  198.   ylabel('Sklepni polozaj [rad]');
  199.   legend('q2r','q2');
  200.  
  201.   subplot(3,1,3);
  202.   plot(data_q_tao.Time, [data_q_tao.Data(:,3), data_q_tao.Data(:,4)]);
  203.   legend('tao 1','tao 2');
  204.   ylabel('Navor [Nm]');
  205.   xlabel('cas [s]');
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top