%   Script IntercMotors.m
%
%   Behaviour of the interconnected motors
%
% J. Lunze
% 20.1.2022
% Version of 24.4.2022
%
%   
clear
close all

%%     Motor parameters
%    www.maccon.de/fileadmin/redaktion/downloads/Produkte/Servomotoren/dc_motoren/
%    for use in a "Quad"
%    Motor M4-2954   115 V, 1119 W, 11.1 Nm constant torque; 52.9 Nm peak torque
%                   for 15.1 A constant current; 90 A peak current
R1=0.481;      % Ohm   winding resistance
L1=0.0031;    % H     3.1 mH  winding inductance
kT1=0.736;     % Nm/A = Vs/rad   torque constant  77.1 V/krpm = 0.7363 Vs/rad
J1=3.36;      % kg m^2  moment of inertia of a wheel (cf. NCS, Appl.Studies)
kL1=0.096;     % Nms/rad  determined from steady-state torque=11.1 Nm for 1500 rpm
A1=[-R1/L1 -kT1/L1; kT1/J1 -kL1/J1];
G1=[0; 1/J1];  % disturbance input
E1=[1/L1; 0];  % coupling input  in V
C1=[0 1];      % output = speed in rad/s
Cz1=[1 0];     % coupling output in A
n1=2;
Motor1=ss(A1, E1, C1, 0);  % input: nominal voltage V1
%
alpha=1.0;      % factor to change the parameters of Motor 1 to get 
                % the parameters of Motor 2
R2=0.481*alpha;       % Ohm   
L2=0.0031;    % H     
kT2=0.736;     % Nm/A   
J2=3.36;   % kg m^2  
kL2=0.096;   % Nm^3/rad  
A2=[-R2/L2 -kT2/L2; kT2/J2 -kL2/J2];
G2=[0; 1/J2];
E2=[1/L2; 0];
C2=[0 1];      % output = speed in rad/s
Cz2=[1 0];     % coupling output
n2=2;
Motor2=ss(A2, E2, C2, 0);  % input: nominal voltage V1
%
RB=0.1;         % 0.3 Ohm
Ubar=115;        % Volt  power supply, nominal voltage
%
%       transformation of rad/s in rpm by multiplying with factor 
ninrpm=60/2/pi;
%
%   Overall model
L=[0 0 1; 0 0 1; 1 1 0];

%%  Step response of a motor
%   
figure(1)
Tend=15;
dt=0.005;
Time=0:dt:Tend;
kend=length(Time);
[Y,Time,X]=step(Motor1,Time);
%
subplot(4,4,[1 2 3 5 6 7])
plot(Time, Ubar*X(:,1), 'b');        %  multiplication with Ubar to 
                                     %  scale for the supply voltage
axis([0 Tend 0 250]);
latexylabel('$$I_1$$ in A');
rotateY;
xleer;
xticksempty([0 5 10 15 20]);
yticks([0 100 200]);
latextitle('$${\tt IntercMotors:}$$ Step response of a motor');
%
subplot(4,4,[4 8])
plot(Time, Ubar*X(:,1), 'b');
axis([0 0.2 0 250]);
%yticks([0 100 200]);
yleer;
xticks([0 0.1 0.2]);
%
subplot(4,4,[9 10 11 13 14 15])
plot(Time, ninrpm*Ubar*X(:,2), 'b');
axis([0 Tend 0 1500]);
latexylabel('$$\omega_1$$ in rpm');
rotateY;
latexxlabel('$$t$$ in s');
xticks([0 5 10 15 20]);
yticks([0 500 1000 1500]);


%%  Overall system
%
figure(2)
Abar=[A1-E1*[RB 0] -E1*[RB 0];
      -E2*[RB 0]  A2-E2*[RB 0]];
Bbar=[E1; E2];
Gbar=[G1 zeros(n1,1); zeros(n2,1) G2];
Bud=[Bbar Gbar];    % inputs: u, d1, d2
Cbar=[0 1 0 0;
      0 0 0 1];
CoupledMotors=ss(Abar, Bbar, Cbar, 0);
[Y,Time,X]=step(CoupledMotors,Time);
%
subplot(4,4,[1 2 3 5 6 7])
plot(Time, Ubar*X(:,1), 'b', Time, Ubar*X(:,3), 'b--');
axis([0 Tend 0 210]);
latexylabel('$$I_1$$ in A');
rotateY;
xleer;
xticksempty([0 5 10 15 20]);
yticks([0 100 200]);
latextitle('$${\tt IntercMotors:}$$ Step response of two coupled motors');
hold on
%
subplot(4,4,[4 8])
plot(Time, Ubar*X(:,1), 'b');
axis([0 0.2 0 210]);
%yticks([0 100 200]);
yleer
xticks([0 0.1 0.2]);
hold on
%
subplot(4,4,[9 10 11 13 14 15])
plot(Time, ninrpm*Ubar*X(:,2), 'b');
axis([0 Tend 0 1500]);
latexylabel('$$\omega_1$$ in rpm');
rotateY;
latexxlabel('$$t$$ in s');
xticks([0 5 10 15 20]);
yticks([0 500 1000 1500]);
hold on

