diff --git a/matlab/Readme.md b/matlab/Readme.md index 7ff8283..0657688 100644 --- a/matlab/Readme.md +++ b/matlab/Readme.md @@ -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 ========= diff --git a/matlab/SCRATCH.m b/matlab/SCRATCH.m index 6f11860..4d2ff3c 100644 --- a/matlab/SCRATCH.m +++ b/matlab/SCRATCH.m @@ -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) diff --git a/matlab/StateSpaceControlDesign.m b/matlab/StateSpaceControlDesign.m index a4cc4ab..a627b6a 100644 --- a/matlab/StateSpaceControlDesign.m +++ b/matlab/StateSpaceControlDesign.m @@ -1,25 +1,31 @@ -%http://ctms.engin.umich.edu/CTMS/index.php?example=Introduction§ion=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§ion=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 diff --git a/matlab/identifyFxFyStage.m b/matlab/identifyFxFyStage.m index bfa22ce..c594ff9 100644 --- a/matlab/identifyFxFyStage.m +++ b/matlab/identifyFxFyStage.m @@ -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 diff --git a/matlab/libESB_MX.slx b/matlab/libESB_MX.slx index 3ea9eb4..ceb311f 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 922620d..de7a0a1 100644 Binary files a/matlab/observer.slx and b/matlab/observer.slx differ diff --git a/matlab/simFxFyStage.m b/matlab/simFxFyStage.m index 85e6f08..9449d75 100644 --- a/matlab/simFxFyStage.m +++ b/matlab/simFxFyStage.m @@ -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 diff --git a/matlab/stage_closed_loop.slx b/matlab/stage_closed_loop.slx index e594048..c0b1669 100644 Binary files a/matlab/stage_closed_loop.slx and b/matlab/stage_closed_loop.slx differ