\documentclass[10pt,a4paper]{article}
\usepackage[margin=1.5cm]{geometry} %other geomentry
\setlength{\parindent}{0cm} %other indent of paragraphs
\usepackage[utf8]{inputenc}%needed for äöüàéè etc.
\usepackage[T1]{fontenc} %needed for äöüàéè etc. read http://de.wikipedia.org/wiki/LaTeX
\usepackage[bookmarks]{hyperref} %adds commands as \url for external links
\usepackage{fancyvrb} %Verbatim
\usepackage[abspath]{currfile} %%works only with PDFLaTeX-argument -recorder 
\usepackage{fancyhdr} %define new headers and footers
\pagestyle{fancy}
\fancyhead{} % sets header to nothing (instead of chapter)
\renewcommand{\headrulewidth}{0pt}%remove header line
\fancyfoot[R]{\currfilename}
\input{|"/home/zamofing_t/Documents/doc-git/git2texInfo.sh"}%works only with PDFLaTeX-argument -shell-escape
\usepackage{graphicx}
\usepackage{hyperref}
\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}
\usepackage{datetime}
\date{\today, \currenttime\\
\texttt{git:\gitAbrHash, ver:\gitVerNo{ }\gitStatus}} 

\begin{document}
\maketitle 
\tableofcontents
\section{Introduction}
This document describes the tuning and modeling process of the ESB-MX fast stages.

\section{Measurements}
The tool used to record data of the fast stages, to plot bode diagrams and to do further analysis is \verb|MXTuning.py.| The main call to collect all data is:\\
\verb|./MXTuning.py --dir MXTuning/19_01_29 --mode 64|\\

The used frequencies on the DeltaTau are:  20 kHz phase loop, 5 kHz servo loop, 6.25MHz AdcAmp. This results in 50us phase time and 0.2ms servo time.\\

According to the amplifier specs \cite[19]{PMAClv} a DAC Value of  $32737=2^{15}$ corresponds to 33.85A current. So 1 \verb|curr_bit| is $33.85/32737A =1.034mA$.\\

The Parker stages are configured to contCur=800mA ,peakCur=2400mA. Specs of the D11 stage \cite[23]{ParkerStage} are 0.8Amp RMS (producing 4N force) and 2.4Amp RMS peak.\\
It should be save to use the higher DC value of 0.92Amp and 2.8Amp instead of the RMS value.

\subsection{Measure Current Step}\label{sec:measCurStep}

The current regulation loop controls the voltage to reach the desired commanded current. It is measured and analyzed with:
\verb|MXTuning.py –mode 1| and \verb|identifyFxFyStage.m|\\

\includegraphics[scale=.5]{../matlab/figures/currstep_1.eps} 
\includegraphics[scale=.5]{/home/zamofing_t/Documents/doc-ext/DeltaTau/UsrMan257.png} 
The current step looks similar for both motors.
The transfer function is: $k \cdot {w_0}^2/({w_0}^2+2 \cdot damp \cdot w_0 \cdot s+s^2)$\\
the rise rise time $(0\rightarrow 100\%)$ is ca. 0.4ms\\
(rise time: \url{https://nptel.ac.in/courses/101108056/module7/lecture20.pdf}\\
$\rightarrow$ \verb|1/(w0*np.sqrt(1-0.75**2))*(np.pi/2+np.arcsin(0.75))|)\\

The loop parameters are:\\
IiGain=5, IpfGain=8, IpbGain=8


In steady state an idCmd=idMeas=406. results in aprox. idVolts=7400\\
This has been measured precisely from gathered data \verb|chirp_all_1a.npz|
at frequencies from 10 to 220 Hz.
The images have been generated with 
\verb|./MXTuning.py --dir MXTuning/19_01_29 --mode 128|.\\

\cite[245-259]{PMACusr} shows how the PwmSf parameter works and explains it with some calculation examples.
The settings of the PwmSf is done in the templates of gpasciiCommander:
\begin{verbatim}
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}\\

The overall aplification $iqCmd \rightarrow iqVolts$ is approx. 18.2.

\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)}
\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)}
\end{figure}

\FloatBarrier
\subsection{Measure Open Loop}

The frequency response has been measured with chirps at different amplitudes  and frequency regimes. The yellow line is averaged measurement data. The black line is the approximated model. The diagram shows \verb|curr_bits| (ca.1mA) to \verb|ActPos| (in um) transfer function.\\
The images have been generated with 
\verb|./MXTuning.py --dir MXTuning/19_01_29 --mode 64|.\\

\begin{figure}[h]
\includegraphics[trim=0cm 1.5cm 0cm 1cm,scale=.45]{../python/MXTuning/19_01_29/img/bode_model_plot0.eps} 
\includegraphics[trim=0cm 1.5cm 0cm 1cm,scale=.45]{../python/MXTuning/19_01_29/img/bode_model_plot2.eps} 
\caption{open loop of motor 1 and 2}
\label{fig:mot_open}
\end{figure}


\emph{Motor 1 (fy) has as expected a better response than Motor 2 (fx), because it has less mass to move.}\\

\hbox{
\parbox[t]{5cm}{
Motor 1:\\
0dB at 20.28Hz\\	
10 Hz: 9.63 dB -134\deg	\\
100Hz: -28.5dB -191.7\deg\\
resonances 198 Hz	\\
}

\parbox[t]{6cm}{
Motor 2\\
0dB at 12.39Hz\\
10 Hz: 4.68 dB -162\deg\\
100 Hz: -38.15 dB -187.5\deg\\
resonances 60Hz,142Hz,414Hz,231Hz\\
}
}

The bode plot drops as expected at approx. 40dB/dec. (due to the equation $F=m\ddot{x}$)\\

Here a example to roughly calculate at which frequency the motor moves 1um at 2A current:\\
1\texttt{curr\_bits}  is approx. 1mA. Hence at 0dB 1mA moves a sine of 1um.\\
10dB=factor 10, 20dB=factor 100, 30dB=factor 1000, 3dB $\approx$ factor 2\\
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\\

The chirp parameters for low frequencies bode measurements (yellow) are: \verb|minFrq=1, maxFrq=15, amp=1000|.\\
In this regime of 0 to 10Hz the bode amplitude is flat, which is mainly the result of the friction:\\
Motor 1 has 9.63dB($\approx$ factor 10) at 10 Hz. This means that an amplitude of 1000um needs roughly 100 \verb|curr_bits|. 
The friction as measured in figure \ref{fig:mot1_frict} is around 100 \verb|curr_bits|. At lower frequencies than 10 Hz with an amplitude of 1000um, the friction free motion consumes much less current (-40dB) than the friction.
Therefore the whole current is used to overcome the friction and the bode amplitude remains constant.


