towards matlab

matlab/SCRATCH.m

@@ -0,0 +1,123 @@
+
+clear;
+clear global;
+[mot1,mot2]=identifyFxFyStage();
+[Kp,Kvfb,Ki,Kvff,Kaff,MaxInt,mot_num,mot_den,Ts,MaxDac,MaxPosErr,A,B,C,D]=simFxFyStage(mot1,1);
+
+[Nbar,A,B,C,Ao,Bo,Co,Do,L,K]=StateSpaceControlDesign(mot1,1);
+
+
+
+
+
+
+
+
+
+
+
+
+function f=SCRATCH()
+open('stage_closed_loop.slx')
+
+
+  [m1,m2]=identifyFxFyStage();
+  controlSystemDesigner(1,m2.tf_py);    % <<<<<<<<< This opens a transferfiûnction that can be edited
+
+  %identification toolbox
+  systemIdentification
+
+  
+  
+  
+  
+  %opt=tfestOptions('Display','off');
+  %opt=tfestOptions('Display','on','initializeMethod','svf');
+  %opt=tfestOptions('Display','on','initializeMethod','iv','WeightingFilter',[]);
+  %opt=tfestOptions('Display','on','initializeMethod','iv','WeightingFilter',[1,5;20,570]);
+  %tf1 = tfest(mot1frq, 6, 4, opt);
+  % Model refinement
+  % Options = tf1.Report.OptionsUsed;
+  % Options.WeightingFilter = 'prediction';
+  % tf1_1 = pem(mot1frq, tf1, Options)
+
+
+
+
+
+  bodeplot(mot1frq,tf1)
+  mag,phase=bode(tf1,frq)
+  figure(1)
+  subplot(211)
+
+
+  bodeplot(tf1)
+
+  Opt = n4sidOptions('N4Horizon',[15 15 15]);
+  n4s3 = n4sid(mot1frq, 3, Opt)
+
+
+
+
+
+  %tf([1 2],[1 0 10])
+  %specifies the transfer function (s+2)/(s^2+10) while
+  sys=tf([1],[1,0,0])
+  bode(sys)
+  step(sys)
+
+  sys=tf([1],[1,-1,2]) %instable
+
+  sys=tf([1],[1,1,2]) %stable
+
+
+  %0dB at 12 Hz=12*2*pi rad/s =75.4=k^2 -> k=8.6833
+
+  sys=tf([10],[1,0,0])
+  %1/s^2 -> 0dB at 1Hz -40dB/decade
+  %10=+20dB
+
+
+  sys=tf([1],[1,0,2]) %not damped constant sine after step
+
+
+  sys=zpk([],[1,0,0],100) %stable
+
+  sys=zpk([],[-10,-10],100)
+
+
+  %parker stage 1 
+  %!encoder_sim(enc=1,tbl=9,mot=9,posSf=13000./2048)
+  %!encoder_inc(enc=1,tbl=1,mot=1,posSf=13000./650000)
+  %!motor_servo(mot=1,ctrl='ServoCtrl',Kp=25,Kvfb=400,Ki=0.02,Kvff=350,Kaff=5000,MaxInt=1000)
+  %!motor(mot=1,dirCur=0,contCur=800,peakCur=2400,timeAtPeak=1,IiGain=5,IpfGain=8,IpbGain=8,JogSpeed=10.,numPhase=3,invDir=True,servo=None,PhasePosSf=1./81250,PhaseFindingDac=100,PhaseFindingTime=50,SlipGain=0,AdvGain=0,PwmSf=10000,FatalFeLimit=200,WarnFeLimit=100,InPosBand=2,homing='enc-index')
+  Ts=2E-4 % discrete sample time (servo period)
+  Kp=25,Kvfb=400,Ki=0.02,Kvff=350,Kaff=5000,MaxInt=1000
+  Kp=25,Kvfb=0,Ki=0,Kvff=0,Kaff=0,MaxInt=0
+
+
+  num=7.32
+  den=[5.995e-04 4.897e-02   1.]
+  open('stage_closed_loop.slx')
+  %sim('stage_closed_loop.slx')
+
+  sys=tf(num,den)
+  bode(sys)
+  G = tf(1.5,[1 14 40.02]);
+  controlSystemDesigner('bode',sys);
+  controlSystemDesigner
+  linearSystemAnalyzer
+  load ltiexamples
+  linearSystemAnalyzer(sys_dc)
+
+  controlSystemDesigner('bode',sys);
+  controlSystemDesigner(1,sys);    % <<<<<<<<< This opens a transferfiûnction that can be edited
+
+  num=[8.32795069e-11, 1.04317228e-08, 6.68431323e-05, 3.31861324e-03, 7.32824533e+00];
+  den=[5.26156641e-18, 1.12897840e-14, 7.67853031e-12, 1.03201301e-08, 2.05154780e-06, 1.34279894e-03, 7.19229912e-02, 1.00000000e+00];
+  mot2=tf(num,den);
+  controlSystemDesigner('bode',mot2);
+  
+  
+
+end

