diff --git a/MXfastStageDoc/MXfastStage.tex b/MXfastStageDoc/MXfastStage.tex index 382d2db..7738626 100644 --- a/MXfastStageDoc/MXfastStage.tex +++ b/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_observer_bode1.eps}\\ -\includegraphics[scale=.45]{../matlab/figures/sim_cl_observer_2.eps} -\includegraphics[scale=.45]{../matlab/figures/sim_cl_observer_bode2.eps} +\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_0_1.eps} +\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_bode0_1.eps}\\ +\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_0_2.eps} +\includegraphics[scale=.45]{../matlab/figures/sim_cl_obs_bode0_2.eps} \caption{Observer sim: Motor 1 Motor 2} \label{fig:mot_observer_sim} \end{figure} @@ -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} diff --git a/matlab/Readme.md b/matlab/Readme.md index 42745fd..931f9a0 100644 --- a/matlab/Readme.md +++ b/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 ``` diff --git a/matlab/StateSpaceControlDesign.m b/matlab/StateSpaceControlDesign.m index 3342f6e..325b0c5 100644 --- a/matlab/StateSpaceControlDesign.m +++ b/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 - %mMdl: mode to select model for observer - %0 ss_plt :real plant (model of real plant) - %1 ss_c1 :current, mechanic, no resonance - %2 ss_d1 :simpl. current, mechanic, no resonance - %3 ss_1 :no current, mechanic, no resonance - %4 ss_0 :no current, simpl. mechanic, no resonance + %plant and model + % ss_plt :real plant (model of real plant) + % ss_c1 :current, mechanic, no resonance + % ss_d1 :simpl. current, mechanic, no resonance + % ss_1 :no current, mechanic, no resonance + % 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 - - %mShow: mode(bits) to plot/simulate - % 0: 1: bode plots of open loop - % 1: 2: step answer on open loop - % 2: 4: 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 disctrete observer controller - % 5:32: plot all closed loop bode and pole-zero diagrams of desPos->actPos - % 6:64: + %verb: mode(bits) to plot/simulate + % 0: 1: poles of model and placed poles of controller + % 1: 2: bode plots of open loop + % 2: 4: step answer on open loop + % 3: 8: step answer on closed loop with space state controller + % 4: 16: step answer on closed loop with observer controller + % 5: 32: step answer on closed loop with disctrete observer controller + % 6: 64: plot all closed loop bode and pole-zero diagrams of desPos->actPos + % 7:128: bode plot of filt_pos_err %use_lqr: use lqr instead of pole placement - - mPlt=0; - mMdl=1; - mPrefilt=2; - mShow=32+64; + verb=1; use_lqr=0; - - switch mPlt + filt_pos_err=Prefilt(mot,2); + %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; - case 3 - ss_plt=mot.ss_1; - case 4 - ss_plt=mot.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 + use_lqr=1 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 - %2500rad/s = 397Hz -> locate poles here - %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); + K = place(Am,Bm,pl); + %K = acker(Am,Bm,pl); 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 - OP=2*P; - L=place(Am',Cm',OP)'; - %L=acker(A',C',OP)'; + %observer poles + if use_lqr %use the lqr controller + Q=eye(length(Am')); % ??????????????? CHANGES NEEDED ???????????? + 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=1; - else - idx=3; - end + idx=length(ss_cl.OutputName); h=bodeplot(ss_cl(idx),ss_t(idx),ss_tz(idx)); setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw'); @@ -221,13 +194,9 @@ function [ssc]=StateSpaceControlDesign(mot) Bo=[Bm L]; 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','actPos'},'OutputName',{'k*xt'}); - else - ss_o = ss(Ao,Bo,Co,Do,'Name','observer controller','InputName',{'desPos','iqMeas','iqVolts','actPos'},'OutputName',{'k*xt'}); - end - + + ss_o = ss(Ao,Bo,Co,Do,'Name','observer controller','InputName',[{'desPos'}; ss_mdl.OutputName ],'OutputName',{'k*xt'}); + %discrete plant %ss_pltz = c2d(ss_plt,Ts); %[Apz,Bpz,Cpz,Dpz]=ssdata(ss_pltz); @@ -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) - h=bodeplot(prefilt,prefiltz); + if bitand(verb,128) + 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,14 +241,13 @@ 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]); %bo = bodeoptions; %bo.FreqUnits = 'Hz'; bo.MagUnits='abs'; bo.Grid='on'; %bode(tf1,bo) - + %k=1.2; aa=2; f=[40 60];w=f*2*pi; tf([1 33 w0^2]; den3=[1 20 w0^2]; %f=277;w0=f*2*pi; num1=[1 20 w0^2]; den1=[1 500 w0^2]; %f=138;w0=f*2*pi; num2=[1 300 w0^2]; den2=[1 100 w0^2]; diff --git a/matlab/documentFunctions.m b/matlab/documentFunctions.m index b838cbf..afaac54 100644 --- a/matlab/documentFunctions.m +++ b/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 - [ssc]=StateSpaceControlDesign(mot{k});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_obs_pf_%d',mot{k}.id),'-depsc'); - f=bodeSamples(desPos_actPos); - print(f,sprintf('figures/sim_cl_obs_pf_bode%d',mot{k}.id),'-depsc'); +for m =0%0:1 + for k =1:2 + [ssc]=StateSpaceControlDesign(mot{k},m);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_obs_%d_%d',m,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; diff --git a/matlab/identifyFxFyStage.m b/matlab/identifyFxFyStage.m index 2e9da8c..0b357a5 100644 --- a/matlab/identifyFxFyStage.m +++ b/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'); diff --git a/matlab/libESB_MX.slx b/matlab/libESB_MX.slx index ceb311f..149fbc4 100644 Binary files a/matlab/libESB_MX.slx and b/matlab/libESB_MX.slx differ diff --git a/matlab/observer.slx b/matlab/observer.slx index 22811d6..69a6764 100644 Binary files a/matlab/observer.slx and b/matlab/observer.slx differ diff --git a/matlab/stage_closed_loop.slx b/matlab/stage_closed_loop.slx index 18da798..956a085 100644 Binary files a/matlab/stage_closed_loop.slx and b/matlab/stage_closed_loop.slx differ