%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}
\subsubsection{DeltaTau schematics}

\includegraphics[scale=.7]{/home/zamofing_t/Documents/doc-ext/DeltaTau/UsrMan290.png}
\\

For now only features of the basic PID loop are used: \verb|Kp, Kvfb, Ki, Kvff, Kaff, MaxInt|.\\
These values have been tweaked as it is recommended by various DeltaTau turorials. But this  is just based on trial and error and not on regulation theory.\\
The settings so far are:
\begin{verbatim}
motor_servo(mot=1,Kp=25,Kvfb=400,Ki=0.02,Kvff=350,Kaff=5000,MaxInt=1000)
motor_servo(mot=2,Kp=22,Kvfb=350,Ki=0.02,Kvff=240,Kaff=1500,MaxInt=1000)
\end{verbatim}

With these setting the 'chirp sine closed loop' bode diagrams have been measured.

\subsubsection{chirp sine closed loop}


Figures \ref{fig:mot1_chirp} to \ref{fig:mot2_chirp_cmd}
shows chirp plot with the input(blue) and the output(green) and its bode plots.
The parameters for that chirp is:\\
\verb| amp: 5, minFrq: 10, maxFrq: 220, ts: 0.0002, tSec: 20|
 
\begin{figure}[h!]
\includegraphics[trim=0cm 1.5cm 0cm 1cm,scale=.45]{../python/MXTuning/19_01_29/img/chirp_all_1b0.eps} 
\includegraphics[trim=0cm 1.5cm 0cm 1cm, scale=.45]{../python/MXTuning/19_01_29/img/chirp_all_1b1.eps} 
\caption{DesPos->ActPos of motor 1}
\label{fig:mot1_chirp}
\end{figure}
\vspace{2cm}

\begin{figure}[h!]
\includegraphics[trim=0cm 1.5cm 0cm 1cm,scale=.45]{../python/MXTuning/19_01_29/img/chirp_all_2b0.eps} 
\includegraphics[trim=0cm 1.5cm 0cm 1cm,scale=.45]{../python/MXTuning/19_01_29/img/chirp_all_2b1.eps}
\caption{DesPos->ActPos of motor 2}
\label{fig:mot2_chirp}
\end{figure}


\begin{figure}[h!]
\includegraphics[trim=0cm 1.5cm 0cm 1cm,scale=.45]{../python/MXTuning/19_01_29/img/chirp_all_1b2.eps} 
\includegraphics[trim=0cm 1.5cm 0cm 1cm,scale=.45]{../python/MXTuning/19_01_29/img/chirp_all_1b3.eps} 
\caption{DesPos->IqCmd of motor 1}
\label{fig:mot1_chirp_cmd}
\end{figure}

\begin{figure}[h!]
\includegraphics[trim=0cm 1.5cm 0cm 1cm,scale=.45]{../python/MXTuning/19_01_29/img/chirp_all_2b2.eps} 
\includegraphics[trim=0cm 1.5cm 0cm 1cm,scale=.45]{../python/MXTuning/19_01_29/img/chirp_all_2b3.eps} 
\caption{DesPos->IqCmd  of motor 2}
\label{fig:mot2_chirp_cmd}
\end{figure}

\begin{tcolorbox}[colback=yellow!5!white,colframe=yellow!75!black,colbacktitle=yellow!50,coltitle=black,title=Conclusion]
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\% )\\
$\rightarrow$ improvements at that level without regulation theory background is based on luck.
\end{tcolorbox}


\FloatBarrier
\subsubsection{using advanced DeltaTau Servo Loop}

The standard PID servo loop (figure\ref{fig:deltatau_std_ctrl}) has some additional features, that were not yet used, especially the polynomial filters and nonlinear components as backlash and friction compensation.

\begin{figure}[h!]
\center
\includegraphics[trim=0cm 2cm 0cm 1cm,scale=.2]{/home/zamofing_t/Documents/doc-ext/DeltaTau/ServoBlockDiag.png}
\caption{position dependent friction of motor 2}
\label{fig:deltatau_std_ctrl}
\end{figure}

\cite[293]{PMACusr} shows details about the standard servo loop but unfortunately there are errors in documentation and the explanations are barely helpful.\\

\paragraph{Polynomial Filters:}
 There are no information in the documentation how these filters should be set.
\begin{description}
\item[A:] $Ka_0+Ka_1z^{-1}+\ldots+Ka_1z^{-7}$ 
\item[B:] $Kb_0+Kb_1z^{-1}+\ldots+Kb_1z^{-7}$ 
\item[C/D:] $\frac{1+Kc_1z^{-1}+\ldots+Kc_1z^{-7}}
                  {1+Kd_1z^{-1}+\ldots+Kd_1z^{-7}}$
\item[E:] $\frac{1}{1+Ke_1z^{-1}+Ke_1z^{-2}}$ 
\item[F:] $\frac{1}{1+Kf_1z^{-1}+Kf_1z^{-2}}$ 
\end{description}

Filters A, B, C, and D are 7th-order polynomials. 
if \texttt{Motor[x].Servo.SwPoly7}  is set to its default value of 0, these only execute as 2nd-order polynomials, saving computation time.
If this element is set to 1, these execute as full 7th-order polynomials.
Filters E and F are always 2nd-order polynomials.\\

\paragraph{$\mathbf{K_{fff}}$:}
The Value of $K_{fff}$ is used to compensate the non linear static friction. It can be estimated out of the bode plot at low frequencies. It is a non linear element, because it only depends on the sign if the velocity.\\

\paragraph{$\mathbf{K_{vff}}$:}
$K_{vff}$ is used to compensate the linear viscose friction.\\

\paragraph{$\mathbf{K_{aff}}$:}
On a friction free and error free plant a pure feed forward control in open loop would only need the parameter $K_{aff}$ to be set. The value of $K_{aff}$ can be calculated out of the bode diagram.\\

\subsubsection{Friction} \label{sec:friction}
The flat amplitude in figure \ref{fig:mot_open} at low frequencies is mostly the result of the static and viscose friction  and not of a linear mechanical spring and element as one could suggest out of the bode plot.\\


To measure the friction, the stage is moved at slow speed from
+lim to -lim. The current is proportional to the force.
Additionally the friction is measured depending on the stage positions and motion directions. This is measured at different speed to see if at these velocities the viscose friction is visible.

\begin{figure}[h!]
\includegraphics[trim=0cm 1.5cm 0cm 1cm,scale=.45]{../python/MXTuning/19_01_29/img/friction10.eps} 
\includegraphics[trim=0cm 1.5cm 0cm 1cm,scale=.45]{../python/MXTuning/19_01_29/img/friction11.eps} 
\caption{position dependant friction of motor 1}
\label{fig:mot1_frict}
\end{figure}

