\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 is the bode plots is MXTuning.py, a script specially developed to record system responses. The main call to collect all data was:\\
\verb|./MXTuning.py --dir MXTuning/19_01_29 --mode 64|\\
The used frequencies 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$.\\

\cite[245-259]{PMACusr} Shows how the PwmSf works and is explained with some calculation examples.\\
This is set in the gpasciiCommander templates:
\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}
Nerverless the documentation is confusing. Therefore PwmSf will be measured to convert idCmd bits values to idVolts bits in section \ref{sec:measCurStep}

The Parker stages are configured to contCur=800mA ,peakCur=2400mA. Specs of the D11 stage 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}
\verb|MXTuning.py –mode 1| $\rightarrow$ \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)$\\
rise time $(0\rightarrow 100\%)$ 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 precisly 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|.\\

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

\begin{figure}[h!]
\includegraphics[scale=.45]{../python/MXTuning/19_01_29/img/iqCmd_TF0.eps} 
\includegraphics[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[scale=.45]{../python/MXTuning/19_01_29/img/iqCmd_TF2.eps} 
\includegraphics[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 regions. 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 32|.\\

\begin{figure}[h]
\includegraphics[scale=.45]{../python/MXTuning/19_01_29/img/bode_model_plot0.eps} 
\includegraphics[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 $F=m\ddot{x}$)

Here a example to roughly calculate at which frequency the motor moves 1um at 2A current:\\
1curr\_bits  is approx. 1mA. 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\\

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

Closed loops have been measured with the following control loop settings.
\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}


\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[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} 
\caption{DesPos->ActPos of motor 1}
\label{fig:mot1_chirp}
\end{figure}

\begin{figure}[h!]
\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}
\caption{DesPos->ActPos of motor 2}
\label{fig:mot2_chirp}
\end{figure}


