%      SyncMotor.M
%   
%  Synchronisation of AC motors
%  Dissipative systems
%  
% J. Lunze
% 
% 2.2.2021
% Version v. 9.7.2021
%
echo off
clear
close all

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%   Motor parameters
%
%   EN 42 series, Canon     https://prec.canon/en/products/ironcore.html
%
V1=24;          % V    nominal voltage
omega0=6500;    % rpm  no load speed
omega1=5600;    % rpm  rated speed
torque=0.049;   % Nm   nominal torque: 49 mNm
iAnom=1.7;      % A    nominal current
power=29;       % W    power
%
r=0.02;           % m   20 mm radius of the roll
cRubber=2.8;      % N/m elasticity of the rubber band 0.024 N/mm
%
%   Detailed motor model
RA=2.1;       % Ohm   winding resistance
LA=0.0019;    % H     1.9 mH  winding inductance
kT=0.034;     % Nm/A   torque constant: 33.9 mNm/A
kM=kT;
J=0.000045;   % kg m^2  moment of inertia: 150 g cm^2 + 400 g cm^2
kL=0.00012;   % Nm^3/rad  (adapted so as to get the required speed)
A=[-RA/LA -kM/LA; kT/J -kL/J];
B=[0; r/J];
G=[1/LA; 0];
C=[0 1];      % output = speed in rad/s
n=2;
Motor=ss(A, G, C, 0);  % input: nominal voltage V1
omega2=dcgain(Motor)*V1;  % speed for nominal voltage
%
MotorF=ss(A, B, C, 0);  % input is the force at the roll
kwf=dcgain(MotorF);     % gain from force to speed

%
alpha=1.15;            % change motor1 -> motor2
beta=0.8;             % change motor1 -> motor3
omega(1)=omega2;  % rad/s initial speed
omega(2)=alpha*omega(1);
omega(3)=beta*omega(1);
%

%%  Simplified model of the coupled motors
%
figure(1)
%
Tend=0.5;
%
B1=kwf;        % rad/N/s coefficient between force F and speed
B2=alpha*B1;       
%
%  Two motors coupled bidirectionally
L=[1 -1; -1 1];
Ltilde=r*cRubber*L;
%   xdot = Abar xdot + Gtilde;  x(0) = x0
%   Abar=Atilde - Btilde*L*Ctilde
N1=2;
Atilde=zeros(N1,N1);
Btilde=zeros(N1,N1);
Btilde(1,1)=B1;
Btilde(2,2)=B2;
Ctilde=eye(N1,N1);
Gtilde=omega(1:2)';    % nominal speed of both motors
Abar=Atilde-Btilde*Ltilde*Ctilde;
CoupledMotors=ss(Abar, Gtilde, Ctilde, 0);
x0=[0; 0];             % initial value of theta1 and theta2
%
[Ystep,Time]=step(CoupledMotors, Tend);   % reaction wrt nominal speed
Yfree=initial(CoupledMotors, x0, Time);   % reaction wrt couplings
Y=Ystep+Yfree;
% 
subplot(4,1,[1,2]);
plot(Time, Y, 'b');
latexylabel('$$\theta_1, \theta_2$$ in rad');
rotateY;
yticks([0 100 200 300]);
xticksempty([0 0.1 0.2 0.3 0.4 0.5]);
axis([0 Tend 0 300]);
latextitle('$${\tt SyncMotor:}$$ Simplified motor models');
%
subplot(4,1,[3 4]);
Omega=[];             % Determine the motor speed
for k1=1:length(Y)
    Omega(k1, 1:2)=Gtilde' - (Btilde*Ltilde*Y(k1,1:2)')';