matlab/StateSpaceControlDesign.m

@@ -0,0 +1,166 @@
+%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
+
+  %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);
+  setoptions(h,'IOGrouping','all')
+  A=sys.A;
+  B=sys.B;
+  C=sys.C;
+  D=sys.D;
+
+  P=ctrb(A,B);
+  if rank(A)==rank(P)
+    disp('sys controlable')
+  else
+    disp('sys not controlable')
+  end
+
+  Q=obsv(A,C);
+  if rank(A)==rank(Q)
+    disp('sys observable')
+  else
+    disp('sys not observable')
+  end
+
+
+  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)
+
+
+  poles = eig(A);
+
+  if motid==1
+    p1=-3300+2800i;
+    p2=-1500+500i;
+    p3=-1200+10i;
+    P=[p1 p1' p2 p2' p3 p3'];
+  else
+  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)
+  %Nbar(2)=1; %the voltage stuff is crap for now
+  if length(Nbar)>1
+    Nbar=Nbar(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)
+
+  %observer poles-> 5 times farther left than system poles
+  if motid==1
+    op1=(p1*5);
+    op2=(p2*5);
+    op3=(p3*5);
+    OP=[op1 op1' op2 op2' op3 op3'];
+  else
+  end
+  L=place(A',C',OP)';
+
+  At = [ A-B*K             B*K
+         zeros(size(A))    A-L*C ];
+  Bt = [    B*Nbar
+         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
+  Ao=A-L*C;
+  Bo=[B L];
+  Co=K;
+  Do=zeros(size(Co,1),size(Bo,2));
+  mdlName='observer';
+  open(mdlName)
+end
+
+function SCRATCH()
+  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
+
+
+

matlab/identifyFxFyStage.m

@@ -73,18 +73,39 @@ function [mot1,mot2]=identifyFxFyStage()
     figure(); 
     mot.tf2_0 = tfest(mot.meas, 2, 0, opt);disp(str2ndOrd(mot.tf2_0));
     mot.tf_mdl=idtf(mot.mdl.num,mot.mdl.den);
+    %ss([g1 mot.tf_mdl],'minimal') this doesn't work as expected
 
-    g11=tf(mot.mdl.numc,mot.mdl.denc);  % iqCmd->iqMeas  
+    numc=myNorm(mot.mdl.numc);
-    g12=tf([1 0],mot.mdl.denc*12); % iqCmd->iqVolts  : iqVolts= i_meas*R+i_meas'*L
+    denc=myNorm(mot.mdl.denc);
-    num=conv(conv(mot.mdl.num1,mot.mdl.num2),mot.mdl.numc);
+    num1=myNorm(mot.mdl.num1);
-    den=conv(conv(mot.mdl.den1,mot.mdl.den2),mot.mdl.denc);
+    den1=myNorm(mot.mdl.den1);
-    g13=tf(num,den); %iqCmd->ActPos
+    num2=myNorm(mot.mdl.num2);
-    %sys=ss([g11;g12])
+    den2=myNorm(mot.mdl.den2);
-    sys=ss([g11;g12;g13])
+    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
+    %tf(s1) % display all transfer functions
+    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);
+    s3.A(3,2)=s3.C(1,2)*s3.B(3,2);
+    mot.ss=ss(s3.A,s3.B(:,1),s3.C,0); % single input, remove input iqMeas
     
+    mot.ss.InputName{1}='iqCmd';
+    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);
+    %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);
     setoptions(h,'FreqUnits','Hz','Grid','on');
     
   end
