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

290
matlab/StateSpaceControlDesign.m
View File

@@ -1,11 +1,10 @@
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.
% 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:
%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
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';
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);
[Am,Bm,Cm,Dm]=ssdata(ssMdl);
%sys=ss(sys.A,sys.B,sys.C(3,:),0); % this would reduce the outputs to position only
if bitand(mShow,1)
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));
xm0 = zeros(1,length(Am));
[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')
if bitand(mShow,2)
% step answer on open loop:
t = 0:1E-4:.5;
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);
%w0=abs(poles);
%ang=angle(-poles);
@@ -50,7 +107,6 @@ function [ssc]=StateSpaceControlDesign(mot)
% *** space state controller ***
%
%place poles for the controller feedback
use_lqr=0;
if use_lqr %use the lqr controller
Q=eye(length(ssMdl.A));
R=1;
@@ -59,42 +115,41 @@ function [ssc]=StateSpaceControlDesign(mot)
if mot.id==1
%2500rad/s = 397Hz -> locate poles here
%6300rad/s = 1027Hz -> locate poles here
if length(poles)==4
p1=-6300+280i;
p2=-6200+150i;
P=[p1 p1' p2 p2'];
P=[-4100 -4000 -1500+10j -1500-10j];
else
p1=-3300+2800i;
p2=-2700+500i;
p3=-2500+10i;
%p1=-3300+2800i;
%p2=-1500+500i;
%p3=-1200+10i;
P=[p1 p1' p2 p2' p3 p3'];
switch mMdl
case 0
p1=-3300+2800i; p2=-2700+500i; p3=-2500+10i;
P=[p1 p1' p2 p2' p3 p3'];
case 1
%p1=-6300+280i; p2=-6200+150i;
%P=[p1 p1' p2 p2'];
P=[-4100 -4000 -1500+10j -1500-10j];
case 2
%p1=-6300+280i; p2=-6200+150i;
%P=[p1 p1' p2 p2'];
P=[-1500+10j -1500-10j];
case 3
%p1=-6300+280i; p2=-6200+150i;
%P=[p1 p1' p2 p2'];
P=[-1500+10j -1500-10j -1400 -1300];
end
else
%2500rad/s = 397Hz -> locate poles here
%6300rad/s = 1027Hz -> locate poles here
if length(poles)==4
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;
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'];
switch mMdl
case 0
p1=-3300+2800i; p2=-1900+130i; p3=-2900+80i;
p4=-2300+450i; p5=-2000+20i; p6=-1500+10i;
P=[p1 p1' p2 p2' p3 p3' p4 p4' p5 p5' p6 p6'];
case 1
%p1=-6300+2800i; p2=-6200+1500i;
%P=[p1 p1' p2 p2'];
P=[-2500 -2800 -1500+10j -1500-10j];
case 2
%p1=-6300+2800i; p2=-6200+1500i;
%P=[p1 p1' p2 p2'];
P=[-2500 -2800 -1500+10j -1500-10j];
end
end
%P=P*.1; % P was too aggressive
K = place(Am,Bm,P);
%K = acker(Am,Bm,Pm);
end %if lqr
@@ -104,19 +159,15 @@ function [ssc]=StateSpaceControlDesign(mot)
if length(V)>1
V=V(3); % only the position scaling needed
end
%prefilter to compensate non observable resonance frequencies
numV=mot.prefilt.Numerator{1};
denV=mot.prefilt.Denominator{1};
% step answer on closed loop with space state controller:
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);
figure();plot(t,y);title('step on closed loop');
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:
t = 0:1E-4:.5;
[y,t,x]=lsim(ss_cl,V*u,t,xm0);
figure();plot(t,y);title('step on closed loop');
end
% *** observer controller ***
%
%observer poles-> 5 times farther left than system poles
@@ -131,54 +182,83 @@ function [ssc]=StateSpaceControlDesign(mot)
Ct = [ Cm zeros(size(Cm)) ];
Dt=0;
% step answer on closed loop with observer controller:
ss_t = ss(At,Bt,Ct,Dt,'Name','observer controller','InputName',{'desPos'},'OutputName',mot.ssMdl.OutputName);
figure();lsim(ss_t,ones(size(t)),t,[xm0 xm0]);title('step on closed loop with observer');
ss_t = ss(At,Bt,Ct,Dt,'Name','observer controller','InputName',{'desPos'},'OutputName',ssMdl.OutputName);
if bitand(mShow,8)
% 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 ***
%
Ts=1/5000; % 5kHz
ss_tz = c2d(ss_t,Ts);
[Atz,Btz,Ctz,Dtz]=ssdata(ss_tz );
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
figure();
h=bodeplot(ss_cl(3),ss_t(3),ss_tz(3));
setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
if bitand(mShow,16)
% 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');
end
figure();
h=pzplot(ss_cl(3),ss_t(3),ss_tz(3));
setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
if bitand(mShow,32)
%plot all bode diagrams of desPos->actPos
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
Ao=Am-L*Cm;
Bo=[Bm L];
Co=K;
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
ssPltz = c2d(ssPlt,Ts);
[Apz,Bpz,Cpz,Dpz]=ssdata(ssPltz);
%discrete observer controller
%discrete observer controller
ss_oz = c2d(ss_o,Ts);
[Aoz,Boz,Coz,Doz]=ssdata(ss_oz);
mdlName='observer';
%mdlName='observer';
%open(mdlName);
%prefilter to compensate non observable resonance frequencies
prefilt=Prefilt(mot,mPrefilt);
numV=prefilt.Numerator{1};
denV=prefilt.Denominator{1};
%discrete prefilter
prefiltz=c2d(mot.prefilt,Ts);
prefiltz=c2d(prefilt,Ts);
numVz=prefiltz.Numerator{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
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"]
@@ -187,6 +267,40 @@ function [ssc]=StateSpaceControlDesign(mot)
save(sprintf('/tmp/ssc%d.mat',mot.id),'-struct','ssc');
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)
function SCRATCH()
@@ -200,16 +314,16 @@ function SCRATCH()
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 ];