sf-mx/PBSwissMX
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

major renaming of transfer functions

Browse Source
This commit is contained in:
zamofing_t
2019-02-15 10:46:29 +01:00
parent f3c5fafe5f
commit e792d0b028
11 changed files with 592 additions and 580 deletions
Download Patch File Download Diff File Expand all files Collapse all files

126
MXfastStageDoc/MXfastStage.tex
View File

@@ -128,6 +128,13 @@ Out of the bode plot we can read approx.:\\
Motor 1: -33dB at 130Hz\\ Motor 1: -33dB at 130Hz\\
Motor 2: -33dB at 84Hz\\ 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 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 %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 %1um at 12Hz with 1 mA $\rightarrow$ with 2000mA $\rightarrow$ sqrt(2000)*12Hz=540Hz
@@ -385,15 +392,15 @@ springs: $k=k_1+k_2+\ldots+k_n$\\
\begin{equation} \begin{equation}
\begin{aligned} \begin{aligned}
\dot{x} = Ax + Bu\\ \mathbf{\dot{x} = Ax + Bu}\\
y = Cx + Du \mathbf{y = Cx + Du}
\end{aligned}\label{eq:mech2} \end{aligned}\label{eq:mech2}
\end{equation} \end{equation}
\eqref{eq:mech2} are the general input output equations in matrix form with x and A defines as \eqref{eq:mech3}: \eqref{eq:mech2} are the general input output equations in matrix form with x and A defines as \eqref{eq:mech3}:
\begin{equation} \begin{equation}
x= \mathbf{x}=
\begin{bmatrix} \begin{bmatrix}
x_1\\ x_1\\
\dot{x}_1\\ \dot{x}_1\\
@@ -404,7 +411,7 @@ x_3\\
x_4\\ x_4\\
\dot{x}_4\\ \dot{x}_4\\
\end{bmatrix},\quad \end{bmatrix},\quad
\dot{x}= \mathbf{\dot{x}}=
\begin{bmatrix} \begin{bmatrix}
\dot{x}_1\\ \dot{x}_1\\
\ddot{x}_1\\ \ddot{x}_1\\
@@ -415,7 +422,7 @@ x_4\\
\dot{x}_4\\ \dot{x}_4\\
\ddot{x}_4\\ \ddot{x}_4\\
\end{bmatrix},\quad \end{bmatrix},\quad
A= \mathbf{A}=
\begin{bmatrix} \begin{bmatrix}
0 & 1 & 0 & 0 & 0 & 0 & 0 & 0\\ 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}\\ -\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}\\
@@ -440,7 +447,14 @@ B=\begin{bmatrix}
\label{eq:mech3} \label{eq:mech3}
\end{equation} \end{equation}
\begin{tcolorbox}[colback=red!5!white,colframe=red!75!black,colbacktitle=red!50,coltitle=black,title=TODO]
$k_1=0$ The non linear friction has to be compensate outside of the control loop.\\
$d_1$ seems to be negligable (section \ref{sec:stageData}).\\
But the system is not observable. $\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
\end{tcolorbox}
\subsection{Stage data} \subsection{Stage data}
\label{sec:stageData}
This data comes from datasheets and construction information \cite{DynParker}. This data comes from datasheets and construction information \cite{DynParker}.
@@ -469,16 +483,19 @@ Inductance &L& 2.4mH\\
\vspace{1pc} \vspace{1pc}
The data in the data sheet are quite confusing. But lets check the motor Konstant Km.\\ The data sheet for the current are quite confusing (sine, trap, rms). But lets check the motor Konstant Km.\\
The data sheet says:\\ The data sheet says:\\
Stall Current Continous 0.92A, Stall force Continous 4N Stall Current Continous 0.92A (trap, DC), Stall force Continous 4N
\[ \[
U=R\cdot I \qquad P=U \cdot I \quad \rightarrow \quad P=R \cdot I^2\\ 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:\\ 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$ $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 negligable, and theerefore the viscose damopling is negligable.
\subsection{identification of stages} \subsection{identification of stages}
@@ -500,20 +517,26 @@ The approximated transfer functions can be tweaked and edited with: e.g. \verb|c
The full transfer function is then split in individual parts that are put back into \verb|MXTuning.py|.\\ 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}\\ the transfer functions for each two motors are separated atomic transfer function as stated in table \ref{tab:trfFunc1}\\
tfc = transfer function current, tfp = transfer function position. Consecutive letters are simplified transfer functions. tf[1-9]=transfer function resonance.
\begin{table}[h!] \begin{table}[h!]
\center \center
\begin{tabular}{|l|l|l|l|} \begin{tabular}{|l|l|l|l|}
\hline \hline
key & description & motor1 (fy) & motor2 (fx) \\ key & description & motor1 (fy) & motor2 (fx) \\
\hline \hline
tfc & curren loop tf & f=694 ? 1389 , d=0.75 & same \\ tfc & current loop tf & f=694 ? 1389 , d=0.75 & same \\
tf1 & \vtop{\hbox{\strut main mechanical tf} \hbox{\strut with -40dB/dec }} tfd & simplified cur.loop tf $PT_1$ & f-3dB, 45\deg at 400Hz & same \\
& mag=6dB f=7.96Hz d=0.6 & mag=12dB f=3.34 d=0.4 \\ \hline
tf2 & mechanical resonance tf & f=[197,199] d=[0.02,0.02] & f=[55|61] d=[0.2|0.2] \\ tfp & \vtop{\hbox{\strut main mechanical tf} \hbox{\strut with -40dB/dec }}
tf3 & mechanical resonance tf & & f=[128|137] d=[0.05|0.05] \\ & mag=6dB f=7.96Hz d=0.6 & mag=12dB f=3.34 d=0.4 \\
tf4 & mechanical resonance tf & & f=[410|417] d=[0.015|0.015] \\ tfq & \vtop{\hbox{\strut simplified mechanical tf} \hbox{\strut 2 integrators with -40dB/dec }}
tf5 & mechanical resonance tf & & f=[230|233] d=[0.04|0.04] \\ & 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 \hline
\end{tabular} \end{tabular}
\caption{plant partial transfer functions} \caption{plant partial transfer functions}
@@ -521,7 +544,7 @@ tf5 & mechanical resonance tf & & f=[23
\end{table} \end{table}
%\vspace{1pc} %\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$ . 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} \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.}} \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.}}
@@ -584,29 +607,17 @@ opposite to the PID cpntroller which is a ‘black-box design’, the state spa
\end{figure} \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. To implement a state space controller with observer the model has to be observable and controllable. The full model of the tranfer 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}\\
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:\\ Figure \ref{fig:mdl_bode1} shows the bode plots of the best model to the most simplified model:\\
\begin{tabular}{ll} \begin{tabular}{ll}
plant & best model tranfer function: $tf1 \cdot tf2 \cdot tf3 \cdot tf4 \cdot tf5 \cdot tfc$\\ plant & best model tranfer function: $tfc \cdot tfp \cdot tf1 \cdot tf2 \cdot tf3 \cdot tf4$\\
mdl1c & main mechanical with second order current loop: $tf1 \cdot tfc$\\ mdlcp & main mechanical with second order current loop: $tfc \cdot tfp$\\
mdl1d & main mechanical with $PT_1$ current loop (first order): $tf1 \cdot tfd$\\ mdldp & main mechanical with $PT_1$ current loop (first order): $tfd \cdot tfp$\\
mdl1 & main mechanical (incl. 'viscose friction'): $tf1$\\ mdlp & main mechanical (incl. 'viscose friction'): $tfp$\\
mdl0 & only 2 integrators: $tf0$\\ mdlq & only 2 integrators: $tfq$\\
\end{tabular} \end{tabular}
\vspace{1pc} \vspace{1pc}
@@ -627,10 +638,10 @@ The matlab models are:\\
\begin{tabular}{ll} \begin{tabular}{ll}
\texttt{ss\_plt:} & best approach of the plant with mechanics, resonance, current loop etc.\\ \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\_cp:} & model without resonance (only current and main mechanical)\\
\texttt{ss\_d1:} & model without resonance (simplified current and main mechanical)\\ \texttt{ss\_dp:} & model without resonance (simplified current and main mechanical)\\
\texttt{ss\_1:} & model without current loop, no resonance (only main mechanical)\\ \texttt{ss\_p:} & model without current loop, no resonance (only main mechanical)\\
\texttt{ss\_0:} & simplified mechanical, no current loop, no resonance\\ \texttt{ss\_q:} & simplified mechanical, no current loop, no resonance\\
\end{tabular}\\ \end{tabular}\\
\vspace{1pc} \vspace{1pc}
@@ -639,13 +650,18 @@ Following code calculates parameters for a observer controller, does a simulatio
\begin{verbatim} \begin{verbatim}
clear;clear global;close all; clear;clear global;close all;
mot=identifyFxFyStage(7); mot=identifyFxFyStage(7);
sscType=0 for sscType=0:1
for k =1:2 for k=1:2
[ssc]=StateSpaceControlDesign(mot{k},sscType);sim('observer'); [ssc]=StateSpaceControlDesign(mot{k},sscType);sim('observer');
f=figure(); h=plot(desPos_actPos.Time,desPos_actPos.Data,'g'); 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]); 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'); 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
\end{verbatim} \end{verbatim}
@@ -659,10 +675,10 @@ end
\begin{figure}[h!] \begin{figure}[h!]
\center \center
\includegraphics[scale=.45]{../matlab/figures/sim_cl_observer_1.eps} \includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_0_1.eps}
\includegraphics[scale=.45]{../matlab/figures/sim_cl_observer_bode1.eps}\\ \includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_bode0_1.eps}\\
\includegraphics[scale=.45]{../matlab/figures/sim_cl_observer_2.eps} \includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_0_2.eps}
\includegraphics[scale=.45]{../matlab/figures/sim_cl_observer_bode2.eps} \includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_bode0_2.eps}
\caption{Observer sim: Motor 1 Motor 2} \caption{Observer sim: Motor 1 Motor 2}
\label{fig:mot_observer_sim} \label{fig:mot_observer_sim}
\end{figure} \end{figure}
@@ -675,10 +691,10 @@ With an additional prefilter resonance and reductions can be suppressed a bit.\\
\begin{figure}[h!] \begin{figure}[h!]
\center \center
\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_pf_1.eps} \includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_1_1.eps}
\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_pf_bode1.eps}\\ \includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_bode1_1.eps}\\
\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_pf_2.eps} \includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_1_2.eps}
\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_pf_bode2.eps} \includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_bode1_2.eps}
\caption{Observer with prefilter sim: Motor 1 Motor 2} \caption{Observer with prefilter sim: Motor 1 Motor 2}
\label{fig:mot_observer_sim} \label{fig:mot_observer_sim}
\end{figure} \end{figure}