@@ -109,10 +130,8 @@ function [mot1,mot2]=identifyFxFyStage()
 
     %create ss from tf MIMO:
     %https://ch.mathworks.com/matlabcentral/answers/37152-how-to-convert-tf2ss-for-mimo-system
-    %Gspm=[tf_iqCmd_actVolts;tf_iqCmd_iqMeas;tf_iqCmd_actPos];
+    %http://ch.mathworks.com/help/control/ug/conversion-between-model-types.html#f3-1039600
-    %sys=ss(Gspm);
+    %https://ch.mathworks.com/help/control/ref/append.html
-    
-    %normalize: [1E6 1E3 1].*mot.mdl.denc
     numc=myNorm(mot.mdl.numc);
     denc=myNorm(mot.mdl.denc);
     num1=myNorm(mot.mdl.num1);
@@ -127,45 +146,36 @@ function [mot1,mot2]=identifyFxFyStage()
     den5=myNorm(mot.mdl.den5);
     num=myNorm(mot.mdl.num);
     den=myNorm(mot.mdl.den);
-
-    %http://ch.mathworks.com/help/control/ug/conversion-between-model-types.html#f3-1039600
-    %tf2ss MIMO
-    %https://ch.mathworks.com/help/control/ref/append.html
     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
-    tf(s1) % display all transfer functions
+    %tf(s1) % display all transfer functions
-    
+    num=conv(conv(conv(conv(num1,num2),num3),num4),num5);%num=1;
-    num=conv(conv(conv(conv(num1,num2),num3),num4),num5);
+    den=conv(conv(conv(conv(den1,den2),den3),den4),den5);%den=[1 0 0];
-    den=conv(conv(conv(conv(den1,den2),den3),den4),den5);  
     g2=tf(num,den); %iqMeas->ActPos
     s2=ss(g2);
-
     s3=append(s1,s2);
-    tf(s3)
+    %t_=tf(s3);
+    %bode(g2);figure;bode(t_(3,2));
     %connect iqMeas from s1 to iqMeas of s2
-    s3.A(3,1)=1 %WHAT NUMBER ??? s3.B(3,2), s3.C2,:)?
+    s3.A(3,2)=s3.C(1,2)*s3.B(3,2);
-    %remove the direct iqMeas input
-    %s3.B(3,2)=0
 
-    t_=tf(s3) 
+    s3.A(3,2)=s3.C(1,2)*s3.B(3,2);
-    t_(3,1)
+    mot.ss=ss(s3.A,s3.B(:,1),s3.C,0); % single input, remove input iqMeas
-    figure;
-    bode(t_(3,1))
-    %compare with tf iqCmd->ActPos
-    num=conv(conv(conv(conv(conv(mot.mdl.num1,mot.mdl.num2),mot.mdl.num3),mot.mdl.num4),mot.mdl.num5),mot.mdl.numc);
-    den=conv(conv(conv(conv(conv(mot.mdl.den1,mot.mdl.den2),mot.mdl.den3),mot.mdl.den4),mot.mdl.den5),mot.mdl.denc);
-    g4=tf(num,den); %iqCmd->ActPos
-    figure;
-    bode(g4)
-    
-    %sys=ss([g11;g12])
-    sys=ss([g11;g12;g13])
-    sys=ss(g13)
     
+    mot.ss.InputName{1}='iqCmd';
+    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',mot.tf13_9,'m',mot.tf_py,'b');
-    h=bodeplot(mot.meas,'r',mot.tf2_0,'b',mot.tf_mdl,'g',mot.w);
+    %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);
     setoptions(h,'FreqUnits','Hz','Grid','on');
     %controlSystemDesigner('bode',1,mot.tf_py);    % <<<<<<<<< This opens a transferfiûnction that can be edited
 

matlab/rscale.m

