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/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
+