clear all

% Parametri mehanizma
  m1 = 0.066; % Masa clena 1 [kg]
  m2 = 0.036; % Masa clena 2 [kg]

  l1 = 0.095;  % Dolzina clena 1 [m]
  l2 = 0.110;  % Dolzina clena 2 [m]
  lc1 = 0.0395; % Razdalja od KS 0 do masnega centra c1 [m]
  lc2 = 0.0237; % Razdalja od KS 1 do masnega centra c2 [m]

  I1 = 11.78e-5; % Vztrajnost clena 1 v masnem centru, okoli z osi (poravnan s sklepno z osjo) [kgm^2]
  I2 = 4.14e-5; % Vztrajnost clena 2 v masnem centru, okoli z osi (poravnan s sklepno z osjo) [kgm^2]

% Parametri gonila
  p = 3.75; % Prestava
  P = p*[1 0;1 1]; % Prestavna matrika
  Jg = (4.5e-7); % Vztrajnost gonila preslikana na stran motorjev [kgm^2]
  iP=inv(P); %Inverz prestavne matrike
% Parametri motorjev
  Jm=17.6e-7 + Jg; % Vztrajnost rotorja okoli lastne osi skupaj z gonilom [kgm^2]
  Bm=1.41e-5; % Viskozno dusenje v lezajih motorja [Nms]

% Parametri bremena (masni delec, brez rotacijske inercije)

  lb = 0.01; % dolzina od KS 2 do masnega centra bremena [m]



% Zacetni pogoji (polozaj in hitrost)
  q0 = [0 0];  % [rad]
  qd0 = [0 0]; % [rad/s]

%% Regulacijski parametri

% konstantni del masne vztrajnostne matrike (izvendiagonalni elementi so
% zanemarjeni)
  mc_11 = m1*lc1*lc1 + m2*(l1*l1+lc2*lc2) + I1 + I2;
  mc_22 = m2*lc2*lc2 + I2;

% Nominalni vztrajnostni moment aktivnega mehanizma (preslikan na rotor motorja)
  Jnoml1=(mc_11 + mc_22)/p^2;
  Jnoml2=mc_22/p^2;

% Ojačanja osnega regulatorja 1

  J1=Jnoml1 + Jm;
  % parameter opazovalnika motenj
  g1=1*100; % mejna frekvenca [rad/s]

% Ojačanja osnega regulatorja 2

  J2=Jnoml2 + Jm;
  % parameter opazovalnika motenj
  g2=1*100; % mejna frekvenca [rad/s]


% Referencne vrednosti za regulator (generator ref. trajektorij)
  q1r = 90*pi/180;   % Zeljena kooridnata 1. sklepa
  q2r = 90*pi/180;   % Zeljena kooridnata 2. sklepa

  % ...za regulacijo...
  phi1r = 1*q1r*p;
  phi2r = 1*(q1r + q2r)*p;

  % ...za sledenje (generator ref. trajektorije)...
  dir1 = 0*sign(q1r-q0(1));
  dir2 = 0*sign(q2r-q0(2));
  Vmax = sqrt(q1r*q1r+q2r*q2r)/2;
  Amax = Vmax/0.5;

%% Simulacija

Tstep = 0.001; % Korak racunanja [s]
T = Tstep;
%% Animacija

global scaraLinkWidth
scaraLinkWidth = 5;

global Tanim
Tanim  = Tstep*8;

global Xmin;
global Xmax;
global Ymin;
global Ymax;

Xmin = -25; % cm
Xmax = 25;  % cm
Ymin = -25; % cm
Ymax = 25;  % cm

%% Izris rezultatov

% Brez bremena, brez opazovalnika, horizontalno
%{
 mb = 0*0.03; % Masa bremena [kg]
 kp1=100;
 kv1=20;
 %kp1=1000;
 %kv1=80;
 kp2=100;
 kv2=20;
 %kp2=1000;
 %kv2=100;
 en=0; % vklop/izklop opazovalnika

    % Gravitacijski pospesek
  G = 0*9.81;   % [m/s2]
  sim('SCARA_PDaxisctrl');

  figure();
  subplot(3,1,1);
  plot(data_qr.Time, data_qr.Data(:,1));
  hold on;
  plot(data_q_tao.Time, data_q_tao.Data(:,1));
  ylabel('Sklepni polozaj [rad]');
  legend('q1r','q1');

  subplot(3,1,2);
  plot(data_qr.Time, data_qr.Data(:,2));
  hold on;
  plot(data_q_tao.Time, data_q_tao.Data(:,2));
  ylabel('Sklepni polozaj [rad]');
  legend('q2r','q2');

  subplot(3,1,3);
  plot(data_q_tao.Time, [data_q_tao.Data(:,3), data_q_tao.Data(:,4)]);
  legend('tao 1','tao 2');
  ylabel('Navor [Nm]');
  xlabel('cas [s]');
  %}


% Z bremenom, brez opazovalnika, horizontalno
%{

 mb = 1*0.03; % Masa bremena [kg]
 kp1=100;
 kv1=20;
 kp2=100;
 kv2=20;
 en=0; % vklop/izklop opazovalnika

    % Gravitacijski pospesek
  G = 0*9.81;   % [m/s2]
  sim('SCARA_PDaxisctrl');

  figure();
  subplot(3,1,1);
  plot(data_qr.Time, data_qr.Data(:,1));
  hold on;
  plot(data_q_tao.Time, data_q_tao.Data(:,1));
  ylabel('Sklepni polozaj [rad]');
  legend('q1r','q1');

  subplot(3,1,2);
  plot(data_qr.Time, data_qr.Data(:,2));
  hold on;
  plot(data_q_tao.Time, data_q_tao.Data(:,2));
  ylabel('Sklepni polozaj [rad]');
  legend('q2r','q2');

  subplot(3,1,3);
  plot(data_q_tao.Time, [data_q_tao.Data(:,3), data_q_tao.Data(:,4)]);
  legend('tao 1','tao 2');
  ylabel('Navor [Nm]');
  xlabel('cas [s]');
  %}


  % Z bremenom, brez opazovalnika, horizontalno

 mb = 1*0.03; % Masa bremena [kg]
 kp1=100;
 kv1=20;
 kp2=100;
 kv2=20;
 en=0; % vklop/izklop opazovalnika

    % Gravitacijski pospesek
  G = 0*9.81;   % [m/s2]
  sim('SCARA_PDaxisctrl');

  figure();
  subplot(3,1,1);
  plot(data_qr.Time, data_qr.Data(:,1));
  hold on;
  plot(data_q_tao.Time, data_q_tao.Data(:,1));
  ylabel('Sklepni polozaj [rad]');
  legend('q1r','q1');

  subplot(3,1,2);
  plot(data_qr.Time, data_qr.Data(:,2));
  hold on;
  plot(data_q_tao.Time, data_q_tao.Data(:,2));
  ylabel('Sklepni polozaj [rad]');
  legend('q2r','q2');

  subplot(3,1,3);
  plot(data_q_tao.Time, [data_q_tao.Data(:,3), data_q_tao.Data(:,4)]);
  legend('tao 1','tao 2');
  ylabel('Navor [Nm]');
  xlabel('cas [s]');