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-03-19 09:10:28 +01:00
parent 0c45705715
commit eda8caf985
10 changed files with 268 additions and 142 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
MXfastStageDoc/MXfastStage.tex
View File

@@ -71,20 +71,22 @@ The settings of the PwmSf is done in the templates of gpasciiCommander:
PwmSf=15134.8909 # =.95*16384. PMAC3-style DSPGATE3 ASIC is being used for the output,
the counter moves between +/- 16384. PwmSf is typically set to 95% of 16384
\end{verbatim}
Nevertheless the documentation is confusing and the PwmSf is not directly a value to scale from cvurrent to voltage. Therefore PwmSf is measured to convert idCmd bits values to idVolts bits in section \ref{sec:measCurStep}\\
Nevertheless the documentation is confusing and the PwmSf is not directly a value to scale from current to voltage. Therefore PwmSf is measured roughly in this section to convert idCmd bits values to idVolts bits\\
The overall aplification $iqCmd \rightarrow iqVolts$ is approx. 18.2.
The overall aplification $iqCmd \rightarrow iqVolts$ is approx. 7400/406=18.2. (s.a. figure \ref{fig:IqCmd->IqVolts}).
\begin{figure}[h!]
\includegraphics[trim=0cm 1.5cm 0cm 1cm,scale=.45]{../python/MXTuning/19_01_29/img/iqCmd_TF0.eps}
\includegraphics[trim=0cm 1.5cm 0cm 1cm,scale=.45]{../python/MXTuning/19_01_29/img/iqCmd_TF1.eps}
\caption{IqCmd->IqMeas of motor 1 (bode same for both motors)}
\label{fig:IqCmd->IqMeas}
\end{figure}
\begin{figure}[h!]
\includegraphics[trim=0cm 1.5cm 0cm 1cm,scale=.45]{../python/MXTuning/19_01_29/img/iqCmd_TF2.eps}
\includegraphics[trim=0cm 1.5cm 0cm 1cm,scale=.45]{../python/MXTuning/19_01_29/img/iqCmd_TF3.eps}
\caption{IqCmd->IqVolts of motor 1 (bode same for both motors)}
\label{fig:IqCmd->IqVolts}
\end{figure}
\FloatBarrier
@@ -304,13 +306,13 @@ Solving in Laplace space:\\
$iqVolts=(R+Ls)\cdot iqMeas$\\
$s \cdot iqMeas =\frac{1}{L}iqVolts - \frac{R}{L}iqMeas$\\
Transferfunction open loop of $G_1(s)=iqVolts \rightarrow iqCmd$\\
Transferfunction open loop of $G_1(s)=iqVolts \rightarrow iqMeas$\\
using Masons rule:
\url{https://en.wikipedia.org/wiki/Mason's_gain_formula}:
\[
G_1(s)=\frac{y_{out}}{y_{in}}=\frac{iqCmd}{iqVolts}=
G_1(s)=\frac{y_{out}}{y_{in}}=\frac{iqMeas}{iqVolts}=
\frac{\frac{1}{Ls}}{1+ \frac{R}{Ls}} = \frac{1}{Ls+R} = \frac{k}{1+Ts} = \frac{\frac{1}{R}}{1+\frac{L}{R}s}
\]
@@ -321,7 +323,7 @@ Transferfunction closed loop of $G_2(s)=iqCmd \rightarrow iqMeas$:
\begin{aligned}
&\text{with}\quad
a=Ipf+\frac{Li}{s} \quad
b=PwmSF \cdot G(s) \quad
b=PwmSF \cdot G_1(s) \quad
c=Ipb \quad
d=1\\
&\text{using Masons rule:} \quad G_2(s)=\frac{ab}{1+bc+abd}\\
@@ -581,7 +583,7 @@ The black line on figure \ref{fig:mot_open} shows the concatenate transfer funct
\section{Enhance controller with simulation and implementation}
It has to be checked if the model matches the real stage. Therefore simulations in MATLAB have been done to validate the identification process of the stages. Further simulations and implementations with better controller types were tested
It has to be checked if the model matches the real stage. Therefore simulations in MATLAB have been done to validate the identification process of the stages. Further simulations and implementations with better controller types were tested.
\subsection{chirp sine closed loop with simulink model}
@@ -627,7 +629,7 @@ Therefore for better regulation quality more sophisticated controllers as state
A standard PID controller uses feedback to detect errors between the desired position and the actual position and applies corrective commands to compensate for those errors. Although PID control is the most common type of industrial controller, it does have limitations.
The idea of a state space controller with observer is to have a model of the plant which follows all internal states of the real plant. All these internal states of the model can then be used states to build an optimal controller.
opposite to the PID controller which is a ‘black-box design’, the state space controller with observer is a ‘white box design’. (Figure \ref{fig:observer1})\\
Opposite to the PID controller which is a ‘black-box design’, the state space controller with observer is a ‘white box design’. (Figure \ref{fig:observer1})\\
\begin{figure}[h!]
\centering
@@ -892,6 +894,11 @@ This works good and enhanced the trajectory following at high frequencies. But t
Much more important is to focus now on the closed loop filters B, C/D and E.
\begin{tcolorbox}[colback=red!5!white,colframe=red!75!black,colbacktitle=red!50,coltitle=black,title=TODO]
\begin{verbatim}
write conclusion and best parameters
\end{verbatim}
\end{tcolorbox}
% Motor 2:11.84Hz 0dB
% Kaff = 1/((11.84*2*np.pi)**2/5000/5000) = 4517.278506241804
@@ -911,18 +918,18 @@ Much more important is to focus now on the closed loop filters B, C/D and E.
In this section the trajectory planing of the x and y Parker stages in the 'shapepath' mode is analyzed. In the 'shapepath' mode the goal is to place crystals with sizes close to 1um at the beam position when the FEL pulse arrives. The FEL pulse is so short that motion at the impact time is not relevant. The FEL repetition rate is 25, 50 or 100Hz.\\
The trajectory planing is very important to achieve good timing and positioning precision. This is crucial for high crystal hit rate of the FEL. DeltaTau has the possibility to use pvt (position-velocity-time) motion. In that mode one can tell to pass a given point at certain time with a certain speed.\\
PVT motion is a function of $ax^3+bx^2+cx+d$ between 2 points. Additionally the derivate (velocity) in a point is same for the incoming and outgoing trajectory segment.\\
PVT motion is a function of $ax^3+bx^2+cx+d$ between 2 points. Additionally the derivate (velocity) in a junction point is same for the incoming and outgoing trajectory segment.\\
Different approaches have been tested and compared to maximize the yield:
\paragraph{p0t motion:}
A first very trivial approach is to stop at each point. This stop and go motion will induce a lot of vibrations because it always accelerates and decelerates. This can also have a negative impact to the sample. Therefore other approaches with smoother trajectory and higher speed at impact time will be compared.
A first very trivial approach is to stop at each point. This stop and go motion will induce a lot of vibrations because it always accelerates and decelerates. This can also have a negative impact to the sample position. Therefore other approaches with smoother trajectory and higher speed at impact time will be compared.
\paragraph{pvt motion:}
Assume that the time between two points is $t_s$ ms.
The velocity in the point $p_n$ is set in a way that the distance from point $p_{n-1}$ to $p_{n+1}$ is reached in $2 \cdot t_s$ ms. This approach gives a very smooth trajectory planing. But it contains still some high frequency components that can be avoided.
\paragraph{ift motion:}
Each new point has to be reached at equidistant time $t_s$. So we can look at the position as sampling values. The trajectory with the least high frequency components is the inverse Fourier transformation of the sampled points. The reconstruction of the trajectory can be done with summing $sin(x)/x$ in the time domain or by adding zeros in the frequency spectrum and the do the inverse Fourier transformation.\\
Each new point has to be reached at equidistant time $t_s$. So we can look at the position as sampling values with a constant time interval. The trajectory with the least high frequency components is the inverse Fourier transformation of the sampled points. The reconstruction of the trajectory can be done with summing $sin(x)/x$ in the time domain or by adding zeros in the frequency spectrum and the do the inverse Fourier transformation.\\
The resulting trajectory is the best achievable in point of view of frequency bandwidth. Unfortunately motion programs do not allow such trajectory planing out of the box.
\paragraph{pft motion:}
@@ -1032,7 +1039,13 @@ shot average error x 0.865708 um, y 0.965126 um, 1.49323 um
\subsection{simulations}
MATLAB simulation by executing \verb|DeltaTauOptimizer.m|
\center
{
\includegraphics[scale=.4]{../matlab/figures/sim_optimize1.eps}
\includegraphics[scale=.4]{../matlab/figures/sim_optimize2.eps}
}
\begin{tcolorbox}[colback=red!5!white,colframe=red!75!black,colbacktitle=red!50,coltitle=black,title=TODO]

2
MXfastStageDoc/traj1.eps
View File

@@ -1,7 +1,7 @@
%!PS-Adobe-3.0 EPSF-3.0
%%Title: /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/MXfastStageDoc/traj1.eps
%%Creator: matplotlib version 1.5.1, http://matplotlib.org/
%%CreationDate: Tue Mar 5 14:20:51 2019
%%CreationDate: Fri Mar 15 09:41:59 2019
%%Orientation: portrait
%%BoundingBox: 18 180 594 612
%%EndComments

2
MXfastStageDoc/traj2.eps
View File

@@ -1,7 +1,7 @@
%!PS-Adobe-3.0 EPSF-3.0
%%Title: /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/MXfastStageDoc/traj2.eps
%%Creator: matplotlib version 1.5.1, http://matplotlib.org/
%%CreationDate: Tue Mar 5 14:20:51 2019
%%CreationDate: Fri Mar 15 09:42:00 2019
%%Orientation: portrait
%%BoundingBox: 18 180 594 612
%%EndComments

2
MXfastStageDoc/traj3.eps
View File

@@ -1,7 +1,7 @@
%!PS-Adobe-3.0 EPSF-3.0
%%Title: /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/MXfastStageDoc/traj3.eps
%%Creator: matplotlib version 1.5.1, http://matplotlib.org/
%%CreationDate: Tue Mar 5 14:20:52 2019
%%CreationDate: Fri Mar 15 09:42:01 2019
%%Orientation: portrait
%%BoundingBox: 18 180 594 612
%%EndComments

120
matlab/DeltaTauOptimizer.m
View File

@@ -10,73 +10,55 @@ function DeltaTauOptimizer()
mot=identifyFxFyStage(7);
end
%SIM1=[1 1; 1 2; 8 2; 9 2;9 3];
%SIM1=[9 1; 9 2; 9 3; 9 4];
%SIM1=[9 1; 9 2; 9 3; 9 4; 9 5; 9 6];
SIM1=[9 1; 9 2; 9 5; 9 6];
%SIM2=[1 1; 1 2; 8 2; 9 2];
SIM1=[9 1; 9 2; 9 3; 9 4];
SIM2=[9 1; 9 2; 9 3; 9 4];
%SIM2=[8 1; 8 2; 8 3; 8 4];
%SIM2=[8 1; 8 2; 9 1; 9 2];
%SIM2=[8 2; 9 2;8 3; 9 3];
%SIM2=[8 3; 9 3;8 4; 9 4];
%SIM2=[9 4;9 0];
%SIM2=[8 3; 9 3];
SIM1=[9 1; 9 2; 9 3; 9 4; 9 5];
%SIM2=[9 1; 9 2; 9 3; 9 4];
SIM2=[9 1; 9 2];
if isempty(simData1)
close all;
simData1=ExecSim(mot{1},SIM1);
end
%if isempty(simData2)
% close all;
% simData2=ExecSim(mot{2},SIM2);
%end
if isempty(simData2)
close all;
simData2=ExecSim(mot{2},SIM2);
end
close all;
test()
%bodeSim(simData1);
%bodeSim(simData2);
%test1();return
%test2();return
bodeSim(simData1);
bodeSim(simData2);
for i =1:2
set(i,'OuterPosition',[80 400 1000 700]);
print(i,sprintf('figures/sim_optimize%d',i),'-depsc');
end
end
function test()
global pb mot simData1 simData2;
%pb=DeltaTauParam(mot{2},8,0);
function test1()
global pb mot simData1;
%pb=DeltaTauParam(mot{1},8,0);
pb=DeltaTauParam(mot{1},9,0);
sim('DeltaTauSim');
i=6;
i=7;
simData1(i).pb=pb;
simData1(i).desPos_actPos=desPos_actPos;
simData1=bodeSim(simData1);
return
%simData2(i).mot_mdl_param=SIM(i,:);
%pb.C=[0.04877];
%pb.D=[1 -0.9512];
%pb.C=[1 -1.3236 6.2472 -11.8555 11.3067 -5.4188 1.0440];
%pb.D=[1.0000 -6.6330 17.6945 -24.5314 18.7409 -7.5020 1.2309];
global tfs
Ts=1/5000;
frq0=55;d0=.5;
frq1=85;d1=.5;
w0=frq0*2*pi;
w1=frq1*2*pi;
tf0=tf([w0^2],[1 2*d0*w0 w0^2 ])
tf1=tf([1 2*d1*w1 w1^2 ],[w1^2])
tfs=tf0*tf1
tfz=c2d(tfs,Ts)
%h=bodeplot(tfz,tfs);setoptions(h,'FreqUnits','Hz','Grid','on');
%pb.C=tfz.Numerator{1};
%pb.D=tfz.Denominator{1};
opt=tfestOptions;
opt.Display='off';
% %tfa=tfest(simData2(i).tfEst, 6, 5,opt);
tfa=tfest(simData2(i).tfEst, 6, 6,opt);
% tfb=1/tfa
% tfc=c2d(tfb,1/5000)
%
% tfs=tf([1],[.001 1])
% tfz=c2d(tfs,1/5000)
% h=bodeplot(tfs,tfz)
% setoptions(h,'FreqUnits','Hz','Grid','on');
% controlSystemDesigner(tfa)
end
function test2()
global pb mot simData2;
%pb=DeltaTauParam(mot{2},8,0); % ss_cq no resonance
pb=DeltaTauParam(mot{2},9,0); % ss_cqr with resonance
sim('DeltaTauSim');
i=3;
simData2(i).pb=pb;
simData2(i).desPos_actPos=desPos_actPos;
simData2=bodeSim(simData2);
return
end
function simData=ExecSim(mot,SIM)
global pb;
@@ -131,3 +113,37 @@ function simData=bodeSim(simData)
linkaxes([ax1,ax2],'x')
legend('Location','best');
end
function SCRATCH()
%simData2(i).mot_mdl_param=SIM(i,:);
%pb.C=[0.04877];
%pb.D=[1 -0.9512];
%pb.C=[1 -1.3236 6.2472 -11.8555 11.3067 -5.4188 1.0440];
%pb.D=[1.0000 -6.6330 17.6945 -24.5314 18.7409 -7.5020 1.2309];
global tfs
Ts=1/5000;
frq0=55;d0=.5;
frq1=85;d1=.5;
w0=frq0*2*pi;
w1=frq1*2*pi;
tf0=tf([w0^2],[1 2*d0*w0 w0^2 ])
tf1=tf([1 2*d1*w1 w1^2 ],[w1^2])
tfs=tf0*tf1
tfz=c2d(tfs,Ts)
%h=bodeplot(tfz,tfs);setoptions(h,'FreqUnits','Hz','Grid','on');
%pb.C=tfz.Numerator{1};
%pb.D=tfz.Denominator{1};
opt=tfestOptions;
opt.Display='off';
% %tfa=tfest(simData2(i).tfEst, 6, 5,opt);
tfa=tfest(simData2(i).tfEst, 6, 6,opt);
% tfb=1/tfa
% tfc=c2d(tfb,1/5000)
%
% tfs=tf([1],[.001 1])
% tfz=c2d(tfs,1/5000)
% h=bodeplot(tfs,tfz)
% setoptions(h,'FreqUnits','Hz','Grid','on');
% controlSystemDesigner(tfa)
end

184
matlab/DeltaTauParam.m
View File

@@ -45,13 +45,7 @@ function [pb]=DeltaTauParam(mot,mdl,param)
switch param
case 0 %scratch
desc=desc+"scratch";
pb.Kp=25;pb.Kvfb=350;pb.Ki=0.02;pb.Kvff=350;pb.Kaff=1/(curr2acc*(pb.Ts^2));pb.MaxInt=1000;
%pure feed forward
pb.Kp=0;pb.Kvfb=0;pb.Ki=0.02;pb.Kvff=0;pb.Kaff=1/(curr2acc*(pb.Ts^2));pb.MaxInt=1000;
%pure feedback
pb.Kp=25;pb.Kvfb=350;pb.Ki=0.02;pb.Kvff=0;pb.Kaff=0;pb.MaxInt=1000;
pl=[-300+350i -300-350i -2513];
pl=[-800+150i -800-150i -2513];
[Am,Bm,Cm,Dm]=ssdata(mot.ss_dq);
K = place(Am,Bm,pl);
V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
@@ -59,33 +53,49 @@ function [pb]=DeltaTauParam(mot,mdl,param)
Kfb=K*(Cm^-1);
pb.V=V;
pb.Kfb=Kfb;
pb.Kp=V;
pb.B=tf(Kfb(3)/V);
%pb.B=tf(Kfb(3)/V);
pb.Kvfb=Kfb(2)/pb.Ts;
pb.Kvff=1*pb.Kvfb;%0;
pb.Kvff=0;%1*pb.Kvfb;
pb.Kafb=Kfb(1)/(curr2acc*(pb.Ts^2));
pb.Kaff=1/(curr2acc*(pb.Ts^2))+pb.Kafb;%0;
pb.Ki=0.002;
pb.Kaff=0;%1/(curr2acc*(pb.Ts^2))+pb.Kafb;
pb.Ki=0.001;
pb.MaxInt=1000;
%https://ch.mathworks.com/help/control/ref/tf.html
%feed forward filter attenuating high frequencies
%fs=1/pb.Ts;%[n,d] = butter(6,fc/(fs/2));
fc=125;%Hz
fc=200;%Hz
[num,den] = butter(2,fc*pb.Ts*2,'low');
Fz2=tf(num,den,pb.Ts);
Fz=tf([1 0 0],den/sum(num),pb.Ts);
h=bodeplot(Fz,Fz2);setoptions(h,'FreqUnits','Hz','Grid','on');
%h=bodeplot(Fz,Fz2);setoptions(h,'FreqUnits','Hz','Grid','on');
pb.F=Fz;
% THE F FILTER HELPS TO NOT OVERSHOOT ON HIGH FREQUENCY TRAJECTORIES,
% BUT THE INPUT TRAJECTORY DOES NOT CONTAIN THESE FREQUENCIES.
% THEREFORE THE FILTERS IN THE CLOSED LOOP MUST BE TWEAKED!
%tf([.2 .8],[1],pb.Ts,'variable','z^-1') = tf([.2 .8],[1 0],pb.Ts)
%pb.A=tf([.0 1],[1 0],pb.Ts)
%pb.F=Fz;
fc=800;%Hz
[num,den] = butter(2,fc*pb.Ts*2,'low');
Ez=tf([1 0 0],den/sum(num),pb.Ts);
Bz=tf([.5 .5],[1 0],pb.Ts)
pb.E=Ez; %less noise sensitive on feedback
pb.B=Bz; %less noise sensitive on feedback
aa=.9;
k=1.8;
T=1/200;
C_D=tf(k*[T 1],[T*aa 1]);
C_Dz=c2d(C_D,pb.Ts)
%h=bodeplot(C_D,C_Dz);setoptions(h,'FreqUnits','Hz','Grid','on');
pb.C_D=C_Dz;
display(pb.B)
display(pb.E)
display(pb.C_D)
display(pb.F)
Fz=tf([1 0 0],den/sum(num),pb.Ts);
fprintf('Kp:%f Kvfb:%f\n',pb.Kp,pb.Kvfb);
case 1 %origin parameters
@@ -137,7 +147,8 @@ function [pb]=DeltaTauParam(mot,mdl,param)
display(pb.B)
fprintf('Kp:%f Kvfb:%f Kafb:%f\n',pb.Kp,pb.Kvfb,pb.Kafb);
case 5 % optimize higher gain and filter
pl=[-600+750i -600-750i -2513];
desc=desc+"opt5";
pl=[-800+150i -800-150i -2513];
[Am,Bm,Cm,Dm]=ssdata(mot.ss_dq);
K = place(Am,Bm,pl);
V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
@@ -145,26 +156,95 @@ function [pb]=DeltaTauParam(mot,mdl,param)
Kfb=K*(Cm^-1);
pb.V=V;
pb.Kfb=Kfb;
pb.Kp=V;
pb.B=tf(Kfb(3)/V);
%pb.B=tf(Kfb(3)/V);
pb.Kvfb=Kfb(2)/pb.Ts;
pb.Kvff=.15*pb.Kvfb;%0;
pb.Kvff=1*pb.Kvfb;
pb.Kafb=Kfb(1)/(curr2acc*(pb.Ts^2));
pb.Kaff=.2/(curr2acc*(pb.Ts^2))+pb.Kafb;%0;
pb.Ki=0.002;
pb.Kaff=1/(curr2acc*(pb.Ts^2))+pb.Kafb;
pb.Ki=0.001;
pb.MaxInt=1000;
%https://ch.mathworks.com/help/control/ref/tf.html
%feed forward filter attenuating high frequencies
%fs=1/pb.Ts;%[n,d] = butter(6,fc/(fs/2));
fc=125;%Hz
fc=200;%Hz
[num,den] = butter(2,fc*pb.Ts*2,'low');
Fz2=tf(num,den,pb.Ts);
Fz=tf([1 0 0],den/sum(num),pb.Ts);
h=bodeplot(Fz,Fz2);setoptions(h,'FreqUnits','Hz','Grid','on');
pb.F=Fz;
%h=bodeplot(Fz,Fz2);setoptions(h,'FreqUnits','Hz','Grid','on');
fc=800;%Hz
[num,den] = butter(2,fc*pb.Ts*2,'low');
Ez=tf([1 0 0],den/sum(num),pb.Ts);
Bz=tf([.5 .5],[1 0],pb.Ts) % fast simple lowpass: -3dB at 1250Hz
%pb.F=Fz; %filtering does not help in the domain below 50Hz. it attenuates at high frq. but makes things worse at low frq.
pb.E=Ez; %less noise sensitive on feedback
pb.B=Bz; %less noise sensitive on feedback
display(pb.B)
display(pb.F)
Fz=tf([1 0 0],den/sum(num),pb.Ts);
fprintf('Kp:%f Kvfb:%f\n',pb.Kp,pb.Kvfb);
case 6
desc=desc+"opt6";
pl=[-800+150i -800-150i -2513];
[Am,Bm,Cm,Dm]=ssdata(mot.ss_dq);
K = place(Am,Bm,pl);
V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
V=V(end);
Kfb=K*(Cm^-1);
pb.V=V;
pb.Kfb=Kfb;
pb.Kp=V;
%pb.B=tf(Kfb(3)/V);
pb.Kvfb=Kfb(2)/pb.Ts;
pb.Kvff=1*pb.Kvfb;
pb.Kafb=Kfb(1)/(curr2acc*(pb.Ts^2));
pb.Kaff=1/(curr2acc*(pb.Ts^2))+pb.Kafb;
pb.Ki=0.001;
pb.MaxInt=1000;
%https://ch.mathworks.com/help/control/ref/tf.html
%feed forward filter attenuating high frequencies
%fs=1/pb.Ts;%[n,d] = butter(6,fc/(fs/2));
fc=200;%Hz
[num,den] = butter(2,fc*pb.Ts*2,'low');
Fz2=tf(num,den,pb.Ts);
Fz=tf([1 0 0],den/sum(num),pb.Ts);
%h=bodeplot(Fz,Fz2);setoptions(h,'FreqUnits','Hz','Grid','on');
%pb.F=Fz;
fc=800;%Hz
[num,den] = butter(2,fc*pb.Ts*2,'low');
Ez=tf([1 0 0],den/sum(num),pb.Ts);
Bz=tf([.5 .5],[1 0],pb.Ts)
pb.E=Ez; %less noise sensitive on feedback
pb.B=Bz; %less noise sensitive on feedback
aa=.9;
k=1.8;
T=1/200;
C_D=tf(k*[T 1],[T*aa 1]);
C_Dz=c2d(C_D,pb.Ts)
%h=bodeplot(C_D,C_Dz);setoptions(h,'FreqUnits','Hz','Grid','on');
pb.C_D=C_Dz;
display(pb.B)
display(pb.E)
display(pb.C_D)
display(pb.F)
Fz=tf([1 0 0],den/sum(num),pb.Ts);
fprintf('Kp:%f Kvfb:%f\n',pb.Kp,pb.Kvfb);
end
%pb.Kp=0.1;pb.Kvfb=0;pb.Ki=0.00;pb.Kvff=0;pb.Kaff=1/(1.548e04*(pb.Ts^2));pb.MaxInt=1000;
%filter [z^0 z^-1 ... z^-n];
@@ -177,28 +257,36 @@ function [pb]=DeltaTauParam(mot,mdl,param)
switch param
case 0 %scratch
desc=desc+"scratch";
pb.Kp=20.3255;
pb.Kvfb=582.4551;pb.Kvff=582.4551;
pb.Kaff=5595.2;pb.Kafb=1077.9;
pb.Ki=0.01;pb.MaxInt=1000;
%pole(mot.ss_dq)
pl=[-300+150i -300-150i -2513];
[Am,Bm,Cm,Dm]=ssdata(mot.ss_dq);
K = place(Am,Bm,pl);
V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
V=V(end);
Kfb=K*(Cm^-1);
pb.V=V;
pb.Kfb=Kfb;
% Ts=pb.Ts;
% frq=75;d=.6;
% w0=frq*2*pi;
% tfs=tf([w0^2],[1 2*d*w0 w0^2 ])
% global currTf
% frq0=55;d0=.5;
% frq1=85;d1=.5;
% w0=frq0*2*pi;
% w1=frq1*2*pi;
% tf0=tf([w0^2],[1 2*d0*w0 w0^2 ])
% tf1=tf([1 2*d1*w1 w1^2 ],[w1^2])
% tfs=tf0*tf1
% tfz=c2d(tfs,Ts)
% h=bodeplot(tfz,tfs);setoptions(h,'FreqUnits','Hz','Grid','on');
% pb.C=tfz.Numerator{1};
% pb.D=tfz.Denominator{1};
% currTf=tfz
pb.Kp=V;
%pb.B=tf(Kfb(3)/V);
pb.Kvfb=Kfb(2)/pb.Ts;
pb.Kvff=1*pb.Kvfb;
pb.Kafb=0.1*Kfb(1)/(curr2acc*(pb.Ts^2));
pb.Kaff=1/(curr2acc*(pb.Ts^2))+pb.Kafb;
pb.Ki=0.001;
pb.MaxInt=1000;
fc=800;%Hz
[num,den] = butter(2,fc*pb.Ts*2,'low');
Ez=tf([1 0 0],den/sum(num),pb.Ts);
%Az=tf([.1 .2 .7],[1 0 0],pb.Ts) %is similar to the phase at feed forward only
%h=bodeplot(Az);setoptions(h,'FreqUnits','Hz','Grid','on');
%Bz=tf([.5 .5],[1 0],pb.Ts)
%pb.E=Ez; %less noise sensitive on feedback
%pb.B=Bz; %less noise sensitive on feedback
fprintf('Kp:%f Kvfb:%f Kafb:%f\n',pb.Kp,pb.Kvfb,pb.Kafb);
case 1 %origin parameters
desc=desc+"orig";

BIN
matlab/DeltaTauSim.slx
View File

Binary file not shown.

11
python/MXMotion.py
View File

@@ -36,7 +36,10 @@ class MotionBase:
self.gather=gather
self.verbose=verbose
#ServoPeriod=0.2 #ms, Sys.ServoPeriod is dependent of !common() macro
ServoPeriod=comm.gpascii.servo_period*1000
if comm is None:
ServoPeriod=.2
else:
ServoPeriod=comm.gpascii.servo_period*1000
self.meta={'srv_per':ServoPeriod,'pt2pt_time':40,'sync_flag':0,'sync_mode':2}
self.meta.update(kwargs)
@@ -106,6 +109,7 @@ class MotionBase:
self.sync_prg = prg
#download and start triggerSync code
comm=self.comm
if comm is None: return
arg=0 #argument to pass to triggerSync program
if sync_mode==2:arg+=1 #synchronize
@@ -144,6 +148,7 @@ class MotionBase:
def run(self, crdId=1):
'runs the code sync_run which has been generated with setup_sync()'
comm = self.comm
if comm is None: return
gpascii = comm.gpascii
try:
cmd=self.sync_run
@@ -157,6 +162,7 @@ class MotionBase:
return
else:
comm = self.comm
if comm is None: return
gpascii = comm.gpascii
while (True):
val = gpascii.get_variable('Coord[{crdId}].Q[0]'.format(crdId=crdId), type_=int)
@@ -172,7 +178,7 @@ class MotionBase:
if ge == 0: return -1
cnt = gpascii.get_variable('Gather.Samples', type_=int)
try:
cnt*=self.meta['srv_per']/(self.meta['acq_per']*self.meta['pt2pt_time'])
cnt*=self.meta['srv_per']*self.meta['acq_per']/self.meta['pt2pt_time']
except KeyError:
pass
return cnt
@@ -184,6 +190,7 @@ class MotionBase:
time.sleep(wait)
if self.meta['sync_flag']&1:
comm = self.comm
if comm is None: return
gpascii = comm.gpascii
gpascii.send_block('Coord[1].Q[10]=1')
else:

10
python/helicalscan.py
View File

@@ -1254,11 +1254,11 @@ if __name__=='__main__':
y=(1405.7, 1019.2),
z=((-1309.6, -1010.9, -2410.3), (-1219.4, -918.8, -2510.4)))
hs.calcParam(x=((-70.25682272433501, 287.48437780001825, -477.99870973215764),
(-41.466601738723284, 274.71565975031103, -504.9508517714285)),
y=(1207.6326052816642, 1704.2138281362475), z=(
(-1196.2847548574225, -1686.284754857423, -1686.284754857423),
(-1196.2847548574225, -1686.284754857423, -1686.284754857423)))
#hs.calcParam(x=((-70.25682272433501, 287.48437780001825, -477.99870973215764),
# (-41.466601738723284, 274.71565975031103, -504.9508517714285)),
# y=(1207.6326052816642, 1704.2138281362475), z=(
# (-1196.2847548574225, -1686.284754857423, -1686.284754857423),
# (-1196.2847548574225, -1686.284754857423, -1686.284754857423)))
### use simulation motors ###

