towards pole placement

MXfastStageDoc/MXfastStage.tex

@@ -827,7 +827,7 @@ To correct errors, posErrFB is increased. But in a discrete system, the regulati
 The Standard Delta Tau controller (figure \ref{fig:deltatau_std_ctrl}), shows a very similar feed forward and feedback loop structure with additional filters. So after all the measurements, we can calculate the optimal Kfff and Kaff values.
 Kp and Ki values will attenuate the errors. Kvff=Kvfb makes the system stable at higher Kp values, but setting the filter B seems to be more appropriate.\\
 
-MATLAB simulations and real test showed similar behaviour and the following error could be reduced roughly by factor 2 with first tests.\\
+MATLAB simulations \verb|(tool: DeltaTauOptimizer.m)| and real test showed similar behaviour and the following error could be reduced roughly by factor 2 with first tests.\\
 
 This should be good enough for the beam time in March 2019 but further improvements still should be possible.
 

matlab/DeltaTauOptimizer.m

@@ -9,32 +9,76 @@ function DeltaTauOptimizer()
   if isempty(mot)
     mot=identifyFxFyStage(7);
   end
-  SIM1=[1 1; 1 2; 8 2; 9 2];
+  %SIM1=[1 1; 1 2; 8 2; 9 2];
-  SIM2=[1 1; 1 2; 8 2; 9 2];
+  %SIM2=[1 1; 1 2; 8 2; 9 2];
-  if isempty(simData1)
+  SIM1=[10 0;7 3];
+  SIM1=[10 0];
+  %SIM2=[9 0];
+  if 1 || isempty(simData1)
     close all;
     simData1=ExecSim(mot{1},SIM1);
   end
-  if isempty(simData2)
+  %if isempty(simData2)
-    close all;
+  %  close all;
-    simData2=ExecSim(mot{2},SIM2);
+  %  simData2=ExecSim(mot{2},SIM2);
-  end
+  %end
   close all;
   %test()
   bodeSim(simData1);
-  bodeSim(simData2);
+  %bodeSim(simData2);
 end
 
 function test()
   global pb mot simData1 simData2;
-  %pb=DeltaTauParam(mot{2},7,0);
+  %pb=DeltaTauParam(mot{2},8,0);
   pb=DeltaTauParam(mot{2},9,0);
+  %pb.C=[0.04877];
+  %pb.D=[1 -0.9512];
+  %pb.C=[1   -1.3236    6.2472  -11.8555   11.3067   -5.4188    1.0440];
+  %pb.D=[1.0000   -6.6330   17.6945  -24.5314   18.7409   -7.5020    1.2309];
+  global tfs
+  tfz=c2d(tf(tfs),1/5000)
+  h=bodeplot(tfz,tfs);setoptions(h,'FreqUnits','Hz','Grid','on');
+  pb.C=tfz.Numerator{1};
+  pb.D=tfz.Denominator{1};
   sim('DeltaTauSim');
-  i=6;
+  i=2;
   %simData2(i).mot_mdl_param=SIM(i,:);
   simData2(i).pb=pb;
   simData2(i).desPos_actPos=desPos_actPos;
-  bodeSim(simData2);
+  simData2=bodeSim(simData2);
+
+  opt=tfestOptions;
+  opt.Display='off';
+%   %tfa=tfest(simData2(i).tfEst, 6, 5,opt);
+  tfa=tfest(simData2(i).tfEst, 6, 6,opt);
+%   tfb=1/tfa
+%   tfc=c2d(tfb,1/5000)
+% 
+%   tfs=tf([1],[.001 1])
+%   tfz=c2d(tfs,1/5000)
+%   h=bodeplot(tfs,tfz)
+%   setoptions(h,'FreqUnits','Hz','Grid','on');
+  
+  
+  %C=num=0   -1.3236    6.2472  -11.8555   11.3067   -5.4188    1.0440
+  %D=den=1.0000   -6.6330   17.6945  -24.5314   18.7409   -7.5020    1.2309
+  % controlSystemDesigner('bode',1,tf(1,1))
+ % controlSystemDesigner( 1/simData2(2).tfEst2)
+ % controlSystemDesigner(tfa)
+ 
+ %C =
+ %  1035.3 (s+217.8)
+ % -------------------
+ % (s+845.3) (s+266.8)
+ 
+ %  0.18956 (z-0.9574)
+ % --------------------
+ % (z-0.948) (z-0.8444)
+ 
+%    0.1896 z - 0.1815
+%  ----------------------
+%  z^2 - 1.792 z + 0.8006
 end
 
 
