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

matlab wip

Browse Source
This commit is contained in:
zamofing_t
2018-10-11 17:19:48 +02:00
parent f9ddf04fdc
commit cb9310ee5c
8 changed files with 228 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
matlab/Readme.md
View File

@@ -13,9 +13,25 @@ this works also on my linux computers
Files overview
--------------
```
identifyFxFyStage.m: a transferfunction is optimized with the acquires bode data from: MXTuning.py --mode 4 (which generates: full_bode_mot[1|2].mat)
identifyFxFyStage.m: read python data and build motor objects. plot bode
simFxFyStage.m: build a pb structure which contains current Okt 2018) Powerbrick controller settings
StateSpaceControlDesign.m: design a discrete observer for Fx,Fy stages
```
important calls
---------------
```
clear;
clear global;
close all;
[mot1,mot2]=identifyFxFyStage();
[pb]=simFxFyStage(mot1,1);
[ssc]=StateSpaceControlDesign(mot1,1);
[pb]=simFxFyStage(mot2,2);
[ssc]=StateSpaceControlDesign(mot2,2);
```
@@ -23,7 +39,6 @@ identifyFxFyStage.m: a transferfunction is optimized with the acquires bode da
OLD STUFF
=========

46
matlab/SCRATCH.m
View File

@@ -1,18 +1,54 @@
clear;
clear global;
[mot1,mot2]=identifyFxFyStage();
[Kp,Kvfb,Ki,Kvff,Kaff,MaxInt,mot_num,mot_den,Ts,MaxDac,MaxPosErr,A,B,C,D]=simFxFyStage(mot1,1);
raw=desPos_actPos.Data
raw(:,2)=raw(:,1)*100
ft=fft(raw);
plot(log(abs(ft)));
plot(log(abs(ft(:,1)./ft(:,2))));
[Nbar,A,B,C,Ao,Bo,Co,Do,L,K]=StateSpaceControlDesign(mot1,1);
i=int(minFrq*tSec); j=int(maxFrq*tSec); #print(w[i],w[j])
x=2+3j
rho=abs(x)
theta=angle(x)
%-------------------
x=rho*exp(j*theta)
figure();
h=pzplot(ssc.ss_cl(3),ssc.ss_o(3),ssc.ss_od(3));
setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
%figure();bode(ssc.ss_o(3));
%figure();tf=tf(ssc.ss_o) pzplot(tf(3));
h=bodeplot(ssc.ss_cl(3),ssc.ss_o(3),ssc.ss_od(3));
setoptions(h,'FreqUnits','Hz','Grid','on');
h=pzplot(ssc.ss_cl(3),ssc.ss_o(3),ssc.ss_od(3));
setoptions(h,'FreqUnits','Hz','Grid','on');
h=bodeplot(ssc.ss_cl(3),ssc.ss_o(3),ssc.ss_od(3));
setoptions(h,'FreqUnits','Hz','Grid','on');
getoptions(h)
continous to discrete
web(fullfile(docroot, 'control/ref/c2d.html'))
h = tf(10,[1 3 10],'IODelay',0.25);
hd = c2d(h,0.1)

152
matlab/StateSpaceControlDesign.m
View File

