%%
% Motor reference data taken from data sheet 
% 942D series plaentary (epicycle) metal gearbox (RE 540/1)
% RS 454-0877 12V with 5:1 gear reduction
Ms = 22*9.81/10000;                  % torque in Nm of stalled motor 
Moptimum = 4.6*9.81/10000;           % torque in Nm for highest efficiency 
Ioptimum = 0.55;                        % current in A for highest efficiency 
n0 = 5300;                             % Leerlaufdrehzahl in rpm
Un = 12;                                % nominal voltage in V 
I0 = 0.15;                              % Leerlaufstrom in A

U_Motor_max = Un;                   % Spannungsbegrenzung
I_Motor_max = Ioptimum;                        % Strombegrenzung ,first guess for safety reason   

% therefrom derived values
Km = Moptimum/(Ioptimum-I0);     % motor constant in Nm/A, EMF  

R2 = Un^2 / (2*pi*n0/60*Ms +I0*Un);         % one possible calculation for internal resistance
R1 = Un/(Ioptimum*Ms/Moptimum);  % alternative calculation for internal resistance
R  = (R1+R2)/2;                          % average(Formel nochmal überprüfen)!!!!!!!!!!!
%R  = R1 ;

% inductivity not know --> time constant estimated at approx. 10ms
% L = 0.001*R;     %Henry   
L_spule = 0.015*R;     %Henry , 15ms Zeitkonstant  

% determination of inertia 
ue = 100;         % gear reduction 

m_rotor = 0.1;      % Masse des Rotors in kg
m_welle = 0.05;        % Masse der Spindel in kg

m_rob = 0.2;                % Masse des Rohrs in kg
radius_rob_a = 0.013;       % Außenradius des Rohrs in m
radius_rob_i = 0.012;       % Innenradius des Rohrs in m
laenge_rob = 0.2;         % Länge des Rohrs in m
radius_rob = 0.025;

radius_rotor = 0.01;       % Radius der Rotors in m
radius_welle = 0.004;         % Radius der Spindel in m
radius_seil = 0.001;       % Radius des Seils in m

Ks = radius_welle + radius_seil;                  % Spindelsteigung in m/rad

J_rotor = 0.5*m_rotor*radius_rotor^2;        % Trägheitsmoment des Motors (Formel für Vollzylinde)
J_g = J_rotor*ue^2;                          % getriebeseitiges Trägheitsmoment des Motors, J*ü^2
J_welle = 0.5*m_welle*radius_welle^2;                 % Trägheitsmoment die Spindel
J_rob = 0.25*m_rob*(radius_rob_a^2+radius_rob_i^2+4*laenge_rob^2/3);    % Trägheitsmoment des Arms (Formel für Hohlzylinde)
J_an = J_g + J_welle + J_rob + J_rotor ;                           % Gesamte Trägheitsmoment der Abtriebsseite
%J_gesamt = J_rob + J_g + J_rotor  + J_sp;     

   % Trägheitsmoment des Arms (Formel für Hohlzylinde)
% mechanical constants of robolinks
d1_an = 0.1;                   % Gleitreibungszahl des Antriebs
d2_an = 0.1;                      % Trockenes Reibmoment des Antriebs
d3_an = 0.5*d2_an;            % Stribeck-Reibmoment des Antriebs
Cv_an = 0.4;                      % Koeffitient der Stribeck-Reibung des Antriebs

%c = 100000;                       % Federkonstant
c_feder = 1000;                       % Federkonstant

radius_rob = 0.025;               % Radius im Gelenk

d1_rob = 0.1;                  % Gleitreibungszahl des Gelenkarms
d2_rob = 0.1;                     % Trockenes Reibmoment des Gelenkarms
d3_rob = 0.5*d2_rob;          % Stribeck Reibmoment des Gelenkarms
Cv_rob = 0.4;                     % Koeffitient der Stribeck-Reibung des Gelenkarms

Tscurrent = 0.002;                 % sampling time current controller 10ms
Ts = 0.01;                        % sampling time velocity controller 20ms