@@ -0,0 +1,43 @@
+function[Nbar]=rscale(a,b,c,d,k)
+% Given the single-input linear system: 
+%       .
+%       x = Ax + Bu
+%       y = Cx + Du
+% and the feedback matrix K,
+% 
+% the function rscale(sys,K) or rscale(A,B,C,D,K) 
+% finds the scale factor N which will 
+% eliminate the steady-state error to a step reference 
+% for a continuous-time, single-input system
+% with full-state feedback using the schematic below:
+%
+%                         /---------\
+%      R         +     u  | .       |
+%      ---> N --->() ---->| X=Ax+Bu |--> y=Cx ---> y
+%                -|       \---------/
+%                 |             | 
+%                 |<---- K <----|
+%
+%8/21/96 Yanjie Sun of the University of Michigan
+%        under the supervision of Prof. D. Tilbury
+%6/12/98 John Yook, Dawn Tilbury revised
+
+error(nargchk(2,5,nargin));
+
+% --- Determine which syntax is being used ---
+nargin1 = nargin;
+if (nargin1==2),	% System form
+		[A,B,C,D] = ssdata(a);
+		K=b;
+elseif (nargin1==5), % A,B,C,D matrices
+		A=a; B=b; C=c; D=d; K=k;
+else error('Input must be of the form (sys,K) or (A,B,C,D,K)')
+end;
+
+% compute Nbar
+s = size(A,1);
+Z = [zeros([1,s]) 1];
+N = inv([A,B;C,D])*Z';
+Nx = N(1:s);
+Nu = N(1+s);
+Nbar=Nu + K*Nx;

matlab/simFxFyStage.m

@@ -1,8 +1,5 @@
-function out=simFxFyStage()
+function [Kp,Kvfb,Ki,Kvff,Kaff,MaxInt,mot_num,mot_den,Ts,MaxDac,MaxPosErr,A,B,C,D]=simFxFyStage(mot,motid)
-  global Kp Kvfb Ki Kvff Kaff MaxInt
+  %global Kp Kvfb Ki Kvff Kaff MaxInt mot_num mot_den Ts MaxDac MaxPosErr A B C D
-  global m1 m2 mot_num mot_den Ts MaxDac MaxPosErr
-  global A B C D
-    function ServoDeltaTau_z(motid)
   Ts=2E-4; % 0.2ms=5kHz
   MaxDac=2011.968;
   MaxPosErr=10000;
@@ -10,30 +7,35 @@ function out=simFxFyStage()
     %!motor_servo(mot=1,ctrl='ServoCtrl',Kp=25,Kvfb=400,Ki=0.02,Kvff=350,Kaff=5000,MaxInt=1000)
     %!motor(mot=1,dirCur=0,contCur=800,peakCur=2400,timeAtPeak=1,IiGain=5,IpfGain=8,IpbGain=8,JogSpeed=10.,numPhase=3,invDir=True,servo=None,PhasePosSf=1./81250,PhaseFindingDac=100,PhaseFindingTime=50,SlipGain=0,AdvGain=0,PwmSf=10000,FatalFeLimit=200,WarnFeLimit=100,InPosBand=2,homing='enc-index')
     Kp=25;Kvfb=400;Ki=0.02;Kvff=350;Kaff=5000;MaxInt=1000;
-      mot_num=m1.tf_mdl.Numerator;
+    mot_num=mot.tf_mdl.Numerator;
-      mot_den=m1.tf_mdl.Denominator;
+    mot_den=mot.tf_mdl.Denominator;
   else
     %!motor_servo(mot=2,ctrl='ServoCtrl',Kp=22,Kvfb=350,Ki=0.02,Kvff=240,Kaff=1500,MaxInt=1000)
     %!motor(mot=2,dirCur=0,contCur=800,peakCur=2400,timeAtPeak=1,IiGain=5,IpfGain=8,IpbGain=8,JogSpeed=10.,numPhase=3,invDir=True,servo=None,PhasePosSf=1./81250,PhaseFindingDac=100,PhaseFindingTime=50,SlipGain=0,AdvGain=0,PwmSf=10000,FatalFeLimit=200,WarnFeLimit=100,InPosBand=2,homing='enc-index')
     Kp=22;Kvfb=350;Ki=0.02;Kvff=240;Kaff=1500;MaxInt=1000;