\begin{figure}[h!]
\includegraphics[trim=0cm 1.5cm 0cm 1cm,scale=.45]{../python/MXTuning/19_01_29/img/friction20.eps} 
\includegraphics[trim=0cm 1.5cm 0cm 1cm,scale=.45]{../python/MXTuning/19_01_29/img/friction21.eps} 
\caption{position dependant friction of motor 2}
\label{fig:mot2_frict}
\end{figure}

The recorded data moves from -10000um to 10000um at speed of 5 to 60 mm/s.\\
Data recording is done with: \verb|./MXTuning.py --dir MXTuning/19_01_29 --mode 16|.\\
The function parameters are: \verb|pMin=-10000, pMax=10000, speed=(5,10,20,30,60), cnt=2, period=10|.\\

Avg current forward: 111.7 Avg current backward: -105.3\\
Avg current forward: 95.3 Avg current backward: -130.7\\

Hence a value of  $K_{fff}\approx100$ should used to compensate friction.\\
Tests with the motors showed that this value will overcompensate and best results are achived with values of $K_{fff}\approx10$\\

\FloatBarrier

\newpage 
\section{Modeling the system}

The goal of this section is to make a mathematical model of the stage. Out of the bode diagram and the physical knowledge, the system consists of a current regulation loop and a mechanical setup with resonance frequencies.

\subsection{Electrical model}
The electrical model is derived from the knowledge that the motor consists of a resistance and a inductance. Further the control structure in the DeltaTau is known (section \ref{sec:measCurStep}).

\label{sec:mdlElec}
\begin{figure}[h!]
\centering
\includegraphics[scale=.45]{model1.eps} 
\includegraphics[scale=.6]{model2.eps} 
\caption{electrical model}
\end{figure}

Basic formulas: $U=R \cdot I$ \hspace{.5cm}
$U=L \cdot \frac{di}{dt}$\\

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$\\

using Masons rule:
\url{https://en.wikipedia.org/wiki/Mason's_gain_formula}:

\[
G_1(s)=\frac{y_{out}}{y_{in}}=\frac{iqCmd}{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}  
\]

\vspace{1pc}

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
c=Ipb \quad
d=1\\
&\text{using Masons rule:} \quad  G_2(s)=\frac{ab}{1+bc+abd}\\
\\
&\text{extending:} \quad =\frac{(Ipf+\frac{Ii}{s}) \cdot PwmSF \cdot G_1(s)}
{1+PwmSF \cdot G_1(s) \cdot Ipb +(Ipf+\frac{Ii}{s}) 
\cdot PwmSF \cdot G_1(s)}\\
\\
&=\left.\frac{(Ipf+\frac{Ii}{s}) \cdot PwmSF \cdot \frac{1}{Ls+R}}
{1+PwmSF \cdot \frac{1}{Ls+R} \cdot Ipb +(Ipf+\frac{Ii}{s}) \cdot PwmSF \cdot \frac{1}{Ls+R}} \right| \cdot  (Ls+R) \cdot s\\
\\
&=\frac{(Ipf \cdot s+Ii) \cdot PwmSF }
{(Ls+R)s+PwmSF \cdot Ipb \cdot s +(Ipf \cdot s+Ii) \cdot PwmSF }\\
\\
&=\frac{Ipf \cdot s+Ii }
{\frac{L}{PwmSF}s^2 +(\frac{R}{PwmSF}+ Ipb+Ipf)s +Ii}\\
\\
\end{aligned}
\]

To use real values we have to consider: The values
$IiGain=5, IpfGain=8, IpbGain=8$ for the DeltaTau are in z-domain.\\
Therefore for the continous domain they have to be scaled: 
$Ii=IiGain/ts \quad Ipf=IpfGain \quad Ipb=IpbGain$ with $ts=50\mu s$ (20kHz)\\

The overall amplification $iqCmd \rightarrow iqVolts$ measured in section \ref{sec:measCurStep} is approx. 18.2.

The resistance of the stage is 8.8 $\Omega$\\
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 matched.\\
Therefore the only approach is to use the second order transfer function as approximated in section \ref{sec:measCurStep}.\\

A further test is to 'turn of' the current loop. This can be done by setting:$IiGain=0, IpfGain=1, IpbGain=-1$.
The resulting transfer function is: 
\[
\frac{Ipf}
{\frac{L}{PwmSF}s +\frac{R}{PwmSF}} =\\
\frac{Ipf \cdot PwmSF}
{L s +R} =\\
\frac{\frac{Ipf \cdot PwmSF}{R}}
{\frac{L}{R} s +1}\\
\\
\]
This is a $PT_1$ element with a time constant of $\frac{L}{R}=\frac{2.4mH}{8.8\Omega}=0.27ms$. Figures \ref{fig:curr_ol} shows a current step of 3000 \verb|curr_bit| resulting in a current of about 150 \verb|curr_bit|. The rise time matches roughly. So the cable resistance has not a major impact. 

\begin{figure}[h!]
\centering
\includegraphics[scale=.25]{../python/MXTuning/19_01_29/img/curr_step_ol_10.eps} 
\includegraphics[scale=.25]{../python/MXTuning/19_01_29/img/curr_step_ol_20.eps} 
\caption{voltage step (current open loop)}
\label{fig:curr_ol}
\end{figure}


\subsection{Mechanical model}

\begin{figure}[h!]
\centering
\includegraphics[scale=.45]{model3.eps} 
\caption{mechanical model}
\end{figure}

Input: Force $u(t)=F$\\
Output: Position $y(t)=x_1(t)$\\
mass $m=m_1+m_2+\ldots+m_n$\\
damping: $d=d_1+d_2+\ldots+d_n$\\
springs: $k=k_1+k_2+\ldots+k_n$\\

Eq.\eqref{eq:mech1} shows the mechanical differential equations:

\begin{equation}
\begin{aligned}
 m_1\ddot{x}_1 = & u(t) -k_1x_1-d_1\dot{x}_1\\
                     & + k_2(x_2-x_1)+d_2(\dot{x}_2-\dot{x}_1)
                       + k_3(x_3-x_1)+d_3(\dot{x}_3-\dot{x}_1)
                       + k_4(x_4-x_1)+d_4(\dot{x}_4-\dot{x}_1)\\
 m_2\ddot{x}_2= & k_2(x_2-x_1)+d_2(\dot{x}_2-\dot{x}_1)\\
 m_3\ddot{x}_3= & k_3(x_3-x_1)+d_3(\dot{x}_3-\dot{x}_1)\\
 m_4\ddot{x}_4= & k_4(x_4-x_1)+d_4(\dot{x}_4-\dot{x}_1)\\
