towards pole placement

2019-03-01 14:48:44 +01:00
parent 6ac4915925
commit 842dc87860
9 changed files with 300 additions and 28 deletions
--- a/matlab/DeltaTauOptimizer.m
+++ b/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];
-  SIM2=[1 1; 1 2; 8 2; 9 2];
-  if isempty(simData1)
+  %SIM1=[1 1; 1 2; 8 2; 9 2];
+  %SIM2=[1 1; 1 2; 8 2; 9 2];
+  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)
-    close all;
-    simData2=ExecSim(mot{2},SIM2);
-  end
+  %if isempty(simData2)
+  %  close all;
+  %  simData2=ExecSim(mot{2},SIM2);
+  %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');
--- a/matlab/DeltaTauParam.m
+++ b/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];
--- a/matlab/DeltaTauSim.slx
+++ b/matlab/DeltaTauSim.slx
--- a/matlab/Readme.md
+++ b/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
 =========

--- a/matlab/SCRATCH2.m
+++ b/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
+
--- a/matlab/SCRATCH3.m
+++ b/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
+
+
--- a/matlab/identifyFxFyStage.m
+++ b/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');
--- a/matlab/sample.slx
+++ b/matlab/sample.slx