280
PBInspect1.pbi
View File

@@ -1,4 +1,9 @@
[ [
[
"StatusGblListCtrl",
[],
"name=g;caption=global;state=67377148;dir=4;layer=0;row=0;pos=0;prop=100000;bestw=200;besth=369;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=41;floaty=486;floatw=208;floath=377;notebookid=-1;transparent=255"
],
[ [
"MotorListCtrl", "MotorListCtrl",
[ [
@@ -24,160 +29,207 @@
[ [
"WatchListCtrl", "WatchListCtrl",
[ [
[
"EncTable[9].PrevEnc",
null
],
[
"EncTable[10].PrevEnc",
null
],
[
"EncTable[11].PrevEnc",
null
],
[
"EncTable[12].PrevEnc",
null
],
[
"EncTable[13].PrevEnc",
null
],
[
"EncTable[14].PrevEnc",
null
],
[
"EncTable[15].PrevEnc",
null
],
[
"EncTable[16].PrevEnc",
null
],
[ [
"Motor[1].idCmd", "Motor[1].idCmd",
"lambda v: '%.8g'%float(v)" null
],
[
"Motor[1].iqCmd",
"lambda v: '%.8g'%float(v)"
],
[
"Motor[1].ServoOut",
"lambda v: '%.8g'%float(v)"
],
[
"Motor[2].iqCmd",
"lambda v: '%.8g'%float(v)"
], ],
[ [
"Motor[2].idCmd", "Motor[2].idCmd",
"lambda v: '%.8g'%float(v)" null
],
[
"Motor[2].ServoOut",
"lambda v: '%.8g'%float(v)"
],
[
"Motor[3].iqCmd",
"lambda v: '%.8g'%float(v)"
], ],
[ [
"Motor[3].idCmd", "Motor[3].idCmd",
"lambda v: '%.8g'%float(v)" null
],
[
"Motor[3].ServoOut",
"lambda v: '%.8g'%float(v)"
],
[
"Motor[4].iqCmd",
"lambda v: '%.8g'%float(v)"
], ],
[ [
"Motor[4].idCmd", "Motor[4].idCmd",
"lambda v: '%.8g'%float(v)" null
],
[
"Motor[4].ServoOut",
"lambda v: '%.8g'%float(v)"
],
[
"Motor[5].iqCmd",
"lambda v: '%.8g'%float(v)"
], ],
[ [
"Motor[5].idCmd", "Motor[5].idCmd",
"lambda v: '%.8g'%float(v)" null
],
[
"Motor[5].ServoOut",
"lambda v: '%.8g'%float(v)"
],
[
"Motor[6].iqCmd",
"lambda v: '%.8g'%float(v)"
], ],
[ [
"Motor[6].idCmd", "Motor[6].idCmd",
"lambda v: '%.8g'%float(v)" null
],
[
"Motor[6].ServoOut",
"lambda v: '%.8g'%float(v)"
],
[
"Motor[7].iqCmd",
"lambda v: '%.8g'%float(v)"
], ],
[ [
"Motor[7].idCmd", "Motor[7].idCmd",
"lambda v: '%.8g'%float(v)"
],
[
"Motor[7].iqCmd",
"lambda v: '%.8g'%float(v)"
],
[
"P1000",
null null
], ],
[ [
"P2000", "Motor[8].idCmd",
null
],
[
"Motor[1].ServoOut",
null
],
[
"Motor[2].ServoOut",
null
],
[
"Motor[3].ServoOut",
null
],
[
"Motor[4].ServoOut",
null
],
[
"Motor[5].ServoOut",
null
],
[
"Motor[6].ServoOut",
null
],
[
"Motor[7].ServoOut",
null
],
[
"Motor[8].ServoOut",
null
],
[
"Gate3[1].Chan[0].UserFlag",
null null
] ]
], ],
"name=w;caption=watch;state=67377148;dir=4;layer=0;row=1;pos=0;prop=100000;bestw=200;besth=369;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=221;floaty=858;floatw=208;floath=377;notebookid=-1;transparent=255" "name=w;caption=watch;state=67377148;dir=4;layer=0;row=2;pos=0;prop=100000;bestw=200;besth=369;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=221;floaty=858;floatw=208;floath=377;notebookid=-1;transparent=255"
], ],
[ [
"StatusMotorListCtrl", "StatusMotorListCtrl",
[ [
3 1
], ],
"name=m3;caption=motor3;state=67377148;dir=4;layer=0;row=6;pos=0;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=694;floaty=941;floatw=208;floath=677;notebookid=-1;transparent=255" "name=m1;caption=motor1;state=67377148;dir=4;layer=0;row=3;pos=0;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=485;floaty=810;floatw=208;floath=677;notebookid=-1;transparent=255"
],
[
"StatusGblListCtrl",
[],
"name=g;caption=global;state=67377148;dir=4;layer=0;row=0;pos=0;prop=100000;bestw=200;besth=369;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=2058;floaty=457;floatw=208;floath=377;notebookid=-1;transparent=255"
], ],
[ [
"StatusMotorListCtrl", "StatusMotorListCtrl",
[ [
2 2
], ],
"name=m2;caption=motor2;state=67377148;dir=4;layer=0;row=5;pos=1;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=2588;floaty=946;floatw=208;floath=677;notebookid=-1;transparent=255" "name=m2;caption=motor2;state=67377148;dir=4;layer=0;row=3;pos=1;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=520;floaty=734;floatw=208;floath=677;notebookid=-1;transparent=255"
], ],
[ [
"StatusMotorListCtrl", "WatchListCtrl",
[ [
4 [
"P2010",
"lambda v: '%.5g'%float(v)"
],
[
"P2011",
"lambda v: '%.5g'%float(v)"
],
[
"P2012",
"lambda v: '%.5g'%float(v)"
],
[
"P2013",
"lambda v: '%.5g'%float(v)"
],
[
"P2014",
"lambda v: '%.5g'%float(v)"
],
[
"P2015",
"lambda v: '%.5g'%float(v)"
],
[
"P2016",
"lambda v: '%.5g'%float(v)"
],
[
"P2017",
"lambda v: '%.5g'%float(v)"
],
[
"P2018",
"lambda v: '%.5g'%float(v)"
],
[
"P2019",
"lambda v: '%.5g'%float(v)"
],
[
"P2020",
"lambda v: '%.5g'%float(v)"
],
[
"P2021",
"lambda v: '%.5g'%float(v)"
],
[
"P2022",
"lambda v: '%.5g'%float(v)"
],
[
"P2023",
"lambda v: '%.5g'%float(v)"
],
[
"P2024",
"lambda v: '%.5g'%float(v)"
],
[
"P2025",
"lambda v: '%.5g'%float(v)"
],
[
"P2026",
"lambda v: '%.5g'%float(v)"
],
[
"P2027",
"lambda v: '%.5g'%float(v)"
],
[
"P2028",
"lambda v: '%.5g'%float(v)"
],
[
"P2029",
"lambda v: '%.5g'%float(v)"
]
], ],
"name=m4;caption=motor4;state=67377148;dir=4;layer=0;row=7;pos=0;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=905;floaty=792;floatw=208;floath=677;notebookid=-1;transparent=255" "name=w-1;caption=watch;state=67377148;dir=4;layer=0;row=1;pos=0;prop=129863;bestw=200;besth=369;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=464;floaty=580;floatw=208;floath=377;notebookid=-1;transparent=255"
],
[
"StatusMotorListCtrl",
[
5
],
"name=m5;caption=motor5;state=67377148;dir=4;layer=0;row=7;pos=1;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=1139;floaty=836;floatw=208;floath=677;notebookid=-1;transparent=255"
],
[
"StatusMotorListCtrl",
[
7
],
"name=m7;caption=motor7;state=67377148;dir=4;layer=0;row=8;pos=1;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=1353;floaty=657;floatw=208;floath=677;notebookid=-1;transparent=255"
],
[
"StatusMotorListCtrl",
[
6
],
"name=m6;caption=motor6;state=67377148;dir=4;layer=0;row=8;pos=0;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=1276;floaty=771;floatw=208;floath=677;notebookid=-1;transparent=255"
],
[
"StatusMotorListCtrl",
[
1
],
"name=m1;caption=motor1;state=67377148;dir=4;layer=0;row=5;pos=0;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=669;floaty=892;floatw=208;floath=677;notebookid=-1;transparent=255"
],
[
"StatusCoordListCtrl",
[
1
],
"name=c1;caption=coord1;state=67377148;dir=4;layer=0;row=2;pos=0;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=527;floaty=439;floatw=208;floath=677;notebookid=-1;transparent=255"
] ]
] ]

108
matlab/StateSpaceControlDesign.m
View File

@@ -39,43 +39,67 @@ function [ssc]=StateSpaceControlDesign(mot,mode)
%use_lqr: use lqr instead of pole placement %use_lqr: use lqr instead of pole placement
verb=1; t=0:1E-4:.5; %time vector for simulations
verb=0;
use_lqr=0; use_lqr=0;
MaxDac=2011.968; MaxDac=2011.968;
filt_pos_err=Prefilt(mot,2); tfDesPos=tf(1,1);
tfPosErr=tf(1,1);
ssc=struct();
ssc.sel1={3,[3]};
%locate poles: 2500rad/s = 397Hz, 6300rad/s = 1027Hz %locate poles: 2500rad/s = 397Hz, 6300rad/s = 1027Hz
switch mode switch mode
case -1 %TESTING case -1 %TESTING
case 0 %for document: best observer without prefilter
ss_plt=mot.ss_plt; %ss_plt ss_c1 ss_d1 ss_1 ss_0 ss_plt=mot.ss_plt; %ss_plt ss_c1 ss_d1 ss_1 ss_0
ss_mdl=mot.ss_d1; %ss_plt ss_c1 ss_d1 ss_1 ss_0 ss_mdl=mot.ss_cp; %ss_plt ss_c1 ss_d1 ss_1 ss_0
if mot.id==1
pl=[-2200 -2100 -2000]; % stable with scaling of .05 .. 1.0;
else
pl=[-2500 -900 -800];
end
plObs=2*pl;
case 0
ss_plt=mot.ss_plt; %ss_plt ss_c1 ss_d1 ss_1 ss_0
ss_mdl=mot.ss_c1; %ss_plt ss_c1 ss_d1 ss_1 ss_0
if mot.id==1 if mot.id==1
pl=[-3300 -3200 -2900 -2800]; pl=[-3300 -3200 -2900 -2800];
else else
pl=[-3300 -3200 -2700 -2600]; pl=[-3300 -3200 -2700 -2600];
end end
plObs=2*pl; plObs=2*pl;
case 1 case 1 %for document: best observer without prefilter
ss_plt=mot.ss_plt; %ss_plt ss_c1 ss_d1 ss_1 ss_0 ss_plt=mot.ss_plt; %ss_plt ss_c1 ss_d1 ss_1 ss_0
ss_mdl=mot.ss_d1; %ss_plt ss_c1 ss_d1 ss_1 ss_0 ss_mdl=mot.ss_cp; %ss_plt ss_c1 ss_d1 ss_1 ss_0
if mot.id==1
pl=[-3300 -3200 -2900 -2800];
else
pl=[-3300 -3200 -2700 -2600];
end
plObs=2*pl;
tfDesPos=Prefilt(mot,2);%user designed envelope
tfPosErr=Prefilt(mot,1);%inverse resonance
case 2 %for document: best observer with prefilter
ss_plt=mot.ss_plt; %ss_plt ss_c1 ss_d1 ss_1 ss_0
ss_mdl=mot.ss_dp; %ss_plt ss_c1 ss_d1 ss_1 ss_0
if mot.id==1 if mot.id==1
pl=[-2200 -2100 -2000]; % stable with scaling of .05 .. 1.0; pl=[-2200 -2100 -2000]; % stable with scaling of .05 .. 1.0;
else else
pl=[-2500 -900 -800]; pl=[-2500 -900 -800];
end end
plObs=2*pl; plObs=2*pl;
case 2 case 3
ss_plt=mot.ss_plt; %ss_plt ss_c1 ss_d1 ss_1 ss_0 ss_plt=mot.ss_plt; %ss_plt ss_c1 ss_d1 ss_1 ss_0
ss_mdl=mot.ss_c1; %ss_plt ss_c1 ss_d1 ss_1 ss_0 ss_mdl=mot.ss_dp; %ss_plt ss_c1 ss_d1 ss_1 ss_0
use_lqr=1 if mot.id==1
pl=[-2200 -2100 -2000]; % stable with scaling of .05 .. 1.0;
else
pl=[-2500 -900 -800];
end
plObs=2*pl;
case 4
%this is the hovering ball model
A = [ 0 1 0; 980 0 -2.8;0 0 -100 ];
B = [ 0 0 100 ]';
C = [ 1 0 0 ];
D = 0;
ssBall=ss(A,B,C,D,'InputName','iCmd','OutputName','actPos');
ss_plt=ssBall; %ss_plt ss_c1 ss_d1 ss_1 ss_0
ss_mdl=ssBall; %ss_plt ss_c1 ss_d1 ss_1 ss_0
pl=[-10+10i -10-10i -50];
plObs=[-100 -101 -102];
end end
[Am,Bm,Cm,Dm]=ssdata(ss_mdl); [Am,Bm,Cm,Dm]=ssdata(ss_mdl);
@@ -100,12 +124,14 @@ function [ssc]=StateSpaceControlDesign(mot,mode)
if bitand(verb,4) if bitand(verb,4)
% step answer on open loop: % step answer on open loop:
t = 0:1E-4:.5;
u = ones(size(t)); u = ones(size(t));
[yp,t,x] = lsim(ss_plt,u,t,xp0); [yp,t,x] = lsim(ss_plt,u,t,xp0);
[ym,t,x] = lsim(ss_mdl,u,t,xm0); [ym,t,x] = lsim(ss_mdl,u,t,xm0);
figure();plot(t,yp,t,ym,'--');title('step on open loop (plant and model)'); figure();hold on; plot(t,yp,'DisplayName',ss_plt.OutputName{1})
legend('plt.iqMeas','plt.iqVolts','plt.actPos','mdl.iqMeas','mdl.iqVolts','mdl.actPos') plot(t,ym,'--','DisplayName',ss_plt.OutputName{1});
title('step on open loop (plant and model)');
%legend('plt.iqMeas','plt.iqVolts','plt.actPos','mdl.iqMeas','mdl.iqVolts','mdl.actPos')
legend('location','best')
end end
%w0=abs(poles); %w0=abs(poles);
%ang=angle(-poles); %ang=angle(-poles);
@@ -126,13 +152,13 @@ function [ssc]=StateSpaceControlDesign(mot,mode)
V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 ) V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
if length(V)>1 if length(V)>1
V=V(3); % only the position scaling needed disp(['scaling (should be actPos): ' ss_mdl.OutputName{end}])
V=V(end); % only the position scaling needed
end end
ss_cl = ss(Am-Bm*K,Bm*V,Cm,0,'Name','space state controller','InputName',ss_mdl.InputName,'OutputName',ss_mdl.OutputName); ss_cl = ss(Am-Bm*K,Bm*V,Cm,0,'Name','space state controller','InputName',ss_mdl.InputName,'OutputName',ss_mdl.OutputName);
if bitand(verb,8) if bitand(verb,8)
% step answer on closed loop with space state controller: % step answer on closed loop with space state controller:
t = 0:1E-4:.5;
[y,t,x]=lsim(ss_cl,V*u,t,xm0); [y,t,x]=lsim(ss_cl,V*u,t,xm0);
figure();plot(t,y);title('step on closed loop'); figure();plot(t,y);title('step on closed loop');
end end
@@ -206,7 +232,8 @@ function [ssc]=StateSpaceControlDesign(mot,mode)
ss_oz = c2d(ss_o,Ts); ss_oz = c2d(ss_o,Ts);
%discrete prefilter %discrete prefilter
filt_pos_err_z=c2d(filt_pos_err,Ts); tfDesPos_z=c2d(tfDesPos,Ts);
tfPosErr_z=c2d(tfPosErr,Ts);
if bitand(verb,128) if bitand(verb,128)
h=bodeplot(filt_pos_err,filt_pos_err_z); h=bodeplot(filt_pos_err,filt_pos_err_z);
@@ -214,11 +241,10 @@ function [ssc]=StateSpaceControlDesign(mot,mode)
end end
%state space controller %state space controller
ssc=struct(); for k=["Ts","ss_plt","ss_o","ss_oz","tfDesPos","tfDesPos_z","tfPosErr","tfPosErr_z","V","MaxDac"]
for k=["Ts","ss_plt","ss_o","ss_oz","filt_pos_err","filt_pos_err_z","V","MaxDac"]
ssc=setfield(ssc,k,eval(k)); ssc=setfield(ssc,k,eval(k));
end end
mat2py=struct(); mat2py=struct();
%[ozA,ozB,ozC,ozD]=ssdata(ss_oz); %[ozA,ozB,ozC,ozD]=ssdata(ss_oz);
%[pos_err_num,pos_err_den]=tfdata(filt_pos_err_z); %[pos_err_num,pos_err_den]=tfdata(filt_pos_err_z);
@@ -230,12 +256,9 @@ function [ssc]=StateSpaceControlDesign(mot,mode)
mat2py.ozB=ss_oz.B; mat2py.ozB=ss_oz.B;
mat2py.ozC=ss_oz.C; mat2py.ozC=ss_oz.C;
mat2py.ozD=ss_oz.D; mat2py.ozD=ss_oz.D;
mat2py.ozInpName=ss_oz.InputName mat2py.ozInpName=ss_oz.InputName;
mat2py.ozOutName=ss_oz.OutputName mat2py.ozOutName=ss_oz.OutputName;
mat2py.pos_err_num=filt_pos_err_z.Numerator{1};
mat2py.pos_err_den=filt_pos_err_z.Denominator{1};
fn=[pwd '/' sprintf( 'ssc%d.mat',mot.id)]; fn=[pwd '/' sprintf( 'ssc%d.mat',mot.id)];
save(fn,'-struct','mat2py'); save(fn,'-struct','mat2py');
disp(['saved ' fn]); disp(['saved ' fn]);
@@ -247,23 +270,26 @@ function pf=Prefilt(mot,mode)
pf=tf(1,1); pf=tf(1,1);
case 1 %inverse resonance case 1 %inverse resonance
if mot.id==1 if mot.id==1
den=mot.mdl.num2;%num=1; den=mot.mdl.num1;%num=1;
num=mot.mdl.den2;%den=[1 0 0]; num=mot.mdl.den1;%den=[1 0 0];
pf=tf(num,den); pf=tf(num,den);
else else
den=conv(conv(conv(mot.mdl.num2,mot.mdl.num3),mot.mdl.num4),mot.mdl.num5);%num=1; den=conv(conv(conv(mot.mdl.num1,mot.mdl.num2),mot.mdl.num3),mot.mdl.num4);%num=1;
num=conv(conv(conv(mot.mdl.den2,mot.mdl.den3),mot.mdl.den4),mot.mdl.den5);%den=[1 0 0]; num=conv(conv(conv(mot.mdl.den1,mot.mdl.den2),mot.mdl.den3),mot.mdl.den4);%den=[1 0 0];
pf=tf(num,den); pf=tf(num,den);
end end
case 2 case 2
if mot.id==1 if mot.id==1
f=200;w0=f*2*pi; num1=[1 300 w0^2]; den1=[1 200 w0^2]; %f=200;w0=f*2*pi; num1=[1 300 w0^2]; den1=[1 200 w0^2];
numV=num1; %numV=num1;
denV=den1; %denV=den1;
pf=tf(numV,denV); %pf=tf(numV,denV);
%Lag
f=[200 300]; w=f*2*pi; T=1./w;
pf=tf([T(1) 1],[T(2) 1]);
else else
%Lag %Lag
f=[100 200]; w=f*2*pi; T=1./w; f=[200 400]; w=f*2*pi; T=1./w;
tf1=tf([T(1) 1],[T(2) 1]); tf1=tf([T(1) 1],[T(2) 1]);
%bo = bodeoptions; %bo = bodeoptions;
%bo.FreqUnits = 'Hz'; bo.MagUnits='abs'; bo.Grid='on'; %bo.FreqUnits = 'Hz'; bo.MagUnits='abs'; bo.Grid='on';

10
matlab/documentFunctions.m
View File

@@ -8,7 +8,7 @@ mot=identifyFxFyStage(7);
close all;disp('simulate stage closed loop...'); close all;disp('simulate stage closed loop...');
for k =1:2 for k =1:2
[pb]=simFxFyStage(mot{k});sim('stage_closed_loop'); [pb]=simFxFyStage(mot{k});sim('stage_closed_loop');
f=figure(); h=plot(desPos_actPos.Time,desPos_actPos.Data,'g'); 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]); 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'); print(f,sprintf('figures/sim_cl_DTGz_%d',mot{k}.id),'-depsc');
f=bodeSamples(desPos_actPos); f=bodeSamples(desPos_actPos);
@@ -16,14 +16,14 @@ for k =1:2
end end
close all;disp('simulate observer...'); close all;disp('simulate observer...');
for sscType=0%0:1 for sscType=1%0:1
for k=1:2 for k=1:2
[ssc]=StateSpaceControlDesign(mot{k},sscType);sim('observer'); [ssc]=StateSpaceControlDesign(mot{k},sscType);sim('observer');
f=figure(); h=plot(desPos_actPos.Time,desPos_actPos.Data,'g'); 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]); set(h(1),'color','b'); set(h(2),'color',[0 0.5 0]);
print(f,sprintf('figures/sim_cl_obs_%d_%d',m,mot{k}.id),'-depsc'); print(f,sprintf('figures/sim_cl_obs_%d_%d',sscType,mot{k}.id),'-depsc');
f=bodeSamples(desPos_actPos); f=bodeSamples(desPos_actPos);
print(f,sprintf('figures/sim_cl_obs_bode%d_%d',m,mot{k}.id),'-depsc'); print(f,sprintf('figures/sim_cl_obs_bode%d_%d',sscType,mot{k}.id),'-depsc');
end end
end end
disp('document figure generation done');close all; disp('document figure generation done');close all;

