sf-mx/PBSwissMX
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

towards matlab

Browse Source
This commit is contained in:
zamofing_t
2018-10-10 17:15:36 +02:00
parent 5543582557
commit f9ddf04fdc
11 changed files with 640 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files

123
matlab/SCRATCH.m Normal file
View File

@@ -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

166
matlab/StateSpaceControlDesign.m Normal file
View File

@@ -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

92
matlab/identifyFxFyStage.m
View File

@@ -73,18 +73,39 @@ function [mot1,mot2]=identifyFxFyStage()
figure(); figure();
mot.tf2_0 = tfest(mot.meas, 2, 0, opt);disp(str2ndOrd(mot.tf2_0)); mot.tf2_0 = tfest(mot.meas, 2, 0, opt);disp(str2ndOrd(mot.tf2_0));
mot.tf_mdl=idtf(mot.mdl.num,mot.mdl.den); 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.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'); setoptions(h,'FreqUnits','Hz','Grid','on');
end end
@@ -109,10 +130,8 @@ function [mot1,mot2]=identifyFxFyStage()
%create ss from tf MIMO: %create ss from tf MIMO:
%https://ch.mathworks.com/matlabcentral/answers/37152-how-to-convert-tf2ss-for-mimo-system %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); numc=myNorm(mot.mdl.numc);
denc=myNorm(mot.mdl.denc); denc=myNorm(mot.mdl.denc);
num1=myNorm(mot.mdl.num1); num1=myNorm(mot.mdl.num1);
@@ -127,45 +146,36 @@ function [mot1,mot2]=identifyFxFyStage()
den5=myNorm(mot.mdl.den5); den5=myNorm(mot.mdl.den5);
num=myNorm(mot.mdl.num); num=myNorm(mot.mdl.num);
den=myNorm(mot.mdl.den); 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 g1=tf(numc,denc); % iqCmd->iqMeas
s1=ss(g1); 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; 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 g2=tf(num,den); %iqMeas->ActPos
s2=ss(g2); s2=ss(g2);
s3=append(s1,s2); s3=append(s1,s2);
tf(s3) %t_=tf(s3);
%bode(g2);figure;bode(t_(3,2));
%connect iqMeas from s1 to iqMeas of s2 %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)
t_(3,1)
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)
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',mot.tf13_9,'m',mot.tf_py,'b'); %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'); setoptions(h,'FreqUnits','Hz','Grid','on');
%controlSystemDesigner('bode',1,mot.tf_py); % <<<<<<<<< This opens a transferfiûnction that can be edited %controlSystemDesigner('bode',1,mot.tf_py); % <<<<<<<<< This opens a transferfiûnction that can be edited

BIN
matlab/observer.slx Normal file
View File

Binary file not shown.

43
matlab/rscale.m Normal file
View File

@@ -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;

54
matlab/simFxFyStage.m
View File

@@ -1,39 +1,41 @@
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 Ts=2E-4; % 0.2ms=5kHz
global A B C D MaxDac=2011.968;
function ServoDeltaTau_z(motid) MaxPosErr=10000;
Ts=2E-4; % 0.2ms=5kHz if motid==1
MaxDac=2011.968; %!motor_servo(mot=1,ctrl='ServoCtrl',Kp=25,Kvfb=400,Ki=0.02,Kvff=350,Kaff=5000,MaxInt=1000)
MaxPosErr=10000; %!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')
if motid==1 Kp=25;Kvfb=400;Ki=0.02;Kvff=350;Kaff=5000;MaxInt=1000;
%!motor_servo(mot=1,ctrl='ServoCtrl',Kp=25,Kvfb=400,Ki=0.02,Kvff=350,Kaff=5000,MaxInt=1000) mot_num=mot.tf_mdl.Numerator;
%!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') mot_den=mot.tf_mdl.Denominator;
Kp=25;Kvfb=400;Ki=0.02;Kvff=350;Kaff=5000;MaxInt=1000; else
mot_num=m1.tf_mdl.Numerator; %!motor_servo(mot=2,ctrl='ServoCtrl',Kp=22,Kvfb=350,Ki=0.02,Kvff=240,Kaff=1500,MaxInt=1000)
mot_den=m1.tf_mdl.Denominator; %!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')
else Kp=22;Kvfb=350;Ki=0.02;Kvff=240;Kaff=1500;MaxInt=1000;
%!motor_servo(mot=2,ctrl='ServoCtrl',Kp=22,Kvfb=350,Ki=0.02,Kvff=240,Kaff=1500,MaxInt=1000) mot_num=mot.tf_mdl.Numerator;
%!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') mot_den=mot.tf_mdl.Denominator;
Kp=22;Kvfb=350;Ki=0.02;Kvff=240;Kaff=1500;MaxInt=1000;
mot_num=m2.tf_mdl.Numerator;
mot_den=m2.tf_mdl.Denominator;
end
end end
mdlName='stage_closed_loop'; mdlName='stage_closed_loop';
open(mdlName) open(mdlName)
ServoDeltaTau_z(2) %ServoDeltaTau_z(motid)
[A,B,C,D]=tf2ss(mot_num,mot_den); [A,B,C,D]=tf2ss(mot_num,mot_den);
%mdlWks=get_param(mdlName,'ModelWorkspace') %mdlWks=get_param(mdlName,'ModelWorkspace');
%whos global %whos global
%whos(mdlWks) %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) %assignin(mdlWks,'Ts',1234)
%getVariable(mdlWks,'Ts') %getVariable(mdlWks,'Ts')
% in global space call: % in global space call:
%global Kp Kvfb Ki Kvff Kaff MaxInt %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 end

