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

matlab wip

Browse Source
This commit is contained in:
zamofing_t
2018-10-11 17:19:48 +02:00
parent f9ddf04fdc
commit cb9310ee5c
8 changed files with 228 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files

152
matlab/StateSpaceControlDesign.m
View File

@@ -1,25 +1,31 @@
%http://ctms.engin.umich.edu/CTMS/index.php?example=Introduction&section=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&section=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