@@ -1,25 +1,31 @@
%http://ctms.engin.umich.edu/CTMS/index.php?example=Introduction&section=ControlStateSpace
%zustandsregler:
% web(fullfile(docroot, 'simulink/examples.html'))
% web(fullfile(docroot, 'simulink/examples/inverted-pendulum-with-animation.html'))
% web(fullfile(docroot, 'simulink/examples/double-spring-mass-system.html'))
function [Nbar,A,B,C,D,Ao,Bo,Co,Do,L,K]=StateSpaceControlDesign(mot,motid)
sys=mot.ss;
sys=ss(sys.A,sys.B,sys.C(3,:),0); % $$$ only output position
function [ssc]=StateSpaceControlDesign(mot,motid)
% !!! first it need to run: [mot1,mot2]=identifyFxFyStage() tobuild a motor object !!!
%
% builds a state space controller designed for the plant.
% shows step answers of open and closed loop, also for the observer controller and the final discrete observer
%
% finally it opens a simulink observer file for testing
%sys=ss(tf(mot1.mdl.num1,mot1.mdl.den1));
%A=sys.A;
%B=sys.B;
%C=sys.C;
%[A,B,C,D]=tf2ss(mot1.mdl.num1,mot1.mdl.den1)
%tf2ss(mot1.mdl.num1,mot1.mdl.den1)
figure();h=bodeplot(sys);
%References:
%http://ctms.engin.umich.edu/CTMS/index.php?example=Introduction&section=ControlStateSpace
%space state controller:
% web(fullfile(docroot, 'simulink/examples.html'))
% web(fullfile(docroot, 'simulink/examples/inverted-pendulum-with-animation.html'))
% web(fullfile(docroot, 'simulink/examples/double-spring-mass-system.html'))
%
% https://www.youtube.com/watch?v=Lax3etc837U
ss_ol=mot.ss;
ss_ol.Name='open loop';
%sys=ss(sys.A,sys.B,sys.C(3,:),0); % this would reduce the outputs to position only
figure();h=bodeplot(ss_ol);
setoptions(h,'IOGrouping','all')
A=sys.A;
B=sys.B;
C=sys.C;
D=sys.D;
A=ss_ol.A;
B=ss_ol.B;
C=ss_ol.C;
D=ss_ol.D;
P=ctrb(A,B);
if rank(A)==rank(P)
@@ -36,38 +42,62 @@ function [Nbar,A,B,C,D,Ao,Bo,Co,Do,L,K]=StateSpaceControlDesign(mot,motid)
end
% step answer on open loop:
t = 0:1E-4:.5;
u = ones(size(t)); %1000um
x0 = zeros(1,length(sys.A));
[y,t,x] = lsim(sys,u,t,x0);
figure();plot(t,y)
u = ones(size(t));
x0 = zeros(1,length(ss_ol.A));
[y,t,x] = lsim(ss_ol,u,t,x0);
figure();plot(t,y);title('step on open loop');
poles = eig(A);
w0=abs(poles);
ang=angle(-poles);
%-------------------
%p=w0.*exp(j.*ang)
% *** space state controller ***
%
%place poles for the controller feedback
if motid==1
%2500rad/s = 397Hz -> locate poles here
p1=-3300+2800i;
p2=-1500+500i;
p3=-1200+10i;
p2=-2700+500i;
p3=-2500+10i;
%p1=-3300+2800i;
%p2=-1500+500i;
%p3=-1200+10i;
P=[p1 p1' p2 p2' p3 p3'];
else
%2500rad/s = 397Hz -> locate poles here
p1=-3300+2800i;
p2=-1900+130i;
p3=-2900+80i;
p4=-2300+450i;
p5=-2000+20i;
p6=-1500+10i;
%p1=-3300+2800i;
%p2=-1500+500i;
%p3=-1200+10i;
P=[p1 p1' p2 p2' p3 p3'];% p4 p4' p5 p5' p6 p6'];
end
K = place(A,B,P);
%K = acker(A,B,P);
%K = acker(A,B,[p1 p1' p2 p2' p3 p3']);
%K = place(A,B,[p1 p1']);
%Nbar = rscale(sys,K);
%Nbar=1;
Nbar=-1./(C*(A-B*K)^-1*B); %from my notes)
V=-1./(C*(A-B*K)^-1*B); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
%Nbar(2)=1; %the voltage stuff is crap for now
if length(Nbar)>1
Nbar=Nbar(3); % only the position scaling needed
if length(V)>1
V=V(3); % only the position scaling needed
end
sys_cl = ss(A-B*K,B,C,0);
[y,t,x]=lsim(sys_cl,Nbar*u,t,x0);
figure();plot(t,y)
% step answer on closed loop with space state controller:
t = 0:1E-4:.5;
ss_cl = ss(A-B*K,B*V,C,0,'Name','space state controller','InputName',mot.ss.InputName,'OutputName',mot.ss.OutputName);
[y,t,x]=lsim(ss_cl,V*u,t,x0);
figure();plot(t,y);title('step on closed loop');
% *** observer controller ***
%
%observer poles-> 5 times farther left than system poles
if motid==1
op1=(p1*5);
@@ -75,26 +105,66 @@ function [Nbar,A,B,C,D,Ao,Bo,Co,Do,L,K]=StateSpaceControlDesign(mot,motid)
op3=(p3*5);
OP=[op1 op1' op2 op2' op3 op3'];
else
op1=(p1*2);
op2=(p2*2);
op3=(p3*2);
op4=(p4*2);
op5=(p5*2);
op6=(p6*2);
OP=[op1 op1' op2 op2' op3 op3'];% op4 op4' op5 op5' op6 op6'];
end
L=place(A',C',OP)';
%L=acker(A',C',OP)';
At = [ A-B*K B*K
zeros(size(A)) A-L*C ];
Bt = [ B*Nbar
Bt = [ B*V
zeros(size(B)) ];
Ct = [ C zeros(size(C)) ];
sys = ss(At,Bt,Ct,0);
lsim(sys,ones(size(t)),t,[x0 x0]);
%https://www.youtube.com/watch?v=Lax3etc837U
% step answer on closed loop with observer controller:
ss_o = ss(At,Bt,Ct,0,'Name','observer controller','InputName',{'desPos'},'OutputName',mot.ss.OutputName);
figure();lsim(ss_o,ones(size(t)),t,[x0 x0]);title('step on closed loop with observer');
% *** disctrete observer controller ***
%
Ts=1/5000; % 5kHz
ss_od = c2d(ss_o,Ts);
ss_od .Name='discrete obsvr ctrl';
% step answer on closed loop with disctrete observer controller:
t = 0:Ts:.05;
figure();lsim(ss_od ,ones(size(t)),t,[x0 x0]);title('step on closed loop with observer discrete');
%plot all bode diagrams of desPos->actPos
figure();
h=bodeplot(ss_cl(3),ss_o(3),ss_od(3));
setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
figure();
h=pzplot(ss_cl(3),ss_o(3),ss_od(3));
setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
%calculate matrices for the simulink system
Ao=A-L*C;
Bo=[B L];
Co=K;
Do=zeros(size(Co,1),size(Bo,2));
mdlName='observer';
open(mdlName)
open(mdlName);
%state space controller
ssc=struct();
for k=["Ts","A","B","C","D","Ao","Bo","Co","Do","V","K","L","ss_cl","ss_o","ss_od"]
ssc=setfield(ssc,k,eval(k));
end
end
%code snipplets from an example on youtube (see reference at top)
function SCRATCH()
A = [ 0 1 0
980 0 -2.8

80
matlab/identifyFxFyStage.m
View File

@@ -1,10 +1,36 @@
function [mot1,mot2]=identifyFxFyStage()
%sample idfrd
%f = logspace(-1,1,100);
%[mag,phase] = bode(idtf([1 .2],[1 2 1 1]),f);
%response = mag.*exp(1j*phase*pi/180);
%m = idfrd(response,f,0.08);
%loads recorded data of the current step and bode diagrams of the stages then plots the bode diagrams and identifies
%the current step transfer function
%
%finally it builds a ss-Model of the stage with:
%
% u +-----------+ y
%iqCmd------->|1 1|-------> iqMeas
% | 2|-------> iqVolts
% | 3|-------> actPos
% +-----------+
%
% the returned motor objects mot1 and mot2 contains:
%
% w,mag,phase : (gathered data with Python)
% meas : a MATLAB idfrd model with data w,mag,phase
% mdl : a structure with the python numerators and denominators for the transfer functions
% tfc,tf_mdl : various transfer functions
% ss : the final continous state space model of the plant
%
% The used data files (generated from Python) are:
% (located for now in: /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/18_10_02/ )
% - curr_step[1|2].mat
% - full_bode_mot[1|2].mat
% - model[1|2].mat
%References:
%create ss from tf MIMO:
%https://ch.mathworks.com/matlabcentral/answers/37152-how-to-convert-tf2ss-for-mimo-system
%http://ch.mathworks.com/help/control/ug/conversion-between-model-types.html#f3-1039600
%https://ch.mathworks.com/help/control/ref/append.html
function obj=loadData(path,motid)
obj=struct();
@@ -12,7 +38,7 @@ function [mot1,mot2]=identifyFxFyStage()
obj.currstep=f;
%prepend sone zeros to stable system identification
obj.currstep=iddata([zeros(10,1); obj.currstep.data(:,2)],[zeros(10,1); obj.currstep.data(:,3)],50E-6);
f=load(strcat(path,sprintf('full_bode_mot%d.mat',motid)));
obj.w=f.frq*2*pi; %convert from Hz to rad/s
if motid==2
@@ -25,7 +51,7 @@ function [mot1,mot2]=identifyFxFyStage()
fMdl=load(strcat(path,sprintf('model%d.mat',motid)));
obj.mdl=fMdl;
end
function tfc=currstep(obj)
@@ -33,9 +59,6 @@ function [mot1,mot2]=identifyFxFyStage()
opt.Display='off';
tfc = tfest(obj.currstep, 2, 0,opt);
s=str2ndOrd(tfc);
%disp(s);
%h = stepplot(tf1);
%l=obj.currstep.OutputData
t=(0:199)*50E-6;
[y,t]=step(tfc,t);
f=figure();
@@ -57,7 +80,7 @@ function [mot1,mot2]=identifyFxFyStage()
k=num(1)/den(3);
w0=sqrt(den(3));
damp=den(2)/2/w0;
s=sprintf('k:%g w0:%g damp:%g\n',k,w0,damp);
s=sprintf('k:%g w0:%g damp:%g',k,w0,damp);
end
@@ -70,7 +93,7 @@ function [mot1,mot2]=identifyFxFyStage()
opt.initializeMethod='iv';
opt.WeightingFilter=[1,5;30,670]*(2*pi); % Hz->rad/s conversion
figure();
figure();
mot.tf2_0 = tfest(mot.meas, 2, 0, opt);disp(str2ndOrd(mot.tf2_0));
mot.tf_mdl=idtf(mot.mdl.num,mot.mdl.den);
%ss([g1 mot.tf_mdl],'minimal') this doesn't work as expected
@@ -81,9 +104,9 @@ function [mot1,mot2]=identifyFxFyStage()
den1=myNorm(mot.mdl.den1);
num2=myNorm(mot.mdl.num2);
den2=myNorm(mot.mdl.den2);
g1=tf(numc,denc); % iqCmd->iqMeas
g1=tf(numc,denc); % iqCmd->iqMeas
s1=ss(g1);
s1.C=[s1.C; 1/s1.B(1) 0]; % add output iqVolts: iqVolts= i_meas*R+i_meas'*L
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)
%tf(s1) % display all transfer functions
num=conv(num1,num2);%num=1;
den=conv(den1,den2);%den=[1 0 0];
@@ -92,26 +115,20 @@ function [mot1,mot2]=identifyFxFyStage()
s3=append(s1,s2);
s3.A(3,2)=s3.C(1,2)*s3.B(3,2);
mot.ss=ss(s3.A,s3.B(:,1),s3.C,0); % single input, remove input iqMeas
mot.ss.InputName{1}='iqCmd';
mot.ss.OutputName{1}='iqMeas';
mot.ss.OutputName{2}='iqVolts';
mot.ss.OutputName{3}='actPos';
% u +-----------+ y
%iqCmd------->|1 1|-------> iqMeas
% | 2|-------> iqVolts
% | 3|-------> actPos
% +-----------+
mot.ss.OutputName{3}='actPos';
%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);
tmp=tf(mot.ss);h=bodeplot(mot.meas,'r',tmp(3,1),'g',mot.w);
setoptions(h,'FreqUnits','Hz','Grid','on');
end
function y=myNorm(y)
%normalizes num and den by factor 1000
%y.*10.^(3*(length(y):-1:1))
%y.*10.^(3*(length(y):-1:1))
end
function mot=fxStage()
@@ -128,10 +145,6 @@ function [mot1,mot2]=identifyFxFyStage()
mot.tf13_9 = tfest(mot.meas, 13, 9, opt);
mot.tf_mdl=idtf(mot.mdl.num,mot.mdl.den);
%create ss from tf MIMO:
%https://ch.mathworks.com/matlabcentral/answers/37152-how-to-convert-tf2ss-for-mimo-system
%http://ch.mathworks.com/help/control/ug/conversion-between-model-types.html#f3-1039600
%https://ch.mathworks.com/help/control/ref/append.html
numc=myNorm(mot.mdl.numc);
denc=myNorm(mot.mdl.denc);
num1=myNorm(mot.mdl.num1);
@@ -146,12 +159,15 @@ function [mot1,mot2]=identifyFxFyStage()
den5=myNorm(mot.mdl.den5);
num=myNorm(mot.mdl.num);
den=myNorm(mot.mdl.den);
g1=tf(numc,denc); % iqCmd->iqMeas
g1=tf(numc,denc); % iqCmd->iqMeas
s1=ss(g1);
s1.C=[s1.C; 1/s1.B(1) 0]; % add output iqVolts: iqVolts= i_meas*R+i_meas'*L
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)
%tf(s1) % display all transfer functions
num=conv(conv(conv(conv(num1,num2),num3),num4),num5);%num=1;
den=conv(conv(conv(conv(den1,den2),den3),den4),den5);%den=[1 0 0];
num=conv(num1,num2);%num=1;
den=conv(den1,den2);%den=[1 0 0];
g2=tf(num,den); %iqMeas->ActPos
s2=ss(g2);
s3=append(s1,s2);
@@ -162,11 +178,11 @@ function [mot1,mot2]=identifyFxFyStage()
s3.A(3,2)=s3.C(1,2)*s3.B(3,2);
mot.ss=ss(s3.A,s3.B(:,1),s3.C,0); % single input, remove input iqMeas
mot.ss.InputName{1}='iqCmd';
mot.ss.OutputName{1}='iqMeas';
mot.ss.OutputName{2}='iqVolts';
mot.ss.OutputName{3}='actPos' ;
mot.ss.OutputName{3}='actPos' ;
% u +-----------+ y
%iqCmd------->|1 1|-------> iqMeas
% | 2|-------> iqVolts

BIN
matlab/libESB_MX.slx
View File

Binary file not shown.

BIN
matlab/observer.slx
View File

Binary file not shown.

28
matlab/simFxFyStage.m
View File

@@ -1,5 +1,11 @@
function [Kp,Kvfb,Ki,Kvff,Kaff,MaxInt,mot_num,mot_den,Ts,MaxDac,MaxPosErr,A,B,C,D]=simFxFyStage(mot,motid)
%global Kp Kvfb Ki Kvff Kaff MaxInt mot_num mot_den Ts MaxDac MaxPosErr A B C D
function [pb]=simFxFyStage(mot,motid)
% !!! first it need to run: [mot1,mot2]=identifyFxFyStage() tobuild a motor object !!!
%
%loads the current (11.10.2018) controller settings of the
%Powerbrick and opens its simulink file
%the returned variable pb contains varous parameters:
% Kp,Kvfb,Ki,Kvff,Kaff,MaxInt,mot_num,mot_den,Ts,MaxDac,MaxPosErr,A,B,C,D
Ts=2E-4; % 0.2ms=5kHz
MaxDac=2011.968;
MaxPosErr=10000;
@@ -22,20 +28,8 @@ function [Kp,Kvfb,Ki,Kvff,Kaff,MaxInt,mot_num,mot_den,Ts,MaxDac,MaxPosErr,A,B,C,
%ServoDeltaTau_z(motid)
[A,B,C,D]=tf2ss(mot_num,mot_den);
%mdlWks=get_param(mdlName,'ModelWorkspace');
%whos global
%whos(mdlWks)
%for k=["Kp","Kvfb","Ki","Kvff","Kaff","MaxInt","mot_num","mot_den","Ts","MaxDac","MaxPosErr"]
% assignin(mdlWks,k,eval(k))
%end
%assignin(mdlWks,'Ts',1234)
%getVariable(mdlWks,'Ts')
% in global space call:
%global Kp Kvfb Ki Kvff Kaff MaxInt
%global mot_num mot_den Ts MaxDac MaxPosErr
%[mot1,mot2]=identifyFxFyStage();
%simFxFyStage(mot1,mot2);
%
pb=struct();
for k=["Kp","Kvfb","Ki","Kvff","Kaff","MaxInt","mot_num","mot_den","Ts","MaxDac","MaxPosErr","A","B","C","D"]
pb=setfield(pb,k,eval(k));
end

BIN
matlab/stage_closed_loop.slx
View File

Binary file not shown.