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, 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 the counter moves between +/- 16384. PwmSf is typically set to 95% of 16384
\end{verbatim} \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!] \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_TF0.eps}
\includegraphics[trim=0cm 1.5cm 0cm 1cm,scale=.45]{../python/MXTuning/19_01_29/img/iqCmd_TF1.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)} \caption{IqCmd->IqMeas of motor 1 (bode same for both motors)}
\label{fig:IqCmd->IqMeas}
\end{figure} \end{figure}
\begin{figure}[h!] \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_TF2.eps}
\includegraphics[trim=0cm 1.5cm 0cm 1cm,scale=.45]{../python/MXTuning/19_01_29/img/iqCmd_TF3.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)} \caption{IqCmd->IqVolts of motor 1 (bode same for both motors)}
\label{fig:IqCmd->IqVolts}
\end{figure} \end{figure}
\FloatBarrier \FloatBarrier
@@ -304,13 +306,13 @@ Solving in Laplace space:\\
$iqVolts=(R+Ls)\cdot iqMeas$\\ $iqVolts=(R+Ls)\cdot iqMeas$\\
$s \cdot iqMeas =\frac{1}{L}iqVolts - \frac{R}{L}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: using Masons rule:
\url{https://en.wikipedia.org/wiki/Mason's_gain_formula}: \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} \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} \begin{aligned}
&\text{with}\quad &\text{with}\quad
a=Ipf+\frac{Li}{s} \quad a=Ipf+\frac{Li}{s} \quad
b=PwmSF \cdot G(s) \quad b=PwmSF \cdot G_1(s) \quad
c=Ipb \quad c=Ipb \quad
d=1\\ d=1\\
&\text{using Masons rule:} \quad G_2(s)=\frac{ab}{1+bc+abd}\\ &\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} \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} \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. 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. 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!] \begin{figure}[h!]
\centering \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. 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 % Motor 2:11.84Hz 0dB
% Kaff = 1/((11.84*2*np.pi)**2/5000/5000) = 4517.278506241804 % 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.\\ 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.\\ 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: Different approaches have been tested and compared to maximize the yield:
\paragraph{p0t motion:} \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:} \paragraph{pvt motion:}
Assume that the time between two points is $t_s$ ms. 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. 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:} \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. 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:} \paragraph{pft motion:}
@@ -1032,7 +1039,13 @@ shot average error x 0.865708 um, y 0.965126 um, 1.49323 um
\subsection{simulations} \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] \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 %!PS-Adobe-3.0 EPSF-3.0
%%Title: /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/MXfastStageDoc/traj1.eps %%Title: /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/MXfastStageDoc/traj1.eps
%%Creator: matplotlib version 1.5.1, http://matplotlib.org/ %%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 %%Orientation: portrait
%%BoundingBox: 18 180 594 612 %%BoundingBox: 18 180 594 612
%%EndComments %%EndComments

2
MXfastStageDoc/traj2.eps
View File

@@ -1,7 +1,7 @@
%!PS-Adobe-3.0 EPSF-3.0 %!PS-Adobe-3.0 EPSF-3.0
%%Title: /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/MXfastStageDoc/traj2.eps %%Title: /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/MXfastStageDoc/traj2.eps
%%Creator: matplotlib version 1.5.1, http://matplotlib.org/ %%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 %%Orientation: portrait
%%BoundingBox: 18 180 594 612 %%BoundingBox: 18 180 594 612
%%EndComments %%EndComments

2
MXfastStageDoc/traj3.eps
View File

@@ -1,7 +1,7 @@
%!PS-Adobe-3.0 EPSF-3.0 %!PS-Adobe-3.0 EPSF-3.0
%%Title: /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/MXfastStageDoc/traj3.eps %%Title: /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/MXfastStageDoc/traj3.eps
%%Creator: matplotlib version 1.5.1, http://matplotlib.org/ %%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 %%Orientation: portrait
%%BoundingBox: 18 180 594 612 %%BoundingBox: 18 180 594 612
%%EndComments %%EndComments

122
matlab/DeltaTauOptimizer.m
View File

