diff --git a/MXfastStageDoc/MXfastStage.tex b/MXfastStageDoc/MXfastStage.tex index 60ce4c6..f56a062 100644 --- a/MXfastStageDoc/MXfastStage.tex +++ b/MXfastStageDoc/MXfastStage.tex @@ -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] diff --git a/MXfastStageDoc/traj1.eps b/MXfastStageDoc/traj1.eps index 7f0a00f..8ea9696 100644 --- a/MXfastStageDoc/traj1.eps +++ b/MXfastStageDoc/traj1.eps @@ -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 diff --git a/MXfastStageDoc/traj2.eps b/MXfastStageDoc/traj2.eps index 678f844..180e52a 100644 --- a/MXfastStageDoc/traj2.eps +++ b/MXfastStageDoc/traj2.eps @@ -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 diff --git a/MXfastStageDoc/traj3.eps b/MXfastStageDoc/traj3.eps index 0528e4a..4f428f6 100644 --- a/MXfastStageDoc/traj3.eps +++ b/MXfastStageDoc/traj3.eps @@ -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 diff --git a/matlab/DeltaTauOptimizer.m b/matlab/DeltaTauOptimizer.m index eb620fd..ef4ffc0 100644 --- a/matlab/DeltaTauOptimizer.m +++ b/matlab/DeltaTauOptimizer.m @@ -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; @@ -130,4 +112,38 @@ function simData=bodeSim(simData) set(ax2, 'XScale', 'log'); linkaxes([ax1,ax2],'x') legend('Location','best'); -end \ No newline at end of file +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 diff --git a/matlab/DeltaTauParam.m b/matlab/DeltaTauParam.m index 4ed99db..b2608a7 100644 --- a/matlab/DeltaTauParam.m +++ b/matlab/DeltaTauParam.m @@ -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; + 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; - % 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) + 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) - 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 %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]; @@ -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') 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; + desc=desc+"scratch"; + %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; + + 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; -% 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 + 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"; diff --git a/matlab/DeltaTauSim.slx b/matlab/DeltaTauSim.slx index fc33cf0..f4e7e4f 100644 Binary files a/matlab/DeltaTauSim.slx and b/matlab/DeltaTauSim.slx differ diff --git a/python/MXMotion.py b/python/MXMotion.py index 1577f94..222175c 100644 --- a/python/MXMotion.py +++ b/python/MXMotion.py @@ -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: diff --git a/python/helicalscan.py b/python/helicalscan.py index ba46894..aed531e 100755 --- a/python/helicalscan.py +++ b/python/helicalscan.py @@ -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 ### diff --git a/python/shapepath.py b/python/shapepath.py index acda3c1..20751e8 100755 --- a/python/shapepath.py +++ b/python/shapepath.py @@ -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)