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
\begin{verbatim}
clear;clear global;close all;
mot=cell(2,1);
[mot{1},mot{2}]=identifyFxFyStage();
mot=identifyFxFyStage(7);
for k =1:2
[pb]=simFxFyStage(mot{k});sim('stage_closed_loop');
f=figure(); h=plot(desPos_actPos.Time,desPos_actPos.Data,'g');
@@ -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:\\
\begin{tabular}{ll}
\texttt{ssPlt:} & best approach of the plant with mechanics, resonance, current loop etc.\\
\texttt{ssMdl\_c1:} & model without resonance (only current and main mechanical)\\
\texttt{ssMdl\_12:} & model without current loop, with one resonance (main mechanical + first resonance)\\
\texttt{ssMdl\_1:} & model without current loop, no resonance (only main mechanical)\\
\texttt{ss\_plt:} & best approach of the plant with mechanics, resonance, current loop etc.\\
\texttt{ss\_c1:} & model without resonance (only current and main mechanical)\\
\texttt{ss\_d1:} & model without resonance (simplified current and main mechanical)\\
\texttt{ss\_1:} & model without current loop, no resonance (only main mechanical)\\
\texttt{ss\_0:} & simplified mechanical, no current loop, no resonance\\
\end{tabular}\\
\vspace{1pc}
@@ -549,9 +549,9 @@ Following code calculates parameters for a observer controller, does a simulatio
\begin{verbatim}
clear;clear global;close all;
mot=cell(2,1);
[mot{1},mot{2}]=identifyFxFyStage();
[mot{1},mot{2}]=identifyFxFyStage(7);
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');
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');

157
matlab/StateSpaceControlDesign.m
View File

