matlab wip

matlab/Readme.md

@@ -13,9 +13,25 @@ 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)
+identifyFxFyStage.m:       read python data and build motor objects. plot bode
+simFxFyStage.m:            build a pb structure which contains current Okt 2018) Powerbrick controller settings
+StateSpaceControlDesign.m: design a discrete observer for Fx,Fy stages
+```
 
 
+important calls
+---------------
+```
+clear;
+clear global;
+close all;
+[mot1,mot2]=identifyFxFyStage();
+[pb]=simFxFyStage(mot1,1);
+[ssc]=StateSpaceControlDesign(mot1,1);
+
+[pb]=simFxFyStage(mot2,2);
+[ssc]=StateSpaceControlDesign(mot2,2);
+
 
 ```
 
@@ -23,7 +39,6 @@ identifyFxFyStage.m:   a transferfunction is optimized with the acquires bode da
 
 
 
-
 OLD STUFF
 =========
 

matlab/SCRATCH.m

@@ -1,18 +1,54 @@
 
-clear;
+raw=desPos_actPos.Data
-clear global;
+raw(:,2)=raw(:,1)*100
-[mot1,mot2]=identifyFxFyStage();
+ft=fft(raw);
-[Kp,Kvfb,Ki,Kvff,Kaff,MaxInt,mot_num,mot_den,Ts,MaxDac,MaxPosErr,A,B,C,D]=simFxFyStage(mot1,1);
+plot(log(abs(ft)));
+plot(log(abs(ft(:,1)./ft(:,2))));
 
-[Nbar,A,B,C,Ao,Bo,Co,Do,L,K]=StateSpaceControlDesign(mot1,1);
+      i=int(minFrq*tSec); j=int(maxFrq*tSec); #print(w[i],w[j])
+
+
+x=2+3j
+rho=abs(x)
+theta=angle(x)
+%-------------------
+x=rho*exp(j*theta)
+
+
+
+  figure();
+  h=pzplot(ssc.ss_cl(3),ssc.ss_o(3),ssc.ss_od(3));
+  setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
 
 
 
 
 
 
+  %figure();bode(ssc.ss_o(3));
+  %figure();tf=tf(ssc.ss_o) pzplot(tf(3));
 
 
+  h=bodeplot(ssc.ss_cl(3),ssc.ss_o(3),ssc.ss_od(3));
+  setoptions(h,'FreqUnits','Hz','Grid','on');
+
+  h=pzplot(ssc.ss_cl(3),ssc.ss_o(3),ssc.ss_od(3));
+  setoptions(h,'FreqUnits','Hz','Grid','on');
+
+
+  h=bodeplot(ssc.ss_cl(3),ssc.ss_o(3),ssc.ss_od(3));
+  setoptions(h,'FreqUnits','Hz','Grid','on');
+  getoptions(h)
+
+
+
+
+continous to discrete
+web(fullfile(docroot, 'control/ref/c2d.html'))
+
+
+h = tf(10,[1 3 10],'IODelay',0.25); 
+hd = c2d(h,0.1)
 
 
 

matlab/StateSpaceControlDesign.m

@@ -1,25 +1,31 @@
-%http://ctms.engin.umich.edu/CTMS/index.php?example=Introduction&section=ControlStateSpace
+function [ssc]=StateSpaceControlDesign(mot,motid)
-%zustandsregler:
+  % !!! first it need to run: [mot1,mot2]=identifyFxFyStage() tobuild a motor object !!!
-% web(fullfile(docroot, 'simulink/examples.html'))
+  %
-% web(fullfile(docroot, 'simulink/examples/inverted-pendulum-with-animation.html'))
+  % builds a state space controller designed for the plant.
-% web(fullfile(docroot, 'simulink/examples/double-spring-mass-system.html'))
+  % shows step answers of open and closed loop, also for the observer controller and the final discrete observer
-function [Nbar,A,B,C,D,Ao,Bo,Co,Do,L,K]=StateSpaceControlDesign(mot,motid)
+  %
-  sys=mot.ss; 
+  % finally it opens a simulink observer file for testing
-  sys=ss(sys.A,sys.B,sys.C(3,:),0); %   $$$ only output position
 
-  %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;
+  A=ss_ol.A;
-  B=sys.B;
+  B=ss_ol.B;
-  C=sys.C;
+  C=ss_ol.C;
-  D=sys.D;
+  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
+  u = ones(size(t));
-  x0 = zeros(1,length(sys.A));
+  x0 = zeros(1,length(ss_ol.A));
-
-  [y,t,x] = lsim(sys,u,t,x0);
-  figure();plot(t,y)
 
+  [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;
+    p2=-2700+500i;
-    p3=-1200+10i;
+    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']);
+  V=-1./(C*(A-B*K)^-1*B); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
-  %K = place(A,B,[p1 p1']);
-  %Nbar = rscale(sys,K);
-  %Nbar=1;
-  Nbar=-1./(C*(A-B*K)^-1*B); %from my notes)
   %Nbar(2)=1; %the voltage stuff is crap for now
-  if length(Nbar)>1
+  if length(V)>1
-    Nbar=Nbar(3); % only the position scaling needed
+    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

matlab/identifyFxFyStage.m

@@ -1,10 +1,36 @@
 
 function [mot1,mot2]=identifyFxFyStage()
-  %sample idfrd
+  %loads recorded data of the current step and bode diagrams of the stages then plots the bode diagrams and identifies
-  %f = logspace(-1,1,100);
+  %the current step transfer function
-  %[mag,phase] = bode(idtf([1 .2],[1 2 1 1]),f);
+  %
-  %response = mag.*exp(1j*phase*pi/180);
+  %finally it builds a ss-Model of the stage with:
-  %m = idfrd(response,f,0.08);
+  %
+  %  u          +-----------+           y
+  %iqCmd------->|1         1|-------> iqMeas
+  %             |          2|-------> iqVolts
+  %             |          3|-------> actPos
+  %             +-----------+
+  %
+  % the returned motor objects mot1 and mot2 contains:
+  %
+  % w,mag,phase : (gathered data with Python)
+  % meas        : a MATLAB idfrd model with data w,mag,phase
+  % mdl         : a structure with the python numerators and denominators for the transfer functions
+  % tfc,tf_mdl  : various transfer functions
+  % ss          : the final continous state space model of the plant
+  %
+  % 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/ )
+  % - curr_step[1|2].mat
+  % - full_bode_mot[1|2].mat
+  % - model[1|2].mat
+
+
+  %References:
+  %create ss from tf MIMO:
+  %https://ch.mathworks.com/matlabcentral/answers/37152-how-to-convert-tf2ss-for-mimo-system
+  %http://ch.mathworks.com/help/control/ug/conversion-between-model-types.html#f3-1039600
+  %https://ch.mathworks.com/help/control/ref/append.html
 
   function obj=loadData(path,motid)
     obj=struct();
@@ -33,9 +59,6 @@ function [mot1,mot2]=identifyFxFyStage()
     opt.Display='off';
     tfc = tfest(obj.currstep, 2, 0,opt);
     s=str2ndOrd(tfc);
-    %disp(s);
-    %h = stepplot(tf1);
-    %l=obj.currstep.OutputData
     t=(0:199)*50E-6;
     [y,t]=step(tfc,t);
     f=figure();
@@ -57,7 +80,7 @@ function [mot1,mot2]=identifyFxFyStage()
     k=num(1)/den(3);
     w0=sqrt(den(3));
     damp=den(2)/2/w0;
-    s=sprintf('k:%g w0:%g damp:%g\n',k,w0,damp);
+    s=sprintf('k:%g w0:%g damp:%g',k,w0,damp);
   end
 
 
@@ -83,7 +106,7 @@ function [mot1,mot2]=identifyFxFyStage()
     den2=myNorm(mot.mdl.den2);
     g1=tf(numc,denc);  % iqCmd->iqMeas
     s1=ss(g1);
-    s1.C=[s1.C; 1/s1.B(1) 0];   % add output iqVolts: iqVolts= i_meas*R+i_meas'*L
+    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];
@@ -97,12 +120,6 @@ function [mot1,mot2]=identifyFxFyStage()
     mot.ss.OutputName{1}='iqMeas';
     mot.ss.OutputName{2}='iqVolts';
     mot.ss.OutputName{3}='actPos';
-    %  u          +-----------+           y
-    %iqCmd------->|1         1|-------> iqMeas
-    %             |          2|-------> iqVolts
-    %             |          3|-------> actPos
-    %             +-----------+
-    
     %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);
     tmp=tf(mot.ss);h=bodeplot(mot.meas,'r',tmp(3,1),'g',mot.w);
@@ -128,10 +145,6 @@ function [mot1,mot2]=identifyFxFyStage()
     mot.tf13_9 = tfest(mot.meas, 13, 9, opt);
     mot.tf_mdl=idtf(mot.mdl.num,mot.mdl.den);
 
-    %create ss from tf MIMO:
-    %https://ch.mathworks.com/matlabcentral/answers/37152-how-to-convert-tf2ss-for-mimo-system
-    %http://ch.mathworks.com/help/control/ug/conversion-between-model-types.html#f3-1039600
-    %https://ch.mathworks.com/help/control/ref/append.html
     numc=myNorm(mot.mdl.numc);
     denc=myNorm(mot.mdl.denc);
     num1=myNorm(mot.mdl.num1);
@@ -148,10 +161,13 @@ function [mot1,mot2]=identifyFxFyStage()
     den=myNorm(mot.mdl.den);
     g1=tf(numc,denc);  % iqCmd->iqMeas
     s1=ss(g1);
-    s1.C=[s1.C; 1/s1.B(1) 0];   % add output iqVolts: iqVolts= i_meas*R+i_meas'*L
+    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];
+    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);

matlab/simFxFyStage.m

@@ -1,5 +1,11 @@
-function [Kp,Kvfb,Ki,Kvff,Kaff,MaxInt,mot_num,mot_den,Ts,MaxDac,MaxPosErr,A,B,C,D]=simFxFyStage(mot,motid)
+function [pb]=simFxFyStage(mot,motid)
-  %global Kp Kvfb Ki Kvff Kaff MaxInt mot_num mot_den Ts MaxDac MaxPosErr A B C D
+  % !!! first it need to run: [mot1,mot2]=identifyFxFyStage() tobuild a motor object !!!
+  %
+  %loads the current (11.10.2018) controller settings of the
+  %Powerbrick and opens its simulink file
+  %the returned variable pb contains varous parameters:
+  %  Kp,Kvfb,Ki,Kvff,Kaff,MaxInt,mot_num,mot_den,Ts,MaxDac,MaxPosErr,A,B,C,D
+
   Ts=2E-4; % 0.2ms=5kHz
   MaxDac=2011.968;
   MaxPosErr=10000;
@@ -22,20 +28,8 @@ function [Kp,Kvfb,Ki,Kvff,Kaff,MaxInt,mot_num,mot_den,Ts,MaxDac,MaxPosErr,A,B,C,
   %ServoDeltaTau_z(motid)
   [A,B,C,D]=tf2ss(mot_num,mot_den);
 
-  %mdlWks=get_param(mdlName,'ModelWorkspace');
-  %whos global
-  %whos(mdlWks)
-  %for k=["Kp","Kvfb","Ki","Kvff","Kaff","MaxInt","mot_num","mot_den","Ts","MaxDac","MaxPosErr"]
-  %  assignin(mdlWks,k,eval(k))
-  %end
 
-  %assignin(mdlWks,'Ts',1234)
+  pb=struct();
-  %getVariable(mdlWks,'Ts')
+  for k=["Kp","Kvfb","Ki","Kvff","Kaff","MaxInt","mot_num","mot_den","Ts","MaxDac","MaxPosErr","A","B","C","D"]
-  % in global space call:
+    pb=setfield(pb,k,eval(k));
-  %global Kp Kvfb Ki Kvff Kaff MaxInt
-  %global mot_num mot_den Ts MaxDac MaxPosErr
-  %[mot1,mot2]=identifyFxFyStage();
-  %simFxFyStage(mot1,mot2);
- 
-  %
 end