\end{aligned}\label{eq:mech1}
\end{equation}

\begin{equation}
\begin{aligned}
\mathbf{\dot{x} = Ax + Bu}\\
\mathbf{y = Cx + Du}
\end{aligned}\label{eq:mech2}
\end{equation}

Eq.\eqref{eq:mech2} are the general input output equations in matrix form with x and A defines as eq.\eqref{eq:mech3}:

\begin{equation}
\mathbf{x}=
\begin{bmatrix}
x_1\\
\dot{x}_1\\
x_2\\
\dot{x}_2\\
x_3\\
\dot{x}_3\\
x_4\\
\dot{x}_4\\
\end{bmatrix},\quad
\mathbf{\dot{x}}=
\begin{bmatrix}
\dot{x}_1\\
\ddot{x}_1\\
\dot{x}_2\\
\ddot{x}_2\\
\dot{x}_3\\
\ddot{x}_3\\
\dot{x}_4\\
\ddot{x}_4\\
\end{bmatrix},\quad
\mathbf{A}=
\begin{bmatrix}
0 & 1 & 0 & 0 & 0 & 0 & 0 & 0\\
-\frac{k}{m_1} & -\frac{d}{m_1} & \frac{k_2}{m_1} & \frac{d_2}{m_1} & \frac{k_3}{m_1} & \frac{d_3}{m_1} & \frac{k_4}{m_1} & \frac{d_4}{m_1}\\

0 & 0 & 0 & 1 & 0 & 0 & 0 & 0\\
-\frac{k_1}{m_2} & -\frac{d_1}{m_2} & \frac{k_2}{m_2} & \frac{d_2}{m_2} & 0 & 0 & 0 & 0 \\

0 & 0 & 0 & 0 & 0 & 1 & 0 & 0\\
-\frac{k_1}{m_3} & -\frac{d_1}{m_3} & 0 & 0 & \frac{k_3}{m_1} & \frac{d_3}{m_3} & 0 & 0 \\

0 & 0 & 0 & 0 & 0 & 0 & 0 & 1\\
-\frac{k_1}{m_4} & -\frac{d_1}{m_4} & 0 & 0 & 0 & 0 & \frac{k_4}{m_1} & \frac{d_4}{m_4} \\
\end{bmatrix},\quad
B=\begin{bmatrix}
\frac{1}{m_1} \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0\\
\end{bmatrix}
%,\quad
%C=\begin{bmatrix}
%\1 \\ 1 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0\\
%\end{bmatrix},\quad
%D=0
\label{eq:mech3}
\end{equation}



\begin{tcolorbox}[colback=yellow!5!white,colframe=yellow!75!black,colbacktitle=yellow!50,coltitle=black,title=Conclusion]
$k_1=0$: There is nothing like a spring that brings the system to a neutral position. The reason of the flat bode diagram at low frequencies is the friction. This non linear friction has to be compensate outside of the control loop. ($K_{fff}$)\\

$d_1$ seems to be negligible (section \ref{sec:stageData}).\\

The system is neither observable nor controllable.
$\rightarrow$ Perhaps the integrators of the system can be removed. having only $\mathbf{x}=[ \dot{x}_1 \dot{x}_2 \dot{x}_3 \dot{x}_4]$ as the state vector.\\

But all that resonance frequency stuff seems hardly controllable. Therefore the only way to have good control seems to avoid any resonance frequency.\\

Neverless that model helped a lot to understand the whole system and to build good suited models in MATLAB.
\end{tcolorbox}

\subsection{Stage data}
\label{sec:stageData}

This data comes from datasheets \cite{ParkerStage} and construction information \cite{DynParker}.

\begin{tabular}{|r|l|}
\hline
Stage Y mass& 340g \\
Stage X mass& 950g \\
Interferometer mirrors & 51g (additional)\\
Aluminum (instead ABS) & 42g (additional)\\
\hline
\end{tabular}

\vspace{1pc}

\begin{tabular}{|r|c|l|}
\hline
Continuous force  && 5.51N \\
Peak force  && 12N \\
Static friction && 1N\\
Viscose damping && 0.5N$\cdot$s/m\\
Motor constant &Km& 1.46N/$\sqrt{watt}$\\
Resistance &R&8.8$\Omega$\\
Inductance &L& 2.4mH\\
\hline
\end{tabular}

\vspace{1pc}

The data sheet for the current are quite confusing (sine, trap, rms). But lets check the motor constant Km.\\
The data sheet says:\\
Stall Current Continuous 0.92A (trap, DC), Stall force Continuous 4N
\[
U=R\cdot I \qquad P=U \cdot I \quad \rightarrow \quad  P=R \cdot I^2\\
\]
at a constant current of 0.92A we have $ 8.8 \cdot 0.92^2 = 7.44 $W the resulting force will be:\\
$1.46N \cdot \sqrt{7.44} = 3.98 N$\\

Lets have a look at the viscose damping:\\
our speed is roughly 50$\mu$m/10ms. this is 5mm/s. The damping force at that speed is: $0.5\frac{Ns}{m}\cdot 5\frac{mm}{s}=2\cdot10^{-3}N$.
The used current for that force is negligible, and therefore the viscose damping is negligible.



\subsection{identification of stages}

The goal is to build an optimal state space model of the plant with following input and output taps:

\begin{figure}[h!]
\center
\includegraphics[scale=.7]{model4.eps} 
\caption{model taps}
\end{figure}


\verb#full_bode_mot[1|2].mat# is generated from \verb|MXTuning.py| in function \verb|bode_model_plot()|. It contains the bode data of real measurements and approximated transfer functions.

\verb|identifyFxFyStage.m| reads the python data \verb#full_bode_mot[1|2].mat# and build motor objects with transfer functions and state space models.

The approximated transfer functions can be tweaked and edited in MATLAB with: e.g.\\
\verb|controlSystemDesigner('bode',1,mot1.tf_xx)| to enhance the model.
The full transfer function is then split in individual parts that are put back into \verb|MXTuning.py|.\\

the transfer functions for each two motors are separated atomic transfer function as stated in table \ref{tab:trfFunc1} :\\
\-\hspace{2em}tfc = transfer function current\\
\-\hspace{2em}tfp = transfer function position\\
\-\hspace{2em}tf[1-9]=transfer function for resonance\\
\-\hspace{2em}Consecutive letters are simplified transfer functions (tfd,tfq). 


