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

first rewrite of ovserver

Browse Source
This commit is contained in:
zamofing_t
2019-02-08 09:47:46 +01:00
parent 3e83b82a5d
commit 6b5b8750d1
6 changed files with 70 additions and 163 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
MXfastStageDoc/MXfastStage.tex
View File

@@ -448,8 +448,7 @@ It has to be checked if the model matches the real stage. Therefore simulations
To compare the measurements with the model following lines were executed in MATLAB To compare the measurements with the model following lines were executed in MATLAB
\begin{verbatim} \begin{verbatim}
clear;clear global;close all; clear;clear global;close all;
mot=cell(2,1); mot=identifyFxFyStage(7);
[mot{1},mot{2}]=identifyFxFyStage();
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');
@@ -537,10 +536,11 @@ As first approach the tf function is just converted to the ss space and the ss m
The matlab models are:\\ The matlab models are:\\
\begin{tabular}{ll} \begin{tabular}{ll}
\texttt{ssPlt:} & 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{ssMdl\_c1:} & model without resonance (only current and main mechanical)\\ \texttt{ss\_c1:} & model without resonance (only current and main mechanical)\\
\texttt{ssMdl\_12:} & model without current loop, with one resonance (main mechanical + first resonance)\\ \texttt{ss\_d1:} & model without resonance (simplified current and main mechanical)\\
\texttt{ssMdl\_1:} & model without current loop, no resonance (only main mechanical)\\ \texttt{ss\_1:} & model without current loop, no resonance (only main mechanical)\\
\texttt{ss\_0:} & simplified mechanical, no current loop, no resonance\\
\end{tabular}\\ \end{tabular}\\
\vspace{1pc} \vspace{1pc}
@@ -549,9 +549,9 @@ 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=cell(2,1); mot=cell(2,1);
[mot{1},mot{2}]=identifyFxFyStage(); [mot{1},mot{2}]=identifyFxFyStage(7);
for k =1:2 for k =1:2
[ssc]=StateSpaceControlDesign(mot1{k});sim('observer'); [ssc]=StateSpaceControlDesign(mot{k});sim('observer');
f=figure(); h=plot(desPos_actPos.Time,desPos_actPos.Data,'g'); f=figure(); h=plot(desPos_actPos.Time,desPos_actPos.Data,'g');
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_observer_%d',mot{k}.id),'-depsc');

157
matlab/StateSpaceControlDesign.m
View File