-      mot_num=m2.tf_mdl.Numerator;
+    mot_num=mot.tf_mdl.Numerator;
-      mot_den=m2.tf_mdl.Denominator;
+    mot_den=mot.tf_mdl.Denominator;
-    end
   end
+
   mdlName='stage_closed_loop';
   open(mdlName)
-  ServoDeltaTau_z(2)
+  %ServoDeltaTau_z(motid)
   [A,B,C,D]=tf2ss(mot_num,mot_den);
 
-  %mdlWks=get_param(mdlName,'ModelWorkspace')
+  %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)
   %getVariable(mdlWks,'Ts')
   % in global space call:
   %global Kp Kvfb Ki Kvff Kaff MaxInt
-  %global m1 m2 mot_num mot_den Ts MaxDac MaxPosErr
+  %global mot_num mot_den Ts MaxDac MaxPosErr
-  %[m1,m2]=identifyFxFyStage();
+  %[mot1,mot2]=identifyFxFyStage();
+  %simFxFyStage(mot1,mot2);
  
   %
 end

python/MXTuning.py

@@ -76,6 +76,11 @@ class MXTuning(Tuning):
         num1=np.poly1d([mag1])
         den1 = np.poly1d([T1**2,2*T1*d1,1])
 
+        #reiner integrator: 30Hz=0dB -> k=30*2*pi=180
+        #num1=np.poly1d([120*120])
+        #den1 = np.poly1d([1,0,0])
+
+
         #first resonance frequency
         f2=np.array([197,199])
         d2=np.array([.02,.02])#daempfung
@@ -101,9 +106,9 @@ class MXTuning(Tuning):
         num=num1*num2*numc#*num3
         den=den1*den2*denc#*den3
         mdl= signal.lti(num, den) #num denum
-        print num
+        print(num)
-        print den
+        print(den)
-        print mdl
+        print(mdl)
         d={'num':num.coeffs,'num1':num1.coeffs,'num2':num2.coeffs,'numc':numc.coeffs,
            'den':den.coeffs,'den1':den1.coeffs,'den2':den2.coeffs,'denc':denc.coeffs}
         fn=os.path.join(base,'model%d.mat'%mot)
@@ -175,9 +180,9 @@ class MXTuning(Tuning):
         num=num1*num2*num3*num4*num5*numc
         den=den1*den2*den3*den4*den5*denc
         mdl= signal.lti(num, den) #num denum
-        print num
+        print(num)
-        print den
+        print(den)
-        print mdl
+        print(mdl)
         d={'num':num.coeffs,'num1':num1.coeffs,'num2':num2.coeffs,'num3':num3.coeffs,'num4':num4.coeffs,'num5':num5.coeffs,'numc':numc.coeffs,
            'den':den.coeffs,'den1':den1.coeffs,'den2':den2.coeffs,'den3':den3.coeffs,'den4':den4.coeffs,'den5':den5.coeffs,'denc':denc.coeffs}
         fn=os.path.join(base,'model%d.mat'%mot)
@@ -196,6 +201,100 @@ class MXTuning(Tuning):
       # tp print see also: print(np.poly1d([1,2,3], variable='s')), print(np.poly1d([1,2,3], r=True, variable='s'))
 
 