\begin{table}[h!]
\center
\begin{tabular}{|l|l|l|l|}
\hline
key & description                          & motor1 (fy)                 & motor2 (fx)                 \\
\hline
tfc & current loop tf                      & f=694 ? 1389 , d=0.75       & same                        \\
tfd & simplified cur.loop tf $PT_1$        & f-3dB, 45\deg at 400Hz      & same                        \\
\hline
tfp & \vtop{\hbox{\strut main mechanical tf} \hbox{\strut with -40dB/dec }}
                                           & mag=6dB f=7.96Hz d=0.6      & mag=12dB f=3.34 d=0.4       \\
tfq & \vtop{\hbox{\strut simplified mechanical tf} \hbox{\strut 2 integrators with -40dB/dec }}
                                           & 0dB at 19.8Hz       & 0dB at 11.84Hz\\
\hline
tf1 & mechanical resonance tf              & f=[197,199] d=[0.02,0.02]   & f=[55|61] d=[0.2|0.2]       \\
tf2 & mechanical resonance tf              &                             & f=[128|137] d=[0.05|0.05]   \\
tf3 & mechanical resonance tf              &                             & f=[410|417] d=[0.015|0.015] \\
tf4 & mechanical resonance tf              &                             & f=[230|233] d=[0.04|0.04]   \\
\hline
\end{tabular}
\caption{plant partial transfer functions}
\label{tab:trfFunc1}
\end{table}
%\vspace{1pc}

The black line on figure \ref{fig:mot_open} shows the concatenate transfer function $tfc \cdot tfp \cdot tf1 \cdot tf2 \cdot tf3 \cdot tf4$.\\
\vspace{1pc}

\fbox{\parbox[t]{15cm}{The current loop frequency from the MATLAB identification looks different than the used one. MATLAB identifies $w_0=8727rad/sec = f0=1389Hz$ but to match the bode plot a value of half frequency need to be taken:$f_0=694$. The reason could be discretization and time delay because the servo loop is processed after the phase loop.}}
\vspace{1pc}

\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

\subsection{chirp sine closed loop with simulink model}

To compare the measurements with the model following lines were executed in MATLAB
\begin{verbatim}
clear;clear global;close all;
mot=identifyFxFyStage(7);
for k =1:2
  [pb]=simFxFyStage(mot{k});sim('stage_closed_loop');
  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_DTGz_%d',mot{k}.id),'-depsc');
end
\end{verbatim}

The Simulink model \verb|stage_closed_loop| contains various controller blocks. as  \verb|PID G(s)|, \verb|PID G(z)|, \verb|ServoDeltaTau_z G(s)| and \verb|ServoDeltaTau_z G(z)|.

With the controller block \verb|ServoDeltaTau_z G(z)| the real measurements can be compared with the simulated model.
Figures \ref{fig:mot_chirp_sim} showed similar response as the real stages (Figures \ref{fig:mot1_chirp}, \ref{fig:mot2_chirp}). Therefore the model matches the real stage well.


\begin{figure}[h!]
\center

\includegraphics[scale=.45]{../matlab/figures/sim_cl_DTGz.png}\\
\includegraphics[scale=.45]{../matlab/figures/sim_cl_DTGz_1.eps} 
\includegraphics[scale=.45]{../matlab/figures/sim_cl_DTGz_bode1.eps}\\ 
\includegraphics[scale=.45]{../matlab/figures/sim_cl_DTGz_2.eps} 
\includegraphics[scale=.45]{../matlab/figures/sim_cl_DTGz_bode2.eps} 
\caption{Motor 1 sim	Motor 2 sim}
\label{fig:mot_chirp_sim}
\end{figure}


The model can be used to imrove the controller in MATLAB/simulink.

Improvement of the regulation with a simple PID loop and with prefilters did not improve the regulation quality.\\
Therefore for better regulation quality more sophisticated controllers as state controllers will be tested.

\subsection{State Space Controller with Observer}

\verb|(s.a. http://ctms.engin.umich.edu/CTMS/index.php?example=Introduction&section=ControlStateSpace)|

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})\\

\begin{figure}[h!]
\centering
\includegraphics[scale=.75]{observer_statefeedback.png} 
\caption{observer controller}\label{fig:observer1}
\end{figure}


To implement a state space controller with observer the model has to be observable and controllable. The full model of the transfer function $tfc \cdot tfp \cdot  tf1 \cdot tf2 \cdot tf3 \cdot tf4$ is not controllable due to the various resonance frequencies. Therefore for the observer design the state space model has to be simplified. (see simplified transfer function in table  \ref{tab:trfFunc1})\\


Figure \ref{fig:mdl_bode1} shows the bode plots of the best model to the most simplified model:\\

\begin{tabular}{ll}
plant & best model transfer function: $tfc \cdot tfp \cdot  tf1 \cdot tf2 \cdot tf3 \cdot tf4$\\
mdlcp & main mechanical with second order current loop: $tfc \cdot tfp$\\
mdldp & main mechanical with $PT_1$ current loop (first order): $tfd \cdot tfp$\\
mdlp  & main mechanical (incl. 'viscose friction'): $tfp$\\
mdlq  & only 2 integrators: $tfq$\\
\end{tabular}
\vspace{1pc}

\begin{figure}[h!]
\center
\includegraphics[scale=.44]{../python/MXTuning/19_01_29/img/bode_model_plot1.eps} 
\includegraphics[scale=.44]{../python/MXTuning/19_01_29/img/bode_model_plot3.eps} 
\caption{bode plots of best and simplified models}
\label{fig:mdl_bode1}
\end{figure}

\verb|identifyFxFyStage.m| is the code that generates 'state space models' of different complexity out of the these transfer functions. All that information is stored in the 'motor objects'.\\


As first approach the tf function is just converted to the ss space and the ss matrices are glued together.

The matlab models are:\\

\begin{tabular}{ll}
\texttt{ss\_plt:} &  best approach of the plant with mechanics, resonance, current loop etc.\\
\texttt{ss\_cp:} & model without resonance (only current and main mechanical)\\
\texttt{ss\_dp:} & model without resonance (simplified current and main mechanical)\\
\texttt{ss\_p:}  & model without current loop, no resonance (only main mechanical)\\
\texttt{ss\_q:} & simplified mechanical, no current loop, no resonance\\
\end{tabular}\\
\vspace{1pc}

The state space controller is calculated by pole placement.\\
Following code calculates parameters for a observer controller, does a simulation and plots the results:
\begin{verbatim}
clear;clear global;close all;
mot=identifyFxFyStage(7);
for sscType=0:1
  for k=1:2
    [ssc]=StateSpaceControlDesign(mot{k},sscType);sim('observer');
    f=figure(); h=plot(desPos_actPos.Time,desPos_actPos.Data,'g');grid on;
    set(h(1),'color','b'); set(h(2),'color',[0 0.5 0]);
    print(f,sprintf('figures/sim_cl_obs_%d_%d',sscType,mot{k}.id),'-depsc');
    f=bodeSamples(desPos_actPos);
    print(f,sprintf('figures/sim_cl_obs_bode%d_%d',sscType,mot{k}.id),'-depsc');
  end
