wip

MXfastStageDoc/MXfastStage.tex

@@ -17,6 +17,7 @@
 \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}
@@ -125,7 +126,13 @@ Here a example to roughly calculate at which frequency the motor moves 1um at 2A
 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
+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}
@@ -184,12 +191,15 @@ 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}
+
+\begin{tcolorbox}[width=15cm,colback=red!5!white,colframe=red!75!black,colbacktitle=red!50,coltitle=black,title=TODO]
+Record the friction (=current) at a slow move from +lim to -lim.\\
+Analyse the friction depending on the positions and motion directions.\\
+Do the records and analysis at different speeds.
+\end{tcolorbox}
 
-%→n times higher mass → n times lower frequency for same amplitude response
-%→n times higher frequency → n times higher velocity → n² times more acceleration==current
-%1um at 12Hz with 1 mA →with 2000mA → sqrt(2000)*12Hz=540Hz
-%
-%A very simplified transfer function of the system is G(s)=k/s²
 
 \FloatBarrier
 \subsubsection{using advanced Deltatau Servo Loop}
@@ -223,10 +233,11 @@ The Value of $K_{fff}$ is used to compensate the non linear static friction. It
 
 $K_{vff}$ is used to compensate the linear viscose friction.\\
 
-\textbf{TODO:}\\
+\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}
 
 
 
@@ -300,8 +311,7 @@ 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}.\\
 
-\textbf{TODO:}
+\begin{tcolorbox}[colback=red!5!white,colframe=red!75!black,colbacktitle=red!50,coltitle=black,title=TODO]
-
 A further test will be to 'remove' the current loop. This can be done by setting:$IiGain=0, IpfGain=1, IpbGain=-1$.
 The resulting transfer function is: 
 \[
@@ -314,6 +324,7 @@ The resulting transfer function is:
 \\
 \]
 This is a $PT_1$ element with a time constant of $\frac{L}{R}=\frac{2.4mH}{8.8\Omega}=0.27ms$. But probably due to additional cables etc. the resistance and therefore also the timeconstant is bigger.
+\end{tcolorbox}
 
 \subsection{Mechanical model}
 
@@ -618,10 +629,10 @@ end
 
 \begin{figure}[h!]
 \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}
 \label{fig:mot_observer_sim}
 \end{figure}
@@ -664,7 +675,8 @@ Finally the real time servo code is compliled for the DeltaTau with:\\
 
 Following lines in gpasciiCommander will activate the user servo loop code:
 
-\verb|TODO...|
+\begin{tcolorbox}[width=15cm,colback=red!5!white,colframe=red!75!black,colbacktitle=red!50,coltitle=black,title=TODO]
+\end{tcolorbox}
 
 \vspace{1pc}
 

matlab/Readme.md

@@ -6,7 +6,8 @@ Matlab Simulink start
 cd ~/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/matlab
 module load matlab/2018a
 matlab&