BIN
matlab/stage_closed_loop.slx
View File

Binary file not shown.

129
python/MXTuning.py
View File

@@ -76,6 +76,11 @@ class MXTuning(Tuning):
num1=np.poly1d([mag1]) num1=np.poly1d([mag1])
den1 = np.poly1d([T1**2,2*T1*d1,1]) 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 #first resonance frequency
f2=np.array([197,199]) f2=np.array([197,199])
d2=np.array([.02,.02])#daempfung d2=np.array([.02,.02])#daempfung
@@ -101,9 +106,9 @@ class MXTuning(Tuning):
num=num1*num2*numc#*num3 num=num1*num2*numc#*num3
den=den1*den2*denc#*den3 den=den1*den2*denc#*den3
mdl= signal.lti(num, den) #num denum 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, 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} 'den':den.coeffs,'den1':den1.coeffs,'den2':den2.coeffs,'denc':denc.coeffs}
fn=os.path.join(base,'model%d.mat'%mot) fn=os.path.join(base,'model%d.mat'%mot)
@@ -175,9 +180,9 @@ class MXTuning(Tuning):
num=num1*num2*num3*num4*num5*numc num=num1*num2*num3*num4*num5*numc
den=den1*den2*den3*den4*den5*denc den=den1*den2*den3*den4*den5*denc
mdl= signal.lti(num, den) #num denum 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, 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} '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) 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')) # 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): def bode(mdl):
w,mag,phase = signal.bode(mdl,1000) w,mag,phase = signal.bode(mdl,1000)
f=w/(2*np.pi) f=w/(2*np.pi)
@@ -214,6 +313,16 @@ def bode(mdl):
if __name__=='__main__': if __name__=='__main__':
from argparse import ArgumentParser,RawDescriptionHelpFormatter 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(): def parse_args():
'main command line interpreter function' 'main command line interpreter function'
#usage: gpasciiCommunicator.py --host=PPMACZT84 myPowerBRICK.cfg #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) tune.bode_sine(openloop=False, file=fn)
if os.path.basename(fn).startswith('chirp_cl_mot'): if os.path.basename(fn).startswith('chirp_cl_mot'):
tune.bode_chirp(openloop=False, file=fn) 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() plt.show()
#------------------ Main Code ---------------------------------- #------------------ Main Code ----------------------------------
#ssh_test() #ssh_test()

32
python/helicalscan.py
View File

@@ -151,19 +151,19 @@ class HelicalScan:
param = self.param param = self.param
cx, cz, w, fy, = (0.2,0.3,0.1,0.4) cx, cz, w, fy, = (0.2,0.3,0.1,0.4)
#cx, cz, w, fy, = (10.,20,3.,40) #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) (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) (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, = (0.2,0.3,0.1,0.4)
#dx, dz, w, y, = (10.,20,3.,40) #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) (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) (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,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,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) 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 ofs=(p[1]+p[0])/2. # = center of the cristal
m=Trf.trans(*ofs); self.hOrig=self.pltOrig(m) 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, 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, 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) 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 ofs = (p[1] + p[0]) / 2. # = center of the cristal
m = Trf.trans(cx,fy,cz) m = Trf.trans(cx,fy,cz)
@@ -375,7 +375,7 @@ class HelicalScan:
p[i,1]=y_i # y= y_i 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) 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 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) m=Trf.trans(0,0,0); self.hOrig=self.pltOrig(m)
hCrist,pt=self.pltCrist(cx=-ofs[0],fy=-ofs[1],cz=-ofs[2]) 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(): for k,v in fh.iteritems():
s+=' '+k+': '+str(v.dtype)+' '+str(v.shape)+'\n' s+=' '+k+': '+str(v.dtype)+' '+str(v.shape)+'\n'
setattr(self,k,v) setattr(self,k,v)
print s print(s)
def fwd_transform(self,cx,cz,w,fy): def fwd_transform(self,cx,cz,w,fy):
#cx,cy: coarse stage #cx,cy: coarse stage
@@ -530,7 +530,7 @@ class HelicalScan:
param[i, 1] = y param[i, 1] = y
param[i, 2:] = HelicalScan.meas_rot_ctr(x) # (bias,ampl,phase) param[i, 2:] = HelicalScan.meas_rot_ctr(x) # (bias,ampl,phase)
(bias, ampl, phase) = param[i][2:] (bias, ampl, phase) = param[i][2:]
print param print(param)
def calcParam(self,x=((-241.,96.,-53.),(-162.,-293.,246.)), def calcParam(self,x=((-241.,96.,-53.),(-162.,-293.,246.)),
@@ -569,15 +569,15 @@ class HelicalScan:
x_ = ampl * np.cos(w + phase) + bias x_ = ampl * np.cos(w + phase) + bias
print(x_) print(x_)
(dx,dz,w,y_) = (0,0,0,y[0]) (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_) (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]) (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_) (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): def pltOrig(self,m,h=None):

1
python/shapepath.py
View File

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

91
python/utilities.py Executable file
View File

@@ -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