From fa00a1ef2524f69901960b1e640f1ad158461227 Mon Sep 17 00:00:00 2001 From: Thierry Zamofing Date: Thu, 4 Oct 2018 09:56:25 +0200 Subject: [PATCH] matlab identification code --- matlab/Readme.md | 98 +++++++++++++++++++ matlab/identifyFxFyStage.m | 189 +++++++++++++++++++++++++++++++++++++ python/MXTuning.py | 8 +- 3 files changed, 294 insertions(+), 1 deletion(-) create mode 100644 matlab/Readme.md create mode 100644 matlab/identifyFxFyStage.m diff --git a/matlab/Readme.md b/matlab/Readme.md new file mode 100644 index 0000000..7ff8283 --- /dev/null +++ b/matlab/Readme.md @@ -0,0 +1,98 @@ +Matlab Simulink start +--------------------- +``` +[gfa-lc6-64 ~] +[-bash INSTBASE=/prod]$ +cd /afs/psi.ch/user/z/zamofing_t/ESB-MX/ +module load matlab/2018a +matlab& +or start directly: /afs/psi.ch/sys/psi.x86_64_slp6/Programming/matlab/2018a/bin/matlab& +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) + + + +``` + + + + + + +OLD STUFF +========= + +Matlab Simulink up to 11.7.2018 +------------------------------- +``` +Open and run ESB_stage.m (takes very long to open simulink...) + +// default frq: 20 kHz Phase, 5 kHz Servo, 6.25MHz AdcAmp + +Values for current loop: +PwmSf-> User MAnual page 245 +PwmSf=15134.8909 # =.95*16384. PMAC3-style DSPGATE3 ASIC is being used for the output, the counter moves between +/- 16384. PwmSf is typically set to 95% of 16384 + +scale = 1 for 2 phase motors + = cos(30)=0.866 for 3 phase motors + + Amplifier specs (Power Brick LV User Manual.pdf p.19) + 5A_rms continous current + 15A_rms peak current + 14 bit ADC resolution + 2us PWM deadBand + 33.85A Maximum ADC Current (corresponds to a DAC Value 32737 ==2^15) + +Tested values from IDE: + 3 Phase, 1A_rms -> MaxDac=1184.895 = np.sqrt(2)*np.sqrt(3)/2*32767/33.850 + 3 Phase, 1A_peak -> MaxDac=837.8478 = np.sqrt(3)/2*32767/33.850 + + A_peak=A_rms*sqrt(2) + np.sqrt(3)/2 because of 3 phase (the coil has not always current) + A_peak is used. (A_rms is more dangerous to burn the motor) + if not kw.has_key('IdCmd'): + kw['IdCmd'] = dirCur*scale + kw['MaxDac'] = peakCur*scale; + +gpasciiCommander --host MOTTEST-CPPM-CRM0573 -i + +$$$*** +!common() +!encoder_inc(enc=1,tbl=9,mot=9) +!encoder_sim(enc=1) +!motor(mot=1,dirCur=200,JogSpeed=1024,invDir=False,InPosBand=1) +Defaut: +IiGain=.2 +IpfGain=6 +IpbGain=6 + +Current loop: +-> Monitor idMead, idVolts +idVolts: -32768..32,767 Value that goes to the PWM + +out{constant} specifying the output magnitude as a percentage of + the maximum output parameter for the motor: Motor[x].MaxDac. + +Motor[1].MaxDac=580.80603 +#1out1 +-> 1 % of 580.8 -> Motor[1].ServoOut=5.808 +-> idVolts= 1500 +-> idMeas = 155 + +https://ch.mathworks.com/de/products/simcontrol.html +https://ch.mathworks.com/help/slcontrol/ug/design-compensator-in-simulink-using-automated-pid-tuning.html#responsive_offcanvas +https://ch.mathworks.com/help/slcontrol/ug/analyze-designs-using-response-plots.html +https://ch.mathworks.com/help/control/ug/bode-diagram-design.html +https://ch.mathworks.com/help/control/ug/edit-compensator-dynamics.html +https://ch.mathworks.com/help/control/ug/root-locus-design.html + +controlSystemDesigner(1,sys); + +controlSystemDesigner('bode',sys); + +``` diff --git a/matlab/identifyFxFyStage.m b/matlab/identifyFxFyStage.m new file mode 100644 index 0000000..e920648 --- /dev/null +++ b/matlab/identifyFxFyStage.m @@ -0,0 +1,189 @@ + +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); + + function obj=loadData(path,motid) + obj=struct(); + f=load(strcat(path,sprintf('curr_step%d.mat',motid))); + 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.frq=f.frq*2*pi; %convert from Hz to rad/s + obj.mag=10.^(f.db_mag/20); %mag not in dB + obj.phase=f.deg_phase*pi/180; %phase in rad + response = obj.mag.*exp(1j*obj.phase); + obj.meas= idfrd(response,obj.frq,0); + end + + function tfc=currstep(obj) + opt=tfestOptions; + opt.Display='off'; + tfc = tfest(obj.currstep, 2, 0,opt); + den=tfc.Denominator; + num=tfc.Numerator; + k=num(1); + w0=sqrt(num(1)); + damp=den(2)/2/w0; + s=sprintf('k:%g w0:%g damp:%g\n',k,w0,damp); + %disp(s); + %h = stepplot(tf1); + %l=obj.currstep.OutputData + t=(0:199)*50E-6; + [y,t]=step(tfc,t); + f=figure(); + subplot(1,2,1); + plot(t,y*1000,'r',t,obj.currstep.OutputData(11:210),'b'); + title(s); + subplot(1,2,2); + h=bodeplot(tfc,'r'); + setoptions(h,'FreqUnits','Hz','Grid','on'); + end + + function mot=fyStage() + mot=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/18_10_02/',1); + mot.tfc=currstep(mot); + + opt=tfestOptions; + opt.Display='off'; + opt.initializeMethod='iv'; + opt.WeightingFilter=[1,5;20,670]*(2*pi); % Hz->rad/s conversion + + figure(); + mot.tf4_2 = tfest(mot.meas, 4, 2, opt); + mot.tf6_4 = tfest(mot.meas, 6, 4, opt); + h=bodeplot(mot.meas,'r',mot.tf4_2,'b',mot.tf6_4,'g'); + setoptions(h,'FreqUnits','Hz','Grid','on'); + + end + + function mot=fxStage() + mot=loadData('/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/18_10_02/',2); + currstep(mot); + + opt=tfestOptions; + opt.Display='off'; + opt.initializeMethod='iv'; + opt.WeightingFilter=[1,5;20,670]*(2*pi); % Hz->rad/s conversion + + figure(); + mot.tf4_2 = tfest(mot.meas, 4, 2, opt); + mot.tf6_4 = tfest(mot.meas, 6, 4, opt); + mot.tf13_9 = tfest(mot.meas, 13, 9, opt); + num=[5.42637491e-24 1.45926254e-21 5.20861422e-17 9.92527094e-15 1.16707977e-10 1.31240975e-08 7.03191689e-05 3.08626613e-03 7.32824533e+00]; + den=[2.01035570e-35,2.33560078e-31,9.14767135e-28,2.52369732e-24,7.42150891e-21,6.89695386e-18,1.65017156e-14,5.77522779e-12,1.08386286e-08,1.13336206e-06,1.27552247e-03,2.19776479e-02,1.00000000e+00]; + mot.tf_py = idtf(num,den); + h=bodeplot(mot.meas,'r',mot.tf4_2,'b',mot.tf6_4,'g',mot.tf13_9,'m',mot.tf_py,'b'); + setoptions(h,'FreqUnits','Hz','Grid','on'); + %controlSystemDesigner('bode',1,mot.tf_py); % <<<<<<<<< This opens a transferfiûnction that can be edited + + end + close all + mot1=fyStage(); + mot2=fxStage(); + +end + + +function f=SCRATCH() + + %identification toolbox + systemIdentification + + %opt=tfestOptions('Display','off'); + %opt=tfestOptions('Display','on','initializeMethod','svf'); + %opt=tfestOptions('Display','on','initializeMethod','iv','WeightingFilter',[]); + %opt=tfestOptions('Display','on','initializeMethod','iv','WeightingFilter',[1,5;20,570]); + %tf1 = tfest(mot1frq, 6, 4, opt); + % Model refinement + % Options = tf1.Report.OptionsUsed; + % Options.WeightingFilter = 'prediction'; + % tf1_1 = pem(mot1frq, tf1, Options) + + + + + + bodeplot(mot1frq,tf1) + mag,phase=bode(tf1,frq) + figure(1) + subplot(211) + + + bodeplot(tf1) + + Opt = n4sidOptions('N4Horizon',[15 15 15]); + n4s3 = n4sid(mot1frq, 3, Opt) + + + + + + %tf([1 2],[1 0 10]) + %specifies the transfer function (s+2)/(s^2+10) while + sys=tf([1],[1,0,0]) + bode(sys) + step(sys) + + sys=tf([1],[1,-1,2]) %instable + + sys=tf([1],[1,1,2]) %stable + + + %0dB at 12 Hz=12*2*pi rad/s =75.4=k^2 -> k=8.6833 + + sys=tf([10],[1,0,0]) + %1/s^2 -> 0dB at 1Hz -40dB/decade + %10=+20dB + + + sys=tf([1],[1,0,2]) %not damped constant sine after step + + + sys=zpk([],[1,0,0],100) %stable + + sys=zpk([],[-10,-10],100) + + + %parker stage 1 + %!encoder_sim(enc=1,tbl=9,mot=9,posSf=13000./2048) + %!encoder_inc(enc=1,tbl=1,mot=1,posSf=13000./650000) + %!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') + Ts=2E-4 % discrete sample time (servo period) + Kp=25,Kvfb=400,Ki=0.02,Kvff=350,Kaff=5000,MaxInt=1000 + Kp=25,Kvfb=0,Ki=0,Kvff=0,Kaff=0,MaxInt=0 + + + num=7.32 + den=[5.995e-04 4.897e-02 1.] + open('stage_closed_loop.slx') + %sim('stage_closed_loop.slx') + + sys=tf(num,den) + bode(sys) + G = tf(1.5,[1 14 40.02]); + controlSystemDesigner('bode',sys); + controlSystemDesigner + linearSystemAnalyzer + load ltiexamples + linearSystemAnalyzer(sys_dc) + + controlSystemDesigner('bode',sys); + controlSystemDesigner(1,sys); % <<<<<<<<< This opens a transferfiûnction that can be edited + + num=[8.32795069e-11, 1.04317228e-08, 6.68431323e-05, 3.31861324e-03, 7.32824533e+00]; + den=[5.26156641e-18, 1.12897840e-14, 7.67853031e-12, 1.03201301e-08, 2.05154780e-06, 1.34279894e-03, 7.19229912e-02, 1.00000000e+00]; + mot2=tf(num,den); + controlSystemDesigner('bode',mot2); + + + [m1,m2]=identifyFxFyStage(); + controlSystemDesigner(1,m2.tf_py); % <<<<<<<<< This opens a transferfiûnction that can be edited + +end \ No newline at end of file diff --git a/python/MXTuning.py b/python/MXTuning.py index f13dcc0..6537354 100755 --- a/python/MXTuning.py +++ b/python/MXTuning.py @@ -27,7 +27,6 @@ import matplotlib as mpl #mpl.use('GTKAgg') import matplotlib.pyplot as plt from scipy import signal -from utilities import * sys.path.insert(0,os.path.expanduser('~/Documents/prj/SwissFEL/PBTools/')) #import pbtools.misc.pp_comm as pp_comm -> pp_comm.PPComm @@ -231,6 +230,13 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n ' fig=plt.figure(mot) tune.bode_current(motor=mot, magMove=1000, magPhase=500, dwell=10, file=fn,fig=fig) plt.show(block=False) + f=np.load(fn) + fn=fn[:-3]+'mat' + import scipy.io + scipy.io.savemat(fn, mdict=f) + print('save to matlab file:'+fn) + + elif mode==2: # full recording open loop for mot in (1, 2): fsin=os.path.join(base, 'sine_ol_mot%d.npz' % (mot))