end
plot(Time, Omega, 'b');
latexylabel('$$\omega_1, \omega_2$$ in $$\frac{\rm rad}{\rm s}$$');
rotateY;
latexxlabel('$$t$$ in s');
yticks([580 620 660 700]);
xticks([0 0.1 0.2 0.3 0.4 0.5]);
axis([0 Tend 560 680]);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%  A more detailed motor models
%
figure(2)
TendMotor=0.6;
[Y, Time]=step(Motor,TendMotor);         % time constant: 10 ms
subplot(4,1,[1,2]);
plot(Time, V1*Y, 'b');
latexylabel('$$\omega$$ in in $$\frac{\rm rad}{\rm s}$$');
rotateY;
latexxlabel('$$t$$ in s');
yticks([0 200 400 600]);
xticks([0 0.2 0.4 0.6 0.8 1]);
axis([0 TendMotor 0 600]);
latextitle('$${\tt SyncMotor:}$$ Step response of the motor');


%%  Two motors coupled
%
figure(3)
%   extend motor model to get the angle as output
A1=[A zeros(n,1); C 0];
B1=[B; 0];
C1=[zeros(1,n) 1];
D1=0;
G1=G;
A2=A1;
B2=B1;
C2=C1;
D2=D1;
G2=alpha*G1;
%
%  Two motors coupled bidirectionally
Ltilde=r*cRubber*[1 -1; -1 1];
%   xdot = Abar xdot + Gtilde;  x(0) = x0
%   Abar=Atilde - Btilde*L*Ctilde
N1=2;
Atilde=[A1 zeros(n+1,n+1); zeros(n+1,n+1) A2];
Btilde=[B1 zeros(n+1,1); zeros(n+1,1) B2];
Ctilde=[C1 zeros(1,n+1); zeros(1,n+1) C2];
Dtilde=0;
Gtilde=[G1; 0; G2; 0]*V1;           
Abar=Atilde-Btilde*Ltilde*Ctilde;
CoupledMotors=ss(Abar, Gtilde, Ctilde, 0);
xbar1=-inv(A)*G1*V1;
xbar2=-inv(A)*G2*V1;
x0=[xbar1; 0; xbar2; 0];
%
Tend2=0.5;
[Ystep,Time,Xstep]=step(CoupledMotors, Tend2);
[Yfree,Time,Xfree]=initial(CoupledMotors, x0, Time);
Y=Ystep+Yfree;
X=Xstep+Xfree;
% 
subplot(4,1,[1,2]);
plot(Time, Y, 'b');
latexylabel('$$\theta_1, \theta_2$$ in rad');
rotateY;
yticks([0 100 200 300]);
xticksempty([0 0.1 0.2 0.3 0.4 0.5]);
axis([0 Tend2 0 300]);
latextitle('$${\tt SyncMotor:}$$ Improved motor models');
%
subplot(4,1,[3 4]);
Omega=[];
for k1=1:length(Y)
    Omega(k1,1)=X(k1,2);
    Omega(k1,2)=X(k1,5);
end
plot(Time, Omega, 'b');
latexylabel('$$\omega_1, \omega_2$$ in $$\frac{\rm rad}{\rm s}$$');
rotateY;
latexxlabel('$$t$$ in s');
yticks([580 620 660 700]);
xticks([0 0.1 0.2 0.3 0.4 0.5]);
axis([0 Tend2 560 680]);




