diff --git a/matlab/SCRATCH.m b/matlab/SCRATCH.m new file mode 100644 index 0000000..6f11860 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/matlab/StateSpaceControlDesign.m b/matlab/StateSpaceControlDesign.m new file mode 100644 index 0000000..a4cc4ab --- /dev/null +++ b/matlab/StateSpaceControlDesign.m @@ -0,0 +1,166 @@ +%http://ctms.engin.umich.edu/CTMS/index.php?example=Introduction§ion=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 + + + diff --git a/matlab/identifyFxFyStage.m b/matlab/identifyFxFyStage.m index 55dc568..bfa22ce 100644 --- a/matlab/identifyFxFyStage.m +++ b/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 - g12=tf([1 0],mot.mdl.denc*12); % iqCmd->iqVolts : iqVolts= i_meas*R+i_meas'*L - num=conv(conv(mot.mdl.num1,mot.mdl.num2),mot.mdl.numc); - den=conv(conv(mot.mdl.den1,mot.mdl.den2),mot.mdl.denc); - g13=tf(num,den); %iqCmd->ActPos - %sys=ss([g11;g12]) - sys=ss([g11;g12;g13]) + numc=myNorm(mot.mdl.numc); + denc=myNorm(mot.mdl.denc); + num1=myNorm(mot.mdl.num1); + den1=myNorm(mot.mdl.den1); + num2=myNorm(mot.mdl.num2); + den2=myNorm(mot.mdl.den2); + 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]; - %sys=ss(Gspm); - - %normalize: [1E6 1E3 1].*mot.mdl.denc + %http://ch.mathworks.com/help/control/ug/conversion-between-model-types.html#f3-1039600 + %https://ch.mathworks.com/help/control/ref/append.html 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 - - num=conv(conv(conv(conv(num1,num2),num3),num4),num5); - den=conv(conv(conv(conv(den1,den2),den3),den4),den5); + %tf(s1) % display all transfer functions + num=conv(conv(conv(conv(num1,num2),num3),num4),num5);%num=1; + den=conv(conv(conv(conv(den1,den2),den3),den4),den5);%den=[1 0 0]; 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,:)? - %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); + 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.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 diff --git a/matlab/observer.slx b/matlab/observer.slx new file mode 100644 index 0000000..922620d Binary files /dev/null and b/matlab/observer.slx differ diff --git a/matlab/rscale.m b/matlab/rscale.m new file mode 100644 index 0000000..0ef5e32 --- /dev/null +++ b/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; diff --git a/matlab/simFxFyStage.m b/matlab/simFxFyStage.m index 1af6fd5..85e6f08 100644 --- a/matlab/simFxFyStage.m +++ b/matlab/simFxFyStage.m @@ -1,39 +1,41 @@ -function out=simFxFyStage() - global Kp Kvfb Ki Kvff Kaff MaxInt - 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; - if motid==1 - %!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_den=m1.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_den=m2.tf_mdl.Denominator; - end +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 mot_num mot_den Ts MaxDac MaxPosErr A B C D + Ts=2E-4; % 0.2ms=5kHz + MaxDac=2011.968; + MaxPosErr=10000; + if motid==1 + %!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=mot.tf_mdl.Numerator; + 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=mot.tf_mdl.Numerator; + mot_den=mot.tf_mdl.Denominator; 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 - %[m1,m2]=identifyFxFyStage(); + %global mot_num mot_den Ts MaxDac MaxPosErr + %[mot1,mot2]=identifyFxFyStage(); + %simFxFyStage(mot1,mot2); % end diff --git a/matlab/stage_closed_loop.slx b/matlab/stage_closed_loop.slx index e11bbe1..e594048 100644 Binary files a/matlab/stage_closed_loop.slx and b/matlab/stage_closed_loop.slx differ diff --git a/python/MXTuning.py b/python/MXTuning.py index 27c368b..c2984d3 100755 --- a/python/MXTuning.py +++ b/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 den - print mdl + print(num) + print(den) + 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 den - print mdl + print(num) + print(den) + 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() diff --git a/python/helicalscan.py b/python/helicalscan.py index 3e681e3..c1d5a7a 100755 --- a/python/helicalscan.py +++ b/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): diff --git a/python/shapepath.py b/python/shapepath.py index 1dfc3f7..5c624bb 100755 --- a/python/shapepath.py +++ b/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): diff --git a/python/utilities.py b/python/utilities.py new file mode 100755 index 0000000..77f83ea --- /dev/null +++ b/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