222 lines
5.3 KiB
Matlab
222 lines
5.3 KiB
Matlab
|
|
https://de.wikipedia.org/wiki/Chirp
|
|
|
|
# amp, minFrq, maxFrq, tSec = (10, 10, 300, 30)
|
|
import numpy as np
|
|
import matplotlib as mpl
|
|
import matplotlib.pyplot as plt
|
|
x=np.arange(1,300000)
|
|
y=10*np.sin(31.415926535897931*( pow(1.058324104020218,(x*0.000199996614513)) -1) /np.log(1.058324104020218))
|
|
plt.show()
|
|
plt.plot(x,y)
|
|
plt.show()
|
|
|
|
y=amp*sin( a* (b**(x*c))-1) / log(d) )
|
|
=amp*sin( a/log(d)* (b**(x*c))-1) )
|
|
|
|
0.000199996614513=samplingrate in sec. (gatherPeriod==1?)
|
|
31.415926535897931 = pi*10 =? 2*pi*f0
|
|
1.058324104020218 = k
|
|
x*0.000199996614513 =t
|
|
1.058324104020218**60 = 30
|
|
|
|
sin( 2*pi*f0/ln(k)*(k**t-1) )
|
|
f(t)=f0*k**t = f0*e**(t*ln(k))
|
|
|
|
k**t=30 (from 10 to 300 -> 300/10=30)
|
|
|
|
log(k**t)=log(30)
|
|
log(k)*t=log(30)
|
|
log(k)=log(30)*(1/t)
|
|
log(k)=log(30**(1/t))
|
|
k=30**(1/t)
|
|
30**(1/60.) = 1.0583241040202176
|
|
|
|
-----------------------------------------------------------------
|
|
|
|
|
|
figure();
|
|
h=pzplot(ssc.ss_cl(3),ssc.ss_o(3),ssc.ss_od(3));
|
|
setoptions(h,'FreqUnits','Hz','Grid','on');legend('location','sw');
|
|
|
|
|
|
%figure();bode(ssc.ss_o(3));
|
|
%figure();tf=tf(ssc.ss_o) pzplot(tf(3));
|
|
|
|
|
|
h=bodeplot(ssc.ss_cl(3),ssc.ss_o(3),ssc.ss_od(3));
|
|
setoptions(h,'FreqUnits','Hz','Grid','on');
|
|
|
|
h=pzplot(ssc.ss_cl(3),ssc.ss_o(3),ssc.ss_od(3));
|
|
setoptions(h,'FreqUnits','Hz','Grid','on');
|
|
|
|
|
|
h=bodeplot(ssc.ss_cl(3),ssc.ss_o(3),ssc.ss_od(3));
|
|
setoptions(h,'FreqUnits','Hz','Grid','on');
|
|
getoptions(h)
|
|
|
|
|
|
|
|
|
|
continous to discrete
|
|
web(fullfile(docroot, 'control/ref/c2d.html'))
|
|
|
|
|
|
h = tf(10,[1 3 10],'IODelay',0.25);
|
|
hd = c2d(h,0.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
|
|
1
|
|
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
|
|
|
|
|
|
m1=10; d1=10; k1=0;
|
|
m2=.3; d2=0.15; k2=100;
|
|
m3=1.2; d3=.04; k3=10;
|
|
|
|
%k2 determines resonance frequency k2 higher -> resfrq higher
|
|
%d2 determines the damping d2=0 no damping d2=10 strong damping
|
|
%m2 determines how much energy is in the resonance
|
|
|
|
%m1 is big compared to the other masses
|
|
%d1 is the speed dependant friction
|
|
%k1 can be set to 0, because these is no position for no force
|
|
%-> but this means for 1/s^2 the system is not observable any more
|
|
|
|
ks=k1+k2+k3;
|
|
ds=d1+d2+d3;
|
|
|
|
A=[ 0 1 0 0 0 0 ;
|
|
-ks/m1 -ds/m1 k2/m1 d2/m1 k3/m1 d3/m1 ;
|
|
0 0 0 1 0 0 ;
|
|
k2/m2 d2/m2 -k2/m2 -d2/m2 0 0 ;
|
|
0 0 0 0 0 1 ;
|
|
k3/m1 d3/m1 0 0 -k3/m3 -d3/m3 ];
|
|
B=[ 0 1 0 0 0 0]';
|
|
C=[ 1 0 0 0 0 0];
|
|
D=[0];
|
|
|
|
ss1=ss(A,B,C,D);
|
|
bodeplot(ss1,{.1,1000});
|
|
tf(ss1)
|
|
|
|
|
|
%simplified no resonance
|
|
A=[ 0 1 ;
|
|
-k1/m1 -d1/m1 ];
|
|
B=[ 0 1 ]';
|
|
C=[ 1 0 ];
|
|
D=[0];
|
|
ss1=ss(A,B,C,D);
|
|
bodeplot(ss1,{.1,1000});
|
|
tf(ss1)
|
|
chkCtrlObsv(ss1,'')
|
|
%but with k1=0 or d1=0 the system is neither controllable nor observable
|
|
%-> the
|
|
p = eig(A);
|
|
disp(p);
|
|
|
|
K = place(A,B,[-5 -10]);
|
|
|