@@ -16,18 +16,12 @@ function [ssc]=StateSpaceControlDesign(mot)
% https://www.youtube.com/watch?v=Lax3etc837U % https://www.youtube.com/watch?v=Lax3etc837U
%mPlt: mode to select plant %mPlt: mode to select plant
%0 real plant (model of real plant)
%1 current, mechanic, no resonance
%2 no current current, mechanic, no resonance
%3 no current current, mechanic, first resonance
%mMdl: mode to select model for observer %mMdl: mode to select model for observer
%0 real plant (NOT RECOMANDED, because not observab,econtrolable) %0 ss_plt :real plant (model of real plant)
%1 current, mechanic, no resonance %1 ss_c1 :current, mechanic, no resonance
%2 no current current, mechanic, no resonance %2 ss_d1 :simpl. current, mechanic, no resonance
%3 no current current, mechanic, first resonance %3 ss_1 :no current, mechanic, no resonance
%4 ss_0 :no current, simpl. mechanic, no resonance
%mPrefilt:prefilter mode %mPrefilt:prefilter mode
%0 no filter %0 no filter
@@ -55,34 +49,38 @@ function [ssc]=StateSpaceControlDesign(mot)
switch mPlt switch mPlt
case 0 case 0
ssPlt=mot.ssPlt;%real plant (model of real plant) ss_plt=mot.ss_plt;
case 1 case 1
ssPlt=mot.ssMdl_c1;%current, mechanic, no resonance ss_plt=mot.ss_c1;
case 2 case 2
ssPlt=mot.ssMdl_1;%no current current, mechanic, no resonance ss_plt=mot.ss_d1;
case 3 case 3
ssPlt=mot.ssMdl_12;%no current current, mechanic, first resonance ss_plt=mot.ss_1;
case 4
ss_plt=mot.ss_0;
end end
ssPlt.Name='open loop plant'; ss_plt.Name='open loop plant';
switch mMdl switch mMdl
case 0 case 0
ssMdl=mot.ssPlt;%real plant (model of real plant) ss_mdl=mot.ss_plt;
case 1 case 1
ssMdl=mot.ssMdl_c1;%current, mechanic, no resonance ss_mdl=mot.ss_c1;
case 2 case 2
ssMdl=mot.ssMdl_1;%no current current, mechanic, no resonance ss_mdl=mot.ss_d1;
case 3 case 3
ssMdl=mot.ssMdl_12;%no current current, mechanic, first resonance ss_mdl=mot.ss_1;
case 4
ss_mdl=mot.ss_0;
end end
ssMdl.Name='open loop model'; %model for observer ss_mdl.Name='open loop model'; %model for observer
[Ap,Bp,Cp,Dp]=ssdata(ssPlt); [Ap,Bp,Cp,Dp]=ssdata(ss_plt);
[Am,Bm,Cm,Dm]=ssdata(ssMdl); [Am,Bm,Cm,Dm]=ssdata(ss_mdl);
if bitand(mShow,1) if bitand(mShow,1)
figure();h=bodeplot(ssPlt,ssMdl); figure();h=bodeplot(ss_plt,ss_mdl);
setoptions(h,'IOGrouping','all') setoptions(h,'IOGrouping','all')
end end
@@ -93,8 +91,8 @@ function [ssc]=StateSpaceControlDesign(mot)
% step answer on open loop: % step answer on open loop:
t = 0:1E-4:.5; t = 0:1E-4:.5;
u = ones(size(t)); u = ones(size(t));
[yp,t,x] = lsim(ssPlt,u,t,xp0); [yp,t,x] = lsim(ss_plt,u,t,xp0);
[ym,t,x] = lsim(ssMdl,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();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') legend('plt.iqMeas','plt.iqVolts','plt.actPos','mdl.iqMeas','mdl.iqVolts','mdl.actPos')
end end
@@ -108,9 +106,9 @@ function [ssc]=StateSpaceControlDesign(mot)
% %
%place poles for the controller feedback %place poles for the controller feedback
if use_lqr %use the lqr controller if use_lqr %use the lqr controller
Q=eye(length(ssMdl.A)); Q=eye(length(ss_mdl.A));
R=1; R=1;
[K,P,E]=lqr(ssMdl,Q,R,0); [K,P,E]=lqr(ss_mdl,Q,R,0);
else else
if mot.id==1 if mot.id==1
%2500rad/s = 397Hz -> locate poles here %2500rad/s = 397Hz -> locate poles here
@@ -155,12 +153,11 @@ function [ssc]=StateSpaceControlDesign(mot)
end %if lqr end %if lqr
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 )
%Nbar(2)=1; %the voltage stuff is crap for now
if length(V)>1 if length(V)>1
V=V(3); % only the position scaling needed V=V(3); % only the position scaling needed
end end
ss_cl = ss(Am-Bm*K,Bm*V,Cm,0,'Name','space state controller','InputName',ssMdl.InputName,'OutputName',ssMdl.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(mShow,4) if bitand(mShow,4)
% step answer on closed loop with space state controller: % step answer on closed loop with space state controller:
t = 0:1E-4:.5; t = 0:1E-4:.5;
@@ -182,7 +179,7 @@ function [ssc]=StateSpaceControlDesign(mot)
Ct = [ Cm zeros(size(Cm)) ]; Ct = [ Cm zeros(size(Cm)) ];
Dt=0; Dt=0;
ss_t = ss(At,Bt,Ct,Dt,'Name','observer controller','InputName',{'desPos'},'OutputName',ssMdl.OutputName); ss_t = ss(At,Bt,Ct,Dt,'Name','observer controller','InputName',{'desPos'},'OutputName',ss_mdl.OutputName);
if bitand(mShow,8) if bitand(mShow,8)
% step answer on closed loop with observer controller: % 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'); figure();lsim(ss_t,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer');
@@ -233,8 +230,8 @@ function [ssc]=StateSpaceControlDesign(mot)
end end
%discrete plant %discrete plant
ssPltz = c2d(ssPlt,Ts); %ss_pltz = c2d(ss_plt,Ts);
[Apz,Bpz,Cpz,Dpz]=ssdata(ssPltz); %[Apz,Bpz,Cpz,Dpz]=ssdata(ss_pltz);
%discrete observer controller %discrete observer controller
ss_oz = c2d(ss_o,Ts); ss_oz = c2d(ss_o,Ts);
@@ -261,7 +258,9 @@ function [ssc]=StateSpaceControlDesign(mot)
%state space controller %state space controller
ssc=struct(); 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","ss_cl","ss_o","ss_oz","numV","denV","numVz","denVz"] %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","ss_cl","ss_o","ss_oz","numV","denV","numVz","denVz"]
%for k=["Ts","Ap","Bp","Cp","Dp","Ao","Bo","Co","Do","Aoz","Boz","Coz","Doz","V","K","L","ss_cl","ss_o","ss_oz","numV","denV","numVz","denVz"]
for k=["Ts","ss_plt","ss_o","ss_oz","prefilt","prefiltz","V"]
ssc=setfield(ssc,k,eval(k)); ssc=setfield(ssc,k,eval(k));
end end
save(sprintf('/tmp/ssc%d.mat',mot.id),'-struct','ssc'); save(sprintf('/tmp/ssc%d.mat',mot.id),'-struct','ssc');
@@ -302,93 +301,3 @@ function pf=Prefilt(mot,mode)
end end
%code snipplets from an example on youtube (see reference at top)
function SCRATCH()
%import numpy as np
%fh=np.load('mode1.npz')
%import scipy.io
%scipy.io.savemat('mode1.mat',fh,do_compression=True)
%matlab:
load('mode1.mat');
plot(pts(:,1),pts(:,2),'.');hold on;
plot(rec(:,5),rec(:,6),'-');%despos
plot(rec(:,2),rec(:,3),'-');%actPos
%sig.time = [0 1 1 5 5 8 8 10];
%sig.signals.values = [0 0 2 2 2 3 3 3]';
%sig.signals.dimensions = 1;
sig.time=0:2E-4:(length(rec)-1)*2E-4;
sig.signals.values=rec(:,5);
sig.signals.dimensions = 1;
sum(desPos_actPos.Data(:,1)-desPos_actPos.Data(:,2))
A = [ 0 1 0
980 0 -2.8
0 0 -100 ];
B = [ 0
0
100 ];
C = [ 1 0 0 ];
poles = eig(A)
t = 0:0.01:2;
u = zeros(size(t));
x0 = [0.01 0 0];
sys = ss(A,B,C,0);
[y,t,x] = lsim(sys,u,t,x0);
plot(t,y)
title('Open-Loop Response to Non-Zero Initial Condition')
xlabel('Time (sec)')
ylabel('Ball Position (m)')
p1 = -10 + 10i;
p2 = -10 - 10i;
p3 = -50;
K = place(A,B,[p1 p2 p3]);
sys_cl = ss(A-B*K,B,C,0);
lsim(sys_cl,u,t,x0);
xlabel('Time (sec)')
ylabel('Ball Position (m)')
p1 = -20 + 20i;
p2 = -20 - 20i;
p3 = -100;
K = place(A,B,[p1 p2 p3]);
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);
lsim(sys_cl,u,t);
xlabel('Time (sec)')
ylabel('Ball Position (m)')
axis([0 2 -4E-6 0])
Nbar = rscale(sys,K)
lsim(sys_cl,Nbar*u,t)
title('Linear Simulation Results (with Nbar)')
xlabel('Time (sec)')
ylabel('Ball Position (m)')
axis([0 2 0 1.2*10^-3])
end