146
matlab/identifyFxFyStage.m
View File

@@ -163,11 +163,11 @@ function motCell=identifyFxFyStage(mode)
catch catch
return return
end end
h=bodeplot(mot.meas,'r',mot.ss_plt(3,1),'g',mot.ss_c1(3,1),'b',mot.ss_d1(3,1),'m',mot.ss_1(2,1),'c',mot.ss_0(2,1),'k',mot.w); h=bodeplot(mot.meas,'r',mot.ss_plt(3,1),'g',mot.ss_cp(3,1),'b',mot.ss_dp(3,1),'m',mot.ss_p(2,1),'c',mot.ss_q(2,1),'k',mot.w);
setoptions(h,'FreqUnits','Hz','Grid','on'); setoptions(h,'FreqUnits','Hz','Grid','on');
p=getoptions(h);p.YLim{2}=[-360 90];p.YLimMode='manual';setoptions(h,p); p=getoptions(h);p.YLim{2}=[-360 90];p.YLimMode='manual';setoptions(h,p);
ax=h.getaxes(); ax=h.getaxes();
legend(ax(1),'Location','sw',{'real','plant','c1','d1','1','0'}); legend(ax(1),'Location','sw',{'real','plant','cp','dp','p','q'});
print(gcf,sprintf('figures/plotBode_%d',mot.id),'-depsc'); print(gcf,sprintf('figures/plotBode_%d',mot.id),'-depsc');
end end
@@ -175,24 +175,24 @@ function motCell=identifyFxFyStage(mode)
%current loop iqCmd->iqMeas %current loop iqCmd->iqMeas
tfc=tf(mot.mdl.numc,mot.mdl.denc,'InputName','iqCmd','OutputName','iqMeas'); tfc=tf(mot.mdl.numc,mot.mdl.denc,'InputName','iqCmd','OutputName','iqMeas');
%simplified current loop iqCmd->iqMeas (first order tf) %simplified current loop iqCmd->iqMeas (first order tf)
tfd=tf(mot.mdl.numd,mot.mdl.dend,'InputName','iqCmd','OutputName','iqMeas'); tfd=tf(mot.mdl.numd,mot.mdl.dend,'InputName','iqCmd','OutputName','iqMeas');
%force(=current) to velocity and position iqForce->(actVel,actPos), actVel=s*actPos
tfp=tf({[mot.mdl.nump 0];mot.mdl.nump},mot.mdl.denp,'InputName','iqForce','OutputName',{'actVel','actPos'});
%simplified force(=current) to velocity and position iqForce->(actVel,actPos), actVel=s*actPos
tfq=tf({[mot.mdl.numq 0];mot.mdl.numq},mot.mdl.denq,'InputName','iqForce','OutputName',{'actVel','actPos'});
%simplified force(=current) to velocity iqForce->(actVel)
tfv=tf(mot.mdl.numv,mot.mdl.denv,'InputName','iqForce','OutputName',{'actVel'});
%resonance iqMeas->iqForce %resonance iqMeas->iqForce
tf2=tf(mot.mdl.num2,mot.mdl.den2,'InputName','iqMeas','OutputName','iqForce'); tf1=tf(mot.mdl.num1,mot.mdl.den1,'InputName','iqMeas','OutputName','iqForce');
%current to position iqForce->actPos %current to position iqForce->actPos
tf1_=tf(mot.mdl.num1,mot.mdl.den1,'InputName','iqForce','OutputName','actPos'); %tf1_=tf(mot.mdl.num1,mot.mdl.den1,'InputName','iqForce','OutputName','actPos');
%force(=current) to velocity and position iqForce->(actVel,actPos), actVel=s*actPos
tf1=tf({[mot.mdl.num1 0];mot.mdl.num1},mot.mdl.den1,'InputName','iqForce','OutputName',{'actVel','actPos'});
%simplified force(=current) to velocity and position iqForce->(actVel,actPos), actVel=s*actPos
tf0=tf({[mot.mdl.num0 0];mot.mdl.num0},mot.mdl.den0,'InputName','iqForce','OutputName',{'actVel','actPos'});
%check observable/controlable of transfer functions %check observable/controlable of transfer functions
ssLst=["tfc","tfd","tf0","tf1","tf2","tfc*tf1*tf2","tfc*tf1","tfd*tf1","tf1*tf2"]; ssLst=["tfc","tfd","tfp","tfq","tfv","tf1","tfc*tfp*tf1","tfc*tfp","tfd*tfp","tfp*tf1","tfc*tfv","tfd*tfv","tfd*tfv*tf1"];
sys=[]; sys=[];
for s = ssLst for s = ssLst
eval('sys=ss('+s+');') eval('sys=ss('+s+');')
@@ -201,23 +201,32 @@ function motCell=identifyFxFyStage(mode)
% sample code: % sample code:
%tfc iqCmd-> iqMeas %tfc iqCmd-> iqMeas
%tf2 resonance iqMeas->iqForce %tfp iqForce->(actVel,actPos)
%tf1 iqForce->(actVel,actPos) %tf1 resonance iqMeas->iqForce
%connect(tfc,tf2,'iqCmd','iqForce'); %connect(tfc,tf2,'iqCmd','iqForce');
%connect(tfc,tf2,'iqCmd',{'iqMeas','iqForce'}); %connect(tfc,tf2,'iqCmd',{'iqMeas','iqForce'});
%connect(tfc,tf2,tf1_,'iqCmd',{'iqMeas','iqForce','actPos'}); %connect(tfc,tf2,tf1_,'iqCmd',{'iqMeas','iqForce','actPos'});
%connect(tfc,tf2,tf1_,'iqCmd',{'iqMeas','actPos'}); %connect(tfc,tf2,tf1_,'iqCmd',{'iqMeas','actPos'});
% best plant approximation % best plant approximation
% u +-----------+ y % u +-----------+ y
%iqCmd------->|1 1|-------> iqMeas %iqCmd------->|1 1|-------> iqMeas
% | 2|-------> actVel % | 2|-------> actVel
% | 3|-------> actPos % | 3|-------> actPos
% +-----------+ % +-----------+
mot.ss_plt=connect(tfc,tf1,tf2,'iqCmd',{'iqMeas','actVel','actPos'}); mot.ss_plt=connect(tfc,tfp,tf1,'iqCmd',{'iqMeas','actVel','actPos'});
mot.ss_plt.Name='best plant approximation'; mot.ss_plt.Name='best plant approximation';
chkCtrlObsv(mot.ss_plt,'ss_plt fyStage'); chkCtrlObsv(mot.ss_plt,'ss_plt fyStage');
% best plant approximation without friction (always -40dB)
% u +-----------+ y
%iqCmd------->|1 1|-------> iqMeas
% | 2|-------> actVel
% | 3|-------> actPos
% +-----------+
mot.ss_cqr=connect(tfc,tfq,tf1,'iqCmd',{'iqMeas','actVel','actPos'});
mot.ss_cqr.Name='plant no friction';
chkCtrlObsv(mot.ss_plt,'ss_cqr fyStage');
%without resonance %without resonance
% u +-----------+ y % u +-----------+ y
@@ -225,44 +234,41 @@ function motCell=identifyFxFyStage(mode)
% | 2|-------> actVel % | 2|-------> actVel
% | 3|-------> actPos % | 3|-------> actPos
% +-----------+ % +-----------+
s=tf1.InputName{1};tf1.InputName{1}='iqMeas'; s=tfp.InputName{1};tfp.InputName{1}='iqMeas';
mot.ss_c1=connect(tfc,tf1,'iqCmd',{'iqMeas','actVel','actPos'}); mot.ss_cp=connect(tfc,tfp,'iqCmd',{'iqMeas','actVel','actPos'});
mot.ss_c1.Name='without resonance'; mot.ss_cp.Name='without resonance';
chkCtrlObsv(mot.ss_c1,'ss_c1 fyStage'); chkCtrlObsv(mot.ss_cp,'ss_cp fyStage');
tf1.InputName{1}=s;%restore tfp.InputName{1}=s;%restore
%simplified current, without resonance %simplified current, without resonance
% u +-----------+ y % u +-----------+ y
%iqCmd------->|1 1|-------> iqMeas %iqCmd------->|1 1|-------> iqMeas
% | 2|-------> actVel % | 2|-------> actVel
% | 3|-------> actPos % | 3|-------> actPos
% +-----------+ % +-----------+
s=tf1.InputName{1};tf1.InputName{1}='iqMeas'; s=tfp.InputName{1};tfp.InputName{1}='iqMeas';
mot.ss_d1=connect(tfd,tf1,'iqCmd',{'iqMeas','actVel','actPos'}); mot.ss_dp=connect(tfd,tfp,'iqCmd',{'iqMeas','actVel','actPos'});
mot.ss_d1.Name='simplified current, without resonance'; mot.ss_dp.Name='simplified current, without resonance';
chkCtrlObsv(mot.ss_d1,'ss_d1 fyStage'); chkCtrlObsv(mot.ss_dp,'ss_dp fyStage');
tf1.InputName{1}=s;%restore tfp.InputName{1}=s;%restore
%no current loop, no resonance %no current loop, no resonance
% u +-----------+ y % u +-----------+ y
%iqCmd------->|1 1|-------> actVel %iqCmd------->|1 1|-------> actVel
% | 2|-------> actPos % | 2|-------> actPos
% +-----------+ % +-----------+
mot.ss_1=ss(tf1); mot.ss_p=ss(tfp);
mot.ss_1.Name='no current loop, no resonance'; mot.ss_p.Name='no current loop, no resonance';
chkCtrlObsv(mot.ss_1,'ss_1 fyStage'); chkCtrlObsv(mot.ss_p,'ss_p fyStage');
%simplified mechanics, no current loop, no resonance %simplified mechanics, no current loop, no resonance
% u +-----------+ y % u +-----------+ y
%iqCmd------->|1 1|-------> actVel %iqCmd------->|1 1|-------> actVel
% | 2|-------> actPos % | 2|-------> actPos
% +-----------+ % +-----------+
mot.ss_0=ss(tf0); mot.ss_q=ss(tfq);
mot.ss_0.Name='simplified mechanics, no current loop, no resonance'; mot.ss_q.Name='simplified mechanics, no current loop, no resonance';
chkCtrlObsv(mot.ss_0,'ss_0 fyStage'); chkCtrlObsv(mot.ss_q,'ss_q fyStage');
%h=bodeplot(mot.meas,'r',mot.tf4_2,'b',mot.tf6_4,'g'); %h=bodeplot(mot.meas,'r',mot.tf4_2,'b',mot.tf6_4,'g');
%h=bodeplot(mot.meas,'r',mot.tf2_0,'b',mot.tf_mdl,'g',mot.w); %h=bodeplot(mot.meas,'r',mot.tf2_0,'b',mot.tf_mdl,'g',mot.w);
@@ -272,58 +278,64 @@ function motCell=identifyFxFyStage(mode)
function mot=fxStage(mot) function mot=fxStage(mot)
%current loop iqCmd->iqMeas %current loop iqCmd->iqMeas
tfc=tf(mot.mdl.numc,mot.mdl.denc,'InputName','iqCmd','OutputName','iqMeas'); tfc=tf(mot.mdl.numc,mot.mdl.denc,'InputName','iqCmd','OutputName','iqMeas');
%simplified current loop iqCmd->iqMeas (first order tf) %simplified current loop iqCmd->iqMeas (first order tf)
tfd=tf(mot.mdl.numd,mot.mdl.dend,'InputName','iqCmd','OutputName','iqMeas'); tfd=tf(mot.mdl.numd,mot.mdl.dend,'InputName','iqCmd','OutputName','iqMeas');
%force(=current) to velocity and position iqForce->(actVel,actPos), actVel=s*actPos
tfp=tf({[mot.mdl.nump 0];mot.mdl.nump},mot.mdl.denp,'InputName','iqForce','OutputName',{'actVel','actPos'});
%simplified force(=current) to velocity and position iqForce->(actVel,actPos), actVel=s*actPos
tfq=tf({[mot.mdl.numq 0];mot.mdl.numq},mot.mdl.denq,'InputName','iqForce','OutputName',{'actVel','actPos'});
%resonance iqMeas->iqForce %resonance iqMeas->iqForce
tf2=tf(mot.mdl.num2,mot.mdl.den2,'InputName','iqMeas','OutputName','iqF1'); tf1=tf(mot.mdl.num1,mot.mdl.den1,'InputName','iqMeas','OutputName','iqF1');
%resonance iqMeas->iqForce %resonance iqMeas->iqForce
tf3=tf(mot.mdl.num3,mot.mdl.den3,'InputName','iqF1','OutputName','iqF2'); tf2=tf(mot.mdl.num2,mot.mdl.den2,'InputName','iqF1','OutputName','iqF2');
%resonance iqMeas->iqForce %resonance iqMeas->iqForce
tf4=tf(mot.mdl.num4,mot.mdl.den4,'InputName','iqF2','OutputName','iqF3'); tf3=tf(mot.mdl.num3,mot.mdl.den3,'InputName','iqF2','OutputName','iqF3');
%resonance iqMeas->iqForce %resonance iqMeas->iqForce
tf5=tf(mot.mdl.num5,mot.mdl.den5,'InputName','iqF3','OutputName','iqForce'); tf4=tf(mot.mdl.num4,mot.mdl.den4,'InputName','iqF3','OutputName','iqForce');
%current to position iqForce->actPos %current to position iqForce->actPos
tf1_=tf(mot.mdl.num1,mot.mdl.den1,'InputName','iqForce','OutputName','actPos'); %tf1_=tf(mot.mdl.num1,mot.mdl.den1,'InputName','iqForce','OutputName','actPos');
%force(=current) to velocity and position iqForce->(actVel,actPos), actVel=s*actPos
tf1=tf({[mot.mdl.num1 0];mot.mdl.num1},mot.mdl.den1,'InputName','iqForce','OutputName',{'actVel','actPos'});
%simplified force(=current) to velocity and position iqForce->(actVel,actPos), actVel=s*actPos
tf0=tf({[mot.mdl.num0 0];mot.mdl.num0},mot.mdl.den0,'InputName','iqForce','OutputName',{'actVel','actPos'});
%check observable/controlable of transfer functions %check observable/controlable of transfer functions
ssLst=["tfc","tfd","tf0","tf1","tf2","tf3","tf4","tf5",... ssLst=["tfc","tfd","tfp","tfq","tf1","tf2","tf3","tf4",...
"tfc*tf1*tf2","tfc*tf1","tfd*tf1","tf1*tf2","tf1*tf2*tf3"]; "tfc*tfp*tf1","tfc*tfp","tfd*tfp","tfp*tf1","tfd*tfp*tf1","tfc*tf1","tfd*tf1","tfc*tf1*tf2","tfd*tf1*tf2"];
sys=[]; sys=[];
for s = ssLst for s = ssLst
eval('sys=ss('+s+');') eval('sys=ss('+s+');')
chkCtrlObsv(sys,char(s)); chkCtrlObsv(sys,char(s));
end end
% best plant approximation % best plant approximation
% u +-----------+ y % u +-----------+ y
%iqCmd------->|1 1|-------> iqMeas %iqCmd------->|1 1|-------> iqMeas
% | 2|-------> actVel % | 2|-------> actVel
% | 3|-------> actPos % | 3|-------> actPos
% +-----------+ % +-----------+
mot.ss_plt=connect(tfc,tf1,tf2,tf3,tf4,tf5,'iqCmd',{'iqMeas','actVel','actPos'}); mot.ss_plt=connect(tfc,tfp,tf1,tf2,tf3,tf4,'iqCmd',{'iqMeas','actVel','actPos'});
chkCtrlObsv(mot.ss_plt,'ss_plt fxStage'); chkCtrlObsv(mot.ss_plt,'ss_plt fxStage');
% best plant approximation without friction (always -40dB)
% u +-----------+ y
%iqCmd------->|1 1|-------> iqMeas
% | 2|-------> actVel
% | 3|-------> actPos
% +-----------+
mot.ss_cqr=connect(tfc,tfq,tf1,tf2,tf3,tf4,'iqCmd',{'iqMeas','actVel','actPos'});
mot.ss_cqr.Name='plant no friction';
chkCtrlObsv(mot.ss_cqr,'ss_plt0 fxStage');
%without resonance %without resonance
% u +-----------+ y % u +-----------+ y
%iqCmd------->|1 1|-------> iqMeas %iqCmd------->|1 1|-------> iqMeas
% | 2|-------> actVel % | 2|-------> actVel
% | 3|-------> actPos % | 3|-------> actPos
% +-----------+ % +-----------+
s=tf1.InputName{1};tf1.InputName{1}='iqMeas'; s=tfp.InputName{1};tfp.InputName{1}='iqMeas';
mot.ss_c1=connect(tfc,tf1,'iqCmd',{'iqMeas','actVel','actPos'}); mot.ss_cp=connect(tfc,tfp,'iqCmd',{'iqMeas','actVel','actPos'});
chkCtrlObsv(mot.ss_c1,'ss_c1 fxStage'); chkCtrlObsv(mot.ss_cp,'ss_cp fxStage');
tf1.InputName{1}=s;%restore tfp.InputName{1}=s;%restore
%simplified current, without resonance %simplified current, without resonance
% u +-----------+ y % u +-----------+ y
@@ -331,28 +343,26 @@ function motCell=identifyFxFyStage(mode)
% | 2|-------> actVel % | 2|-------> actVel
% | 3|-------> actPos % | 3|-------> actPos
% +-----------+ % +-----------+
s=tf1.InputName{1};tf1.InputName{1}='iqMeas'; s=tfp.InputName{1};tfp.InputName{1}='iqMeas';
mot.ss_d1=connect(tfd,tf1,'iqCmd',{'iqMeas','actVel','actPos'}); mot.ss_dp=connect(tfd,tfp,'iqCmd',{'iqMeas','actVel','actPos'});
chkCtrlObsv(mot.ss_d1,'ss_d1 fxStage'); chkCtrlObsv(mot.ss_dp,'ss_dp fxStage');
tf1.InputName{1}=s;%restore tfp.InputName{1}=s;%restore
%no current loop, no resonance %no current loop, no resonance
% u +-----------+ y % u +-----------+ y
%iqCmd------->|1 1|-------> actVel %iqCmd------->|1 1|-------> actVel
% | 2|-------> actPos % | 2|-------> actPos
% +-----------+ % +-----------+
mot.ss_1=ss(tf1); mot.ss_p=ss(tfp);
chkCtrlObsv(mot.ss_1,'ss_1 fxStage'); chkCtrlObsv(mot.ss_p,'ss_p fxStage');
%simplified mechanics, no current loop, no resonance %simplified mechanics, no current loop, no resonance
% u +-----------+ y % u +-----------+ y
%iqCmd------->|1 1|-------> actVel %iqCmd------->|1 1|-------> actVel
% | 2|-------> actPos % | 2|-------> actPos
% +-----------+ % +-----------+
mot.ss_0=ss(tf0); mot.ss_q=ss(tfq);
chkCtrlObsv(mot.ss_0,'ss_0 fxStage'); chkCtrlObsv(mot.ss_q,'ss_q fxStage');
plotBode(mot) plotBode(mot)
end end