@@ -16,18 +16,12 @@ function [ssc]=StateSpaceControlDesign(mot)
% https://www.youtube.com/watch?v=Lax3etc837U
%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
%0 real plant (NOT RECOMANDED, because not observab,econtrolable)
%1 current, mechanic, no resonance
%2 no current current, mechanic, no resonance
%3 no current current, mechanic, first resonance
%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
%mPrefilt:prefilter mode
%0 no filter
@@ -55,34 +49,38 @@ function [ssc]=StateSpaceControlDesign(mot)
switch mPlt
case 0
ssPlt=mot.ssPlt;%real plant (model of real plant)
ss_plt=mot.ss_plt;
case 1
ssPlt=mot.ssMdl_c1;%current, mechanic, no resonance
ss_plt=mot.ss_c1;
case 2
ssPlt=mot.ssMdl_1;%no current current, mechanic, no resonance
ss_plt=mot.ss_d1;
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
ssPlt.Name='open loop plant';
ss_plt.Name='open loop plant';
switch mMdl
case 0
ssMdl=mot.ssPlt;%real plant (model of real plant)
ss_mdl=mot.ss_plt;
case 1
ssMdl=mot.ssMdl_c1;%current, mechanic, no resonance
ss_mdl=mot.ss_c1;
case 2
ssMdl=mot.ssMdl_1;%no current current, mechanic, no resonance
ss_mdl=mot.ss_d1;
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
ssMdl.Name='open loop model'; %model for observer
ss_mdl.Name='open loop model'; %model for observer
[Ap,Bp,Cp,Dp]=ssdata(ssPlt);
[Am,Bm,Cm,Dm]=ssdata(ssMdl);
[Ap,Bp,Cp,Dp]=ssdata(ss_plt);
[Am,Bm,Cm,Dm]=ssdata(ss_mdl);
if bitand(mShow,1)
figure();h=bodeplot(ssPlt,ssMdl);
figure();h=bodeplot(ss_plt,ss_mdl);
setoptions(h,'IOGrouping','all')
end
@@ -93,8 +91,8 @@ function [ssc]=StateSpaceControlDesign(mot)
% step answer on open loop:
t = 0:1E-4:.5;
u = ones(size(t));
[yp,t,x] = lsim(ssPlt,u,t,xp0);
[ym,t,x] = lsim(ssMdl,u,t,xm0);
[yp,t,x] = lsim(ss_plt,u,t,xp0);
[ym,t,x] = lsim(ss_mdl,u,t,xm0);
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
@@ -108,9 +106,9 @@ function [ssc]=StateSpaceControlDesign(mot)
%
%place poles for the controller feedback
if use_lqr %use the lqr controller
Q=eye(length(ssMdl.A));
Q=eye(length(ss_mdl.A));
R=1;
[K,P,E]=lqr(ssMdl,Q,R,0);
[K,P,E]=lqr(ss_mdl,Q,R,0);
else
if mot.id==1
%2500rad/s = 397Hz -> locate poles here
@@ -155,12 +153,11 @@ function [ssc]=StateSpaceControlDesign(mot)
end %if lqr
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
V=V(3); % only the position scaling needed
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)
% step answer on closed loop with space state controller:
t = 0:1E-4:.5;
@@ -182,7 +179,7 @@ function [ssc]=StateSpaceControlDesign(mot)
Ct = [ Cm zeros(size(Cm)) ];
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)
% 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');
@@ -233,8 +230,8 @@ function [ssc]=StateSpaceControlDesign(mot)
end
%discrete plant
ssPltz = c2d(ssPlt,Ts);
[Apz,Bpz,Cpz,Dpz]=ssdata(ssPltz);
%ss_pltz = c2d(ss_plt,Ts);
%[Apz,Bpz,Cpz,Dpz]=ssdata(ss_pltz);
%discrete observer controller
ss_oz = c2d(ss_o,Ts);
@@ -261,7 +258,9 @@ function [ssc]=StateSpaceControlDesign(mot)
%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","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));
end
save(sprintf('/tmp/ssc%d.mat',mot.id),'-struct','ssc');
@@ -302,93 +301,3 @@ function pf=Prefilt(mot,mode)
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
%
% mode bits:
% 0 1 : add ss-models and do checks for motor 1 fy
% 1 2 : add ss-models and do checks for setup motor 2 fx
% 2 4 : identify_currstep
% 3 8 : identify_tf2
% The default value for mode is 3
% 0 1 : select motor 1 fy
% 1 2 : select motor 2 fx
% 2 4 : add ss-models and do obser/contr checks
% 3 8 : identify_currstep
% 4 16 : identify_tf (TODO!)
% The default value for mode is 7
%References:
@@ -67,6 +68,7 @@ function motCell=identifyFxFyStage(mode)
opt=tfestOptions;
opt.Display='off';
tfc = tfest(obj.currstep, 2, 0,opt);
s=splitlines(string(evalc('tfc')));disp(join(s(5:7),newline));
s=str2ndOrd(tfc);
t=(0:199)*50E-6;
[y,t]=step(tfc,t);
@@ -85,7 +87,7 @@ function motCell=identifyFxFyStage(mode)
print(f,sprintf('figures/currstep_%d',obj.id),'-depsc');
end
function tf2=identify_tf2(obj)
function tf2=identify_tf(obj)
opt=tfestOptions;
opt.Display='off';
opt.initializeMethod='iv';
@@ -351,19 +353,22 @@ function motCell=identifyFxFyStage(mode)
for motid= 1:2
mot=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/19_01_29/',motid);
mot.id=motid;
if bitand(mode,4)
%identification of second order transfer function out of the current step recorded data.
identify_currstep(mot{motid});
end
if bitand(mode,8)
%identification of second order transfer function out of the position recorded data.
identify_tf2(mot);
end
if motid==1 && bitand(mode,1)
mot=fyStage(mot);
end
if motid==2 && bitand(mode,2)
mot=fxStage(mot);
if bitand(mode,motid)
if bitand(mode,4)
if motid==1
mot=fyStage(mot);
else
mot=fxStage(mot);
end
end
if bitand(mode,8)
%identification of second order transfer function out of the current step recorded data.
identify_currstep(mot);
end
if bitand(mode,16)
%identification of second order transfer function out of the position recorded data.
identify_tf(mot);
end
end
motCell{motid}=mot;
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(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;
mot_num=mot.tf_mdl.Numerator;
mot_den=mot.tf_mdl.Denominator;
else
%!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')
%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;
mot_num=mot.tf_mdl.Numerator;
mot_den=mot.tf_mdl.Denominator;
end
mdlName='stage_closed_loop';
%open(mdlName)
%ServoDeltaTau_z(motid)
[A,B,C,D]=tf2ss(mot_num,mot_den);
ss_plt=mot.ss_plt;
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));
end
%mdlName='stage_closed_loop';
%open(mdlName)
%sim(mdlName)
end

BIN
matlab/stage_closed_loop.slx
View File

Binary file not shown.