+    def custom_chirp(self):
+      motor = 1
+      amp, minFrq, maxFrq, tSec = (10, 10, 300, 30)
+      file='/tmp/gather.npz'
+      # if not os.path.isfile(f): tune.init_stage();plt.close('all')
+      # tune.bode_chirp(openloop=True, file=f, motor=mot, amp=amp, minFrq=minFrq, maxFrq=maxFrq, tSec=tSec)
+      prog = '''
+      &0 //cout works only in coord 0
+      open prog 999
+      L11=0
+      L10=0
+      L12=0
+      L13=0
+      L14=0
+      Gather.Enable=2
+      while(L10<300005)
+      {
+      L12=10*sin(31.415926535897931*(pow(1.058324104020218,(L10*0.000199996614513))-1)/log(1.058324104020218))
+      cout%d:(L12)
+      L10=L10+1
+      }
+      Gather.Enable=0
+      close
+      b999r
+      '''%motor
+      gpascii = self.comm.gpascii
+      gt = self.gather
+      print(gpascii.servo_period)
+      gt.set_phasemode(False)
+      address=("Motor[1].IqCmd", "Motor[1].ActPos",)
+      gt.set_address(*address)
+      #Gather.Enable=1
+      gt.set_property(MaxSamples=300000, Period=1)
+
+      # gt.enable(2)
+      gpascii.send_line(prog)
+      gpascii.sync()
+
+      gt.wait_stopped()
+      self.data=data=gt.upload()
+      meta={'motor':motor,'date':time.asctime(),'minFrq':minFrq,'maxFrq':maxFrq,'tSec':tSec,'amp':amp,'address':address}
+      np.savez_compressed(file, data=data, meta=meta)
+      meta['file'] = file
+      self.bode_chirp_plot(data, meta,True)
+      pass
+
+
+    def bode_sine(self,openloop=True,motor=1,minFrq=1,maxFrq=20,numFrq=15,amp=10,file='/tmp/gather.npz'):
+      '''calculates phase and amplitude at different frequencies and
+         saves:#loads and plots the bode diagram'''
+      if False:# os.path.isfile(file):
+        f=np.load(file)
+        bode=f['bode']
+        meta=f['meta'].item()
+        meta['file']=file
+      else:
+        gpascii=self.comm.gpascii
+        #motor 1 maximum: 13750
+        #amp= percentage of maximum amplitude
+        fig = plt.figure()
+        ax = fig.add_subplot(1, 1, 1)
+        frqLst=np.logspace(np.log10(minFrq),np.log10(maxFrq),numFrq)
+        n=len(frqLst)
+        #frqLst=(10,15,20,25,30)
+        bode=np.ndarray((n,3))
+        bode[:, 0]=frqLst
+        #for i in range(n):
+        for i in range(n-1,-1,-1):
+          frq=frqLst[i]
+          t=1
+          rep=max(1,frq*t)
+          if openloop:
+            data=self.do_command('openloopsine',motor,amp,frq,rep,0)
+          else:
+            data=self.do_command('sinusoidal',motor,amp,frq,rep,0)
+            data=data[:,(1,2)]
+          gpascii.send_line('#1j=0')
+          time.sleep(1)
+          ax.clear()
+          avg=data.mean(0)
+          print(avg)
+          ax.plot(data[:, 0]-avg[0] , 'b-', label='input')
+          ax.plot(data[:, 1]-avg[1], 'g-', label='output')
+          #plt.pause(.05)
+          bode[i,1:]=self.phase_amp(frq, rep)
+          print('frq %g ampl %g phase %g'%tuple(bode[i,:]))
+          plt.show(block=False);plt.pause(.05)
+
+        meta={'motor':motor,'date':time.asctime()}
+        np.savez_compressed(file, bode=bode, meta=meta)
+        meta['file']=file
+      self.bode_sine_plot(bode, meta)
+
+
 def bode(mdl):
   w,mag,phase = signal.bode(mdl,1000)
   f=w/(2*np.pi)
@@ -214,6 +313,16 @@ def bode(mdl):
 
 if __name__=='__main__':
   from argparse import ArgumentParser,RawDescriptionHelpFormatter
+  import logging
+  logger = logging.getLogger(__name__)
+  logger = logging.getLogger('pbtools.misc.pp_comm')
+  logger.setLevel(logging.DEBUG)
+  logging.basicConfig(format=('%(asctime)s %(name)-12s '
+                              '%(levelname)-8s %(message)s'),
+                      datefmt='%m-%d %H:%M',
+                      )
+
+
   def parse_args():
     'main command line interpreter function'
     #usage: gpasciiCommunicator.py --host=PPMACZT84 myPowerBRICK.cfg
@@ -315,7 +424,13 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
           tune.bode_sine(openloop=False, file=fn)
         if os.path.basename(fn).startswith('chirp_cl_mot'):
           tune.bode_chirp(openloop=False, file=fn)