BIN
matlab/observer.slx
View File

Binary file not shown.

23
matlab/simFxFyStage.m
View File

@@ -6,24 +6,27 @@ function [pb]=simFxFyStage(mot)
%the returned variable pb contains varous parameters: %the returned variable pb contains varous parameters:
% Kp,Kvfb,Ki,Kvff,Kaff,MaxInt,mot_num,mot_den,Ts,MaxDac,MaxPosErr,A,B,C,D % Kp,Kvfb,Ki,Kvff,Kaff,MaxInt,mot_num,mot_den,Ts,MaxDac,MaxPosErr,A,B,C,D
Ts=2E-4; % 0.2ms=5kHz pb=struct(...
MaxDac=2011.968; 'Ts', 2E-4, ... % 0.2ms=5kHz
MaxPosErr=10000; 'MaxDac' ,2011.968, ...
'MaxPosErr', 10000);
if mot.id==1 if mot.id==1
%!motor_servo(mot=1,ctrl='ServoCtrl',Kp=25,Kvfb=400,Ki=0.02,Kvff=350,Kaff=5000,MaxInt=1000) %!motor_servo(mot=1,ctrl='ServoCtrl',Kp=25,Kvfb=400,Ki=0.02,Kvff=350,Kaff=5000,MaxInt=1000)
%!motor(mot=1,dirCur=0,contCur=800,peakCur=2400,timeAtPeak=1,IiGain=5,IpfGain=8,IpbGain=8,JogSpeed=10.,numPhase=3,invDir=True,servo=None,PhasePosSf=1./81250,PhaseFindingDac=100,PhaseFindingTime=50,SlipGain=0,AdvGain=0,PwmSf=10000,FatalFeLimit=200,WarnFeLimit=100,InPosBand=2,homing='enc-index') %!motor(mot=1,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')
Kp=25;Kvfb=400;Ki=0.02;Kvff=350;Kaff=5000;MaxInt=1000; pb.Kp=25;pb.Kvfb=400;pb.Ki=0.02;pb.Kvff=350;pb.Kaff=5000;pb.MaxInt=1000;
else else
%!motor_servo(mot=2,ctrl='ServoCtrl',Kp=22,Kvfb=350,Ki=0.02,Kvff=240,Kaff=1500,MaxInt=1000) %!motor_servo(mot=2,ctrl='ServoCtrl',Kp=22,Kvfb=350,Ki=0.02,Kvff=240,Kaff=1500,MaxInt=1000)
%!motor(mot=2,dirCur=0,contCur=800,peakCur=2400,timeAtPeak=1,IiGain=5,IpfGain=8,IpbGain=8,JogSpeed=10.,numPhase=3,invDir=True,servo=None,PhasePosSf=1./81250,PhaseFindingDac=100,PhaseFindingTime=50,SlipGain=0,AdvGain=0,PwmSf=10000,FatalFeLimit=200,WarnFeLimit=100,InPosBand=2,homing='enc-index') %!motor(mot=2,dirCur=0,contCur=800,peakCur=2400,timeAtPeak=1,IiGain=5,IpfGain=8,IpbGain=8,JogSpeed=10.,numPhase=3,invDir=True,servo=None,PhasePosSf=1./81250,PhaseFindingDac=100,PhaseFindingTime=50,SlipGain=0,AdvGain=0,PwmSf=10000,FatalFeLimit=200,WarnFeLimit=100,InPosBand=2,homing='enc-index')
%Kp=22;Kvfb=350;Ki=0.02;Kvff=240;Kaff=1500;MaxInt=1000; %Kp=22;Kvfb=350;Ki=0.02;Kvff=240;Kaff=1500;MaxInt=1000;
Kp=22;Kvfb=350;Ki=0.02;Kvff=240;Kaff=3500;MaxInt=1000; pb.Kp=22;pb.Kvfb=350;pb.Ki=0.02;pb.Kvff=240;pb.Kaff=3500;pb.MaxInt=1000;
end
ss_plt=mot.ss_plt;
pb=struct();
for k=["Kp","Kvfb","Ki","Kvff","Kaff","MaxInt","Ts","MaxDac","MaxPosErr","ss_plt"]
pb=setfield(pb,k,eval(k));
end end
pb.ss_plt=mot.ss_plt; pb.sel={3,[3]};
%pb.ss_plt=mot.ss_plt0; pb.sel={3,[3]};
%pb.ss_plt=mot.ss_c1; pb.sel={3,[3]};
%pb.ss_plt=mot.ss_d1; pb.sel={3,[3]};
%pb.ss_plt=mot.ss_1; pb.sel={2,[2]};
%pb.ss_plt=mot.ss_p; pb.sel={2,[2]};
%mdlName='stage_closed_loop'; %mdlName='stage_closed_loop';
%open(mdlName) %open(mdlName)
%sim(mdlName) %sim(mdlName)

BIN
matlab/stage_closed_loop.slx
View File

Binary file not shown.

252
matlab/testObserver.m
View File

@@ -1,50 +1,16 @@
function [ssc]=testObserver(mot) function [ssc]=testObserver()
%http://ctms.engin.umich.edu/CTMS/index.php?example=Introduction&section=ControlStateSpace %http://ctms.engin.umich.edu/CTMS/index.php?example=Introduction&section=ControlStateSpace
mdl=1; % 0:hovering ball 1:fast stage mdl=1; % 0:hovering ball 1:fast stage
if mdl==0 A = [ 0 1 0
A = [ 0 1 0 980 0 -2.8
980 0 -2.8 0 0 -100 ];
100
0 0 -100 ];
B = [ 0 B = [ 0
0]; 0
100 ];
C = [ 1 0 0 ]; C = [ 1 0 0 ];
D=0; D=0;
else
[A,B,C,D]=ssdata(mot.ssMdl);
C=C(3,:);D=D(3);
numc=(mot.mdl.numc);
denc=(mot.mdl.denc);
num1=(mot.mdl.num1);
den1=(mot.mdl.den1);
g1=tf(numc,denc); % iqCmd->iqMeas
s1=ss(g1);
s1.C=[s1.C; 1E5* 2.4E-3 1E-3*s1.C(2)*8.8]; % add output iqVolts: iqVolts= i_meas*R+i_meas'*L 2.4mH 8.8Ohm (took random scaling values)
g2=tf(num1,den1); %iqMeas->ActPos without resonance frequencies
s2=ss(g2);
s3=append(s1,s2);
s3.A(3,2)=s3.C(1,2)*s3.B(3,2);
%d=.77;w0=9100; %1.44kHz -> T0=6.9046e-04
%numc=[w0^2];
%denc=[1 d*2*w0 w0^2];
%T0=6E-4; %w=1/1E-3=1E3 -> f=w/(2pi)=1000/6.2=159Hz
%numc=[1];
%denc=[T0^2 d*2*T0 1];
%bode(numc,denc)
num=conv(numc,num1);%num=1;
den=conv(denc,den1);%den=[1 0 0];
g4=tf(num,den); %iqMeas->ActPos
s4=ss(g4);
mot.ssMdl
s3
s4
[A,B,C,D]=ssdata(s4);
end
Ap=A;Am=A; Ap=A;Am=A;
Bp=B;Bm=B; Bp=B;Bm=B;
@@ -52,159 +18,79 @@ function [ssc]=testObserver(mot)
Dp=D;Dm=D;Dt=D; Dp=D;Dm=D;Dt=D;
poles = eig(A); poles = eig(A);
disp(poles); poles
if mdl==0 t = 0:0.01:2;
t = 0:0.01:2; u = zeros(size(t));
u = zeros(size(t)); x0 = [0.01 0 0];
x0 = [0.01 0 0];
sys = ss(A,B,C,D); sys = ss(A,B,C,D);
[y,t,x] = lsim(sys,u,t,x0); [y,t,x] = lsim(sys,u,t,x0);
plot(t,y) plot(t,y)
title('Open-Loop Response to Non-Zero Initial Condition') title('Open-Loop Response to Non-Zero Initial Condition')
xlabel('Time (sec)') xlabel('Time (sec)')
ylabel('Ball Position (m)') ylabel('Ball Position (m)')
p1 = -10 + 10i; p2 = -10 - 10i; p3 = -50; p1 = -10 + 10i; p2 = -10 - 10i; p3 = -50;
K = place(A,B,[p1 p2 p3]); K = place(A,B,[p1 p2 p3]);
sys_cl = ss(A-B*K,B,C,0); sys_cl = ss(A-B*K,B,C,0);
lsim(sys_cl,u,t,x0); lsim(sys_cl,u,t,x0);
xlabel('Time (sec)') xlabel('Time (sec)')
ylabel('Ball Position (m)') ylabel('Ball Position (m)')
p1 = -20 + 20i; p2 = -20 - 20i; p3 = -100; t = 0:0.01:2;
P=[p1 p2 p3]; u = 0.001*ones(size(t));
K = place(A,B,P);
sys_cl = ss(A-B*K,B,C,0);
lsim(sys_cl,u,t,x0);
xlabel('Time (sec)')
ylabel('Ball Position (m)')
t = 0:0.01:2;
u = 0.001*ones(size(t));
sys_cl = ss(A-B*K,B,C,0); sys_cl = ss(A-B*K,B,C,0);
lsim(sys_cl,u,t); lsim(sys_cl,u,t);
xlabel('Time (sec)') xlabel('Time (sec)')
ylabel('Ball Position (m)') ylabel('Ball Position (m)')
axis([0 2 -4E-6 0]) axis([0 2 -4E-6 0])
V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 ) V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
lsim(sys_cl,V*u,t) lsim(sys_cl,V*u,t)
title('Linear Simulation Results (with Nbar)') title('Linear Simulation Results (with Nbar)')
xlabel('Time (sec)') xlabel('Time (sec)')
ylabel('Ball Position (m)') ylabel('Ball Position (m)')
axis([0 2 0 1.2*10^-3]) axis([0 2 0 1.2*10^-3])
else
x0 = [0 0 0 0];
p1=-63+2.80i;
p2=-62+1.50i;
P=[p1 p1' p2 p2'];
P=[-4000+100j -4000-100j -500+10j -500-10j];
K = place(A,B,P);
Ts=1/5000
t = 0:Ts:2;
u = 0.001*ones(size(t));
V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
sys_cl = ss(A-B*K,B*V,C,0);
lsim(sys_cl,u,t,x0); op1 = -100;
xlabel('Time (sec)') op2 = -101;
ylabel('Motor Position (m)') op3 = -102;
axis([0 2 0 1.2E-3]) OP=[op1,op2,op3];
end %OP=P*2;
L = place(A',C',OP)';
At = [ A-B*K B*K
zeros(size(A)) A-L*C ];
Bt = [ B*V
zeros(size(B)) ];
Ct = [ C zeros(size(C)) ];
if mdl==0 sys = ss(At,Bt,Ct,0);
op1 = -100; %lsim(sys,zeros(size(t)),t,[x0 x0]);
op2 = -101; lsim(sys,u,t,[x0 x0]);
op3 = -102; title('Linear Simulation Results (with observer)')
OP=[op1,op2,op3]; xlabel('Time (sec)')
OP=P*2; ylabel('Ball Position (m)')
L = place(A',C',OP)';
At = [ A-B*K B*K
zeros(size(A)) A-L*C ];
Bt = [ B*V
zeros(size(B)) ];
Ct = [ C zeros(size(C)) ];
sys = ss(At,Bt,Ct,0); t = 0:1E-6:0.1;
%lsim(sys,zeros(size(t)),t,[x0 x0]); x0 = [0.01 0.5 -5];
lsim(sys,u,t,[x0 x0]*0); [y,t,x] = lsim(sys,zeros(size(t)),t,[x0 x0]);
title('Linear Simulation Results (with observer)')
xlabel('Time (sec)')
ylabel('Ball Position (m)')
else
OP=P*2;
L = place(A',C',OP)';
At = [ A-B*K B*K
zeros(size(A)) A-L*C ];
Bt = [ B*V
zeros(size(B)) ];
Ct = [ C zeros(size(C)) ];
sys = ss(At,Bt,Ct,0); n = 3;
lsim(sys,u,t,[x0 x0]*0); e = x(:,n+1:end);
title('Linear Simulation Results (with observer)') x = x(:,1:n);
xlabel('Time (sec)') x_est = x - e;
ylabel('Motor Position (m)')
axis([0 2 0 1.2E-3])
end
if mdl==0
t = 0:1E-6:0.1;
x0 = [0.01 0.5 -5];
[y,t,x] = lsim(sys,zeros(size(t)),t,[x0 x0]);
n = 3; % Save state variables explicitly to aid in plotting
e = x(:,n+1:end); h = x(:,1); h_dot = x(:,2); i = x(:,3);
x = x(:,1:n); h_est = x_est(:,1); h_dot_est = x_est(:,2); i_est = x_est(:,3);
x_est = x - e;
% Save state variables explicitly to aid in plotting plot(t,h,'-r',t,h_est,':r',t,h_dot,'-b',t,h_dot_est,':b',t,i,'-g',t,i_est,':g')
h = x(:,1); h_dot = x(:,2); i = x(:,3); legend('h','h_{est}','hdot','hdot_{est}','i','i_{est}')
h_est = x_est(:,1); h_dot_est = x_est(:,2); i_est = x_est(:,3); xlabel('Time (sec)')
K,L,V
plot(t,h,'-r',t,h_est,':r',t,h_dot,'-b',t,h_dot_est,':b',t,i,'-g',t,i_est,':g')
legend('h','h_{est}','hdot','hdot_{est}','i','i_{est}')
xlabel('Time (sec)')
end
%discrete
Ts=1/5000; % deltatau std. frq. is 5kHz
%The discrete system works with sampling >100Hz,. the bigger the frequency the better the result
%With sampling = 80Hz hte system already becomes instable.
ss_t=ss(At,Bt,Ct,Dt);
%figure();pzplot(ss_t)
ss_tz=c2d(ss_t,Ts);
%figure();pzplot(ss_tz)
[Atz,Btz,Ctz,Dtz]=ssdata(ss_tz);
[Apz,Bpz,Cpz,Dpz]=ssdata(c2d(ss(Ap,Bp,Cp,Dp),Ts));
[Amz,Bmz,Cmz,Dmz]=ssdata(c2d(ss(Am,Bm,Cm,Dm),Ts));
Ao=Am-L*Cm;
Bo=[Bm L];
Co=K;
Do=zeros(size(Co,1),size(Bo,2));
[Aoz,Boz,Coz,Doz]=ssdata(c2d(ss(Ao,Bo,Co,Do),Ts));
%state space controller
ssc=struct();
for k=["Ts","At","Bt","Ct","Dt","Atz","Btz","Ctz","Dtz","Ap","Bp","Cp","Dp","Am","Bm","Cm","Dm","Ao","Bo","Co","Do","Apz","Bpz","Cpz","Dpz","Aoz","Boz","Coz","Doz","V","K","L"]
ssc=setfield(ssc,k,eval(k));
end
mdlName='testObserverSim';
open(mdlName);
end end