@@ -55,7 +99,7 @@ function simData=ExecSim(mot,SIM)
   end
 end
 
-function bodeSim(simData)
+function simData=bodeSim(simData)
   fig=figure();
   ax1=subplot(2,1,1);  hold(ax1,'on')
   ax2=subplot(2,1,2);  hold(ax2,'on')
@@ -83,6 +127,7 @@ function bodeSim(simData)
     h=plot(ax1,frq,abs(tfn), 'DisplayName',simData(i).pb.desc);
     p=unwrap(phase(tfn))/(2*pi)*360;
     h=plot(ax2,frq,p, 'DisplayName',simData(i).pb.desc);
+    simData(i).tfEst=idfrd(tfn,frq*2*pi,0);
   end
   grid(ax1,'on');grid(ax2,'on');
   set(ax1, 'XScale', 'log');

matlab/DeltaTauParam.m

@@ -9,7 +9,9 @@ function [pb]=DeltaTauParam(mot,mdl,param)
   pb=struct(...
     'Ts', 2E-4, ... % 0.2ms=5kHz
     'MaxDac' ,2011.968, ...
-    'MaxPosErr', 10000);
+    'MaxPosErr', 10000, ...
+    'A',1,'B',1,'C',1,'D',1,'E',1,'F',1, ...
+    'Kafb',0);
   desc=sprintf('mot:%d mdl:',mot.id);
   switch mdl
     case 1
@@ -30,9 +32,9 @@ function [pb]=DeltaTauParam(mot,mdl,param)
       pb.ss_plt=mot.ss_cq;desc=desc+"cq"; pb.sel={3,[3]};
     case 9
       pb.ss_plt=mot.ss_cqr;desc=desc+"cqr"; pb.sel={3,[3]};
+    case 10
+      pb.ss_plt=mot.ss_dq;desc=desc+"dq"; pb.sel={3,[3]};
   end
-  pb.A=[1];pb.B=[1];pb.C=[1];pb.D=[1];pb.E=[1];pb.F=[1];
-
   desc=desc+" param:";
   if mot.id==1
     %!motor_servo(mot=1,ctrl='ServoCtrl',Kp=25,Kvfb=400,Ki=0.02,Kvff=350,Kaff=5000,MaxInt=1000)
@@ -40,12 +42,52 @@ function [pb]=DeltaTauParam(mot,mdl,param)
     switch param
       case 0 %scratch
         desc=desc+"scratch";
+        pl=[-300+550i -300-550i -2513];
+        [Am,Bm,Cm,Dm]=ssdata(mot.ss_dq);
+        K = place(Am,Bm,pl);
+        V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
+        V=V(end);
+        Kfb=K*(Cm^-1);
+        pb.V=V;
+        pb.Kfb=Kfb;
+
+        pb.Kp=V;
+        pb.B=Kfb(3)/V;
+        pb.Kvfb=Kfb(2)/pb.Ts;
+        fprintf('Kp:%f B:%f Kvfb:%f Kafb:%f\n',pb.Kp,pb.B,pb.Kvfb,pb.Kafb);
+
+        pb.Kvff=1*pb.Kvfb;
+        pb.Kafb=0;%Kfb(1)/(pb.Ts^2);
+        pb.Kaff=1*1/(1.548e04*(pb.Ts^2));
+        pb.Ki=0.02;
+        pb.MaxInt=1000;      
       case 1 %origin parameters
         desc=desc+"orig";
         pb.Kp=25;pb.Kvfb=400;pb.Ki=0.02;pb.Kvff=350;pb.Kaff=5000;pb.MaxInt=1000;
       case 2%enhances first step
         desc=desc+"2";
         pb.Kp=25;pb.Kvfb=350;pb.Ki=0.02;pb.Kvff=350;pb.Kaff=1/(1.548e04*(pb.Ts^2));pb.MaxInt=1000;
+      case 3%pole placement on simplified motion tf
+        pl=[-1400 -1440];
+        pl=[-300+350i -300-350i];
+        [Am,Bm,Cm,Dm]=ssdata(mot.ss_q);
+        K = place(Am,Bm,pl);
+        V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
+        V=V(end);
+        Kfb=K*(Cm^-1);
+        pb.V=V;
+        pb.Kfb=Kfb;
+
+        pb.Kp=V;
+        pb.B=Kfb(2)/V;
+        pb.Kvfb=Kfb(1)/pb.Ts;
+        fprintf('Kp:%f B:%f Kvfb:%f\n',pb.Kp,pb.B,pb.Kvfb);
+
+        pb.Kvff=pb.Kvfb;
+        pb.Kafb=0;
+        pb.Kaff=1/(1.548e04*(pb.Ts^2));
+        pb.Ki=0.01; % lower 
+        pb.MaxInt=1000;      
     end
     %pb.Kp=0.1;pb.Kvfb=0;pb.Ki=0.00;pb.Kvff=0;pb.Kaff=1/(1.548e04*(pb.Ts^2));pb.MaxInt=1000;
     %filter  [z^0 z^-1 ... z^-n];