-or start directly: /afs/psi.ch/sys/psi.x86_64_slp6/Programming/matlab/2018a/bin/matlab&
+or start directly:
+/afs/psi.ch/sys/psi.x86_64_slp6/Programming/matlab/2018a/bin/matlab&
 this works also on my linux computers
 ```
 

matlab/StateSpaceControlDesign.m

@@ -1,4 +1,4 @@
-function [ssc]=StateSpaceControlDesign(mot)
+function [ssc]=StateSpaceControlDesign(mot,mode)
   % !!! first it need to run: [mot1,mot2]=identifyFxFyStage() to build a motor object !!!
   %
   % builds a state space controller designed for the plant.
@@ -15,69 +15,81 @@ function [ssc]=StateSpaceControlDesign(mot)
   %
   % https://www.youtube.com/watch?v=Lax3etc837U
 
-  %mPlt: mode to select plant
+  %plant and model
-  %mMdl: mode to select model for observer
+  % ss_plt :real plant (model of real plant)
-  %0 ss_plt :real plant (model of real plant)
+  % ss_c1  :current, mechanic, no resonance
-  %1 ss_c1  :current, mechanic, no resonance
+  % ss_d1  :simpl. current, mechanic, no resonance
-  %2 ss_d1  :simpl. current, mechanic, no resonance
+  % ss_1  :no current, mechanic, no resonance
-  %3 ss_1  :no current, mechanic, no resonance
+  % ss_0  :no current, simpl. mechanic, no resonance
-  %4 ss_0  :no current, simpl. mechanic, no resonance
 
-  %mPrefilt:prefilter mode
+  %prefilt:prefilter mode
   %0 no filter
   %1 inverse resonance filter
   %2 manual setup filter
 
-
+  %verb: mode(bits) to plot/simulate
-  %mShow: mode(bits) to plot/simulate
+  % 0:  1: poles of model and placed poles of controller
-  % 0: 1: bode plots of open loop
+  % 1:  2: bode plots of open loop
-  % 1: 2: step answer on open loop
+  % 2:  4: step answer on open loop
-  % 2: 4: step answer on closed loop with space state controller
+  % 3:  8: step answer on closed loop with space state controller
-  % 3: 8: step answer on closed loop with observer controller
+  % 4: 16: step answer on closed loop with observer controller
-  % 4:16: step answer on closed loop with disctrete observer controller
+  % 5: 32: step answer on closed loop with disctrete observer controller
-  % 5:32: plot all closed loop bode and pole-zero diagrams of desPos->actPos
+  % 6: 64: plot all closed loop bode and pole-zero diagrams of desPos->actPos
-  % 6:64:
+  % 7:128: bode plot of filt_pos_err
 
 
   %use_lqr: use lqr instead of pole placement
-
+  verb=1;
-  mPlt=0;
-  mMdl=1;
-  mPrefilt=2;
-  mShow=32+64;
   use_lqr=0;
-
+  filt_pos_err=Prefilt(mot,2);
-  switch mPlt
+  %locate poles: 2500rad/s = 397Hz, 6300rad/s = 1027Hz
+  switch mode
+    case -1 %TESTING
+      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
+      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=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
+        pl=[-3300 -3200 -2900 -2800];
+      else
+        pl=[-3300 -3200 -2700 -2600];
+      end
+      plObs=2*pl;
     case 1
-      ss_plt=mot.ss_c1;
+      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
+      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 2
-      ss_plt=mot.ss_d1;
+      ss_plt=mot.ss_plt; %ss_plt ss_c1 ss_d1 ss_1 ss_0
-    case 3
+      ss_mdl=mot.ss_c1;  %ss_plt ss_c1 ss_d1 ss_1 ss_0
-      ss_plt=mot.ss_1;
+      use_lqr=1 
-    case 4
-      ss_plt=mot.ss_0;
   end
-  ss_plt.Name='open loop plant';
-
-  switch mMdl
-    case 0
-      ss_mdl=mot.ss_plt;
-    case 1
-      ss_mdl=mot.ss_c1;
-    case 2
-      ss_mdl=mot.ss_d1;
-    case 3
-      ss_mdl=mot.ss_1;
-    case 4
-      ss_mdl=mot.ss_0;
-  end
-  ss_mdl.Name='open loop model'; %model for observer
 
   [Am,Bm,Cm,Dm]=ssdata(ss_mdl);
   
-  if bitand(mShow,1)
+  if bitand(verb,1) && use_lqr==0
+    format compact
+    format shortG
+    disp(pole(ss_mdl)) %==eig(Am)
+    %damp(ss_mdl) %further informations
+    disp(pl)
+    disp(plObs)
+    format short
+  end  
+  
+  if bitand(verb,2)
     figure();h=bodeplot(ss_plt,ss_mdl);
     setoptions(h,'IOGrouping','all')
   end
@@ -85,7 +97,7 @@ function [ssc]=StateSpaceControlDesign(mot)
   xp0 = zeros(1,length(ss_plt.A));
   xm0 = zeros(1,length(Am));
 
-  if bitand(mShow,2)
+  if bitand(verb,4)
     % step answer on open loop:
     t = 0:1E-4:.5;
     u = ones(size(t));
@@ -94,7 +106,6 @@ function [ssc]=StateSpaceControlDesign(mot)
     figure();plot(t,yp,t,ym,'--');title('step on open loop (plant and model)');
     legend('plt.iqMeas','plt.iqVolts','plt.actPos','mdl.iqMeas','mdl.iqVolts','mdl.actPos')
   end
-  poles = eig(Am);
   %w0=abs(poles);
   %ang=angle(-poles);
   %-------------------
@@ -104,51 +115,12 @@ function [ssc]=StateSpaceControlDesign(mot)
   %
   %place poles for the controller feedback
   if use_lqr %use the lqr controller
-    Q=eye(length(ss_mdl.A));
+    Q=eye(length(Am));
     R=1;
-    [K,P,E]=lqr(ss_mdl,Q,R,0);
+    [K,P,E]=lqr(Am,Bm,Q,R,0);
   else
-    if mot.id==1
+    K = place(Am,Bm,pl);
-      %2500rad/s = 397Hz -> locate poles here
+    %K = acker(Am,Bm,pl);
-      %6300rad/s = 1027Hz -> locate poles here
-      switch mMdl
-        case 0
-          p1=-3300+2800i;  p2=-2700+500i;  p3=-2500+10i;
-          P=[p1 p1' p2 p2' p3 p3'];
-        case 1
-          %p1=-6300+280i;    p2=-6200+150i;
-          %P=[p1 p1' p2 p2'];
-          P=[-4100 -4000 -1500+10j -1500-10j];
-        case 2
-          %p1=-6300+280i;    p2=-6200+150i;
-          %P=[p1 p1' p2 p2'];
-          P=[-1500+10j -1500-10j];
-        case 3
-          %p1=-6300+280i;    p2=-6200+150i;
-          %P=[p1 p1' p2 p2'];
-          P=[-1500+10j -1500-10j -1400 -1300];
-      end
-    else
-      %2500rad/s = 397Hz -> locate poles here
-      %6300rad/s = 1027Hz -> locate poles here
-      switch mMdl
-        case 0
-          p1=-3300+2800i;    p2=-1900+130i;       p3=-2900+80i;
-          p4=-2300+450i;     p5=-2000+20i;        p6=-1500+10i;
-          P=[p1 p1' p2 p2' p3 p3' p4 p4' p5 p5' p6 p6'];
-        case 1
-          %p1=-6300+2800i;   p2=-6200+1500i;
-          %P=[p1 p1' p2 p2'];
-          P=[-2500 -2800 -1500+10j -1500-10j];
-          P=[-2500 -2800 -1100+10j -1100-10j];
-        case 2
-          %p1=-6300+2800i;   p2=-6200+1500i;
-          %P=[p1 p1' p2 p2'];
-          P=[-2500 -2800 -1500+10j -1500-10j];
-      end
-    end
-    K = place(Am,Bm,P);
-    %K = acker(Am,Bm,Pm);
   end %if lqr
 
   V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
@@ -157,7 +129,7 @@ function [ssc]=StateSpaceControlDesign(mot)
   end
 
   ss_cl = ss(Am-Bm*K,Bm*V,Cm,0,'Name','space state controller','InputName',ss_mdl.InputName,'OutputName',ss_mdl.OutputName);
-  if bitand(mShow,4)
+  if bitand(verb,8)
     % step answer on closed loop with space state controller:
     t = 0:1E-4:.5;
     [y,t,x]=lsim(ss_cl,V*u,t,xm0);
@@ -166,10 +138,15 @@ function [ssc]=StateSpaceControlDesign(mot)
 
   % *** observer controller ***
   %
-  %observer poles-> 5 times farther left than system poles
+  %observer poles
-  OP=2*P;
+  if use_lqr %use the lqr controller
-  L=place(Am',Cm',OP)';
+    Q=eye(length(Am'));   % ??????????????? CHANGES NEEDED ????????????
-  %L=acker(A',C',OP)';
+    R=eye(size(Cm,1));
+    [L,P,E]=lqr(Am',Cm',Q,R,0);
+  else
+    L=place(Am',Cm',plObs)';
+    %L=acker(A',C',plObs)';
+  end
 
   At = [ Am-Bm*K             Bm*K
          zeros(size(Am))    Am-L*Cm ];
@@ -179,7 +156,7 @@ function [ssc]=StateSpaceControlDesign(mot)
 
   Dt=0;
   ss_t = ss(At,Bt,Ct,Dt,'Name','observer controller','InputName',{'desPos'},'OutputName',ss_mdl.OutputName);
-  if bitand(mShow,8)
+  if bitand(verb,16)
     % step answer on closed loop with observer controller:
     figure();lsim(ss_t,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer');
   end
@@ -191,20 +168,16 @@ function [ssc]=StateSpaceControlDesign(mot)
   [Atz,Btz,Ctz,Dtz]=ssdata(ss_tz );
   ss_tz.Name='discrete obsvr ctrl';
 
-  if bitand(mShow,16)
+  if bitand(verb,32)
     % step answer on closed loop with disctrete observer controller:
     t = 0:Ts:.05;
     figure();lsim(ss_tz ,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer discrete');
   end
 
-  if bitand(mShow,32)
+  if bitand(verb,64)
     %plot all bode diagrams of desPos->actPos
     figure();
-    if mMdl==2 || mMdl==3
+    idx=length(ss_cl.OutputName);
-      idx=1;
-    else
-      idx=3;
-    end
     h=bodeplot(ss_cl(idx),ss_t(idx),ss_tz(idx));
     setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
 
@@ -222,11 +195,7 @@ function [ssc]=StateSpaceControlDesign(mot)
   Co=K;
   Do=zeros(size(Co,1),size(Bo,2));
 
-  if mMdl==2 || mMdl==3
+  ss_o = ss(Ao,Bo,Co,Do,'Name','observer controller','InputName',[{'desPos'}; ss_mdl.OutputName ],'OutputName',{'k*xt'});
-    ss_o = ss(Ao,Bo,Co,Do,'Name','observer controller','InputName',{'desPos','actPos'},'OutputName',{'k*xt'});
-  else
-    ss_o = ss(Ao,Bo,Co,Do,'Name','observer controller','InputName',{'desPos','iqMeas','iqVolts','actPos'},'OutputName',{'k*xt'});
-  end
 
   %discrete plant
   %ss_pltz = c2d(ss_plt,Ts);
@@ -235,20 +204,17 @@ function [ssc]=StateSpaceControlDesign(mot)
   %discrete observer controller
   ss_oz = c2d(ss_o,Ts);
 
-  %prefilter to compensate non observable resonance frequencies
-  prefilt=Prefilt(mot,mPrefilt);
-
   %discrete prefilter
-  prefiltz=c2d(prefilt,Ts);
+  filt_pos_err_z=c2d(filt_pos_err,Ts);
 
-  if bitand(mShow,64)
+  if bitand(verb,128)
-    h=bodeplot(prefilt,prefiltz);
+    h=bodeplot(filt_pos_err,filt_pos_err_z);
     setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
   end
 
   %state space controller
   ssc=struct();
-  for k=["Ts","ss_plt","ss_o","ss_oz","prefilt","prefiltz","V"]
+  for k=["Ts","ss_plt","ss_o","ss_oz","filt_pos_err","filt_pos_err_z","V"]
     ssc=setfield(ssc,k,eval(k));
   end
   save(sprintf('/tmp/ssc%d.mat',mot.id),'-struct','ssc');
@@ -275,7 +241,6 @@ function pf=Prefilt(mot,mode)
         denV=den1;
         pf=tf(numV,denV);
       else
-
         %Lag
         f=[100 200]; w=f*2*pi; T=1./w;
         tf1=tf([T(1) 1],[T(2) 1]);

matlab/documentFunctions.m

@@ -28,13 +28,15 @@ disp('document figure generation done');close all;
 
 
 close all;disp('simulate observer with prefilter...');
-for k =1:2
+for m =0%0:1
-  [ssc]=StateSpaceControlDesign(mot{k});sim('observer');
+  for k =1:2
-  f=figure(); h=plot(desPos_actPos.Time,desPos_actPos.Data,'g');
+    [ssc]=StateSpaceControlDesign(mot{k},m);sim('observer');
-  set(h(1),'color','b'); set(h(2),'color',[0 0.5 0]);
+    f=figure(); h=plot(desPos_actPos.Time,desPos_actPos.Data,'g');
-  print(f,sprintf('figures/sim_cl_obs_pf_%d',mot{k}.id),'-depsc');
+    set(h(1),'color','b'); set(h(2),'color',[0 0.5 0]);
-  f=bodeSamples(desPos_actPos);
+    print(f,sprintf('figures/sim_cl_obs_%d_%d',m,mot{k}.id),'-depsc');
-  print(f,sprintf('figures/sim_cl_obs_pf_bode%d',mot{k}.id),'-depsc');
+    f=bodeSamples(desPos_actPos);
+    print(f,sprintf('figures/sim_cl_obs_bode%d_%d',m,mot{k}.id),'-depsc');
+  end
 end
 disp('document figure generation done');close all;
 

matlab/identifyFxFyStage.m

@@ -212,6 +212,7 @@ function motCell=identifyFxFyStage(mode)
     %             |          3|-------> actPos
     %             +-----------+
     mot.ss_plt=connect(tfc,tf1,tf2,'iqCmd',{'iqMeas','actVel','actPos'});
+    mot.ss_plt.Name='best plant approximation';
     chkCtrlObsv(mot.ss_plt,'ss_plt fyStage');     
     
     %without resonance
@@ -222,6 +223,7 @@ function motCell=identifyFxFyStage(mode)
     %             +-----------+
     s=tf1.InputName{1};tf1.InputName{1}='iqMeas';
     mot.ss_c1=connect(tfc,tf1,'iqCmd',{'iqMeas','actVel','actPos'});
+    mot.ss_c1.Name='without resonance';
     chkCtrlObsv(mot.ss_c1,'ss_c1 fyStage');
     tf1.InputName{1}=s;%restore
 
@@ -234,6 +236,7 @@ function motCell=identifyFxFyStage(mode)
     %             +-----------+
     s=tf1.InputName{1};tf1.InputName{1}='iqMeas';
     mot.ss_d1=connect(tfd,tf1,'iqCmd',{'iqMeas','actVel','actPos'});
+    mot.ss_d1.Name='simplified current, without resonance';
     chkCtrlObsv(mot.ss_d1,'ss_d1 fyStage');
     tf1.InputName{1}=s;%restore
     
@@ -244,6 +247,7 @@ function motCell=identifyFxFyStage(mode)
     %             |          2|-------> actPos
     %             +-----------+
     mot.ss_1=ss(tf1);
+    mot.ss_1.Name='no current loop, no resonance';
     chkCtrlObsv(mot.ss_1,'ss_1 fyStage');
 
     
@@ -253,6 +257,7 @@ function motCell=identifyFxFyStage(mode)
     %             |          2|-------> actPos
     %             +-----------+
     mot.ss_0=ss(tf0);
+    mot.ss_0.Name='simplified mechanics, no current loop, no resonance';
     chkCtrlObsv(mot.ss_0,'ss_0 fyStage');
 
     %h=bodeplot(mot.meas,'r',mot.tf4_2,'b',mot.tf6_4,'g');