sf-mx/PBSwissMX
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

wip

Browse Source
This commit is contained in:
zamofing_t
2019-02-11 16:55:58 +01:00
parent 583d86d4d6
commit 3412e33595
8 changed files with 130 additions and 145 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
MXfastStageDoc/MXfastStage.tex
View File

@@ -17,6 +17,7 @@
\usepackage{amsmath}
\renewcommand{\deg}{$^\circ$}
\usepackage[section]{placeins} %place images in section
\usepackage{tcolorbox}
\title{Tuning/modeling fast stages of ESB-MX}
\author{Thierry Zamofing}
@@ -125,7 +126,13 @@ Here a example to roughly calculate at which frequency the motor moves 1um at 2A
A factor 2000 is $1000 \cdot 2 =30dB+3dB=33dB$.
Out of the bode plot we can read approx.:\\
Motor 1: -33dB at 130Hz\\
Motor 2: -33dB at 84Hz
Motor 2: -33dB at 84Hz\\
%n times higher mass $\rightarrow$ n times lower frequency for same amplitude response\\
%n times higher frequency $\rightarrow$ n times higher velocity $\rightarrow$ $n^2$ times more acceleration==current
%1um at 12Hz with 1 mA $\rightarrow$ with 2000mA $\rightarrow$ sqrt(2000)*12Hz=540Hz
%
%A very simplified transfer function of the system is $G(s)=k/s^2$
\subsection{Closed Loop}
@@ -184,12 +191,15 @@ Moving 5um with frequencies from 10 to 220Hz\\
$\rightarrow$ at frequencies above 200 Hz, the current increses up to 2 amps, and the the following error kicks in\\
$\rightarrow$ The closed loop response becomes bad above 20Hz (motor 1 ca. -10\%, motor 2 +5\% )\\
\FloatBarrier
\subsubsection{Friction}
\begin{tcolorbox}[width=15cm,colback=red!5!white,colframe=red!75!black,colbacktitle=red!50,coltitle=black,title=TODO]
Record the friction (=current) at a slow move from +lim to -lim.\\
Analyse the friction depending on the positions and motion directions.\\
Do the records and analysis at different speeds.
\end{tcolorbox}
%→n times higher mass → n times lower frequency for same amplitude response
%→n times higher frequency → n times higher velocity → n² times more acceleration==current
%1um at 12Hz with 1 mA →with 2000mA → sqrt(2000)*12Hz=540Hz
%
%A very simplified transfer function of the system is G(s)=k/s²
\FloatBarrier
\subsubsection{using advanced Deltatau Servo Loop}
@@ -223,10 +233,11 @@ The Value of $K_{fff}$ is used to compensate the non linear static friction. It
$K_{vff}$ is used to compensate the linear viscose friction.\\
\textbf{TODO:}\\
\begin{tcolorbox}[colback=red!5!white,colframe=red!75!black,colbacktitle=red!50,coltitle=black,title=TODO]
Make simulations in MATLAB. Set C/D filter to compensate resonance and the current loop.\\
This sshould be mostly the inverse of the figures: \ref{fig:mot1_chirp} and \ref{fig:mot2_chirp}.\\
Use $K_{fff}$
\end{tcolorbox}
@@ -300,8 +311,7 @@ The inductance of the stage is 2.4 mH.\\
Nevertheless simulations with \verb|current_loop.slx| showed, that the current loop only works in the discrete domain. In continous domain neither the amplification nor the shape mached.\\
Therefore the only approach is to use the second order transfer function as approximated in section \ref{sec:measCurStep}.\\
\textbf{TODO:}
\begin{tcolorbox}[colback=red!5!white,colframe=red!75!black,colbacktitle=red!50,coltitle=black,title=TODO]
A further test will be to 'remove' the current loop. This can be done by setting:$IiGain=0, IpfGain=1, IpbGain=-1$.
The resulting transfer function is:
\[
@@ -314,6 +324,7 @@ The resulting transfer function is:
\\
\]
This is a $PT_1$ element with a time constant of $\frac{L}{R}=\frac{2.4mH}{8.8\Omega}=0.27ms$. But probably due to additional cables etc. the resistance and therefore also the timeconstant is bigger.
\end{tcolorbox}
\subsection{Mechanical model}
@@ -618,10 +629,10 @@ end
\begin{figure}[h!]
\center
\includegraphics[scale=.45]{../matlab/figures/sim_cl_observer_1.eps}
\includegraphics[scale=.45]{../matlab/figures/sim_cl_observer_bode1.eps}\\
\includegraphics[scale=.45]{../matlab/figures/sim_cl_observer_2.eps}
\includegraphics[scale=.45]{../matlab/figures/sim_cl_observer_bode2.eps}
\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_0_1.eps}
\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_bode0_1.eps}\\
\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_0_2.eps}
\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_bode0_2.eps}
\caption{Observer sim: Motor 1 Motor 2}
\label{fig:mot_observer_sim}
\end{figure}
@@ -664,7 +675,8 @@ Finally the real time servo code is compliled for the DeltaTau with:\\
Following lines in gpasciiCommander will activate the user servo loop code:
\verb|TODO...|
\begin{tcolorbox}[width=15cm,colback=red!5!white,colframe=red!75!black,colbacktitle=red!50,coltitle=black,title=TODO]
\end{tcolorbox}
\vspace{1pc}