@@ -10,73 +10,55 @@ function DeltaTauOptimizer()
mot=identifyFxFyStage(7); mot=identifyFxFyStage(7);
end end
%SIM1=[1 1; 1 2; 8 2; 9 2;9 3]; %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]; %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=[9 1; 9 2; 9 3; 9 4]; SIM2=[9 1; 9 2];
%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];
if isempty(simData1) if isempty(simData1)
close all; close all;
simData1=ExecSim(mot{1},SIM1); simData1=ExecSim(mot{1},SIM1);
end end
%if isempty(simData2) if isempty(simData2)
% close all; close all;
% simData2=ExecSim(mot{2},SIM2); simData2=ExecSim(mot{2},SIM2);
%end end
close all; close all;
test() %test1();return
%bodeSim(simData1); %test2();return
%bodeSim(simData2); 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 end
function test() function test1()
global pb mot simData1 simData2; global pb mot simData1;
%pb=DeltaTauParam(mot{2},8,0); %pb=DeltaTauParam(mot{1},8,0);
pb=DeltaTauParam(mot{1},9,0); pb=DeltaTauParam(mot{1},9,0);
sim('DeltaTauSim'); sim('DeltaTauSim');
i=6; i=7;
simData1(i).pb=pb; simData1(i).pb=pb;
simData1(i).desPos_actPos=desPos_actPos; simData1(i).desPos_actPos=desPos_actPos;
simData1=bodeSim(simData1); 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 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) function simData=ExecSim(mot,SIM)
global pb; global pb;
@@ -130,4 +112,38 @@ function simData=bodeSim(simData)
set(ax2, 'XScale', 'log'); set(ax2, 'XScale', 'log');
linkaxes([ax1,ax2],'x') linkaxes([ax1,ax2],'x')
legend('Location','best'); legend('Location','best');
end 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

188
matlab/DeltaTauParam.m
View File