\begin{figure}[h!]
\includegraphics[scale=.45]{../python/MXTuning/19_01_29/img/chirp_all_1b2.eps} 
\includegraphics[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[scale=.45]{../python/MXTuning/19_01_29/img/chirp_all_2b2.eps} 
\includegraphics[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}

Moving 5um with frequencies from 10 to 220Hz\\
$\rightarrow$ at frequencies above 200 Hz, the current increses up to 2 amps, and the the following error kicks in\\
$\rightarrow$ The closed loop response becomes bad above 20Hz (motor 1 ca. -10\%, motor 2 +5\% )\\

\FloatBarrier
\subsubsection{Friction}

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[scale=.45]{../python/MXTuning/19_01_29/img/friction10.eps} 
\includegraphics[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[scale=.45]{../python/MXTuning/19_01_29/img/friction20.eps} 
\includegraphics[scale=.45]{../python/MXTuning/19_01_29/img/friction21.eps} 
\caption{position dependant friction of motor 2}
\label{fig:mot2_frict}
\end{figure}

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|.
The recorded data moves from -10000um to 10000um at speed of 5 to 60 mm/s.\\

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

These values should be used for the $K_{fff}$ parameter of the Servo loop:\\

Motor 1,2: $K_{fff}\approx100$\\

\FloatBarrier
\subsubsection{using advanced Deltatau Servo Loop}

For now only following settings of the servo loop are used:Kp, Kvfb, Ki, Kvff, Kaff, MaxInt.\\
The standard PID servo loop has some additional features, that were not yet used, especially the polynomial filters.

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

\cite[293]{PMACusr} shows details about the standard servo loop but unfortunately there are errors in documentation and the explanations are not really helpful. Hopefully the implementation is correct.\\

\textbf{Polynomial Filters:}
\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{Kc_0+Kc_1z^{-1}+\ldots+Kc_1z^{-7}}
                  {Kd_0+Kd_1z^{-1}+\ldots+Kd_1z^{-7}}$
\item[E:] $\frac{1}{Ke_0+Ke_1z^{-1}+Ke_1z^{-2}}$ 
\item[F:] $\frac{1}{Kf_0+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.\\

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

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

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

\begin{tcolorbox}[colback=red!5!white,colframe=red!75!black,colbacktitle=red!50,coltitle=black,title=TODO]
Make simulations in MATLAB. Set C/D filter to compensate resonance and the current loop.\\
This sshould be mostly the inverse of the figures: \ref{fig:mot1_chirp} and \ref{fig:mot2_chirp}.\\
Use $K_{fff}$
\end{tcolorbox}




\newpage 
\section{Modeling the system}
\subsection{Electrical model}
\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 aplification $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 mached.\\
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$\\

\eqref{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}
\dot{x} = Ax + Bu\\
y = Cx + Du
\end{aligned}\label{eq:mech2}
\end{equation}

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

\begin{equation}
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
\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
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}

\subsection{Stage data}

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

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

\vspace{1pc}

\begin{tabular}{|r|c|l|}
\hline
Continous 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 in the data sheet are quite confusing. But lets check the motor Konstant Km.\\
The data sheet says:\\
Stall Current Continous 0.92A, Stall force Continous 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$


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

\begin{table}[h!]
\center
\begin{tabular}{|l|l|l|l|}
\hline
key & description                          & motor1 (fy)                 & motor2 (fx)              \\
\hline
tfc & curren loop tf                       & f=694 ? 1389 , d=0.75       & same                     \\
tf1 & \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    \\
tf2 & mechanical resonance tf              & f=[197,199] d=[0.02,0.02]   & f=[55|61] d=[0.2|0.2]    \\
tf3 & mechanical resonance tf              &                             & f=[128|137] d=[0.05|0.05]   \\
tf4 & mechanical resonance tf              &                             & f=[410|417] d=[0.015|0.015] \\
tf5 & 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 $tf1 \cdot tf2 \cdot tf3 \cdot tf4 \cdot tf5 \cdot tfc$ .
\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{Simulink/MATLAB simulations}

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.

\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 cpntroller 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 tranfer function $tf1 \cdot tf2 \cdot tf3 \cdot tf4 \cdot tf5 \cdot tfc$ is not controllable due to the various resonance frequencies. Therefore for the observer design the state space model has to be simplified.

In addition to the transferfunction in table  \ref{tab:trfFunc1} following simplified transfer functions have been added:\\

\begin{tabular}{|l|l|l|l|}
\hline
key & description                          & motor1 (fy)                 & motor2 (fx)              \\
\hline
tfd & simplified loop tf  $PT_1$           & f-3dB, 45\deg at 400Hz       & same                     \\
tf0 & \vtop{\hbox{\strut simplified mechanical tf} \hbox{\strut 2 integrators with -40dB/dec }}
                                           & 0dB at 19.8Hz       & 0dB at 11.84Hz\\
\hline
\end{tabular}
\vspace{1pc}

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

\begin{tabular}{ll}
plant & best model tranfer function: $tf1 \cdot tf2 \cdot tf3 \cdot tf4 \cdot tf5 \cdot tfc$\\
mdl1c & main mechanical with second order current loop: $tf1 \cdot tfc$\\
mdl1d & main mechanical with $PT_1$ current loop (first order): $tf1 \cdot tfd$\\
mdl1  & main mechanical (incl. 'viscose friction'): $tf1$\\
mdl0  & only 2 integrators: $tf0$\\
\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\_c1:} & model without resonance (only current and main mechanical)\\
\texttt{ss\_d1:} & model without resonance (simplified current and main mechanical)\\
\texttt{ss\_1:}  & model without current loop, no resonance (only main mechanical)\\
\texttt{ss\_0:} & 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);
sscType=0
for k =1:2
  [ssc]=StateSpaceControlDesign(mot{k},sscType);sim('observer');
  f=figure(); h=plot(desPos_actPos.Time,desPos_actPos.Data,'g');
  set(h(1),'color','b'); set(h(2),'color',[0 0.5 0]);
  print(f,sprintf('figures/sim_cl_observer_%d',mot{k}.id),'-depsc');
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_observer_1.eps} 
\includegraphics[scale=.45]{../matlab/figures/sim_cl_observer_bode1.eps}\\
\includegraphics[scale=.45]{../matlab/figures/sim_cl_observer_2.eps} 
\includegraphics[scale=.45]{../matlab/figures/sim_cl_observer_bode2.eps} 
\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_pf_1.eps} 
\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_pf_bode1.eps}\\
\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_pf_2.eps} 
\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_pf_bode2.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 continious 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
\subsection{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 compliled for the DeltaTau with:\\

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

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

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}





\vspace{1pc}

\begin{appendix}
\section{Appendix}

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