43
matlab/identifyFxFyStage.m
View File

@@ -31,11 +31,12 @@ function motCell=identifyFxFyStage(mode)
% loadData reads members currstep,w,mag,phase,meas % loadData reads members currstep,w,mag,phase,meas
% %
% mode bits: % mode bits:
% 0 1 : add ss-models and do checks for motor 1 fy % 0 1 : select motor 1 fy
% 1 2 : add ss-models and do checks for setup motor 2 fx % 1 2 : select motor 2 fx
% 2 4 : identify_currstep % 2 4 : add ss-models and do obser/contr checks
% 3 8 : identify_tf2 % 3 8 : identify_currstep
% The default value for mode is 3 % 4 16 : identify_tf (TODO!)
% The default value for mode is 7
%References: %References:
@@ -67,6 +68,7 @@ function motCell=identifyFxFyStage(mode)
opt=tfestOptions; opt=tfestOptions;
opt.Display='off'; opt.Display='off';
tfc = tfest(obj.currstep, 2, 0,opt); tfc = tfest(obj.currstep, 2, 0,opt);
s=splitlines(string(evalc('tfc')));disp(join(s(5:7),newline));
s=str2ndOrd(tfc); s=str2ndOrd(tfc);
t=(0:199)*50E-6; t=(0:199)*50E-6;
[y,t]=step(tfc,t); [y,t]=step(tfc,t);
@@ -85,7 +87,7 @@ function motCell=identifyFxFyStage(mode)
print(f,sprintf('figures/currstep_%d',obj.id),'-depsc'); print(f,sprintf('figures/currstep_%d',obj.id),'-depsc');
end end
function tf2=identify_tf2(obj) function tf2=identify_tf(obj)
opt=tfestOptions; opt=tfestOptions;
opt.Display='off'; opt.Display='off';
opt.initializeMethod='iv'; opt.initializeMethod='iv';
@@ -351,19 +353,22 @@ function motCell=identifyFxFyStage(mode)
for motid= 1:2 for motid= 1:2
mot=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/19_01_29/',motid); mot=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/19_01_29/',motid);
mot.id=motid; mot.id=motid;
if bitand(mode,4) if bitand(mode,motid)
%identification of second order transfer function out of the current step recorded data. if bitand(mode,4)
identify_currstep(mot{motid}); if motid==1
end mot=fyStage(mot);
if bitand(mode,8) else
%identification of second order transfer function out of the position recorded data. mot=fxStage(mot);
identify_tf2(mot); end
end end
if motid==1 && bitand(mode,1) if bitand(mode,8)
mot=fyStage(mot); %identification of second order transfer function out of the current step recorded data.
end identify_currstep(mot);
if motid==2 && bitand(mode,2) end
mot=fxStage(mot); if bitand(mode,16)
%identification of second order transfer function out of the position recorded data.
identify_tf(mot);
end
end end
motCell{motid}=mot; motCell{motid}=mot;
end end

