% PARAMROB.M parametri robota za vezbu dinamickog skliranja trajektorije

% motor od 80 W
Nom_volt(1)=24; % [V] 2
No_load_speed=2850; % [o/min] 3
Stall_torque=2200; % [mNm] 4
Term_resist(1)=0.885; % [Ohm] 8
Torq_const(1)=78.3; % [mNm/A] 14
Speed_const(1)=122; % [o/min/V] 15
Mech_time_const(1)=16.5; % [ms] 16
Rot_inert(1)=1190; % [g*cm^2] 17

% motor od 250 W
Nom_volt(1)=24; % [V] 2
No_load_speed=2770; % [o/min] 3
Stall_torque=10.9e3; % [mNm] 4
Term_resist(1)=0.176; % [Ohm] 8
Torq_const(1)=79.9; % [mNm/A] 14
Speed_const(1)=119; % [o/min/V] 15
Mech_time_const(1)=4.03; % [ms] 16
Rot_inert(1)=1460; % [g*cm^2] 17

Reduct(1)=100;
B(1)=0; % [Nm/rad/s]

Nom_volt(2)=Nom_volt(1);
No_load_speed(2)=No_load_speed(1);
Stall_torque(2)=Stall_torque(1);
Term_resist(2)=Term_resist(1);
Torq_const(2)=Torq_const(1);
Speed_const(2)=Speed_const(1);
Rot_inert(2)=Rot_inert(1);
Reduct(2)=Reduct(1);
B(2)=B(1);
Mech_time_const(2)=Mech_time_const(1);

U=[];
Rr=[];
Kem=[];
Kme=[];
Jm=[];
N=[];
Tem=[];
Mmax=[];
nmax=[];

for ii=1:numcols(Nom_volt)
U=[U, Nom_volt(ii)]; % nominalni napon rotora [V]
Rr=[Rr Term_resist(ii)]; % otpornost rotora [Ohm]
Kem=[Kem 1e-3*Torq_const(ii)]; % elektromehanicka const. motora Kem [Nm/A]
Kme=[Kme 30/(pi*Speed_const(ii))]; % mehanickoelektricna const. motora Kme [V/rad/s]
Jm=[Jm 1e-7*Rot_inert(ii)]; % moment inercije rotora [kg*m^2]
N=[N Reduct(ii)]; % redukcioni odnos primenjenog reduktora
Tem=[Tem 1000*Jm(ii)*Rr(ii)/( Kem(ii)*Kme(ii) )]; % elektromehanicka const. motora [ms]
Mmax=[Mmax 1e-3*Stall_torque(ii)]; % max moment motora pri nultoj brzini [Nm]
nmax=[nmax (pi/30)*No_load_speed(ii)]; % max brzina pri nultom momentu [rad/s]
end

Mmax=Mmax.*N; % svodjenje na stranu segmenta (iza reduktora)
nmax=nmax./N; % svodjenje na stranu segmenta (iza reduktora)

l1=0.75;
l2=0.75;
m1=40;
m2=20;

% alfa a theta d sigma m rx ry rz Ixx Iyy Izz Ixy Iyz Ixz Jm G B Tc+ Tc-
ee2rob=[ 0 l1 0 0 0 m1 -l1/2 0 0 0 m1*l1*l1/12 m1*l1*l1/12 0 0 0 Jm(1) N(1) B(1) 0 0
0 l2 0 0 0 m2 -l2/2 0 0 0 m2*l2*l2/12 m2*l2*l2/12 0 0 0 Jm(2) N(2) B(2) 0 0];

% Rr Kem Kme Jm N B U
ee2acd=[Rr(1) Kem(1) Kme(1) Jm(1) N(1) B(1) U(1)
Rr(2) Kem(2) Kme(2) Jm(2) N(2) B(2) U(2)];

l=l1;
g=9.81;