diff --git a/matlab/Readme.md b/matlab/Readme.md index c4f23e2..de41318 100644 --- a/matlab/Readme.md +++ b/matlab/Readme.md @@ -3,7 +3,7 @@ Matlab Simulink start ``` [gfa-lc6-64 ~] [-bash INSTBASE=/prod]$ -cd /afs/psi.ch/user/z/zamofing_t/ESB-MX/ +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& @@ -31,6 +31,29 @@ close all; [pb]=simFxFyStage(mot2); [ssc]=StateSpaceControlDesign(mot2); +Very good trajectory-filter motor2: +f=268;w0=f*2*pi; +num1=[1 20 w0^2]; +den1=[1 500 w0^2]; +bode(numV,denV) + + +f=140;w0=f*2*pi; +num2=[1 300 w0^2]; +den2=[1 100 w0^2]; +bode(numV,denV) + + +f=62;w0=f*2*pi; +num3=[1 40 w0^2]; +den3=[1 20 w0^2]; +bode(numV,denV) + + +numV=conv(num1,num2) +denV=conv(den1,den2) +%numV=conv(conv(num1,num2),num3) +%denV=conv(conv(den1,den2),den3) ``` diff --git a/matlab/StateSpaceControlDesign.m b/matlab/StateSpaceControlDesign.m index 26575cf..28b73cd 100644 --- a/matlab/StateSpaceControlDesign.m +++ b/matlab/StateSpaceControlDesign.m @@ -16,7 +16,7 @@ function [ssc]=StateSpaceControlDesign(mot) % % https://www.youtube.com/watch?v=Lax3etc837U - ssPlt=mot.ssMdl;%ssPlt; %real plant (model of real plant) + ssPlt=mot.ssPlt;%ssPlt; %real plant (model of real plant) ssPlt.Name='open loop plant'; ssMdl=mot.ssMdl;%ssMdl; %simplified model (observable,controlable) for observer ssMdl.Name='open loop model'; @@ -63,7 +63,7 @@ function [ssc]=StateSpaceControlDesign(mot) p1=-6300+280i; p2=-6200+150i; P=[p1 p1' p2 p2']; - P=[-4000 -4100 -1500+10j -1500-10j]; + P=[-4100 -4000 -1500+10j -1500-10j]; else p1=-3300+2800i; p2=-2700+500i; @@ -80,6 +80,7 @@ function [ssc]=StateSpaceControlDesign(mot) p1=-6300+2800i; p2=-6200+1500i; P=[p1 p1' p2 p2']; + P=[-2500 -2800 -1500+10j -1500-10j]; else p1=-3300+2800i; p2=-1900+130i; @@ -171,14 +172,16 @@ function [ssc]=StateSpaceControlDesign(mot) ss_oz = c2d(ss_o,Ts); [Aoz,Boz,Coz,Doz]=ssdata(ss_oz); mdlName='observer'; - open(mdlName); - - + %open(mdlName); + %discrete prefilter + prefiltz=c2d(mot.prefilt,Ts); + numVz=prefiltz.Numerator{1}; + denVz=prefiltz.Denominator{1}; %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"] + 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"] ssc=setfield(ssc,k,eval(k)); end save(sprintf('/tmp/ssc%d.mat',mot.id),'-struct','ssc'); @@ -187,6 +190,26 @@ 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 ]; diff --git a/matlab/observer.slx b/matlab/observer.slx index 974f9b3..a28f64e 100644 Binary files a/matlab/observer.slx and b/matlab/observer.slx differ diff --git a/matlab/simFxFyStage.m b/matlab/simFxFyStage.m index 72e0652..f268b5c 100644 --- a/matlab/simFxFyStage.m +++ b/matlab/simFxFyStage.m @@ -18,13 +18,14 @@ function [pb]=simFxFyStage(mot) 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=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) + %open(mdlName) %ServoDeltaTau_z(motid) [A,B,C,D]=tf2ss(mot_num,mot_den); @@ -32,4 +33,5 @@ function [pb]=simFxFyStage(mot) 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 end diff --git a/matlab/stage_closed_loop.slx b/matlab/stage_closed_loop.slx index 6afde47..240023d 100644 Binary files a/matlab/stage_closed_loop.slx and b/matlab/stage_closed_loop.slx differ diff --git a/matlab/testObserver.m b/matlab/testObserver.m index ce963dd..c479342 100644 --- a/matlab/testObserver.m +++ b/matlab/testObserver.m @@ -104,7 +104,7 @@ function [ssc]=testObserver(mot) p1=-63+2.80i; p2=-62+1.50i; P=[p1 p1' p2 p2']; - P=[-4000 -4100 -500+10j -500-10j]; + P=[-4000+100j -4000-100j -500+10j -500-10j]; K = place(A,B,P); Ts=1/5000 t = 0:Ts:2; diff --git a/matlab/testObserverSim.slx b/matlab/testObserverSim.slx index 72b479a..d719af7 100644 Binary files a/matlab/testObserverSim.slx and b/matlab/testObserverSim.slx differ