BIN
matlab/observer.slx
View File

Binary file not shown.

17
matlab/simFxFyStage.m
View File

@@ -13,25 +13,18 @@ function [pb]=simFxFyStage(mot)
%!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; Kp=25;Kvfb=400;Ki=0.02;Kvff=350;Kaff=5000;MaxInt=1000;
mot_num=mot.tf_mdl.Numerator;
mot_den=mot.tf_mdl.Denominator;
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; Kp=22;Kvfb=350;Ki=0.02;Kvff=240;Kaff=3500;MaxInt=1000;
mot_num=mot.tf_mdl.Numerator;
mot_den=mot.tf_mdl.Denominator;
end end
ss_plt=mot.ss_plt;
mdlName='stage_closed_loop';
%open(mdlName)
%ServoDeltaTau_z(motid)
[A,B,C,D]=tf2ss(mot_num,mot_den);
pb=struct(); pb=struct();
for k=["Kp","Kvfb","Ki","Kvff","Kaff","MaxInt","mot_num","mot_den","Ts","MaxDac","MaxPosErr","A","B","C","D"] for k=["Kp","Kvfb","Ki","Kvff","Kaff","MaxInt","Ts","MaxDac","MaxPosErr","ss_plt"]
pb=setfield(pb,k,eval(k)); pb=setfield(pb,k,eval(k));
end end
%mdlName='stage_closed_loop';
%open(mdlName)
%sim(mdlName)
end end

BIN
matlab/stage_closed_loop.slx
View File

Binary file not shown.