end


\end{verbatim}


\begin{figure}[h!]
\center
\includegraphics[scale=.35]{../matlab/figures/sim_cl_observer.png} 
\caption{Observer flow chart}
\label{fig:mot_observer_sim}
\end{figure}


\begin{figure}[h!]
\center
\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}



Comparing figures \ref{fig:mot_chirp_sim} and \ref{fig:mot_observer_sim} shows the better behavious of the observer model.

With an additional prefilter resonance and reductions can be suppressed a bit.\\

\begin{figure}[h!]
\center
\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_1_1.eps} 
\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_bode1_1.eps}\\
\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_1_2.eps} 
\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_bode1_2.eps} 
\caption{Observer with prefilter sim: Motor 1 Motor 2}
\label{fig:mot_observer_sim}
\end{figure}



The system was simulated with 5kHz servo loop frequency.
Higher servo loop frequency does not help, because a continuous state space controller behaves not relevantly better than a sampled state space controller at 5kHz.\\

Now the controller must be implemented on DeltaTau to check the performance on the real stage.

\FloatBarrier
\subsubsection{Implementing state space controller on DeltaTau}

In MATLAB the function \verb|StateSpaceControlDesign()| produces files \verb%/tmp/ssc[1|2].mat%.\\

The servo loop code is generated with: \verb|MXTuning.py –mode 256|.\\
This python code reads \verb%/tmp/ssc[1|2].mat% and substitutes part of 
\verb%usr_code/usrcode_template.[h|c]% to build the servo loop code \verb%usr_code/usrcode.[h|c]%.\\

Finally the real time servo code is compiled and installed on the DeltaTau with:\\

\verb|/epics_ioc_modules/ESB_MX/python/usr_code$ make install|\\

Following lines in gpasciiCommander will activate the user servo loop code.
\vspace{1pc}

The \verb|make install| process consists of many sub parts:\\
%\begin{itemize}
%\setlength\itemsep{-1em}
%\item asfgd
%\item asfgd
%\item asfgd
%\end{itemize}
- generate observer date in MATLAB with \verb|StateSpaceControlDesign()|\\
- generate code in python with \verb|MXTuning.py|\\
- compile the code with \verb|MXTuning.py| (make all)\\
- upload the module to DeltaTau (make install)\\
- activate the user servo loop (make install)\\

Here the full process:

\begin{tcolorbox}[width=15cm,colback=yellow!5!white,colframe=yellow!75!black,colbacktitle=yellow!50,coltitle=black,title=MATLAB]
\begin{verbatim}
clear;clear global;close all;
mot=identifyFxFyStage(7);
sscType=0;
for k =1:2
  [ssc]=StateSpaceControlDesign(mot{k},sscType);
end
\end{verbatim}
\end{tcolorbox}

\begin{tcolorbox}[width=15cm,colback=yellow!5!white,colframe=yellow!75!black,colbacktitle=yellow!50,coltitle=black,title=Shell]
\begin{verbatim}
cd /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python
./MXTuning.py -m512

cd /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/usr_code
make

cd /home/zamofing_t/Documents/prj/SwissFEL/PBTools/pbtools/usr_servo_phase
scp userservo_util userphase_util usrServoSample/usralgo.ko root@SAR-CPPM-EXPMX1:/tmp


\end{verbatim}
\end{tcolorbox}

\begin{tcolorbox}[width=15cm,colback=yellow!5!white,colframe=yellow!75!black,colbacktitle=yellow!50,coltitle=black,title=DeltaTau Shell]
\begin{verbatim}
root@:/opt/ppmac#
LD_LIBRARY_PATH=/opt/ppmac/libppmac/ /tmp/userservo_util -d 1
rmmod usralgo
insmod /tmp/usralgo.ko
LD_LIBRARY_PATH=/opt/ppmac/libppmac/ /tmp/userservo_util -l 1 usr_servo_ctrl_2
LD_LIBRARY_PATH=/opt/ppmac/libppmac/ /tmp/userservo_util -e 1

gpascii:
Motor[1].Ctrl =UserAlgo.ServoCtrlAddr[1]
\end{verbatim}
\end{tcolorbox}


\FloatBarrier
\subsubsection{The reality of the state space controller}

The state space controller assumes that the system is observable and controllable. The bode plot shows a flat amplitude at low frequencies, which makes the feeling, that the system is observable and controllable. But in fact the reason of that flat amplitude is friction (section \ref{sec:friction}).\\
This results to the fact that the stage consists of really 2 integrators and behaves without friction roughly like $F=m \cdot \ddot{x}$. But an integrator $\frac{1}{s}$ is neither observable nor controllable. Therefore we have to check, how to implement an optimal controller for such a system. A state space observer controller seems not to be appropriate in stat case.\\


\subsection{Theoretical approach for $\frac{1}{s^2}$ plant}

A controller consists of a feed forward H(s) and a feed back G(s) path.


\begin{figure}[h!]
\center
\includegraphics[scale=.9]{model5.eps} 
\caption{$1/s^2$ plant}
\end{figure}


Assume a plant consists of a integrator $\frac{1}{s}$ the overall transfer functions are $\frac{y}{u}=\frac{\frac{H}{s}+\frac{G}{s}}{1+\frac{G}{s}}=\frac{H+G}{s+G}$ and $\frac{y}{e}=\frac{\frac{G}{s}}{1+\frac{G}{s}}=\frac{G}{s+G}$. Setting H(s)=s and G(s)=k results in a overall transfer function of $\frac{y}{u}=\frac{s+G}{s+G}=1$ and $\frac{y}{e}=\frac{k}{s+k}$. So the output follows the input and k defines how fast errors will be compensated.\\


Simulating a single integrator plant in MATLAB works. But with 2 integrators the system is unstable because of the two derivate elements. With a discrete differentiator it becomes stable again. In fact a derivate element is a very critical element, because it can create instability and adds a lot of noise. But in this case it is acceptable, because is acts on the trajectory and not on the feedback loop. The only thing that has to be granted is that the trajectory is twice differentiable. This is the case with limited jerk or with the pvt motion. A step response should be avoided and not used to tweak the parameters.\\

\includegraphics[scale=.45]{FF_FB_ctrl.png} 
The optimal parameters for a pure feed forward systems are calculated
in equation \ref{eq:calc_Kaff}:

\def\ccdot{\negthinspace\cdot\negthinspace}