40
python/shapepath.py
View File

@@ -240,9 +240,6 @@ class DebugPlot:
plt.show(block=False)
plt.show(block=False)
def plot_pos_error(self):
rec = self.rec # yA,xA,yD,xD,trig
ts=self.meta['srv_per']*self.meta['acq_per']
@@ -455,7 +452,7 @@ class ShapePath(MotionBase):
verb=self.verbose
if verb&2:
self.plot_points(pts)
plt.show()
plt.show(block=False)
def gen_swissfel_points(self,flipx=False,flipy=False,ofs=(0,0),width=1000):
@@ -480,7 +477,7 @@ class ShapePath(MotionBase):
verb=self.verbose
if verb&2:
self.plot_points(pts)
plt.show()
plt.show(block=False)
def gen_rand_points(self,n=107,scale=1000,ofs=(0,0)):
'generate random distributed points'
@@ -500,10 +497,10 @@ class ShapePath(MotionBase):
pts+=ofs
self.points=pts
def gen_spiral_points(self,rStart=1.,rInc=.2,numSeg=4,numCir=6, ofs=(0, 0)):
def gen_spiral_points(self,rStart=1.,rInc=.2,numSeg=4,numCir=6, phase=0, ofs=(0, 0)):
#rInc radius increment per circle
r=rStart+np.arange(numSeg*numCir)*(float(rInc)/numSeg)
ang=2.*np.pi/numSeg*np.arange(numSeg*numCir)
ang=2.*np.pi/numSeg*np.arange(numSeg*numCir)+phase*np.pi/180
pts=np.vstack((np.sin(ang)*r,np.cos(ang)*r)).T
pts+=ofs
@@ -586,7 +583,7 @@ class ShapePath(MotionBase):
if verb&2:
DebugPlot.plot_points(pts)
plt.show()
plt.show(block=False)
self.points=pts
@@ -713,7 +710,7 @@ class ShapePath(MotionBase):
verb=self.verbose
if verb&16:
dp=DebugPlot(self);self.pvt=dp.plot_gen_pvt(pv)
plt.show()
plt.show(block=False)
pv[1:-1, (2, 3)]*=CoordFeedTime #scaling for Deltatau
prg.append(' linear abs')
@@ -857,32 +854,37 @@ if __name__=='__main__':
#sp.gen_closed_shifted()
#sp.gen_swissmx_points(width=1000,ofs=(-500,0))
#sp.gen_swissfel_points(width=1000,ofs=(-500,0))
sp.gen_rand_points(n=200, scale=1000);sp.sort_points(xy=False)
sp.gen_rand_points(n=2000, scale=1000);sp.sort_points(xy=False)
#sp.gen_swissmx_points(width=1000, ofs=(-500, 0));
#sp.gen_spiral_points(rStart=100,rInc=10,numSeg=4,numCir=60, ofs=(0, 0))
#sp.gen_spiral_points(rStart=100,rInc=130,numSeg=4,numCir=2, ofs=(0, 0))
#sp.gen_grid_points(w=10,h=10,pitch=100,rnd=0,ofs=(0,0));sp.sort_points(False);
#sp.gen_grid_points(w=1,h=10,pitch=100,rnd=0,ofs=(0,0))
#sp.gen_spiral_points(rStart=100,rInc=20,numSeg=8,numCir=32, ofs=(0, 0))
#sp.gen_spiral_points(rStart=100,rInc=10,numSeg=2,numCir=32, phase=45, ofs=(0, 0))
#sp.gen_spiral_points(rStart=100,rInc=10,numSeg=4,numCir=32, ofs=(0, 0))
#sp.gen_closed_shifted()
sp.setup_gather(acq_per=1)
gtMaxLn=116508;ovhdTime=100
acq_per=int(np.ceil((sp.meta['pt2pt_time']*sp.points.shape[0]+ovhdTime)/(gtMaxLn*sp.meta['srv_per'])))
sp.setup_gather(acq_per=acq_per)
sp.setup_sync(verbose=args.verbose&32)
sp.setup_coord_trf() # reset to shape path system
sp.setup_motion(fnPrg=fn+'.prg', mode=3, scale=1,dwell=10)
#sp.meta['pt2pt_time']=10 #put between setup_sync and setup_motion to have more motion points than FEL syncs
sp.setup_motion(fnPrg=fn+'.prg', mode=3, scale=1.,dwell=10)
#sp.setup_motion(fnPrg=fn + '.prg', mode=1, scale=1,dwell=10)
#sp.setup_motion(fnPrg=fn + '.prg', mode=1, scale=0,dwell=10)
sp.homing() #homing if needed
sp.run() #start motion program
sp.wait_armed() # wait until motors are at first position
sp.trigger(0.5) #send a start trigger (if needed) ater given time
while True:
p=sp.progress()
if p<0: break
print('progress %d'%p);time.sleep(.1)
sp.gather_upload(fnRec=fn+'.npz')
dp=DebugPlot(sp);dp.plot_gather(mode=11)
if not comm is None:
while True:
p=sp.progress()
if p<0: break
print('progress %d/%d'%(p,sp.points.shape[0]));time.sleep(.1)
sp.gather_upload(fnRec=fn+'.npz')
dp=DebugPlot(sp);dp.plot_gather(mode=11)
print('done')
plt.show(block=False)