225
python/MXTuning.py
View File

@@ -95,31 +95,82 @@ class MXTuning(Tuning):
# Hz -> kHz # Hz -> kHz
# rad/s -> rad/ms # rad/s -> rad/ms
if mot==1: if mot==1:
#current loop 2nd order approx
#identification with matlab: k=1, w0=8725, d=0.75
dc=.75 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
wc=8725.*_N # rad/sec
#...but phase lag seems to have earlier effect -> reduce wc
wc*=.5 # rad/sec
fc=wc/2/np.pi # ca 1388Hz
Tc=1/wc
numc = np.poly1d([1.])
denc = np.poly1d([Tc**2,2*Tc*dc,1])
#simplified current loop 1st order approx
wd=2*np.pi*400. #45deg at 360Hz
Td=1/wd
numd = np.poly1d([1.])
dend = np.poly1d([Td,1])
#force(idMeas)->position
#identify matlab: k:0.671226 w0:134.705 damp:0.191004 #identify matlab: k:0.671226 w0:134.705 damp:0.191004
mag1=6 #10**(db_mag1/20) magp=6 #10**(db_mag1/20)
db_mag1=20*np.log10(mag1)#dB db_magp=20*np.log10(magp)#dB
w1=50.*_N #rad/sec wp=50.*_N #rad/sec
f1=w1/2/np.pi; # ca. 6.5Hz fp=wp/2/np.pi; # ca. 6.5Hz
T1=1/w1 Tp=1/wp
d1=0.6 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2 dp=0.6 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
num1=np.poly1d([mag1]) nump=np.poly1d([magp])
den1 = np.poly1d([T1**2,2*T1*d1,1]) denp = np.poly1d([Tp**2,2*Tp*dp,1])
#force->position (simplified)
#reiner integrator: 19.8Hz=0dB -> k=frq*2*pi=180
numq=np.poly1d([(19.8*2*np.pi)**2])
denq=np.poly1d([1,0,0])
#reiner integrator: 19.8Hz=0dB -> k=30*2*pi=180 #force->velocity (simplified)
num0=np.poly1d([(19.8*2*np.pi)**2]) #reiner integrator: 19.8Hz=0dB -> k=frq*2*pi
den0=np.poly1d([1,0,0]) numv=np.poly1d([19.8*2*np.pi])
denv=np.poly1d([1,0])
#first resonance frequency #first resonance frequency
f2=np.array([197,199]) f1=np.array([197,199])
d2=np.array([.02,.02])#daempfung d1=np.array([.02,.02])#daempfung
w2=f2*2*np.pi*_N #rad/sec w1=f1*2*np.pi*_N #rad/sec
T2=1/w2 T1=1/w1
num2 = np.poly1d([T2[0]**2,2*T2[0]*d2[0],1]) num1 = np.poly1d([T1[0]**2,2*T1[0]*d1[0],1])
den2 = np.poly1d([T2[1]**2,2*T2[1]*d2[1],1]) den1 = np.poly1d([T1[1]**2,2*T1[1]*d1[1],1])
num=numc*nump*num1#*num3
den=denc*denp*den1#*den3
mdl= signal.lti(num, den) #num denum
#print(num);print(den);print(mdl)
w=np.logspace(0, np.log10(2000), 1000)*2*np.pi
mdlcp= signal.lti(numc*nump, denc*denp)
mdldp= signal.lti(numd*nump, dend*denp)
mdlp= signal.lti(nump, denp)
mdlq= signal.lti(numq, denq)
bode(((mdl,'plant'),
(mdlcp,'mdl_cp'),
(mdldp,'mdl_dp'),
(mdlp,'mdl_p'),
(mdlq,'mdl_q'),
),w)
d={'num':num.coeffs,'numc':numc.coeffs,'numd':numd.coeffs,'nump':nump.coeffs,'numq':numq.coeffs,'numv':numv.coeffs,'num1':num1.coeffs,
'den':den.coeffs,'denc':denc.coeffs,'dend':dend.coeffs,'denp':denp.coeffs,'denq':denq.coeffs,'denv':denv.coeffs,'den1':den1.coeffs}
fn=os.path.join(self.baseDir,'model%d.mat'%mot)
import scipy.io
scipy.io.savemat(fn, mdict=d)
print('save to matlab file:'+fn)
elif mot==2:
#current loop 2nd order approx #current loop 2nd order approx
#identification with matlab: k=1, w0=8725, d=0.75 #identification with matlab: k=1, w0=8725, d=0.75
dc=.75 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2 dc=.75 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
@@ -139,52 +190,39 @@ class MXTuning(Tuning):
dend = np.poly1d([Td,1]) dend = np.poly1d([Td,1])
num=num1*num2*numc#*num3 #force(idMeas)->position
den=den1*den2*denc#*den3
mdl= signal.lti(num, den) #num denum
#print(num);print(den);print(mdl)
w=np.logspace(0, np.log10(2000), 1000)*2*np.pi
mdl1c= signal.lti(num1*numc, den1*denc)
mdl1d= signal.lti(num1*numd, den1*dend)
mdl1= signal.lti(num1, den1)
mdl0= signal.lti(num0, den0)
bode(((mdl,'plant'),
(mdl1c,'mdl1c'),
(mdl1d,'mdl1d'),
(mdl1,'mdl1'),
(mdl0,'mdl0'),
),w)
d={'num':num.coeffs,'num1':num1.coeffs,'num2':num2.coeffs,'numc':numc.coeffs,'num0':num0.coeffs,'numd':numd.coeffs,
'den':den.coeffs,'den1':den1.coeffs,'den2':den2.coeffs,'denc':denc.coeffs,'den0':den0.coeffs,'dend':dend.coeffs}
fn=os.path.join(self.baseDir,'model%d.mat'%mot)
import scipy.io
scipy.io.savemat(fn, mdict=d)
print('save to matlab file:'+fn)
elif mot==2:
#identify matlab: k:1.7282 w0:51.069 damp:0.327613 #identify matlab: k:1.7282 w0:51.069 damp:0.327613
mag1=12. #10**(db_mag1/20) magp=12. #10**(db_mag1/20)
db_mag1=20*np.log10(mag1)#dB db_magp=20*np.log10(magp)#dB
w1=21.*_N #rad/sec wp=21.*_N #rad/sec
f1=w1/2/np.pi; # ca. 6.5Hz fp=wp/2/np.pi; # ca. 6.5Hz
T1=1/w1 Tp=1/wp
d1=0.4 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2 dp=0.4 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
num1=np.poly1d([mag1]) nump=np.poly1d([magp])
den1 = np.poly1d([T1**2,2*T1*d1,1]) denp = np.poly1d([Tp**2,2*Tp*dp,1])
#reiner integrator: 11.84Hz=0dB -> k=30*2*pi=180 #reiner integrator: 11.84Hz=0dB -> k=30*2*pi=180
num0=np.poly1d([(11.84*2*np.pi)**2]) numq=np.poly1d([(11.84*2*np.pi)**2])
den0=np.poly1d([1,0,0]) denq=np.poly1d([1,0,0])
#force->velocity (simplified)
#reiner integrator: 19.8Hz=0dB -> k=frq*2*pi
numv=np.poly1d([19.8*2*np.pi])
denv=np.poly1d([1,0])
#resonance frequency #resonance frequency
f2=np.array([55.,61.]) f1=np.array([55.,61.])
d2=np.array([.2,.2])#daempfung d1=np.array([.2,.2])#daempfung
w1=f1*2*np.pi #rad/sec
T1=1/w1
num1 = np.poly1d([T1[0]**2,2*T1[0]*d1[0],1])
den1 = np.poly1d([T1[1]**2,2*T1[1]*d1[1],1])
#resonance frequency
f2=np.array([128,137])
d2=np.array([.05,.05])#daempfung
w2=f2*2*np.pi #rad/sec w2=f2*2*np.pi #rad/sec
T2=1/w2 T2=1/w2
num2 = np.poly1d([T2[0]**2,2*T2[0]*d2[0],1]) num2 = np.poly1d([T2[0]**2,2*T2[0]*d2[0],1])
@@ -192,8 +230,8 @@ class MXTuning(Tuning):
#resonance frequency #resonance frequency
f3=np.array([128,137]) f3=np.array([410,417])
d3=np.array([.05,.05])#daempfung d3=np.array([.015,.015])#daempfung
w3=f3*2*np.pi #rad/sec w3=f3*2*np.pi #rad/sec
T3=1/w3 T3=1/w3
num3 = np.poly1d([T3[0]**2,2*T3[0]*d3[0],1]) num3 = np.poly1d([T3[0]**2,2*T3[0]*d3[0],1])
@@ -201,64 +239,36 @@ class MXTuning(Tuning):
#resonance frequency #resonance frequency
f4=np.array([410,417]) f4=np.array([230,233])
d4=np.array([.015,.015])#daempfung d4=np.array([.04,.04])#daempfung
w4=f4*2*np.pi #rad/sec w4=f4*2*np.pi #rad/sec
T4=1/w4 T4=1/w4
num4 = np.poly1d([T4[0]**2,2*T4[0]*d4[0],1]) num4 = np.poly1d([T4[0]**2,2*T4[0]*d4[0],1])
den4 = np.poly1d([T4[1]**2,2*T4[1]*d4[1],1]) den4 = np.poly1d([T4[1]**2,2*T4[1]*d4[1],1])
#resonance frequency num=numc*nump*num1*num2*num3*num4
f5=np.array([230,233]) den=denc*denp*den1*den2*den3*den4
d5=np.array([.04,.04])#daempfung
w5=f5*2*np.pi #rad/sec
T5=1/w5
num5 = np.poly1d([T5[0]**2,2*T5[0]*d5[0],1])
den5 = np.poly1d([T5[1]**2,2*T5[1]*d5[1],1])
#current loop 2nd order approx
#identification with matlab: k=1, w0=8725, d=0.75
dc=.75 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
wc=8725.*_N # rad/sec
#...but phase lag seems to have earlier effect -> reduce wc
wc*=.5 # rad/sec
fc=wc/2/np.pi # ca 1388Hz
Tc=1/wc
numc = np.poly1d([1.])
denc = np.poly1d([Tc**2,2*Tc*dc,1])
#simplified current loop 1st order approx
wd=2*np.pi*400. #45deg at 360Hz
Td=1/wd
numd = np.poly1d([1.])
dend = np.poly1d([Td,1])
num=num1*num2*num3*num4*num5*numc
den=den1*den2*den3*den4*den5*denc
mdl= signal.lti(num, den) #num denum mdl= signal.lti(num, den) #num denum
#print(num);print(den);print(mdl) #print(num);print(den);print(mdl)
w=np.logspace(0, np.log10(2000), 1000)*2*np.pi w=np.logspace(0, np.log10(2000), 1000)*2*np.pi
mdl1c= signal.lti(num1*numc, den1*denc) mdlcp= signal.lti(numc*nump, denc*denp)
mdl1d= signal.lti(num1*numd, den1*dend) mdldp= signal.lti(numd*nump, dend*denp)
mdl1= signal.lti(num1, den1) mdlp= signal.lti(nump, denp)
mdl0= signal.lti(num0, den0) mdlq= signal.lti(numq, denq)
bode(((mdl,'plant'), bode(((mdl,'plant'),
(mdl1c,'mdl1c'), (mdlcp,'mdlcp'),
(mdl1d,'mdl1d'), (mdldp,'mdldp'),
(mdl1,'mdl1'), (mdlp,'mdlp'),
(mdl0,'mdl0'), (mdlq,'mdlq'),
),w) ),w)
d={'num':num.coeffs,'num1':num1.coeffs,'num2':num2.coeffs,'num3':num3.coeffs,'num4':num4.coeffs,'num5':num5.coeffs,'numc':numc.coeffs,'num0':num0.coeffs,'numd':numd.coeffs, d={'num':num.coeffs,'numc':numc.coeffs,'numd':numd.coeffs,'nump':nump.coeffs,'numq':numq.coeffs,'numv':numv.coeffs,'num1':num1.coeffs,'num2':num2.coeffs,'num3':num3.coeffs,'num4':num4.coeffs,
'den':den.coeffs,'den1':den1.coeffs,'den2':den2.coeffs,'den3':den3.coeffs,'den4':den4.coeffs,'den5':den5.coeffs,'denc':denc.coeffs,'den0':den0.coeffs,'dend':dend.coeffs} 'den':den.coeffs,'denc':denc.coeffs,'dend':dend.coeffs,'denp':denp.coeffs,'denq':denq.coeffs,'denv':denv.coeffs,'den1':den1.coeffs,'den2':den2.coeffs,'den3':den3.coeffs,'den4':den4.coeffs}
fn=os.path.join(self.baseDir,'model%d.mat'%mot) fn=os.path.join(self.baseDir,'model%d.mat'%mot)
import scipy.io import scipy.io
scipy.io.savemat(fn, mdict=d) scipy.io.savemat(fn, mdict=d)
@@ -357,7 +367,16 @@ class MXTuning(Tuning):
prog+=''' //set output prog+=''' //set output
iqCmd={u}*{V}-{y}; iqCmd={u}*{V}-{y};
//return iqCmd; //return iqCmd;
pshm->P[200{motid}]=iqCmd; pshm->P[20{motid}0]=DesPos;
pshm->P[20{motid}1]=IqMeas;
pshm->P[20{motid}2]=ActVel;
pshm->P[20{motid}3]=ActPos;
pshm->P[20{motid}4]=_x[0];
pshm->P[20{motid}5]=_x[1];
pshm->P[20{motid}6]=_x[2];
pshm->P[20{motid}7]=_x[3];
pshm->P[20{motid}8]=iqCmd;
return pshm->ServoCtrl(Mptr); return pshm->ServoCtrl(Mptr);
}} }}
else else
@@ -723,7 +742,7 @@ EXPORT_SYMBOL(obsvr_servo_ctrl_{motid});'''.format(motid=motid)
bm+=32 #amplitude linear (not dB) bm+=32 #amplitude linear (not dB)
self.bode_plot(data, mode=bm, kwargs=meta) self.bode_plot(data, mode=bm, kwargs=meta)
plt.show(block=False) plt.show(block=False)
save_figs(fn) #save_figs(fn)
#wait_key() #wait_key()
raw_input('press return') raw_input('press return')

2
python/usr_code/Makefile
View File

@@ -112,7 +112,7 @@ tags:
Debug: all Debug: all
../../matlab/ssc%.mat: ../../matlab/StateSpaceControlDesign.m ../../matlab/identifyFxFyStage.m ../../matlab/ssc%.mat: ../../matlab/StateSpaceControlDesign.m ../../matlab/identifyFxFyStage.m Makefile
cd ../../matlab; /afs/psi.ch/sys/psi.x86_64_slp6/Programming/matlab/2018a/bin/matlab -nodisplay -nojvm -nosplash -nodesktop -r \ cd ../../matlab; /afs/psi.ch/sys/psi.x86_64_slp6/Programming/matlab/2018a/bin/matlab -nodisplay -nojvm -nosplash -nodesktop -r \
"clear;clear global;close all;\ "clear;clear global;close all;\
mot=identifyFxFyStage(7);\ mot=identifyFxFyStage(7);\