\begin{equation}
\begin{aligned}
&DesPosFB=DesVelFF=0,\quad posErrFB=velErrFB=accErrFB=0\\
&\text{Motor 1: 19.8Hz$\hat{=}$0dB, Ts=5kHz=.2ms,}\quad
k=(19.8 \ccdot 2 \ccdot \pi)^2\\
&Kaff = 1/(Ts \ccdot Ts \ccdot k) = 1/((19.8 \ccdot 2 \ccdot \pi)^2/5000^2) = 1615.2\\
&\text{Motor 2:11.84Hz$\hat{=}$0dB, Ts=5kHz=.2ms,}\quad
k=(11.84 \ccdot 2 \ccdot \pi)^2\\
&Kaff = 1/(Ts \ccdot Ts \ccdot k) = 1/((11.84 \ccdot 2 \ccdot \pi)^2/5000^2) = 4517.28\\
\end{aligned}\label{eq:calc_Kaff}
\end{equation}

To correct errors, posErrFB is increased. But in a discrete system, the regulation becomes instable, because the actPos always lags the desPos for one sample. To avoid instability, a filter can attenuate this problem.\\

The Standard Delta Tau controller (figure \ref{fig:deltatau_std_ctrl}), shows a very similar feed forward and feedback loop structure with additional filters. So after all the measurements, we can calculate the optimal Kfff and Kaff values.
Kp and Ki values will attenuate the errors. Kvff=Kvfb makes the system stable at higher Kp values, but setting the filter B seems to be more appropriate.\\
Now that we had some experimental experiences the focus is to use regulation theory knowledge to find the best parameters.\\

\subsection{Controller with pole placement}\label{sec:ctr_pp}

As a full observer controller is not possible, the goal is to implement a controller with pole placement with a simplified plant model. The simplified motion (tfq) with simplified current (tfd) loop has all the needed states directly as measurement values or as calculated values in the default DeltaTau servo controller. The used states are: position, velocity, acceleration(proportional to measured current). With these states a pole placement controller can be implemented. The calculated values are: Kp Kvfb and Kafb. The result is a quite good looking controller.\\
Additional feed forward gains follows the trajectory even better by setting of these values:
\begin{itemize}
\setlength\itemsep{0em}
\item optimize Kfff by tests to roughly 10
\item Kvff=Kvfb
\item Kaff as calculated in equation \ref{eq:calc_Kaff}.
\item Ki value will attenuate the static error, but also make the dynamic regulation worse
\end{itemize}


MATLAB simulations \verb|(tool: DeltaTauOptimizer.m)| and real test showed similar behaviour and the following error could be reduced roughly by factor 2. (Appendix \ref{ax:optimize}). Further the behavior at higher frequencies (50Hz, 100Hz FEL repetition rate) is much better.\\

Further improvements could be done with filters to shape the bode diagram. But first of all lets focus on the induced frequencies from the trajectory: The trajectory contains frequencies of half of the FEL repetition rate. so e.g. 0-50Hz at a FEL rate of 100 Hz. (section \ref{sec:trajPlan}). Therefore to follow the trajectory only frequencies below 50Hz are relevant. In this regime the bode plot looks very good. Only at higher frequencies the bode plot looks worse, mainly because of the impact of the resonance frequencies. Therefore filters in the feed forward path would make no sense. Only filters in the closed loop can enhance the regulation quality.\\

Therefore in the figure \ref{fig:deltatau_std_ctrl} the filter $\frac{C}{D}$ is the main filter that can enhance the regulation quality. Up to now, this was not achived, because the regulation seems to be stronger than the effect of the filter.\\

One point that is very important to keep in mind: The response analyses focused so far the input trajectory to the measured position. But the frequencies relevant for that are only below 50Hz, because this is the maximal frequency of the trajectory. An other point is the ability to correct position errors. For that the bode plot without any feed forward gains has to be analyzed. That should be the next point to focus on and to optimize.\\

For the time being, these regulation parameters should be good enough for the beam time in March 2019 but still some further minor improvements should be possible.\\

In a test following situations have been observed:\\
Use parameters as stated in section: \ref{sec:ctr_pp} with poles \verb|[-600+750i -600-750i -2513]| and \verb|Ki=0|

$\rightarrow$ The pure feed forward controller will be close to 1 at high frequencies\\
$\rightarrow$ The pure feed back controller will be close to 1 at high frequencies\\
$\rightarrow$ The feed forward with feed back controller will overshoot at high frequencies\\

The reason for that is that the delayed position is feed back and overcompensated then by the feedback\\
To avoid this behaviour, the feed forward filter F can be tweaked as a lowpass filter to attenuate this  overcompensation.\\

This works good and enhanced the trajectory following at high frequencies. But this will not enhance a lot the real application, because high frequencies are suppressed in the trajectory planing.\\

Much more important is to focus now on the closed loop filters B, C/D and E.


%   Motor 2:11.84Hz 0dB
%   Kaff = 1/((11.84*2*np.pi)**2/5000/5000) = 4517.278506241804


%   Motor 1: 19.8Hz 0dB
%   K=(19.8*2*np.pi)**2
%   Ts=5kHz=.2ms
%   Kaff = 1/(Ts*Ts*K) = 1/((19.8*2*np.pi)**2/5000/5000) = 1615.2877200403302

%   Motor 2:11.84Hz 0dB
%   Kaff = 1/((11.84*2*np.pi)**2/5000/5000) = 4517.278506241804
\FloatBarrier

\section{Shapepath: trajectory planing}

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.\\
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.

\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.\\
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:}
pft means 'position-velocity-time with fft velocity'. This approach makes the best approximation of the ift-motion with a pvt motion. For that the steepness (derivate) of the ift has to be calculated in each sampling point. The derivate in time domain corresponds to scale the Fourier coefficients in frequency domain: 