%%  Two motors coupled for t \in [t1, t2]
%
figure(4)
t0=0;
t1=0.1;
t2=1.2;
Tend=1.7;
%   uncoupled motors
A01=[A zeros(n,1); C 0];
B01=[B; 0];             % not used
C01=[zeros(1,n) 1];
D01=0;
G01=G;
A02=A1;
B02=B1;
C02=C1;
D02=D1;
G02=alpha*G1;
%
%   xdot = A0 xdot + G0;  x(0) = x0
%   y = C0 x
N1=2;
A0=[A01 zeros(n+1,n+1); zeros(n+1,n+1) A02];
B0=[B01 zeros(n+1,1); zeros(n+1,1) B02];  % not used
C0=[C01 zeros(1,n+1); zeros(1,n+1) C02];
D0=0;
G0=[G01; 0; G02; 0]*V1;           
UncoupledMotors=ss(A0, G0, C0, 0);
xbar1=-inv(A)*G1*V1;
xbar2=-inv(A)*G2*V1;
x0=[xbar1; 0; xbar2; 2];
%
[Ystep,Time,Xstep]=step(UncoupledMotors, t1);
[Yfree,Time,Xfree]=initial(UncoupledMotors, x0, Time);
Y=Ystep+Yfree;
X=Xstep+Xfree;
%   coupled motors for t=t1...t2
[Ystep,Time2,Xstep]=step(CoupledMotors, t2-t1);
[Yfree,Time2,Xfree]=initial(CoupledMotors, X(end,:), Time2);
Y=[Y;Ystep+Yfree];
X=[X;Xstep+Xfree];
Time=[Time; Time2+t1];
%   uncoupled motors for t>t2
[Ystep,Time2,Xstep]=step(UncoupledMotors, Tend-t2);
[Yfree,Time2,Xfree]=initial(UncoupledMotors, X(end,:), Time2);
Y=[Y;Ystep+Yfree];
X=[X;Xstep+Xfree];
Time=[Time; Time2+t2];
% 
subplot(4,1,[1,2]);
fill([t1 t2 t2 t1 t1],[0 0 1100 1100 0],...
        [0.9 0.9 0.9], 'EdgeColor', 'none');
hold on
plot(Time, Y, 'b');
plot([t1 t1],[0 1100],'k:', [t2 t2],[0 1100],'k:');
latexylabel('$$\theta_1, \theta_2$$ in rad');
rotateY;
yticks([0 200 400 600 800 1000 1200]);
xticksempty([0 0.2 0.4 0.6 0.8 1 1.2 1.4]);
axis([0 Tend 0 1100]);
latextitle('$${\tt SyncMotor:}$$ Improved motor models');
hold off
%
subplot(4,1,[3 4]);
Omega=[];
for k1=1:length(Y)
    Omega(k1,1)=X(k1,2);
    Omega(k1,2)=X(k1,5);
end
fill([t1 t2 t2 t1 t1],[0 0 1100 1100 0],...
        [0.9 0.9 0.9], 'EdgeColor', 'none');
hold on
plot(Time, Omega, 'b');
plot([t1 t1],[0 800],'k:', [t2 t2],[0 800],'k:');
latexylabel('$$\omega_1, \omega_2$$ in $$\frac{\rm rad}{\rm s}$$');
rotateY;
latexxlabel('$$t$$ in s');
yticks([580 620 660 700]);
xticks([0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8]);
axis([0 Tend 560 680]);
hold off

%%   Power of the motors
%
figure(5)
%   determine the power as the product V*i_A
%   i_A is the first state variable
plot(Time, X(:,1)*V1, 'b', Time, X(:,4)*V1*alpha, 'r');
hold on
plot(Time, X(:,1)*V1+X(:,4)*V1*alpha, 'g');
hold off



%%  Sinusoidal trajectory of the synchronised motors
%   y_i = sin(theta_i(t))
%
figure(6)
%  Two motors coupled bidirectionally as in figure(3)
%  increased rubber constant to show practical sync
Ltilde=2*r*cRubber*[1 -1; -1 1];      
Abar=Atilde-Btilde*Ltilde*Ctilde;
CoupledMotors=ss(Abar, Gtilde, Ctilde, 0);
%
Tend3=0.7;
dt=0.0001;
Time3=[0:dt:Tend3];
[Ystep,Time2,Xstep]=step(CoupledMotors, Time3);
[Yfree,Time2,Xfree]=initial(CoupledMotors, x0, Time2);
Y=Ystep+Yfree;
X=Xstep+Xfree;
% 
subplot(4,1,[1,2]);
plot(Time3, sin(Y), 'b');
latexylabel('$$\sin\theta_1, \sin\theta_2$$');
rotateY;
yticks([-1 0 1]);
xticks([0.6 0.65 0.7]);
axis([0.6 Tend3 -1.1 1.1]);
latextitle('$${\tt SyncMotor:}$$ Sinusoidal motor outputs');
latexxlabel('$$t$$ in s');


%%  Figures
%
epsfigc15('SyncMotor');

