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

wip

Browse Source
This commit is contained in:
zamofing_t
2018-11-19 15:54:16 +01:00
parent 7e867b9316
commit 0a5ec9b004
22 changed files with 76507 additions and 236 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Makefile
View File

@@ -4,7 +4,7 @@ BUILDCLASSES = Linux
ARCH_FILTER = eldk42% SL6-x86_64 ARCH_FILTER = eldk42% SL6-x86_64
EXCLUDE_VERSIONS = 3.14.8 EXCLUDE_VERSIONS = 3.14.8
#SCRIPTS+=$(wildcard add_EXPMX*.cmd cfg/*.cfg cfg/*.py cfg/*.pbi python/*.py) #SCRIPTS+=$(wildcard add_EXPMX*.cmd cfg/*.cfg cfg/*.py cfg/*.pbi python/*.py)
SCRIPTS+=$(wildcard add_EXPMX*.cmd cfg/*.cfg cfg/*.pbi) SCRIPTS+=$(wildcard add_EXPMX*.cmd cfg/*.cfg cfg/*.py cfg/*.pbi)
#SOURCES+=src/DHVSaSub.cpp #SOURCES+=src/DHVSaSub.cpp
#DBDS+=src/DHVSaSub.dbd #DBDS+=src/DHVSaSub.dbd
USR_CXXFLAGS+= -fno-operator-names USR_CXXFLAGS+= -fno-operator-names

35
matlab/Readme.md
View File

@@ -26,35 +26,16 @@ clear;
clear global; clear global;
close all; close all;
[mot1,mot2]=identifyFxFyStage(); [mot1,mot2]=identifyFxFyStage();
[pb]=simFxFyStage(mot1); [pb]=simFxFyStage(mot1);sim('stage_closed_loop');
[ssc]=StateSpaceControlDesign(mot1); close all;[ssc]=StateSpaceControlDesign(mot1);sim('observer');
[pb]=simFxFyStage(mot2); [pb]=simFxFyStage(mot2);sim('stage_closed_loop');
[ssc]=StateSpaceControlDesign(mot2); close all;[ssc]=StateSpaceControlDesign(mot2);sim('observer');
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; mögliche tf:
num2=[1 300 w0^2]; iqCmd---->iqMeas
den2=[1 100 w0^2]; iqVolts-->iqMeas
bode(numV,denV) iqMeas--->actPos
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)
``` ```

290
matlab/StateSpaceControlDesign.m
View File

@@ -1,11 +1,10 @@
function [ssc]=StateSpaceControlDesign(mot) function [ssc]=StateSpaceControlDesign(mot)
% !!! first it need to run: [mot1,mot2]=identifyFxFyStage() tobuild a motor object !!! % !!! first it need to run: [mot1,mot2]=identifyFxFyStage() to build a motor object !!!
% %
% builds a state space controller designed for the plant. % 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 % 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 % the matchich simulink model is: 'observer'
%References: %References:
%http://ctms.engin.umich.edu/CTMS/index.php?example=Introduction&section=ControlStateSpace %http://ctms.engin.umich.edu/CTMS/index.php?example=Introduction&section=ControlStateSpace
@@ -16,31 +15,89 @@ function [ssc]=StateSpaceControlDesign(mot)
% %
% https://www.youtube.com/watch?v=Lax3etc837U % https://www.youtube.com/watch?v=Lax3etc837U
ssPlt=mot.ssPlt;%ssPlt; %real plant (model of real plant) %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
%mPrefilt:prefilter mode
%0 no filter
%1 inverse resonance filter
%2 manual setup filter
%mShow: mode(bits) to plot/simulate
% 0: 1: bode plots of open loop
% 1: 2: step answer on open loop
% 2: 4: step answer on closed loop with space state controller
% 3: 8: step answer on closed loop with observer controller
% 4:16: step answer on closed loop with disctrete observer controller
% 5:32: plot all closed loop bode and pole-zero diagrams of desPos->actPos
% 6:64:
%use_lqr: use lqr instead of pole placement
mPlt=0;
mMdl=1;
mPrefilt=2;
mShow=32+64;
use_lqr=0;
switch mPlt
case 0
ssPlt=mot.ssPlt;%real plant (model of real plant)
case 1
ssPlt=mot.ssMdl_c1;%current, mechanic, no resonance
case 2
ssPlt=mot.ssMdl_1;%no current current, mechanic, no resonance
case 3
ssPlt=mot.ssMdl_12;%no current current, mechanic, first resonance
end
ssPlt.Name='open loop plant'; ssPlt.Name='open loop plant';
ssMdl=mot.ssMdl;%ssMdl; %simplified model (observable,controlable) for observer
ssMdl.Name='open loop model'; switch mMdl
case 0
ssMdl=mot.ssPlt;%real plant (model of real plant)
case 1
ssMdl=mot.ssMdl_c1;%current, mechanic, no resonance
case 2
ssMdl=mot.ssMdl_1;%no current current, mechanic, no resonance
case 3
ssMdl=mot.ssMdl_12;%no current current, mechanic, first resonance
end
ssMdl.Name='open loop model'; %model for observer
[Ap,Bp,Cp,Dp]=ssdata(ssPlt); [Ap,Bp,Cp,Dp]=ssdata(ssPlt);
[Am,Bm,Cm,Dm]=ssdata(ssMdl); [Am,Bm,Cm,Dm]=ssdata(ssMdl);
if bitand(mShow,1)
%sys=ss(sys.A,sys.B,sys.C(3,:),0); % this would reduce the outputs to position only figure();h=bodeplot(ssPlt,ssMdl);
setoptions(h,'IOGrouping','all')
end
figure();h=bodeplot(ssPlt,ssMdl);
setoptions(h,'IOGrouping','all')
% step answer on open loop:
t = 0:1E-4:.5;
u = ones(size(t));
xp0 = zeros(1,length(Ap)); xp0 = zeros(1,length(Ap));
xm0 = zeros(1,length(Am)); xm0 = zeros(1,length(Am));
[yp,t,x] = lsim(ssPlt,u,t,xp0); if bitand(mShow,2)
[ym,t,x] = lsim(ssMdl,u,t,xm0); % step answer on open loop:
figure();plot(t,yp,t,ym,'--');title('step on open loop (plant and model)'); t = 0:1E-4:.5;
legend('plt.iqMeas','plt.iqVolts','plt.actPos','mdl.iqMeas','mdl.iqVolts','mdl.actPos') u = ones(size(t));
[yp,t,x] = lsim(ssPlt,u,t,xp0);
[ym,t,x] = lsim(ssMdl,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
poles = eig(Am); poles = eig(Am);
%w0=abs(poles); %w0=abs(poles);
%ang=angle(-poles); %ang=angle(-poles);
@@ -50,7 +107,6 @@ function [ssc]=StateSpaceControlDesign(mot)
% *** space state controller *** % *** space state controller ***
% %
%place poles for the controller feedback %place poles for the controller feedback
use_lqr=0;
if use_lqr %use the lqr controller if use_lqr %use the lqr controller
Q=eye(length(ssMdl.A)); Q=eye(length(ssMdl.A));
R=1; R=1;
@@ -59,42 +115,41 @@ function [ssc]=StateSpaceControlDesign(mot)
if mot.id==1 if mot.id==1
%2500rad/s = 397Hz -> locate poles here %2500rad/s = 397Hz -> locate poles here
%6300rad/s = 1027Hz -> locate poles here %6300rad/s = 1027Hz -> locate poles here
if length(poles)==4 switch mMdl
p1=-6300+280i; case 0
p2=-6200+150i; p1=-3300+2800i; p2=-2700+500i; p3=-2500+10i;
P=[p1 p1' p2 p2']; P=[p1 p1' p2 p2' p3 p3'];
P=[-4100 -4000 -1500+10j -1500-10j]; case 1
else %p1=-6300+280i; p2=-6200+150i;
p1=-3300+2800i; %P=[p1 p1' p2 p2'];
p2=-2700+500i; P=[-4100 -4000 -1500+10j -1500-10j];
p3=-2500+10i; case 2
%p1=-3300+2800i; %p1=-6300+280i; p2=-6200+150i;
%p2=-1500+500i; %P=[p1 p1' p2 p2'];
%p3=-1200+10i; P=[-1500+10j -1500-10j];
P=[p1 p1' p2 p2' p3 p3']; case 3
%p1=-6300+280i; p2=-6200+150i;
%P=[p1 p1' p2 p2'];
P=[-1500+10j -1500-10j -1400 -1300];
end end
else else
%2500rad/s = 397Hz -> locate poles here %2500rad/s = 397Hz -> locate poles here
%6300rad/s = 1027Hz -> locate poles here %6300rad/s = 1027Hz -> locate poles here
if length(poles)==4 switch mMdl
p1=-6300+2800i; case 0
p2=-6200+1500i; p1=-3300+2800i; p2=-1900+130i; p3=-2900+80i;
P=[p1 p1' p2 p2']; p4=-2300+450i; p5=-2000+20i; p6=-1500+10i;
P=[-2500 -2800 -1500+10j -1500-10j]; P=[p1 p1' p2 p2' p3 p3' p4 p4' p5 p5' p6 p6'];
else case 1
p1=-3300+2800i; %p1=-6300+2800i; p2=-6200+1500i;
p2=-1900+130i; %P=[p1 p1' p2 p2'];
p3=-2900+80i; P=[-2500 -2800 -1500+10j -1500-10j];
p4=-2300+450i; case 2
p5=-2000+20i; %p1=-6300+2800i; p2=-6200+1500i;
p6=-1500+10i; %P=[p1 p1' p2 p2'];
%p1=-3300+2800i; P=[-2500 -2800 -1500+10j -1500-10j];
%p2=-1500+500i;
%p3=-1200+10i;
P=[p1 p1' p2 p2' p3 p3'];% p4 p4' p5 p5' p6 p6'];
end end
end end
%P=P*.1; % P was too aggressive
K = place(Am,Bm,P); K = place(Am,Bm,P);
%K = acker(Am,Bm,Pm); %K = acker(Am,Bm,Pm);
end %if lqr end %if lqr
@@ -104,19 +159,15 @@ function [ssc]=StateSpaceControlDesign(mot)
if length(V)>1 if length(V)>1
V=V(3); % only the position scaling needed V=V(3); % only the position scaling needed
end end
%prefilter to compensate non observable resonance frequencies
numV=mot.prefilt.Numerator{1};
denV=mot.prefilt.Denominator{1};
ss_cl = ss(Am-Bm*K,Bm*V,Cm,0,'Name','space state controller','InputName',ssMdl.InputName,'OutputName',ssMdl.OutputName);
if bitand(mShow,4)
% step answer on closed loop with space state controller: % step answer on closed loop with space state controller:
t = 0:1E-4:.5; t = 0:1E-4:.5;
ss_cl = ss(Am-Bm*K,Bm*V,Cm,0,'Name','space state controller','InputName',mot.ssMdl.InputName,'OutputName',mot.ssMdl.OutputName); [y,t,x]=lsim(ss_cl,V*u,t,xm0);
[y,t,x]=lsim(ss_cl,V*u,t,xm0); figure();plot(t,y);title('step on closed loop');
figure();plot(t,y);title('step on closed loop'); end
% *** observer controller *** % *** observer controller ***
% %
%observer poles-> 5 times farther left than system poles %observer poles-> 5 times farther left than system poles
@@ -131,54 +182,83 @@ function [ssc]=StateSpaceControlDesign(mot)
Ct = [ Cm zeros(size(Cm)) ]; Ct = [ Cm zeros(size(Cm)) ];
Dt=0; Dt=0;
% step answer on closed loop with observer controller: 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',mot.ssMdl.OutputName); if bitand(mShow,8)
figure();lsim(ss_t,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer'); % 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');
end
% *** disctrete observer controller *** % *** disctrete observer controller ***
% %
Ts=1/5000; % 5kHz Ts=1/5000; % 5kHz
ss_tz = c2d(ss_t,Ts); ss_tz = c2d(ss_t,Ts);
[Atz,Btz,Ctz,Dtz]=ssdata(ss_tz ); [Atz,Btz,Ctz,Dtz]=ssdata(ss_tz );
ss_tz.Name='discrete obsvr ctrl'; ss_tz.Name='discrete obsvr ctrl';
% step answer on closed loop with disctrete observer controller:
t = 0:Ts:.05;
figure();lsim(ss_tz ,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer discrete');
%plot all bode diagrams of desPos->actPos if bitand(mShow,16)
figure(); % step answer on closed loop with disctrete observer controller:
h=bodeplot(ss_cl(3),ss_t(3),ss_tz(3)); t = 0:Ts:.05;
setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw'); figure();lsim(ss_tz ,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer discrete');
end
figure(); if bitand(mShow,32)
h=pzplot(ss_cl(3),ss_t(3),ss_tz(3)); %plot all bode diagrams of desPos->actPos
setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw'); figure();
if mMdl==2 || mMdl==3
idx=1;
else
idx=3;
end
h=bodeplot(ss_cl(idx),ss_t(idx),ss_tz(idx));
setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
figure();
h=pzplot(ss_cl(idx),ss_t(idx));
setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
figure();
h=pzplot(ss_tz(idx));
setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
end
%calculate matrices for the simulink system %calculate matrices for the simulink system
Ao=Am-L*Cm; Ao=Am-L*Cm;
Bo=[Bm L]; Bo=[Bm L];
Co=K; Co=K;
Do=zeros(size(Co,1),size(Bo,2)); Do=zeros(size(Co,1),size(Bo,2));
ss_o = ss(Ao,Bo,Co,Do,'Name','observer controller','InputName',{'desPos','iqMeas','iqVolts','actPos'},'OutputName',{'k*xt'}); if mMdl==2 || mMdl==3
ss_o = ss(Ao,Bo,Co,Do,'Name','observer controller','InputName',{'desPos','actPos'},'OutputName',{'k*xt'});
else
ss_o = ss(Ao,Bo,Co,Do,'Name','observer controller','InputName',{'desPos','iqMeas','iqVolts','actPos'},'OutputName',{'k*xt'});
end
%discrete plant %discrete plant
ssPltz = c2d(ssPlt,Ts); ssPltz = c2d(ssPlt,Ts);
[Apz,Bpz,Cpz,Dpz]=ssdata(ssPltz); [Apz,Bpz,Cpz,Dpz]=ssdata(ssPltz);
%discrete observer controller %discrete observer controller
ss_oz = c2d(ss_o,Ts); ss_oz = c2d(ss_o,Ts);
[Aoz,Boz,Coz,Doz]=ssdata(ss_oz); [Aoz,Boz,Coz,Doz]=ssdata(ss_oz);
mdlName='observer'; %mdlName='observer';
%open(mdlName); %open(mdlName);
%prefilter to compensate non observable resonance frequencies
prefilt=Prefilt(mot,mPrefilt);
numV=prefilt.Numerator{1};
denV=prefilt.Denominator{1};
%discrete prefilter %discrete prefilter
prefiltz=c2d(mot.prefilt,Ts); prefiltz=c2d(prefilt,Ts);
numVz=prefiltz.Numerator{1}; numVz=prefiltz.Numerator{1};
denVz=prefiltz.Denominator{1}; denVz=prefiltz.Denominator{1};
if bitand(mShow,64)
h=bodeplot(prefilt,prefiltz);
setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
end
%state space controller %state space controller
ssc=struct(); 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"]
@@ -187,6 +267,40 @@ function [ssc]=StateSpaceControlDesign(mot)
save(sprintf('/tmp/ssc%d.mat',mot.id),'-struct','ssc'); save(sprintf('/tmp/ssc%d.mat',mot.id),'-struct','ssc');
end end
function pf=Prefilt(mot,mode)
switch mode
case 0 %no filter
pf=tf(1,1);
case 1 %inverse resonance
if mot.id==1
den=mot.mdl.num2;%num=1;
num=mot.mdl.den2;%den=[1 0 0];
pf=tf(num,den);
else
den=conv(conv(conv(mot.mdl.num2,mot.mdl.num3),mot.mdl.num4),mot.mdl.num5);%num=1;
num=conv(conv(conv(mot.mdl.den2,mot.mdl.den3),mot.mdl.den4),mot.mdl.den5);%den=[1 0 0];
pf=tf(num,den);
end
case 2
if mot.id==1
f=200;w0=f*2*pi; num1=[1 300 w0^2]; den1=[1 200 w0^2];
numV=num1;
denV=den1;
pf=tf(numV,denV);
else
f=277;w0=f*2*pi; num1=[1 20 w0^2]; den1=[1 500 w0^2];
f=138;w0=f*2*pi; num2=[1 300 w0^2]; den2=[1 100 w0^2];
f=60;w0=f*2*pi; num3=[1 33 w0^2]; den3=[1 20 w0^2];
numV=conv(num1,num2);
denV=conv(den1,den2);
numV=conv(conv(num1,num2),num3);
denV=conv(conv(den1,den2),den3) ;
pf=tf(numV,denV);
end
end
%controlSystemDesigner('bode',1,pf); % <<<<<<<<< This opens a transferfunction that can be edited
end
%code snipplets from an example on youtube (see reference at top) %code snipplets from an example on youtube (see reference at top)
function SCRATCH() function SCRATCH()
@@ -200,16 +314,16 @@ function SCRATCH()
plot(pts(:,1),pts(:,2),'.');hold on; plot(pts(:,1),pts(:,2),'.');hold on;
plot(rec(:,5),rec(:,6),'-');%despos plot(rec(:,5),rec(:,6),'-');%despos
plot(rec(:,2),rec(:,3),'-');%actPos plot(rec(:,2),rec(:,3),'-');%actPos
%sig.time = [0 1 1 5 5 8 8 10]; %sig.time = [0 1 1 5 5 8 8 10];
%sig.signals.values = [0 0 2 2 2 3 3 3]'; %sig.signals.values = [0 0 2 2 2 3 3 3]';
%sig.signals.dimensions = 1; %sig.signals.dimensions = 1;
sig.time=0:2E-4:(length(rec)-1)*2E-4; sig.time=0:2E-4:(length(rec)-1)*2E-4;
sig.signals.values=rec(:,5); sig.signals.values=rec(:,5);
sig.signals.dimensions = 1; sig.signals.dimensions = 1;
sum(desPos_actPos.Data(:,1)-desPos_actPos.Data(:,2)) sum(desPos_actPos.Data(:,1)-desPos_actPos.Data(:,2))
A = [ 0 1 0 A = [ 0 1 0
980 0 -2.8 980 0 -2.8
0 0 -100 ]; 0 0 -100 ];

39
matlab/documentFunctions.m Normal file
View File

@@ -0,0 +1,39 @@
baseDir='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuningDoc/';
%plots bode of desPos_actPos
[mot1,mot2]=identifyFxFyStage();
close all;
[pb]=simFxFyStage(mot2);sim('stage_closed_loop');
t=desPos_actPos.Time;
u=desPos_actPos.Data(:,1);
y=desPos_actPos.Data(:,2);
figure(1);
clf;
plot(t,u,'b');hold on;
plot(t,y,'color',[0 .5 0]);
grid on;ylim([-4 4]);
grid on;ylim([-2 2]);
saveas(gca, baseDir+"m2_sim_pb.eps",'epsc');
saveas(gca, baseDir+"m1_sim_ss.eps",'epsc');
saveas(gca, baseDir+"m2_sim_ss.eps",'epsc');
saveas(gca, baseDir+"m1_sim_ss_pref.eps",'epsc');
saveas(gca, baseDir+"m2_sim_ss_pref.eps",'epsc');
saveas(gca, baseDir+"m2_mdl_bode.eps",'epsc');
minFrq=1;maxFrq=1000;
tSec=t(length(t));
f=(1:length(t))/tSec;
fu=fft(u);
fy=fft(y);
ph=phase(fy./fu)
mag=abs(fy)./abs(fu);
magDb=20*log10(mag);
figure(2);
subplot(2,1,1);
semilogx(f,magDb);
subplot(2,1,2);
semilogx(f,ph);
grid on;

238
matlab/identifyFxFyStage.m
View File

@@ -19,6 +19,7 @@ function [mot1,mot2]=identifyFxFyStage()
% tfc,tf_mdl : various transfer functions % tfc,tf_mdl : various transfer functions
% ssPlt : the final continous state space model of the plant (not observable, not controlable) % ssPlt : the final continous state space model of the plant (not observable, not controlable)
% ssMdl : the simplified continous state space model for the observer (observable, controlable) % ssMdl : the simplified continous state space model for the observer (observable, controlable)
% ssMdlNC : model without resonance and current loop
% %
% The used data files (generated from Python) are: % 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/ ) % (located for now in: /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/MXTuning/18_10_02/ )
@@ -97,7 +98,19 @@ function [mot1,mot2]=identifyFxFyStage()
else else
ob='not ';%not observable ob='not ';%not observable
end end
disp([s,' is ',ct,'controlable and ',ob,'observable.']); disp([s,' is ',ct,'controlable and ',ob,'observable.']);
end
function y=myNorm(y)
%normalizes num and den by factor 1000
%y.*10.^(3*(length(y):-1:1))
end
function plotBode(mot)
t1=tf(mot.ssPlt);t2=tf(mot.ssMdl_c1);t3=tf(mot.ssMdl_12);h=bodeplot(mot.meas,'r',t1(3,1),'g',t2(3,1),'b',t3(1,1),'m',mot.w);
setoptions(h,'FreqUnits','Hz','Grid','on');
ax=h.getaxes();
legend(ax(1),'Location','sw',{'real','plant','no res','no cur + 1 res'});
end end
function mot=fyStage() function mot=fyStage()
@@ -116,60 +129,80 @@ function [mot1,mot2]=identifyFxFyStage()
mot.tf_mdl=idtf(mot.mdl.num,mot.mdl.den); mot.tf_mdl=idtf(mot.mdl.num,mot.mdl.den);
%ss([g1 mot.tf_mdl],'minimal') this doesn't work as expected %ss([g1 mot.tf_mdl],'minimal') this doesn't work as expected
numc=myNorm(mot.mdl.numc); tfc=tf(mot.mdl.numc,mot.mdl.denc); %current loop iqCmd->iqMeas
denc=myNorm(mot.mdl.denc); tf1=tf(mot.mdl.num1,mot.mdl.den1); %current to position
num1=myNorm(mot.mdl.num1); tf2=tf(mot.mdl.num2,mot.mdl.den2); %resonance
den1=myNorm(mot.mdl.den1); %state -space model: ssc:current ssm:mechanics ssa:all (current+mechanics)
num2=myNorm(mot.mdl.num2); %resonance
den2=myNorm(mot.mdl.den2); % plant
g1=tf(numc,denc); % iqCmd->iqMeas
s1=ss(g1);
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];
g2=tf(num,den); %iqMeas->ActPos
s2=ss(g2);
s3=append(s1,s2);
s3.A(3,2)=s3.C(1,2)*s3.B(3,2);
mot.ssPlt=ss(s3.A,s3.B(:,1),s3.C,0); % single input, remove input iqMeas
mot.ssPlt.InputName{1}='iqCmd';
mot.ssPlt.OutputName{1}='iqMeas';
mot.ssPlt.OutputName{2}='iqVolts';
mot.ssPlt.OutputName{3}='actPos';
chkCtrlObsv(mot.ssPlt,'ssPlt fyStage');
% u +-----------+ y % u +-----------+ y
%iqCmd------->|1 1|-------> iqMeas %iqCmd------->|1 1|-------> iqMeas
% | 2|-------> iqVolts % | 2|-------> iqVolts
% | 3|-------> actPos % | 3|-------> actPos
% +-----------+ % +-----------+
ssc=ss(tfc);
ssc.C=[ssc.C; 1E5* 2.4E-3 1E-3*ssc.C(2)*8.8]; % add output iqVolts: iqVolts= i_meas*R+i_meas'*L 2.4mH 8.8Ohm (took random scaling values)
ssm=ss(tf1*tf2); %iqMeas->ActPos
ssa=append(ssc,ssm);
ssa.A(3,2)=ssa.C(1,2)*ssa.B(3,2);
mot.ssPlt=ss(ssa.A,ssa.B(:,1),ssa.C,0); % single input, remove input iqMeas
mot.ssPlt.InputName{1}='iqCmd';
mot.ssPlt.OutputName{1}='iqMeas';
mot.ssPlt.OutputName{2}='iqVolts';
mot.ssPlt.OutputName{3}='actPos';
chkCtrlObsv(mot.ssPlt,'ssPlt fyStage');
%tf(ssa) % display all transfer functions
%simplified model without resonance %simplified model without resonance
g2=tf(num1,den1); %iqMeas->ActPos without resonance frequencies % u +-----------+ y
s2=ss(g2); %iqCmd------->|1 1|-------> iqMeas
s3=append(s1,s2); % | 2|-------> iqVolts
s3.A(3,2)=s3.C(1,2)*s3.B(3,2); % | 3|-------> actPos
mot.ssMdl=ss(s3.A,s3.B(:,1),s3.C,0); % single input, remove input iqMeas % +-----------+
mot.ssMdl.InputName{1}='iqCmd'; ssm=ss(tf1); %iqMeas->ActPos
mot.ssMdl.OutputName{1}='iqMeas'; ssa=append(ssc,ssm);
mot.ssMdl.OutputName{2}='iqVolts'; ssa.A(3,2)=ssa.C(1,2)*ssa.B(3,2);
mot.ssMdl.OutputName{3}='actPos'; mot.ssMdl_c1=ss(ssa.A,ssa.B(:,1),ssa.C,0); % single input, remove input iqMeas
chkCtrlObsv(mot.ssMdl,'ssMdl fyStage'); mot.ssMdl_c1.InputName{1}='iqCmd';
mot.ssMdl_c1.OutputName{1}='iqMeas';
mot.ssMdl_c1.OutputName{2}='iqVolts';
mot.ssMdl_c1.OutputName{3}='actPos';
chkCtrlObsv(mot.ssMdl_c1,'ssMdl_c1 fyStage');
%filter in front of plant to suppress resonances (inverse of reonance) %model without current loop, with one resonance
den=num2;%num=1; %this assumes that the iqCmd->iqMeas is not relevant for motion
num=den2;%den=[1 0 0]; % u +-----------+ y
mot.prefilt=tf(num,den); %iqMeas------>|1 1|-------> actPos
% +-----------+
ssm=ss(tf1*tf2); %iqMeas->ActPos
mot.ssMdl_12=ssm; %iqMeas->ActPos without resonance frequencies
mot.ssMdl_12.InputName{1}='iqMeas';
mot.ssMdl_12.OutputName{1}='actPos';
chkCtrlObsv(mot.ssMdl_12,'ssMdl_12 fyStage');
%model without current loop, no resonance
%this assumes that the iqCmd->iqMeas is not relevant for motion
% u +-----------+ y
%iqMeas------>|1 1|-------> actPos
% +-----------+
ssm=ss(tf1); %iqMeas->ActPos
mot.ssMdl_1=ssm; %iqMeas->ActPos without resonance frequencies
mot.ssMdl_1.InputName{1}='iqMeas';
mot.ssMdl_1.OutputName{1}='actPos';
chkCtrlObsv(mot.ssMdl_1,'ssMdl_1 fyStage');
ssLst=["tfc","tf1","tf2","tfc*tf1","tf1*tf2","tfc*tf1*tf2"];
sys=[];
for s = ssLst
eval('sys=ss('+s+');')
%t=tf(sys);
%disp(evalc('t'))
chkCtrlObsv(sys,char(s));
end
%h=bodeplot(mot.meas,'r',mot.tf4_2,'b',mot.tf6_4,'g'); %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); %h=bodeplot(mot.meas,'r',mot.tf2_0,'b',mot.tf_mdl,'g',mot.w);
t1=tf(mot.ssPlt);t2=tf(mot.ssMdl);h=bodeplot(mot.meas,'r',t1(3,1),'g',t2(3,1),'b',mot.w); plotBode(mot)
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))
end end
function mot=fxStage() function mot=fxStage()
@@ -188,73 +221,88 @@ function [mot1,mot2]=identifyFxFyStage()
mot.tf13_9 = tfest(mot.meas, 13, 9, opt); mot.tf13_9 = tfest(mot.meas, 13, 9, opt);
mot.tf_mdl=idtf(mot.mdl.num,mot.mdl.den); mot.tf_mdl=idtf(mot.mdl.num,mot.mdl.den);
numc=myNorm(mot.mdl.numc); tfc=tf(mot.mdl.numc,mot.mdl.denc); %current loop iqCmd->iqMeas
denc=myNorm(mot.mdl.denc); tf1=tf(mot.mdl.num1,mot.mdl.den1); %current to position
num1=myNorm(mot.mdl.num1); tf2=tf(mot.mdl.num2,mot.mdl.den2); %resonance
den1=myNorm(mot.mdl.den1); tf3=tf(mot.mdl.num3,mot.mdl.den3); %resonance
num2=myNorm(mot.mdl.num2); %resonance tf4=tf(mot.mdl.num4,mot.mdl.den4); %resonance
den2=myNorm(mot.mdl.den2); tf5=tf(mot.mdl.num5,mot.mdl.den5); %resonance
num3=myNorm(mot.mdl.num3); %resonance
den3=myNorm(mot.mdl.den3);
num4=myNorm(mot.mdl.num4); %resonance
den4=myNorm(mot.mdl.den4);
num5=myNorm(mot.mdl.num5); %resonance
den5=myNorm(mot.mdl.den5);
%num=myNorm(mot.mdl.num);
%den=myNorm(mot.mdl.den);
g1=tf(numc,denc); % iqCmd->iqMeas
s1=ss(g1);
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];
g2=tf(num,den); %iqMeas->ActPos %state -space model: ssc:current ssm:mechanics ssa:all (current+mechanics)
s2=ss(g2);
s3=append(s1,s2);
%t_=tf(s3);
%bode(g2);figure;bode(t_(3,2));
%connect iqMeas from s1 to iqMeas of s2
s3.A(3,2)=s3.C(1,2)*s3.B(3,2);
s3.A(3,2)=s3.C(1,2)*s3.B(3,2); % plant
mot.ssPlt=ss(s3.A,s3.B(:,1),s3.C,0); % single input, remove input iqMeas
mot.ssPlt.InputName{1}='iqCmd';
mot.ssPlt.OutputName{1}='iqMeas';
mot.ssPlt.OutputName{2}='iqVolts';
mot.ssPlt.OutputName{3}='actPos' ;
chkCtrlObsv(mot.ssPlt,'ssPlt fxStage');
% u +-----------+ y % u +-----------+ y
%iqCmd------->|1 1|-------> iqMeas %iqCmd------->|1 1|-------> iqMeas
% | 2|-------> iqVolts % | 2|-------> iqVolts
% | 3|-------> actPos % | 3|-------> actPos
% +-----------+ % +-----------+
ssc=ss(tfc);
ssc.C=[ssc.C; 1E5* 2.4E-3 1E-3*ssc.C(2)*8.8]; % add output iqVolts: iqVolts= i_meas*R+i_meas'*L 2.4mH 8.8Ohm (took random scaling values)
ssm=ss(tf1*tf2*tf3*tf4*tf5); %iqMeas->ActPos
ssa=append(ssc,ssm);
ssa.A(3,2)=ssa.C(1,2)*ssa.B(3,2);
mot.ssPlt=ss(ssa.A,ssa.B(:,1),ssa.C,0); % single input, remove input iqMeas
mot.ssPlt.InputName{1}='iqCmd';
mot.ssPlt.OutputName{1}='iqMeas';
mot.ssPlt.OutputName{2}='iqVolts';
mot.ssPlt.OutputName{3}='actPos' ;
chkCtrlObsv(mot.ssPlt,'ssPlt fxStage');
%simplified model without resonance %simplified model without resonance
g2=tf(num1,den1); %iqMeas->ActPos without resonance frequencies % u +-----------+ y
s2=ss(g2); %iqCmd------->|1 1|-------> iqMeas
s3=append(s1,s2); % | 2|-------> iqVolts
s3.A(3,2)=s3.C(1,2)*s3.B(3,2); % | 3|-------> actPos
mot.ssMdl=ss(s3.A,s3.B(:,1),s3.C,0); % single input, remove input iqMeas % +-----------+
mot.ssMdl.InputName=mot.ssPlt.InputName; ssm=ss(tf1); %iqMeas->ActPos
mot.ssMdl.OutputName=mot.ssPlt.OutputName; ssa=append(ssc,ssm);
chkCtrlObsv(mot.ssMdl,'ssMdl fxStage'); ssa.A(3,2)=ssa.C(1,2)*ssa.B(3,2);
mot.ssMdl_c1=ss(ssa.A,ssa.B(:,1),ssa.C,0); % single input, remove input iqMeas
mot.ssMdl_c1.InputName{1}='iqCmd';
mot.ssMdl_c1.OutputName{1}='iqMeas';
mot.ssMdl_c1.OutputName{2}='iqVolts';
mot.ssMdl_c1.OutputName{3}='actPos';
chkCtrlObsv(mot.ssMdl_c1,'ssMdl_c1 fxStage');
%filter in front of plant to suppress resonances (inverse of reonance) %model without current loop, with one resonance
den=conv(conv(conv(num2,num3),num4),num5);%num=1; %this assumes that the iqCmd->iqMeas is not relevant for motion
num=conv(conv(conv(den2,den3),den4),den5);%den=[1 0 0]; % u +-----------+ y
mot.prefilt=tf(num,den); %iqMeas------>|1 1|-------> actPos
% +-----------+
ssm=ss(tf1*tf2); %iqMeas->ActPos
mot.ssMdl_12=ssm; %iqMeas->ActPos without resonance frequencies
mot.ssMdl_12.InputName{1}='iqMeas';
mot.ssMdl_12.OutputName{1}='actPos';
chkCtrlObsv(mot.ssMdl_12,'ssMdl_12 fxStage');
%model without current loop, no resonance
%this assumes that the iqCmd->iqMeas is not relevant for motion
% u +-----------+ y
%iqMeas------>|1 1|-------> actPos
% +-----------+
ssm=ss(tf1); %iqMeas->ActPos
mot.ssMdl_1=ssm; %iqMeas->ActPos without resonance frequencies
mot.ssMdl_1.InputName{1}='iqMeas';
mot.ssMdl_1.OutputName{1}='actPos';
chkCtrlObsv(mot.ssMdl_1,'ssMdl_1 fxStage');
ssLst=["tfc","tf1","tf2","tf3","tf4","tf5","tfc*tf1","tf1*tf2","tf1*tf2*tf3","tfc*tf1*tf2"];
sys=[];
for s = ssLst
eval('sys=ss('+s+');')
%t=tf(sys);
%disp(evalc('t'))
chkCtrlObsv(sys,char(s));
end
%h=bodeplot(mot.meas,'r',mot.tf4_2,'b',mot.tf6_4,'g',mot.tf13_9,'m',mot.tf_py,'b'); %h=bodeplot(mot.meas,'r',mot.tf4_2,'b',mot.tf6_4,'g',mot.tf13_9,'m',mot.tf_py,'b');
%h=bodeplot(mot.meas,'r',mot.tf2_0,'b',mot.tf_mdl,'g',mot.w); %h=bodeplot(mot.meas,'r',mot.tf2_0,'b',mot.tf_mdl,'g',mot.w);
t1=tf(mot.ssPlt);t2=tf(mot.ssMdl);h=bodeplot(mot.meas,'r',t1(3,1),'g',t2(3,1),'b',mot.w); plotBode(mot)
setoptions(h,'FreqUnits','Hz','Grid','on');
%controlSystemDesigner('bode',1,mot.tf_py); % <<<<<<<<< This opens a transferfiûnction that can be edited
end end
close all close all
mot1=fyStage(); mot1=fyStage();
mot2=fxStage(); mot2=fxStage();
%controlSystemDesigner('bode',1,mot1.tf_py); % <<<<<<<<< This opens a transferfiûnction that can be edited
end end

BIN
matlab/observer.slx
View File

Binary file not shown.

BIN
matlab/stage_closed_loop.slx
View File

Binary file not shown.

4
matlab/testObserver.m
View File

@@ -4,11 +4,11 @@ function [ssc]=testObserver(mot)
if mdl==0 if mdl==0
A = [ 0 1 0 A = [ 0 1 0
980 0 -2.8 980 0 -2.8
100
0 0 -100 ]; 0 0 -100 ];
B = [ 0 B = [ 0
0 0];
100 ];
C = [ 1 0 0 ]; C = [ 1 0 0 ];
D=0; D=0;

47
python/MXTuning.py
View File

@@ -238,21 +238,14 @@ class MXTuning(Tuning):
s=' //input values\n' s=' //input values\n'
for i in range(len(u)): for i in range(len(u)):
s+=' double {u}=Mptr->{u};\n'.format(u=u[i]) s+=' double {u}=Mptr->{u};\n'.format(u=u[i])
prog+=s+'''\n prog+=s+'''
if (Mptr->ClosedLoop) if (Mptr->ClosedLoop)
{ {
return iqCmd;
}
else
{
Mptr->Servo.Integrator=0.0;
return 0.0;
}
''' '''
s=' //x[n+1]=A*x[n]+B*u;\n' s=' //x[n+1]=A*x[n]+B*u;\n'
for i in range(A.shape[0]): for i in range(A.shape[0]):
s+=' x[%d]='%i s+=' x[%d]='%i
for j in range(A.shape[1]): for j in range(A.shape[1]):
s+='%+28.22g*_x[%d]'%(A[i,j],j) s+='%+28.22g*_x[%d]'%(A[i,j],j)
for j in range(B.shape[0]): for j in range(B.shape[0]):
@@ -260,29 +253,37 @@ class MXTuning(Tuning):
s+=';\n' s+=';\n'
prog+=s+'\n' prog+=s+'\n'
s=' //y=C*x[n]+D*x[n];\n' s=' //y=C*x[n]+D*x[n];\n'
for i in range(C.shape[0]): for i in range(C.shape[0]):
s+=' %s='%y[i] s+=' %s='%y[i]
for j in range(C.shape[1]): for j in range(C.shape[1]):
s+='%+28.22g*_x[%d]'%(C[i,j],j) s+='%+28.22g*_x[%d]'%(C[i,j],j)
s+=';\n' s+=';\n'
prog+=s+'\n' prog+=s+'\n'
prog+=''' iqCmd=DesPos*{V}-{y}; prog+=''' iqCmd=DesPos*{V}-{y};
if (iqCmd>maxDac) if (iqCmd>maxDac)
{{ {{
iqCmd=maxDac; iqCmd=maxDac;
}}
else
{{
if (iqCmd<-maxDac)
{{
iqCmd=-maxDac;
}}
}}
//return iqCmd;
pshm->P[200{motid}]=iqCmd; //lowpass of Position error
return pshm->ServoCtrl(Mptr);
}} }}
else else
{{ {{
if (iqCmd<-maxDac) Mptr->Servo.Integrator=0.0;
{{ return 0.0;
iqCmd=-maxDac;
}}
}} }}
return iqCmd; }}'''.format(V=V[0,0],y=y[0],motid=motid)
}}'''.format(V=V[0,0],y=y[0])
hdr='''double obsvr_servo_ctrl_{motid}(MotorData *Mptr); hdr='''double obsvr_servo_ctrl_{motid}(MotorData *Mptr);
EXPORT_SYMBOL(obsvr_servo_ctrl_{motid});'''.format(motid=motid) EXPORT_SYMBOL(obsvr_servo_ctrl_{motid});'''.format(motid=motid)
@@ -336,7 +337,7 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
#plt.ion() #plt.ion()
#args.host='MOTTEST-CPPM-CRM0573' #args.host='MOTTEST-CPPM-CRM0573'
#args.host=None args.host=None
if args.host is None: if args.host is None:
comm=gt=None comm=gt=None
else: else:

2535
python/MXTuningDoc/curr_step.eps Normal file
View File

File diff suppressed because it is too large Load Diff

BIN
python/MXTuningDoc/curr_step.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 33 KiB

BIN
python/MXTuningDoc/current_step_of_motor_1.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 24 KiB

BIN
python/MXTuningDoc/fastStageTune.odt
View File

Binary file not shown.

11870
python/MXTuningDoc/m1_sim_pb.eps Normal file
View File

File diff suppressed because it is too large Load Diff

10011
python/MXTuningDoc/m1_sim_ss.eps Normal file
View File

File diff suppressed because it is too large Load Diff

10017
python/MXTuningDoc/m1_sim_ss_pref.eps Normal file
View File

File diff suppressed because it is too large Load Diff

11242
python/MXTuningDoc/m2_mdl_bode.eps Normal file
View File

File diff suppressed because it is too large Load Diff

9860
python/MXTuningDoc/m2_sim_pb.eps Normal file
View File

File diff suppressed because it is too large Load Diff

10299
python/MXTuningDoc/m2_sim_ss.eps Normal file
View File

File diff suppressed because it is too large Load Diff

10157
python/MXTuningDoc/m2_sim_ss_pref.eps Normal file
View File

File diff suppressed because it is too large Load Diff

BIN
python/MXTuningDoc/observer.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 28 KiB

97
python/swissmx.svg Normal file
View File

File diff suppressed because one or more lines are too long
Side by Side

After

Width:  |  Height:  |  Size: 152 KiB