\begin{align*}
\mathcal{F}(f')(\xi)&=\int_{-\infty}^{\infty}e^{-2\pi i\xi t}f'(t)\,dt\\
&=e^{-2\pi i\xi t}f(t)\bigr\vert_{t=-\infty}^{\infty}-\int_{-\infty}^{\infty}-2\pi i\xi e^{-2\pi i \xi t}f(t)\,dt\\
&=2\pi i\xi\cdot\mathcal{F}(f)(\xi)
\end{align*}

With that function the velocities in each point can be calculated easily.\\
But high speed on FEL impact makes the system very time sensitive.
Therefore the trajectory planing is additional implemented in such a way, that the calculated speed can be scaled from 1 to 0 (1=pft 0=p0t).
With that an optimal trade off between 'best bandwith (pft)' and 'low speed on FEL impact(p0t)' can be achived.\\

\begin{figure}[h!]
\center
\includegraphics[trim=0cm 0.3cm 0cm 1.5cm,scale=.45]{traj1.eps}\\ 
\includegraphics[trim=0cm 0cm 0cm 1cm,scale=.40]{traj2.eps}
\includegraphics[trim=0cm 0cm 0cm 1cm,scale=.40]{traj3.eps} 
\label{fig:traj}
\caption{trajectory planing}
\end{figure}


Figure \ref{fig:traj} shows one axis trajectory planing with 40ms equidistant sampling points. The displayed trajectories are ift, pvt, p0t and pft. The other figure shows the frequency spectrum. The blue line shows a strong drop at 12.5Hz as expected (40ms sampling $\rightarrow$ 25Hz $\rightarrow$ Nyquist frequency = 12.5Hz). The last figure shows the difference of the pvt, p0t and pft frequency spectrum compared to ift (y scale not significant).\\
As expected pft matches the best to the ift spectrum and has better bandwidth behaviors than the other trajectory planing approaches.

\subsection{Timing}

The FEL time is synchronized with the 50Hz of the SwissGrid network. \url{(https://www.swissgrid.ch/de/home/operation/regulation/frequency.html}). But this frequency is changing in a range of about 
$\pm0.1\%$. This variation if fatal for precise hitting of the target. Therefore the speed of motion must be adapted to match the SwissGrid frequency. This is done with a real time process at 10kHz on the DeltaTau that monitors the timing signal of the EVR. After each shot it guesses the time of next impact and the motion speed is adapted. With that we can achieve a timing jitter of around 0.2ms. If this is not enough it could be enhanced in future with hardware approaches (EVR on encoder entry).


\FloatBarrier
\section{HelicalScan coordinates}

For helical scan the motors cx, cz, rot and fy must be moved. As this coordinates are not simple to interpret at all, a new coordinate system is introduced. That coordinate system is the crystal based.\\
it has components rotation fy, dz, dy. It is done in a way, that dx and dz is the deviation in x and z of the 'crystal axis'.
For this coordinate system the forward and inverse kinematic has to be calculated.
This is done in \verb|helicalscan.py| before the motion program is started.\\

To find the rotation center the Fourier transformation is used:\\
x positions are measured at equidistant angles between 0\deg and 360\deg. The base frequency of the Fourier transformation will give the amplitude and the phase of crystal motion relative to the rotation center. This needs at leas 3 points e.g. 0\deg, 120\deg 240\deg.

As it is very inconvenient to align 6 points per crystal (bottom 3 points, top 3 points) a faster way was searched.

After a first precise alignment of a needle the precise position of the rotation center is known. This information never changes. The only thing that changes is the position of the crystal relative to that rotation center.\\
With only one point (1 at bottom one at top) the crystam position can be caldulated using the x and z values.

With an additional point at an other angle (90\deg is the best) the crystal position can be enhanced.

With that approach crystal alignment is very fast and user friendly.


\begin{figure}[h!]
\includegraphics[scale=.4]{helicalscan1a.eps} 
\includegraphics[scale=.4]{helicalscan2a.eps} 
\includegraphics[scale=.4]{helicalscan3a.eps} 
\caption{coordinate transformation}
\end{figure}



\FloatBarrier
\begin{appendix}
\section{Appendix}

\subsection{real measurements motor 1,2}
\label{sec:real_meas}

Real measurements have been done. The quality of the measurement is done with:\\
$\rightarrow$  spiral motion move at 25Hz (quantitative values)\\
$\rightarrow$ following trajectory of a chirp (graphics) signal \verb|amp: 5, minFrq: 10, maxFrq: 220, ts: 0.0002, tSec: 20|

\begin{verbatim}

***ORIG 19_01_29***
Motor[1] Kp=25 Kvfb=400 Ki=0.02 Kvff=350 Kaff=5000 MaxInt=1000 Kfff=0
Motor[2] Kp=22 Kvfb=350 Ki=0.02 Kvff=240 Kaff=1500 MaxInt=1000 Kfff=0
(spiral motion 25 Hz)
motion average error x 1.54099 um, y 1.04259 um, 2.0391 um
shot average error x 1.68984 um, y 1.42837 um, 2.47304 um
\end{verbatim}
\includegraphics[scale=.45]{../python/MXTuning/19_01_29/img/chirp_all_1b0.eps} 
\includegraphics[scale=.45]{../python/MXTuning/19_01_29/img/chirp_all_1b1.eps} 

\includegraphics[scale=.45]{../python/MXTuning/19_01_29/img/chirp_all_2b0.eps} 
\includegraphics[scale=.45]{../python/MXTuning/19_01_29/img/chirp_all_2b1.eps} 


\begin{verbatim}
**OPTIMIZED opt1***
Motor[1] Kp=25 Kvfb=350 Ki=0.02 Kvff=350 Kaff=1615. MaxInt=1000 Kfff=10
Motor[2] Kp=22 Kvfb=450 Ki=0.02 Kvff=450 Kaff=4517. MaxInt=1000 Kfff=10
motion average error x 0.663518 um, y 0.585719 um, 1.01806 um
shot average error x 0.865708 um, y 0.965126 um, 1.49323 um
\end{verbatim}
\includegraphics[scale=.45]{../python/MXTuning/opt1/img/chirp_all_1b0.eps} 
\includegraphics[scale=.45]{../python/MXTuning/opt1/img/chirp_all_1b1.eps}

\includegraphics[scale=.45]{../python/MXTuning/opt1/img/chirp_all_2b0.eps} 
\includegraphics[scale=.45]{../python/MXTuning/opt1/img/chirp_all_2b1.eps} 



\subsection{simulations}




\begin{tcolorbox}[colback=red!5!white,colframe=red!75!black,colbacktitle=red!50,coltitle=black,title=TODO]
\begin{verbatim}
images of spiral motion: with pvt,pvt0,ift motion
\end{verbatim}
\end{tcolorbox}




\textbf{Overview of code:}\\

\begin{tabular}{ll}
\texttt{MXTuning.py} &  tuning functions to gather/plot data and to generate c servo loop code\\

\texttt{helicalscan.py} & functions for helical scan motion (motion program, coordinate transformation, calibration)\\
\texttt{shapepath.py} & functions for x,y-scan motion (trajectory planing, motion program)\\
\texttt{ShapePathAnalyser.py}   & interactive analyze tool of recorded x,y-scan motion\\
\\
\texttt{identifyFxFyStage.m}&       read python data and build motor objects. plot bode\\
\texttt{simFxFyStage.m}&           build a pb structure which contains current (Jan 2019) Powerbrick controller settings\\
\texttt{StateSpaceControlDesign.m}& design a discrete observer for Fx,Fy stages\\
\end{tabular}


\bibliographystyle{alpha}
\bibliography{myBib}
%\printbibliography
\end{appendix}

%\include{Scratch}

\end{document}