matlab/Readme.md

@@ -76,16 +76,10 @@ Motor[2].Servo.Kaff=1500
 Motor[2].Servo.MaxInt=1000
 Motor[2].Servo.Kfff=0
 
-
-
-
 ********** OPTIMIZED **************
-
 motion average error x 0.663518 um, y 0.585719 um, 1.01806 um
 shot average error x 0.865708 um, y 0.965126 um, 1.49323 um
 
-
-
 Motor[1].Servo.Kp=25
 Motor[1].Servo.Kvfb=350
 Motor[1].Servo.Ki=0.02
@@ -103,12 +97,6 @@ Motor[2].Servo.MaxInt=1000
 Motor[2].Servo.Kfff=10 //try on real system to optimize
 
 
-
-
-
-
-
-
 OLD STUFF
 =========
 

matlab/SCRATCH2.m

@@ -0,0 +1,99 @@
+%http://ctms.engin.umich.edu/CTMS/index.php?example=Introduction&section=ControlStateSpace
+
+%code snipplets from an example on youtube (see reference at top)
+function SCRATCH()
+%import numpy as np
+%fh=np.load('mode1.npz')
+%import scipy.io
+%scipy.io.savemat('mode1.mat',fh,do_compression=True)
+
+%matlab:
+  load('mode1.mat');
+  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 ];
+
+  B = [ 0
+        0
+        100 ];
+
+  C = [ 1 0 0 ];
+
+  poles = eig(A)
+
+  t = 0:0.01:2;
+  u = zeros(size(t));
+  x0 = [0.01 0 0];
+
+  sys = ss(A,B,C,0);
+
+  [y,t,x] = lsim(sys,u,t,x0);
+  plot(t,y)
+  title('Open-Loop Response to Non-Zero Initial Condition')
+  xlabel('Time (sec)')
+  ylabel('Ball Position (m)')
+
+  p1 = -10 + 10i;
+  p2 = -10 - 10i;
+  p3 = -50;
+
+  K = place(A,B,[p1 p2 p3]);
+  sys_cl = ss(A-B*K,B,C,0);
+
+  lsim(sys_cl,u,t,x0);
+  xlabel('Time (sec)')
+  ylabel('Ball Position (m)')
+
+  p1 = -20 + 20i;
+  p2 = -20 - 20i;
+  p3 = -100;
+
+  K = place(A,B,[p1 p2 p3]);
+  sys_cl = ss(A-B*K,B,C,0);
+
+  lsim(sys_cl,u,t,x0);
+  xlabel('Time (sec)')
+  ylabel('Ball Position (m)')
+
+
+  t = 0:0.01:2;
+  u = 0.001*ones(size(t));
+
+  sys_cl = ss(A-B*K,B,C,0);
+
+  lsim(sys_cl,u,t);
+  xlabel('Time (sec)')
+  ylabel('Ball Position (m)')
+  axis([0 2 -4E-6 0])
+
+  Nbar = rscale(sys,K)
+
+  lsim(sys_cl,Nbar*u,t)
+  title('Linear Simulation Results (with Nbar)')
+  xlabel('Time (sec)')
+  ylabel('Ball Position (m)')
+  axis([0 2 0 1.2*10^-3])
+end
+
+clear;clear global;close all;
+mot=identifyFxFyStage(7);
+for k =1:2
+  [pb]=simFxFyStage(mot{k});sim('stage_closed_loop');
+  f=figure(); h=plot(desPos_actPos.Time,desPos_actPos.Data,'g');
+  set(h(1),'color','b'); set(h(2),'color',[0 0.5 0]);
+  print(f,sprintf('figures/sim_cl_DTGz_%d',mot{k}.id),'-depsc');
+end
+

matlab/SCRATCH3.m

