towards matlab
This commit is contained in:
123
matlab/SCRATCH.m
Normal file
123
matlab/SCRATCH.m
Normal 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
166
matlab/StateSpaceControlDesign.m
Normal file
@@ -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
|
||||
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
BIN
matlab/observer.slx
Normal file
BIN
matlab/observer.slx
Normal file
Binary file not shown.
43
matlab/rscale.m
Normal file
43
matlab/rscale.m
Normal 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;
|
||||
@@ -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
|
||||
|
||||
Binary file not shown.
Reference in New Issue
Block a user