@@ -45,13 +45,7 @@ function [pb]=DeltaTauParam(mot,mdl,param)
switch param switch param
case 0 %scratch case 0 %scratch
desc=desc+"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; pl=[-800+150i -800-150i -2513];
%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];
[Am,Bm,Cm,Dm]=ssdata(mot.ss_dq); [Am,Bm,Cm,Dm]=ssdata(mot.ss_dq);
K = place(Am,Bm,pl); K = place(Am,Bm,pl);
V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 ) 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); Kfb=K*(Cm^-1);
pb.V=V; pb.V=V;
pb.Kfb=Kfb; pb.Kfb=Kfb;
pb.Kp=V; pb.Kp=V;
pb.B=tf(Kfb(3)/V); %pb.B=tf(Kfb(3)/V);
pb.Kvfb=Kfb(2)/pb.Ts; 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.Kafb=Kfb(1)/(curr2acc*(pb.Ts^2));
pb.Kaff=1/(curr2acc*(pb.Ts^2))+pb.Kafb;%0; pb.Kaff=0;%1/(curr2acc*(pb.Ts^2))+pb.Kafb;
pb.Ki=0.002; pb.Ki=0.001;
pb.MaxInt=1000; pb.MaxInt=1000;
%https://ch.mathworks.com/help/control/ref/tf.html %https://ch.mathworks.com/help/control/ref/tf.html
%feed forward filter attenuating high frequencies %feed forward filter attenuating high frequencies
%fs=1/pb.Ts;%[n,d] = butter(6,fc/(fs/2)); %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'); [num,den] = butter(2,fc*pb.Ts*2,'low');
Fz2=tf(num,den,pb.Ts); Fz2=tf(num,den,pb.Ts);
Fz=tf([1 0 0],den/sum(num),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;
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
pb.F=Fz; aa=.9;
% THE F FILTER HELPS TO NOT OVERSHOOT ON HIGH FREQUENCY TRAJECTORIES, k=1.8;
% BUT THE INPUT TRAJECTORY DOES NOT CONTAIN THESE FREQUENCIES. T=1/200;
% THEREFORE THE FILTERS IN THE CLOSED LOOP MUST BE TWEAKED! C_D=tf(k*[T 1],[T*aa 1]);
%tf([.2 .8],[1],pb.Ts,'variable','z^-1') = tf([.2 .8],[1 0],pb.Ts) C_Dz=c2d(C_D,pb.Ts)
%pb.A=tf([.0 1],[1 0],pb.Ts) %h=bodeplot(C_D,C_Dz);setoptions(h,'FreqUnits','Hz','Grid','on');
pb.C_D=C_Dz;
display(pb.B) display(pb.B)
display(pb.E)
display(pb.C_D)
display(pb.F) display(pb.F)
Fz=tf([1 0 0],den/sum(num),pb.Ts);
fprintf('Kp:%f Kvfb:%f\n',pb.Kp,pb.Kvfb); fprintf('Kp:%f Kvfb:%f\n',pb.Kp,pb.Kvfb);
case 1 %origin parameters case 1 %origin parameters
@@ -137,7 +147,8 @@ function [pb]=DeltaTauParam(mot,mdl,param)
display(pb.B) display(pb.B)
fprintf('Kp:%f Kvfb:%f Kafb:%f\n',pb.Kp,pb.Kvfb,pb.Kafb); fprintf('Kp:%f Kvfb:%f Kafb:%f\n',pb.Kp,pb.Kvfb,pb.Kafb);
case 5 % optimize higher gain and filter 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); [Am,Bm,Cm,Dm]=ssdata(mot.ss_dq);
K = place(Am,Bm,pl); K = place(Am,Bm,pl);
V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 ) 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); Kfb=K*(Cm^-1);
pb.V=V; pb.V=V;
pb.Kfb=Kfb; pb.Kfb=Kfb;
pb.Kp=V; pb.Kp=V;
pb.B=tf(Kfb(3)/V); %pb.B=tf(Kfb(3)/V);
pb.Kvfb=Kfb(2)/pb.Ts; 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.Kafb=Kfb(1)/(curr2acc*(pb.Ts^2));
pb.Kaff=.2/(curr2acc*(pb.Ts^2))+pb.Kafb;%0; pb.Kaff=1/(curr2acc*(pb.Ts^2))+pb.Kafb;
pb.Ki=0.002; pb.Ki=0.001;
pb.MaxInt=1000; pb.MaxInt=1000;
%https://ch.mathworks.com/help/control/ref/tf.html %https://ch.mathworks.com/help/control/ref/tf.html
%feed forward filter attenuating high frequencies %feed forward filter attenuating high frequencies
%fs=1/pb.Ts;%[n,d] = butter(6,fc/(fs/2)); %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'); [num,den] = butter(2,fc*pb.Ts*2,'low');
Fz2=tf(num,den,pb.Ts); Fz2=tf(num,den,pb.Ts);
Fz=tf([1 0 0],den/sum(num),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;
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.B)
display(pb.F) display(pb.F)
fprintf('Kp:%f Kvfb:%f\n',pb.Kp,pb.Kvfb); 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 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; %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]; %filter [z^0 z^-1 ... z^-n];
@@ -176,29 +256,37 @@ function [pb]=DeltaTauParam(mot,mdl,param)
%!motor(mot=2,dirCur=0,contCur=800,peakCur=2400,timeAtPeak=1,IiGain=5,IpfGain=8,IpbGain=8,JogSpeed=10.,numPhase=3,invDir=True,servo=None,PhasePosSf=1./81250,PhaseFindingDac=100,PhaseFindingTime=50,SlipGain=0,AdvGain=0,PwmSf=10000,FatalFeLimit=200,WarnFeLimit=100,InPosBand=2,homing='enc-index') %!motor(mot=2,dirCur=0,contCur=800,peakCur=2400,timeAtPeak=1,IiGain=5,IpfGain=8,IpbGain=8,JogSpeed=10.,numPhase=3,invDir=True,servo=None,PhasePosSf=1./81250,PhaseFindingDac=100,PhaseFindingTime=50,SlipGain=0,AdvGain=0,PwmSf=10000,FatalFeLimit=200,WarnFeLimit=100,InPosBand=2,homing='enc-index')
switch param switch param
case 0 %scratch case 0 %scratch
desc=desc+"scratch"; desc=desc+"scratch";
pb.Kp=20.3255; %pole(mot.ss_dq)
pb.Kvfb=582.4551;pb.Kvff=582.4551; pl=[-300+150i -300-150i -2513];
pb.Kaff=5595.2;pb.Kafb=1077.9; [Am,Bm,Cm,Dm]=ssdata(mot.ss_dq);
pb.Ki=0.01;pb.MaxInt=1000; 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=0.1*Kfb(1)/(curr2acc*(pb.Ts^2));
pb.Kaff=1/(curr2acc*(pb.Ts^2))+pb.Kafb;
pb.Ki=0.001;
pb.MaxInt=1000;
% Ts=pb.Ts; fc=800;%Hz
% frq=75;d=.6; [num,den] = butter(2,fc*pb.Ts*2,'low');
% w0=frq*2*pi; Ez=tf([1 0 0],den/sum(num),pb.Ts);
% tfs=tf([w0^2],[1 2*d*w0 w0^2 ]) %Az=tf([.1 .2 .7],[1 0 0],pb.Ts) %is similar to the phase at feed forward only
% global currTf %h=bodeplot(Az);setoptions(h,'FreqUnits','Hz','Grid','on');
% frq0=55;d0=.5;
% frq1=85;d1=.5;
% w0=frq0*2*pi; %Bz=tf([.5 .5],[1 0],pb.Ts)
% w1=frq1*2*pi; %pb.E=Ez; %less noise sensitive on feedback
% tf0=tf([w0^2],[1 2*d0*w0 w0^2 ]) %pb.B=Bz; %less noise sensitive on feedback
% tf1=tf([1 2*d1*w1 w1^2 ],[w1^2]) fprintf('Kp:%f Kvfb:%f Kafb:%f\n',pb.Kp,pb.Kvfb,pb.Kafb);
% 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
case 1 %origin parameters case 1 %origin parameters
desc=desc+"orig"; 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.gather=gather
self.verbose=verbose self.verbose=verbose
#ServoPeriod=0.2 #ms, Sys.ServoPeriod is dependent of !common() macro #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={'srv_per':ServoPeriod,'pt2pt_time':40,'sync_flag':0,'sync_mode':2}
self.meta.update(kwargs) self.meta.update(kwargs)
@@ -106,6 +109,7 @@ class MotionBase:
self.sync_prg = prg self.sync_prg = prg
#download and start triggerSync code #download and start triggerSync code
comm=self.comm comm=self.comm
if comm is None: return
arg=0 #argument to pass to triggerSync program arg=0 #argument to pass to triggerSync program
if sync_mode==2:arg+=1 #synchronize if sync_mode==2:arg+=1 #synchronize
@@ -144,6 +148,7 @@ class MotionBase:
def run(self, crdId=1): def run(self, crdId=1):
'runs the code sync_run which has been generated with setup_sync()' 'runs the code sync_run which has been generated with setup_sync()'
comm = self.comm comm = self.comm
if comm is None: return
gpascii = comm.gpascii gpascii = comm.gpascii
try: try:
cmd=self.sync_run cmd=self.sync_run
@@ -157,6 +162,7 @@ class MotionBase:
return return
else: else:
comm = self.comm comm = self.comm
if comm is None: return
gpascii = comm.gpascii gpascii = comm.gpascii
while (True): while (True):
val = gpascii.get_variable('Coord[{crdId}].Q[0]'.format(crdId=crdId), type_=int) val = gpascii.get_variable('Coord[{crdId}].Q[0]'.format(crdId=crdId), type_=int)
@@ -172,7 +178,7 @@ class MotionBase:
if ge == 0: return -1 if ge == 0: return -1
cnt = gpascii.get_variable('Gather.Samples', type_=int) cnt = gpascii.get_variable('Gather.Samples', type_=int)
try: 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: except KeyError:
pass pass
return cnt return cnt
@@ -184,6 +190,7 @@ class MotionBase:
time.sleep(wait) time.sleep(wait)
if self.meta['sync_flag']&1: if self.meta['sync_flag']&1:
comm = self.comm comm = self.comm
if comm is None: return
gpascii = comm.gpascii gpascii = comm.gpascii
gpascii.send_block('Coord[1].Q[10]=1') gpascii.send_block('Coord[1].Q[10]=1')
else: else:

10
python/helicalscan.py
View File

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

40
python/shapepath.py
View File

@@ -240,9 +240,6 @@ class DebugPlot:
plt.show(block=False) plt.show(block=False)
plt.show(block=False)
def plot_pos_error(self): def plot_pos_error(self):
rec = self.rec # yA,xA,yD,xD,trig rec = self.rec # yA,xA,yD,xD,trig
ts=self.meta['srv_per']*self.meta['acq_per'] ts=self.meta['srv_per']*self.meta['acq_per']
@@ -455,7 +452,7 @@ class ShapePath(MotionBase):
verb=self.verbose verb=self.verbose
if verb&2: if verb&2:
self.plot_points(pts) self.plot_points(pts)
plt.show() plt.show(block=False)
def gen_swissfel_points(self,flipx=False,flipy=False,ofs=(0,0),width=1000): def gen_swissfel_points(self,flipx=False,flipy=False,ofs=(0,0),width=1000):
@@ -480,7 +477,7 @@ class ShapePath(MotionBase):
verb=self.verbose verb=self.verbose
if verb&2: if verb&2:
self.plot_points(pts) self.plot_points(pts)
plt.show() plt.show(block=False)
def gen_rand_points(self,n=107,scale=1000,ofs=(0,0)): def gen_rand_points(self,n=107,scale=1000,ofs=(0,0)):
'generate random distributed points' 'generate random distributed points'
@@ -500,10 +497,10 @@ class ShapePath(MotionBase):
pts+=ofs pts+=ofs
self.points=pts 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 #rInc radius increment per circle
r=rStart+np.arange(numSeg*numCir)*(float(rInc)/numSeg) 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=np.vstack((np.sin(ang)*r,np.cos(ang)*r)).T
pts+=ofs pts+=ofs
@@ -586,7 +583,7 @@ class ShapePath(MotionBase):
if verb&2: if verb&2:
DebugPlot.plot_points(pts) DebugPlot.plot_points(pts)
plt.show() plt.show(block=False)
self.points=pts self.points=pts
@@ -713,7 +710,7 @@ class ShapePath(MotionBase):
verb=self.verbose verb=self.verbose
if verb&16: if verb&16:
dp=DebugPlot(self);self.pvt=dp.plot_gen_pvt(pv) 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 pv[1:-1, (2, 3)]*=CoordFeedTime #scaling for Deltatau
prg.append(' linear abs') prg.append(' linear abs')
@@ -857,32 +854,37 @@ if __name__=='__main__':
#sp.gen_closed_shifted() #sp.gen_closed_shifted()
#sp.gen_swissmx_points(width=1000,ofs=(-500,0)) #sp.gen_swissmx_points(width=1000,ofs=(-500,0))
#sp.gen_swissfel_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_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=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_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=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_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=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.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_sync(verbose=args.verbose&32)
sp.setup_coord_trf() # reset to shape path system 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=1,dwell=10)
#sp.setup_motion(fnPrg=fn + '.prg', mode=1, scale=0,dwell=10) #sp.setup_motion(fnPrg=fn + '.prg', mode=1, scale=0,dwell=10)
sp.homing() #homing if needed sp.homing() #homing if needed
sp.run() #start motion program sp.run() #start motion program
sp.wait_armed() # wait until motors are at first position sp.wait_armed() # wait until motors are at first position
sp.trigger(0.5) #send a start trigger (if needed) ater given time sp.trigger(0.5) #send a start trigger (if needed) ater given time
while True: if not comm is None:
p=sp.progress() while True:
if p<0: break p=sp.progress()
print('progress %d'%p);time.sleep(.1) if p<0: break
sp.gather_upload(fnRec=fn+'.npz') print('progress %d/%d'%(p,sp.points.shape[0]));time.sleep(.1)
dp=DebugPlot(sp);dp.plot_gather(mode=11) sp.gather_upload(fnRec=fn+'.npz')
dp=DebugPlot(sp);dp.plot_gather(mode=11)
print('done') print('done')
plt.show(block=False) plt.show(block=False)