@@ -0,0 +1,89 @@
+Ts=1/5000
+tf1=tf([1],[1 0])
+%tfz1=tf([1],[1 0],Ts)
+tfz1a=c2d(tf1,Ts);%%'zoh'=default
+tfz1b=c2d(tf1,Ts,'foh');
+tfz1c=c2d(tf1,Ts,'impulse');
+tfz1d=c2d(tf1,Ts,'tustin');
+tfz1e=c2d(tf1,Ts,'matched');
+tfz1f=c2d(tf1,Ts,'least-squares');
+%      'zoh'           Zero-order hold on the inputs
+%       'foh'           Linear interpolation of inputs
+%       'impulse'       Impulse-invariant discretization
+%       'tustin'        Bilinear (Tustin) approximation.
+%       'matched'       Matched pole-zero method (for SISO systems only).
+%       'least-squares' Least-squares minimization of the error between
+ 
+
+
+bo = bodeoptions;
+%bo.FreqUnits = 'Hz'; bo.MagUnits='abs'; bo.Grid='on';
+bo.FreqUnits = 'Hz'; bo.Grid='on';
+bodeplot(tf1,tfz1a,tfz1b,tfz1c,tfz1d,tfz1e,tfz1f,bo)
+bodeplot(tf1,bo)
+
+%k=1.548e04
+k=1:
+tf1=tf(k ,[1 0 0]);
+ss1=ss(tf1)
+[Am,Bm,Cm,Dm]=ssdata(ss1);
+pl=[-3 -4];
+pl=[-30 -40];
+pl=[-100 -110];
+pl=[-1000 -1100];
+%pl=[-10+40i -10-40i];
+K = place(Am,Bm,pl);
+V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
+ss_cl = ss(Am-Bm*K,Bm*V,Cm,0,'Name','space state controller','InputName','u','OutputName','y');
+step(ss_cl);
+
+bodeplot(ss_cl)
+
+t=0:1E-4:.5; %time vector for simulations
+u = ones(size(t));
+xm0=[0 0];
+[y,t,x]=lsim(ss_cl,V*u,t,xm0);
+
+
+k=1.548e04
+%tf1=tf(k ,[1 0 0]);
+tf1=tf({[k 0]; k},[1 0 0]); %not controlabe, not observable
+%tf1=tf({[k 0]; k},[1 .3 .5]); %controlabe, observable
+%chkCtrlObsv(ss1,'ss1');
+ss1=ss(tf1);
+[Am,Bm,Cm,Dm]=ssdata(ss1);
+pl=[-1500 -1600];
+K = place(Am,Bm,pl);
+V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
+V=V(end);
+ss_cl = ss(Am-Bm*K,Bm*V,Cm,0); %THIS WORKS
+step(ss_cl)
+
+pb.ss_plt=ss1
+pb.Kfb=K*(Cm^-1);
+pb.V=V;
+bode(ss_cl)
+
+
+pb.ss_plt=mot{1}.ss_q;
+pl=[-1000 -1100]*1;
+[Am,Bm,Cm,Dm]=ssdata(pb.ss_plt);
+K = place(Am,Bm,pl);
+V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
+V=V(end);
+pb.Kfb=K*(Cm^-1);
+pb.V=V;
+
+%k/s^2 -> w^2/s^2 ->frq=sqrt(k)/(w/2*pi)=sqrt(1.548e04)/(2*pi)=19.8Hz
+
+[Am,Bm,Cm,Dm]=ssdata(mot.ss_dq);
+pl=[-3300 -2100 -2500];
+K = place(Am,Bm,pl);
+V=-1./(Cm*(Am-Bm*K)^-1*Bm); %(from Lineare Regelsysteme2 (Glattfelder) page:173 )
+
+KK=K*(Cm^-1);
+KK=KK(1,:);
+pb.Kfb=KK
+pb.V=V
+
+

matlab/identifyFxFyStage.m

@@ -289,6 +289,15 @@ function motCell=identifyFxFyStage(mode)
     mot.ss_qr.Name='simplified mechanics, no current loop, resonance';
     tfp.InputName{1}=s;%restore
 
+    %  u          +-----------+           y
+    %iqCmd------->|1         1|-------> iqMeas
+    %             |          2|-------> actVel
+    %             |          3|-------> actPos
+    %             +-----------+
+    s=tfq.InputName{1};tfq.InputName{1}='iqMeas';
+    mot.ss_dq=connect(tfd,tfq,'iqCmd',{'iqMeas','actVel','actPos'});
+    mot.ss_dq.Name='simplified mechanics, simplified current loop, no resonance';
+    tfq.InputName{1}=s;%restore
     
     
     %h=bodeplot(mot.meas,'r',mot.tf4_2,'b',mot.tf6_4,'g');