3
matlab/Readme.md
View File

@@ -6,7 +6,8 @@ Matlab Simulink start
cd ~/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/matlab
module load matlab/2018a
matlab&
or start directly: /afs/psi.ch/sys/psi.x86_64_slp6/Programming/matlab/2018a/bin/matlab&
or start directly:
/afs/psi.ch/sys/psi.x86_64_slp6/Programming/matlab/2018a/bin/matlab&
this works also on my linux computers
```

211
matlab/StateSpaceControlDesign.m
View File

@@ -1,4 +1,4 @@
function [ssc]=StateSpaceControlDesign(mot)
function [ssc]=StateSpaceControlDesign(mot,mode)
% !!! first it need to run: [mot1,mot2]=identifyFxFyStage() to build a motor object !!!
%
% builds a state space controller designed for the plant.
@@ -15,69 +15,81 @@ function [ssc]=StateSpaceControlDesign(mot)
%
% https://www.youtube.com/watch?v=Lax3etc837U
%mPlt: mode to select plant
%mMdl: mode to select model for observer
%0 ss_plt :real plant (model of real plant)
%1 ss_c1 :current, mechanic, no resonance
%2 ss_d1 :simpl. current, mechanic, no resonance
%3 ss_1 :no current, mechanic, no resonance
%4 ss_0 :no current, simpl. mechanic, no resonance
%plant and model
% ss_plt :real plant (model of real plant)
% ss_c1 :current, mechanic, no resonance
% ss_d1 :simpl. current, mechanic, no resonance
% ss_1 :no current, mechanic, no resonance
% ss_0 :no current, simpl. mechanic, no resonance
%mPrefilt:prefilter mode
%prefilt:prefilter mode
%0 no filter
%1 inverse resonance filter
%2 manual setup filter
%mShow: mode(bits) to plot/simulate
% 0: 1: bode plots of open loop
% 1: 2: step answer on open loop
% 2: 4: step answer on closed loop with space state controller
% 3: 8: step answer on closed loop with observer controller
% 4:16: step answer on closed loop with disctrete observer controller
% 5:32: plot all closed loop bode and pole-zero diagrams of desPos->actPos
% 6:64:
%verb: mode(bits) to plot/simulate
% 0: 1: poles of model and placed poles of controller
% 1: 2: bode plots of open loop
% 2: 4: step answer on open loop
% 3: 8: step answer on closed loop with space state controller
% 4: 16: step answer on closed loop with observer controller
% 5: 32: step answer on closed loop with disctrete observer controller
% 6: 64: plot all closed loop bode and pole-zero diagrams of desPos->actPos
% 7:128: bode plot of filt_pos_err
%use_lqr: use lqr instead of pole placement
mPlt=0;
mMdl=1;
mPrefilt=2;
mShow=32+64;
verb=1;
use_lqr=0;
switch mPlt
filt_pos_err=Prefilt(mot,2);
%locate poles: 2500rad/s = 397Hz, 6300rad/s = 1027Hz
switch mode
case -1 %TESTING
ss_plt=mot.ss_plt; %ss_plt ss_c1 ss_d1 ss_1 ss_0
ss_mdl=mot.ss_d1; %ss_plt ss_c1 ss_d1 ss_1 ss_0
if mot.id==1
pl=[-2200 -2100 -2000]; % stable with scaling of .05 .. 1.0;
else
pl=[-2500 -900 -800];
end
plObs=2*pl;
case 0
ss_plt=mot.ss_plt;
ss_plt=mot.ss_plt; %ss_plt ss_c1 ss_d1 ss_1 ss_0
ss_mdl=mot.ss_c1; %ss_plt ss_c1 ss_d1 ss_1 ss_0
if mot.id==1
pl=[-3300 -3200 -2900 -2800];
else
pl=[-3300 -3200 -2700 -2600];
end
plObs=2*pl;
case 1
ss_plt=mot.ss_c1;
ss_plt=mot.ss_plt; %ss_plt ss_c1 ss_d1 ss_1 ss_0
ss_mdl=mot.ss_d1; %ss_plt ss_c1 ss_d1 ss_1 ss_0
if mot.id==1
pl=[-2200 -2100 -2000]; % stable with scaling of .05 .. 1.0;
else
pl=[-2500 -900 -800];
end
plObs=2*pl;
case 2
ss_plt=mot.ss_d1;
case 3
ss_plt=mot.ss_1;
case 4
ss_plt=mot.ss_0;
ss_plt=mot.ss_plt; %ss_plt ss_c1 ss_d1 ss_1 ss_0
ss_mdl=mot.ss_c1; %ss_plt ss_c1 ss_d1 ss_1 ss_0
use_lqr=1
end
ss_plt.Name='open loop plant';
switch mMdl
case 0
ss_mdl=mot.ss_plt;
case 1
ss_mdl=mot.ss_c1;
case 2
ss_mdl=mot.ss_d1;
case 3
ss_mdl=mot.ss_1;
case 4
ss_mdl=mot.ss_0;
end
ss_mdl.Name='open loop model'; %model for observer
[Am,Bm,Cm,Dm]=ssdata(ss_mdl);
if bitand(mShow,1)
if bitand(verb,1) && use_lqr==0
format compact
format shortG
disp(pole(ss_mdl)) %==eig(Am)
%damp(ss_mdl) %further informations
disp(pl)
disp(plObs)
format short
end
if bitand(verb,2)
figure();h=bodeplot(ss_plt,ss_mdl);
setoptions(h,'IOGrouping','all')
end
@@ -85,7 +97,7 @@ function [ssc]=StateSpaceControlDesign(mot)
xp0 = zeros(1,length(ss_plt.A));
xm0 = zeros(1,length(Am));
if bitand(mShow,2)
if bitand(verb,4)
% step answer on open loop:
t = 0:1E-4:.5;
u = ones(size(t));
@@ -94,7 +106,6 @@ function [ssc]=StateSpaceControlDesign(mot)
figure();plot(t,yp,t,ym,'--');title('step on open loop (plant and model)');
legend('plt.iqMeas','plt.iqVolts','plt.actPos','mdl.iqMeas','mdl.iqVolts','mdl.actPos')
end
poles = eig(Am);
%w0=abs(poles);
%ang=angle(-poles);
%-------------------
@@ -104,51 +115,12 @@ function [ssc]=StateSpaceControlDesign(mot)
%
%place poles for the controller feedback
if use_lqr %use the lqr controller
Q=eye(length(ss_mdl.A));
Q=eye(length(Am));
R=1;
[K,P,E]=lqr(ss_mdl,Q,R,0);
[K,P,E]=lqr(Am,Bm,Q,R,0);
else
if mot.id==1
%2500rad/s = 397Hz -> locate poles here
%6300rad/s = 1027Hz -> locate poles here
switch mMdl
case 0
p1=-3300+2800i; p2=-2700+500i; p3=-2500+10i;
P=[p1 p1' p2 p2' p3 p3'];
case 1
%p1=-6300+280i; p2=-6200+150i;
%P=[p1 p1' p2 p2'];
P=[-4100 -4000 -1500+10j -1500-10j];
case 2
%p1=-6300+280i; p2=-6200+150i;
%P=[p1 p1' p2 p2'];
P=[-1500+10j -1500-10j];
case 3
%p1=-6300+280i; p2=-6200+150i;
%P=[p1 p1' p2 p2'];
P=[-1500+10j -1500-10j -1400 -1300];
end
else
%2500rad/s = 397Hz -> locate poles here
%6300rad/s = 1027Hz -> locate poles here
switch mMdl
case 0
p1=-3300+2800i; p2=-1900+130i; p3=-2900+80i;
p4=-2300+450i; p5=-2000+20i; p6=-1500+10i;
P=[p1 p1' p2 p2' p3 p3' p4 p4' p5 p5' p6 p6'];
case 1
%p1=-6300+2800i; p2=-6200+1500i;
%P=[p1 p1' p2 p2'];
P=[-2500 -2800 -1500+10j -1500-10j];
P=[-2500 -2800 -1100+10j -1100-10j];
case 2
%p1=-6300+2800i; p2=-6200+1500i;
%P=[p1 p1' p2 p2'];
P=[-2500 -2800 -1500+10j -1500-10j];
end
end
K = place(Am,Bm,P);
%K = acker(Am,Bm,Pm);
K = place(Am,Bm,pl);
%K = acker(Am,Bm,pl);
end %if lqr
V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
@@ -157,7 +129,7 @@ function [ssc]=StateSpaceControlDesign(mot)
end
ss_cl = ss(Am-Bm*K,Bm*V,Cm,0,'Name','space state controller','InputName',ss_mdl.InputName,'OutputName',ss_mdl.OutputName);
if bitand(mShow,4)
if bitand(verb,8)
% step answer on closed loop with space state controller:
t = 0:1E-4:.5;
[y,t,x]=lsim(ss_cl,V*u,t,xm0);
@@ -166,10 +138,15 @@ function [ssc]=StateSpaceControlDesign(mot)
% *** observer controller ***
%
%observer poles-> 5 times farther left than system poles
OP=2*P;
L=place(Am',Cm',OP)';
%L=acker(A',C',OP)';
%observer poles
if use_lqr %use the lqr controller
Q=eye(length(Am')); % ??????????????? CHANGES NEEDED ????????????
R=eye(size(Cm,1));
[L,P,E]=lqr(Am',Cm',Q,R,0);
else
L=place(Am',Cm',plObs)';
%L=acker(A',C',plObs)';
end
At = [ Am-Bm*K Bm*K
zeros(size(Am)) Am-L*Cm ];
@@ -179,7 +156,7 @@ function [ssc]=StateSpaceControlDesign(mot)
Dt=0;
ss_t = ss(At,Bt,Ct,Dt,'Name','observer controller','InputName',{'desPos'},'OutputName',ss_mdl.OutputName);
if bitand(mShow,8)
if bitand(verb,16)
% step answer on closed loop with observer controller:
figure();lsim(ss_t,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer');
end
@@ -191,20 +168,16 @@ function [ssc]=StateSpaceControlDesign(mot)
[Atz,Btz,Ctz,Dtz]=ssdata(ss_tz );
ss_tz.Name='discrete obsvr ctrl';
if bitand(mShow,16)
if bitand(verb,32)
% step answer on closed loop with disctrete observer controller:
t = 0:Ts:.05;
figure();lsim(ss_tz ,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer discrete');
end
if bitand(mShow,32)
if bitand(verb,64)
%plot all bode diagrams of desPos->actPos
figure();
if mMdl==2 || mMdl==3
idx=1;
else
idx=3;
end
idx=length(ss_cl.OutputName);
h=bodeplot(ss_cl(idx),ss_t(idx),ss_tz(idx));
setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
@@ -221,13 +194,9 @@ function [ssc]=StateSpaceControlDesign(mot)
Bo=[Bm L];
Co=K;
Do=zeros(size(Co,1),size(Bo,2));
if mMdl==2 || mMdl==3
ss_o = ss(Ao,Bo,Co,Do,'Name','observer controller','InputName',{'desPos','actPos'},'OutputName',{'k*xt'});
else
ss_o = ss(Ao,Bo,Co,Do,'Name','observer controller','InputName',{'desPos','iqMeas','iqVolts','actPos'},'OutputName',{'k*xt'});
end
ss_o = ss(Ao,Bo,Co,Do,'Name','observer controller','InputName',[{'desPos'}; ss_mdl.OutputName ],'OutputName',{'k*xt'});
%discrete plant
%ss_pltz = c2d(ss_plt,Ts);
%[Apz,Bpz,Cpz,Dpz]=ssdata(ss_pltz);
@@ -235,20 +204,17 @@ function [ssc]=StateSpaceControlDesign(mot)
%discrete observer controller
ss_oz = c2d(ss_o,Ts);
%prefilter to compensate non observable resonance frequencies
prefilt=Prefilt(mot,mPrefilt);
%discrete prefilter
prefiltz=c2d(prefilt,Ts);
filt_pos_err_z=c2d(filt_pos_err,Ts);
if bitand(mShow,64)
h=bodeplot(prefilt,prefiltz);
if bitand(verb,128)
h=bodeplot(filt_pos_err,filt_pos_err_z);
setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
end
%state space controller
ssc=struct();
for k=["Ts","ss_plt","ss_o","ss_oz","prefilt","prefiltz","V"]
for k=["Ts","ss_plt","ss_o","ss_oz","filt_pos_err","filt_pos_err_z","V"]
ssc=setfield(ssc,k,eval(k));
end
save(sprintf('/tmp/ssc%d.mat',mot.id),'-struct','ssc');
@@ -275,14 +241,13 @@ function pf=Prefilt(mot,mode)
denV=den1;
pf=tf(numV,denV);
else
%Lag
f=[100 200]; w=f*2*pi; T=1./w;
tf1=tf([T(1) 1],[T(2) 1]);
%bo = bodeoptions;
%bo.FreqUnits = 'Hz'; bo.MagUnits='abs'; bo.Grid='on';
%bode(tf1,bo)
%k=1.2; aa=2; f=[40 60];w=f*2*pi; tf([1 33 w0^2]; den3=[1 20 w0^2];
%f=277;w0=f*2*pi; num1=[1 20 w0^2]; den1=[1 500 w0^2];
%f=138;w0=f*2*pi; num2=[1 300 w0^2]; den2=[1 100 w0^2];

16
matlab/documentFunctions.m
View File

@@ -28,13 +28,15 @@ disp('document figure generation done');close all;
close all;disp('simulate observer with prefilter...');
for k =1:2
[ssc]=StateSpaceControlDesign(mot{k});sim('observer');
f=figure(); h=plot(desPos_actPos.Time,desPos_actPos.Data,'g');
set(h(1),'color','b'); set(h(2),'color',[0 0.5 0]);
print(f,sprintf('figures/sim_cl_obs_pf_%d',mot{k}.id),'-depsc');
f=bodeSamples(desPos_actPos);
print(f,sprintf('figures/sim_cl_obs_pf_bode%d',mot{k}.id),'-depsc');
for m =0%0:1
for k =1:2
[ssc]=StateSpaceControlDesign(mot{k},m);sim('observer');
f=figure(); h=plot(desPos_actPos.Time,desPos_actPos.Data,'g');
set(h(1),'color','b'); set(h(2),'color',[0 0.5 0]);
print(f,sprintf('figures/sim_cl_obs_%d_%d',m,mot{k}.id),'-depsc');
f=bodeSamples(desPos_actPos);
print(f,sprintf('figures/sim_cl_obs_bode%d_%d',m,mot{k}.id),'-depsc');
end
end
disp('document figure generation done');close all;

5
matlab/identifyFxFyStage.m
View File

@@ -212,6 +212,7 @@ function motCell=identifyFxFyStage(mode)
% | 3|-------> actPos
% +-----------+
mot.ss_plt=connect(tfc,tf1,tf2,'iqCmd',{'iqMeas','actVel','actPos'});
mot.ss_plt.Name='best plant approximation';
chkCtrlObsv(mot.ss_plt,'ss_plt fyStage');
%without resonance
@@ -222,6 +223,7 @@ function motCell=identifyFxFyStage(mode)
% +-----------+
s=tf1.InputName{1};tf1.InputName{1}='iqMeas';
mot.ss_c1=connect(tfc,tf1,'iqCmd',{'iqMeas','actVel','actPos'});
mot.ss_c1.Name='without resonance';
chkCtrlObsv(mot.ss_c1,'ss_c1 fyStage');
tf1.InputName{1}=s;%restore
@@ -234,6 +236,7 @@ function motCell=identifyFxFyStage(mode)
% +-----------+
s=tf1.InputName{1};tf1.InputName{1}='iqMeas';
mot.ss_d1=connect(tfd,tf1,'iqCmd',{'iqMeas','actVel','actPos'});
mot.ss_d1.Name='simplified current, without resonance';
chkCtrlObsv(mot.ss_d1,'ss_d1 fyStage');
tf1.InputName{1}=s;%restore
@@ -244,6 +247,7 @@ function motCell=identifyFxFyStage(mode)
% | 2|-------> actPos
% +-----------+
mot.ss_1=ss(tf1);
mot.ss_1.Name='no current loop, no resonance';
chkCtrlObsv(mot.ss_1,'ss_1 fyStage');
@@ -253,6 +257,7 @@ function motCell=identifyFxFyStage(mode)
% | 2|-------> actPos
% +-----------+
mot.ss_0=ss(tf0);
mot.ss_0.Name='simplified mechanics, no current loop, no resonance';
chkCtrlObsv(mot.ss_0,'ss_0 fyStage');
%h=bodeplot(mot.meas,'r',mot.tf4_2,'b',mot.tf6_4,'g');

BIN
matlab/libESB_MX.slx
View File

Binary file not shown.

BIN
matlab/observer.slx
View File

Binary file not shown.

BIN
matlab/stage_closed_loop.slx
View File

Binary file not shown.