-      print 'done'
+      print('done')
+    elif mode==7: #further tests
+        tune.init_stage();
+        plt.close('all')
+        tune.bode_sine()
+        tune.custom_chirp()
+
     plt.show()
   #------------------ Main Code ----------------------------------
   #ssh_test()

python/helicalscan.py

@@ -151,19 +151,19 @@ class HelicalScan:
       param = self.param
       cx, cz, w, fy,  = (0.2,0.3,0.1,0.4)
       #cx, cz, w, fy,  = (10.,20,3.,40)
-      print 'input  : cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r*1000.,fy)
+      print('input  : cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r*1000.,fy))
       (dx,dz,w,y) = self.fwd_transform(cx,cz,w,fy)
-      print 'fwd_trf: dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r*1000.,y)
+      print('fwd_trf: dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r*1000.,y))
       (cx,cz,w,fy) = self.inv_transform(dx,dz,w,y)
-      print 'inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r*1000.,fy)
+      print('inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r*1000.,fy))
 
       dx, dz, w, y,  = (0.2,0.3,0.1,0.4)
       #dx, dz, w, y,  = (10.,20,3.,40)
-      print 'input  : dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r*1000.,y)
+      print('input  : dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r*1000.,y))
       (cx,cz,w,fy) = self.inv_transform(dx,dz,w,y)
-      print 'inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r*1000.,fy)
+      print('inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r*1000.,fy))
       (dx,dz,w,y) = self.fwd_transform(cx,cz,w,fy)
-      print 'fwd_trf: dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r*1000.,y)
+      print('fwd_trf: dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r*1000.,y))
 
 
 
@@ -306,7 +306,7 @@ class HelicalScan:
       p[i,0]=x_i+r_i*np.sin(phi_i) # x= x_i+r_i*cos(phi_i+w)+cx
       p[i,1]=y_i # y= y_i
       p[i,2]=z_i+r_i*np.cos(phi_i)  # z= z_i+r_i*sin(phi_i*w)
-    print p
+    print(p)
     ofs=(p[1]+p[0])/2. # = center of the cristal
 
     m=Trf.trans(*ofs); self.hOrig=self.pltOrig(m)
@@ -327,7 +327,7 @@ class HelicalScan:
         p[i, 0] = x_i + r_i * np.cos(phi_i)  # x= x_i+r_i*cos(phi_i+w)+cx
         p[i, 1] = y_i  # y= y_i
         p[i, 2] = z_i + r_i * np.sin(phi_i)  # z= z_i+r_i*sin(phi_i*w)
-      print p
+      print(p)
       ofs = (p[1] + p[0]) / 2.  # = center of the cristal
 
       m = Trf.trans(cx,fy,cz)
@@ -375,7 +375,7 @@ class HelicalScan:
       p[i,1]=y_i # y= y_i
       p[i,2]=z_i+r_i*np.cos(phi_i)  # z= z_i+r_i*sin(phi_i*w)
     ofs=(p[1]+p[0])/2. # = center of the cristal
-    print 'p, ofs',p,ofs
+    print('p, ofs',p,ofs)
 
     m=Trf.trans(0,0,0); self.hOrig=self.pltOrig(m)
     hCrist,pt=self.pltCrist(cx=-ofs[0],fy=-ofs[1],cz=-ofs[2])
@@ -458,7 +458,7 @@ class HelicalScan:
     for k,v in fh.iteritems():
       s+='   '+k+': '+str(v.dtype)+' '+str(v.shape)+'\n'
       setattr(self,k,v)
-    print s
+    print(s)
 
   def fwd_transform(self,cx,cz,w,fy):
     #cx,cy: coarse stage
@@ -530,7 +530,7 @@ class HelicalScan:
       param[i, 1] = y
       param[i, 2:] = HelicalScan.meas_rot_ctr(x)  # (bias,ampl,phase)
       (bias, ampl, phase) = param[i][2:]
-    print param
+    print(param)
 
 
   def calcParam(self,x=((-241.,96.,-53.),(-162.,-293.,246.)),
@@ -569,15 +569,15 @@ class HelicalScan:
       x_ = ampl * np.cos(w + phase) + bias
       print(x_)
     (dx,dz,w,y_) = (0,0,0,y[0])
-    print 'input  : dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r*1000.,y_)
+    print('input  : dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r*1000.,y_))
     (cx,cz,w,fy) = self.inv_transform(dx,dz,w,y_)
-    print 'inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r*1000.,fy)
+    print('inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r*1000.,fy))
     (dx, dz, w, y_) = (0,0,0,y[1])
-    print 'input  : dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx, dz, w / d2r * 1000., y_)
+    print('input  : dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx, dz, w / d2r * 1000., y_))
     (cx, cz, w, fy) = self.inv_transform(dx, dz, w, y_)
-    print 'inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx, cz, w / d2r * 1000., fy)
+    print('inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx, cz, w / d2r * 1000., fy))
 
-    print param
+    print(param)
 
 
   def pltOrig(self,m,h=None):

python/shapepath.py

@@ -56,7 +56,6 @@ import matplotlib as mpl
 import matplotlib.pyplot as plt
 import subprocess as sprc
 import telnetlib
-from utilities import *
 
 class ShapePath:
     def __init__(self,args):

python/utilities.py

@@ -0,0 +1,91 @@
+#!/usr/bin/env python
+#*-----------------------------------------------------------------------*
+#|                                                                       |
+#|  Copyright (c) 2016 by Paul Scherrer Institute (http://www.psi.ch)    |
+#|                                                                       |
+#|              Author Thierry Zamofing (thierry.zamofing@psi.ch)        |
+#*-----------------------------------------------------------------------*
+'''
+utilities classes
+'''
+import json
+import numpy as np
+import time,os
+
+
+class dotdict(dict):
+  """dot.notation access to dictionary attributes"""
+  def __init__(self,arg=None,**kwargs):
+    if arg!=None:
+      self.__fill__(arg)
+    self.__fill__(kwargs)
+
+  def __fill__(self,kw):
+      for k,v in kw.iteritems():
+        if type(v)==dict:
+          self[k]=dotdict(v)
+        else:
+          self[k]=v
+          if type(v)==list:
+            for i,w in enumerate(v):
+              if type(w)==dict:
+                v[i]=dotdict(w)
+            pass
+
+  def __dir__(self):
+    l=dir(object)
+    #l.extend(self.keys())
+    l.extend(map(str,self.keys()))
+    return l
+
+  def __getattr__(self, attr):
+      #return self.get(attr)
+      try:
+        return self[attr]
+      except KeyError as e:
+        raise AttributeError("%r instance has no attribute %r" % (self.__class__, attr))
+
+  def __repr__(self):
+    return '<' + dict.__repr__(self)[1:-1] + '>'
+
+  def PrettyPrint(self,indent=0):
+    for k,v in self.iteritems():
+      if type(v)==dotdict:
+        print(' '*indent,str(k)+':')
+        v.PrettyPrint(indent+2)
+      else:
+        print(' '*indent+str(k)+'\t'+str(v))
+
+  __setattr__= dict.__setitem__
+  __delattr__= dict.__delitem__
+  #__getattr__= dict.__getattr__
+
+
+def ConvUtf8(s):
+  'convert unicoded json object to ASCII encoded'
+  #http://stackoverflow.com/questions/956867/how-to-get-string-objects-instead-of-unicode-ones-from-json-in-python
+  if isinstance(s, dict):
+    return {ConvUtf8(key): ConvUtf8(value) for key, value in s.items()}
+  elif isinstance(s, list):
+    return [ConvUtf8(element) for element in s]
+  elif isinstance(s, str):
+    return s.encode('utf-8')
+  else:
+    return s
+
+class GpasciiCommunicator():
+  '''Communicates with the Delta Tau gpascii programm
+  '''
+  gpascii_ack="\x06\r\n"
+  gpascii_inp='Input\r\n'
+
+  def connect(self, host, username='root', password='deltatau',prompt='ppmac# '):
+    p=telnetlib.Telnet(host)
+    print(p.read_until('login: '))
+    p.write(username+'\n')
+    print(p.read_until('Password: '))
+    p.write(password+'\n')
+    print(p.read_until(prompt))  # command prompt
+    p.write('gpascii -2\n')  # execute gpascii command
+    print(p.read_until(self.gpascii_inp))
+    return p