%%  Simplified model
%
figure(2)
denom=1/R1+1/R2+1/RB;
Asimpl=[-1/J1*(kL1 + kT1*kT1/R1 - kT1*kT1/denom/R1/R1) kT1*kT1/J1/denom/R1/R2;
        kT2*kT2/J2/denom/R1/R2  -1/J2*(kL2+kT2*kT2/R2-kT2*kT2/denom/R2/R2)];
Bsimpl=[kT1/J1/R1/RB/denom; kT2/J2/R2/RB/denom];  % input u(t)
Gsimpl=[-1/J1 0; 0 -1/J2];                  % input d_1, d_2
Budsimpl=[Bsimpl Gsimpl];            % inputs u, d1, d2
Csimpl=eye(2,2);
CI=[kT1/R1*(1/denom/R1-1) kT1/denom/R1/R2; kT2/denom/R1/R2  kT2/R2*(1/denom/R2-1)];  % outputs: I_1 I_2
FI=[1/denom/R1/RB; 1/denom/R1/RB];          % feedthrough u -> I1, I2
SimplMotors=ss(Asimpl, Bsimpl, Csimpl, 0);
[Y,Time,X]=step(SimplMotors,Time);
%
subplot(4,4,[9 10 11 13 14 15])
plot(Time, ninrpm*Ubar*X(:,1), 'b--');
hold off
%
subplot(4,4,[1 2 3 5 6 7])
SimplMotorsI=ss(Asimpl, Bsimpl, CI, FI);
Y=step(SimplMotorsI,Time);
plot(Time, Ubar*Y(:,1), 'b--');
hold off
%
subplot(4,4,[4 8])
plot(Time, Ubar*Y(:,1), 'r--');
axis([0 0.2 0 210]);
%yticks([0 100 200]);
yleer;
xticks([0 0.1 0.2]);
hold off

%%  Behaviour of the disturbed motor
%
figure(3)
%   Input voltage u(t) reaches max on a ramp
T2=0.3;
k2=ceil(T2/dt);
U=[];
for k1=1:k2
    U(k1)=Ubar/k2*k1;
end
for k1=k2+1:kend
    U(k1)=Ubar;
end
%  Disturbance d1(t) on the first motor with magnitude dbar
dbar=-20;   % Nm
T3=5;
T4=7;
k3=ceil(T3/dt);
k4=ceil(T4/dt);
D1=zeros(1,kend);
for k1=k3:k4
    D1(k1)=dbar;
end
D2=zeros(1,kend);
CoupledMotorsud=ss(Abar, Bud, Cbar, 0);
[Y,Time,X]=lsim(CoupledMotorsud, [U;D1;D2], Time);
%
subplot(4,4,[1 2 3 5 6 7])
fill([k3*dt k4*dt k4*dt k3*dt k3*dt],[-0.2 -0.2 Ubar*2.6 Ubar*2.6 -0.2],...
        [0.9 0.9 0.9], 'EdgeColor', 'none');
hold on
plot(Time, X(:,1), 'b');
axis([0 Tend 0 210]);
latexylabel('$$I_1$$ in A');
rotateY;
xleer;
xticksempty([0 5 10 15 20]);
yticks([0 100 200]);
latextitle('$${\tt IntercMotors:}$$ Disturbance behaviour of two coupled motors');
%
% subplot(4,4,[4 8])
% plot(Time, X(:,1), 'b');
% axis([0 0.2 0 210]);
% %yticks([0 100 200]);
% yleer
% xticks([0 0.1 0.2]);
% hold on
%
subplot(4,4,[9 10 11 13 14 15])
fill([k3*dt k4*dt k4*dt k3*dt k3*dt],[-0.2 -0.2 1600 1600 -0.2],...
        [0.9 0.9 0.9], 'EdgeColor', 'none');
hold on
plot(Time, ninrpm*X(:,2), 'b', Time, ninrpm*X(:,4), 'b--');
axis([0 Tend 0 1500]);
latexylabel('$$\omega_1, \omega_2$$ in rpm');
rotateY;
latexxlabel('$$t$$ in s');
xticks([0 5 10 15 20]);
yticks([0 500 1000 1500]);
latextext(6,1250, '$$\omega_2$$');
latextext(6,850, '$$\omega_1$$');
%
subplot(4,4,[12 16])
fill([k3*dt k4*dt k4*dt k3*dt k3*dt],[-0.2 -0.2 1600 1600 -0.2],...
        [0.9 0.9 0.9], 'EdgeColor', 'none');
hold on
plot(Time, ninrpm*X(:,2), 'b', Time, ninrpm*X(:,4), 'b--');
axis([4.5 15 950 1300]);
latexxlabel('$$t$$ in s');
xticks([5 10 15]);
yleer
%
%    Comparison with the simplified model
CoupledMotorsudsimpl=ss(Abar, Bud, Cbar, 0);
[Y,Time,X]=lsim(CoupledMotorsudsimpl, [U;D1;D2], Time);
%
subplot(4,4,[1 2 3 5 6 7])
plot(Time, X(:,1), 'r--');
hold off
%
% subplot(4,4,[4 8])
% plot(Time, X(:,1), 'r--');
% hold off
%   Comparison of the disturbed system with the undisturbed system
D1=zeros(1,kend);
[Y,Time,X]=lsim(CoupledMotorsud, [U;D1;D2], Time);
subplot(4,4,[12 16])
plot(Time, ninrpm*X(:,2), 'r');
hold off

%%  EPS figures
%
epsfigc15('IntercMotors');