sf-mx/PBSwissMX
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
5a05b7355e27a74d374aa107ec6d06ef8a1429fa
PBSwissMX/python/MXTuning.py
Thierry Zamofing 5a05b7355e wip
2019-02-12 16:52:55 +01:00

899 lines
31 KiB
Python
Executable File
Raw Blame History

#!/usr/bin/env python
# *-----------------------------------------------------------------------*
# | |
# | Copyright (c) 2016 by Paul Scherrer Institute (http://www.psi.ch) |
# | |
# | Author Thierry Zamofing (thierry.zamofing@psi.ch) |
# *-----------------------------------------------------------------------*
'''
tuning functions for ESB-MX
Modes:
bit 0=1: record/plot current step
bit 1=2: custom chirp record/plot for IdCmd->ActPos transfer function
bit 2=4: custom chirp record/plot for DesPos->ActPos transfer function
bit 3=8: custom chirp record/plot for various transfer function
bit 4=16: slow linear move record/plot friction
bit 5=32: plot the full bode recording
bit 6=64: plot the full bode recording with an approximation model
bit 7=128: plot all raw acquired data files
bit 8=256: custom plots
bit 9=512: generate observer code (after files generated with matlab)
-> check https://github.com/klauer/ppmac for fast data gathering server which supports
phase gathering -> not yet compiling: /home/zamofing_t/Documents/prj/SwissFEL/PowerBrickInspector/ppmac/fast_gather
BUT data acquired and stored in: /media/zamofing_t/DataUbuHD/VirtualBox/shared/data
'''
import os, sys, json, time
import numpy as np
import matplotlib as mpl
#mpl.use('GTKAgg')
import matplotlib.pyplot as plt
from scipy import signal
sys.path.insert(0,os.path.expanduser('~/Documents/prj/SwissFEL/PBTools/'))
#import pbtools.misc.pp_comm as pp_comm -> pp_comm.PPComm
from pbtools.misc.pp_comm import PPComm,GpasciiChannel
from pbtools.misc.gather import Gather
from pbtools.misc.tuning import Tuning
# import sys, os
def wait_key():
''' Wait for a key press on the console and return it. '''
result = None
import termios
fd = sys.stdin.fileno()
oldterm = termios.tcgetattr(fd)
newattr = termios.tcgetattr(fd)
newattr[3] = newattr[3] & ~termios.ICANON & ~termios.ECHO
termios.tcsetattr(fd, termios.TCSANOW, newattr)
try:
result = sys.stdin.read(1)
except IOError:
pass
finally:
termios.tcsetattr(fd, termios.TCSAFLUSH, oldterm)
return result
class MXTuning(Tuning):
tuneDir='/opt/ppmac/tune/'
def __init__(self,comm,gather):
Tuning.__init__(self,comm,gather)
self.homed=False
def init_stage(self,mot=3,fn=''):
print(fn)
comm=self.comm
if comm is None or (self.homed&mot)==mot or os.path.isfile(fn):
return
gpascii=comm.gpascii
sys.stdout.write('homing stage');sys.stdout.flush()
gpascii.send_line('enable plc1')
time.sleep(.2)
while True:
act=gpascii.get_variable('Plc[1].Active',type_=int)
if act==0:
sys.stdout.write('\n')
break
sys.stdout.write('.');sys.stdout.flush()
time.sleep(.2)
self.homed=3
def bode_model_plot(self, mot):
self.bode_full_plot(mot,self.baseDir)
fig=plt.gcf()
_N=1.#E-3 # normalization factor: -> moves 3 decades to right but has factors around 1
# s -> ms
# Hz -> kHz
# rad/s -> rad/ms
if mot==1:
#identify matlab: k:0.671226 w0:134.705 damp:0.191004
mag1=6 #10**(db_mag1/20)
db_mag1=20*np.log10(mag1)#dB
w1=50.*_N #rad/sec
f1=w1/2/np.pi; # ca. 6.5Hz
T1=1/w1
d1=0.6 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
num1=np.poly1d([mag1])
den1 = np.poly1d([T1**2,2*T1*d1,1])
#reiner integrator: 19.8Hz=0dB -> k=30*2*pi=180
num0=np.poly1d([(19.8*2*np.pi)**2])
den0=np.poly1d([1,0,0])
#first resonance frequency
f2=np.array([197,199])
d2=np.array([.02,.02])#daempfung
w2=f2*2*np.pi*_N #rad/sec
T2=1/w2
num2 = np.poly1d([T2[0]**2,2*T2[0]*d2[0],1])
den2 = np.poly1d([T2[1]**2,2*T2[1]*d2[1],1])
#current loop 2nd order approx
#identification with matlab: k=1, w0=8725, d=0.75
dc=.75 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
wc=8725.*_N # rad/sec
#...but phase lag seems to have earlier effect -> reduce wc
wc*=.5 # rad/sec
fc=wc/2/np.pi # ca 1388Hz
Tc=1/wc
numc = np.poly1d([1.])
denc = np.poly1d([Tc**2,2*Tc*dc,1])
#simplified current loop 1st order approx
wd=2*np.pi*400. #45deg at 360Hz
Td=1/wd
numd = np.poly1d([1.])
dend = np.poly1d([Td,1])
num=num1*num2*numc#*num3
den=den1*den2*denc#*den3
mdl= signal.lti(num, den) #num denum
#print(num);print(den);print(mdl)
w=np.logspace(0, np.log10(2000), 1000)*2*np.pi
mdl1c= signal.lti(num1*numc, den1*denc)
mdl1d= signal.lti(num1*numd, den1*dend)
mdl1= signal.lti(num1, den1)
mdl0= signal.lti(num0, den0)
bode(((mdl,'plant'),
(mdl1c,'mdl1c'),
(mdl1d,'mdl1d'),
(mdl1,'mdl1'),
(mdl0,'mdl0'),
),w)
d={'num':num.coeffs,'num1':num1.coeffs,'num2':num2.coeffs,'numc':numc.coeffs,'num0':num0.coeffs,'numd':numd.coeffs,
'den':den.coeffs,'den1':den1.coeffs,'den2':den2.coeffs,'denc':denc.coeffs,'den0':den0.coeffs,'dend':dend.coeffs}
fn=os.path.join(self.baseDir,'model%d.mat'%mot)
import scipy.io
scipy.io.savemat(fn, mdict=d)
print('save to matlab file:'+fn)
elif mot==2:
#identify matlab: k:1.7282 w0:51.069 damp:0.327613
mag1=12. #10**(db_mag1/20)
db_mag1=20*np.log10(mag1)#dB
w1=21.*_N #rad/sec
f1=w1/2/np.pi; # ca. 6.5Hz
T1=1/w1
d1=0.4 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
num1=np.poly1d([mag1])
den1 = np.poly1d([T1**2,2*T1*d1,1])
#reiner integrator: 11.84Hz=0dB -> k=30*2*pi=180
num0=np.poly1d([(11.84*2*np.pi)**2])
den0=np.poly1d([1,0,0])
#resonance frequency
f2=np.array([55.,61.])
d2=np.array([.2,.2])#daempfung
w2=f2*2*np.pi #rad/sec
T2=1/w2
num2 = np.poly1d([T2[0]**2,2*T2[0]*d2[0],1])
den2 = np.poly1d([T2[1]**2,2*T2[1]*d2[1],1])
#resonance frequency
f3=np.array([128,137])
d3=np.array([.05,.05])#daempfung
w3=f3*2*np.pi #rad/sec
T3=1/w3
num3 = np.poly1d([T3[0]**2,2*T3[0]*d3[0],1])
den3 = np.poly1d([T3[1]**2,2*T3[1]*d3[1],1])
#resonance frequency
f4=np.array([410,417])
d4=np.array([.015,.015])#daempfung
w4=f4*2*np.pi #rad/sec
T4=1/w4
num4 = np.poly1d([T4[0]**2,2*T4[0]*d4[0],1])
den4 = np.poly1d([T4[1]**2,2*T4[1]*d4[1],1])
#resonance frequency
f5=np.array([230,233])
d5=np.array([.04,.04])#daempfung
w5=f5*2*np.pi #rad/sec
T5=1/w5
num5 = np.poly1d([T5[0]**2,2*T5[0]*d5[0],1])
den5 = np.poly1d([T5[1]**2,2*T5[1]*d5[1],1])
#current loop 2nd order approx
#identification with matlab: k=1, w0=8725, d=0.75
dc=.75 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
wc=8725.*_N # rad/sec
#...but phase lag seems to have earlier effect -> reduce wc
wc*=.5 # rad/sec
fc=wc/2/np.pi # ca 1388Hz
Tc=1/wc
numc = np.poly1d([1.])
denc = np.poly1d([Tc**2,2*Tc*dc,1])
#simplified current loop 1st order approx
wd=2*np.pi*400. #45deg at 360Hz
Td=1/wd
numd = np.poly1d([1.])
dend = np.poly1d([Td,1])
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)
w=np.logspace(0, np.log10(2000), 1000)*2*np.pi
mdl1c= signal.lti(num1*numc, den1*denc)
mdl1d= signal.lti(num1*numd, den1*dend)
mdl1= signal.lti(num1, den1)
mdl0= signal.lti(num0, den0)
bode(((mdl,'plant'),
(mdl1c,'mdl1c'),
(mdl1d,'mdl1d'),
(mdl1,'mdl1'),
(mdl0,'mdl0'),
),w)
d={'num':num.coeffs,'num1':num1.coeffs,'num2':num2.coeffs,'num3':num3.coeffs,'num4':num4.coeffs,'num5':num5.coeffs,'numc':numc.coeffs,'num0':num0.coeffs,'numd':numd.coeffs,
'den':den.coeffs,'den1':den1.coeffs,'den2':den2.coeffs,'den3':den3.coeffs,'den4':den4.coeffs,'den5':den5.coeffs,'denc':denc.coeffs,'den0':den0.coeffs,'dend':dend.coeffs}
fn=os.path.join(self.baseDir,'model%d.mat'%mot)
import scipy.io
scipy.io.savemat(fn, mdict=d)
print('save to matlab file:'+fn)
#bode(mdl)
w=np.logspace(0,np.log10(2000),1000)*2*np.pi
w,mag,phase = signal.bode(mdl,w)
f=w/(2*np.pi)
ax=fig.axes[0]
ax.semilogx(f, mag,'-k',lw=1) # Bode magnitude plot
ax=fig.axes[1]
ax.semilogx(f, phase,'-k',lw=1) # Bode phase plot
# tp print see also: print(np.poly1d([1,2,3], variable='s')), print(np.poly1d([1,2,3], r=True, variable='s'))
def usr_servo_gen_code(self,fn='/tmp/ssc1.mat'):
import scipy.io,re
#the file ssc[1|2].mat has been generated with matlab:
#[mot1, mot2]=identifyFxFyStage();
#[pb]=simFxFyStage(mot1);
#[ssc]=StateSpaceControlDesign(mot1);
mat=scipy.io.loadmat(fn)
motid=int(re.search('(\d)\.mat',fn).group(1))
A=mat['ozA']
B=mat['ozB']
C=mat['ozC']
D=mat['ozD']
V=mat['V']
u=('DesPos','IqMeas','IqVolts','ActPos')
y=('obsvOut',)
progSample='''double usr_servo_ctrl_{motid}(MotorData *Mptr)
{{
pshm->P[2000]=pshm->P[2000]*.9999+abs(Mptr->PosError)*0.0001; //lowpass of Position error
return pshm->ServoCtrl(Mptr);
}}'''.format(motid=motid)
prog='''double obsvr_servo_ctrl_{motid}(MotorData *Mptr)
{{
//x[n+1]=A*x[n]+B*u
//y=C*x[n]+D*x[n]
//u=[{u}].T
double x[{A.shape[0]}]; //new state
static double _x[{A.shape[0]}]={{0,0,0,0}}; //old state
//double {u}; // input values
double {y},iqCmd; // output values
double maxDac=Mptr->MaxDac;
'''.format(motid=motid,A=A,xInit=','.join('0'*A.shape[0]),u=', '.join(u),y=', '.join(y))
s=' //input values\n'
for i in range(len(u)):
s+=' double {u}=Mptr->{u};\n'.format(u=u[i])
prog+=s+'''
if (Mptr->ClosedLoop)
{
'''
s=' //x[n+1]=A*x[n]+B*u;\n'
for i in range(A.shape[0]):
s+=' x[%d]='%i
for j in range(A.shape[1]):
s+='%+28.22g*_x[%d]'%(A[i,j],j)
for j in range(B.shape[0]):
s+='%+28.22g*%s'%(B[i,j],u[j])
s+=';\n'
prog+=s+'\n'
s=' //y=C*x[n]+D*x[n];\n'
for i in range(C.shape[0]):
s+=' %s='%y[i]
for j in range(C.shape[1]):
s+='%+28.22g*_x[%d]'%(C[i,j],j)
s+=';\n'
prog+=s+'\n'
prog+=''' iqCmd=DesPos*{V}-{y};
//return iqCmd;
pshm->P[200{motid}]=iqCmd; //lowpass of Position error
return pshm->ServoCtrl(Mptr);
}}
else
{{
Mptr->Servo.Integrator=0.0;
return 0.0;
}}
}}'''.format(V=V[0,0],y=y[0],motid=motid)
hdr='''double obsvr_servo_ctrl_{motid}(MotorData *Mptr);
EXPORT_SYMBOL(obsvr_servo_ctrl_{motid});'''.format(motid=motid)
return (hdr,prog)
def friction(self, motor=1, pMin=-10000, pMax=10000, speed=(5,10,20,30,60), cnt=2, period=10, phase=False,
file='/tmp/gather.npz'):
if os.path.isfile(file):
f=np.load(file)
data=f['data']
meta=f['meta'].item()
meta['file']=file
else:
gpascii=self.comm.gpascii
gt=self.gather
gt.set_phasemode(phase)
m='Motor[%d].'%motor
address=('ActPos','IqMeas')
address=tuple(map(lambda x: m+x, address))
gt.set_address(*address)
# gt.set_property(MaxSamples=300000, Period=1)
gt.set_property(Period=period)
tSrv=gpascii.servo_period
tSrv=2E-4 # seconds
phOsv=gpascii.get_variable('sys.PhaseOverServoPeriod', float)
homePos=gpascii.get_variable('Motor[%d].HomePos'%motor, float)
JogTa=gpascii.get_variable('Motor[%d].JogTa'%motor, float)
gpascii.set_variable('Motor[%d].JogTa'%motor, -5)
subs={'prgId':999,'mot':motor,'num':100,'phase':'Phase' if phase else '','pMin':pMin,'pMax':pMax}
prog = ['''
&1
open prog {prgId}
linear abs
jog{mot}={pMin}
dwell 100
Gather.{phase}Enable=2
'''.format(**subs),]
for spd in sorted(speed*cnt):
prog.append('''
Motor[{mot}].JogSpeed=%d
jog{mot}={pMax}
dwell 0
jog{mot}={pMin}
dwell 0'''.format(**subs)%spd)
prog.append('''
dwell 10
Gather.{phase}Enable=0
close
&1
b{prgId}r
'''.format(**subs))
gpascii.send_block('\n'.join(prog))
res=gpascii.sync()
res=res.replace(GpasciiChannel.ACK+'\r\n','')
print(res)
t=time.time()
gt.wait_stopped()
print('time %f'%(time.time()-t))
self.data=data=gt.upload()
gpascii.set_variable('Motor[%d].JogTa'%motor, JogTa)
data[:,0]-=homePos
meta={'motor':motor,'date':time.asctime(),'ts':tSrv*period if not phase else tSrv*phOsv*period,'address':address}
np.savez_compressed(file, data=data, meta=meta)
meta['file'] = file
ts=meta['ts']
t=ts*np.arange(data.shape[0])
fig, ax1 = plt.subplots()
ax2=ax1.twinx()
ax3=ax1.twinx()
ax3.spines["right"].set_position(("axes", 1.12))
fig.subplots_adjust(right=0.80)
actPos=data[:,0]
iqMeas=data[:,1]
actVel=np.diff(actPos)/ts/1000.
actVel=np.hstack((actVel, actVel[-1]))
h1,=ax1.plot(t,actPos,'b',label="actPos (um)")
h2,=ax2.plot(t,iqMeas,'g',label="IqMeas (curr_bit)")
h3,=ax3.plot(t,actVel,'r',label="actVel (mm/s)")
ax1.yaxis.label.set_color(h1.get_color())
ax2.yaxis.label.set_color(h2.get_color())
ax3.yaxis.label.set_color(h3.get_color())
ax1.tick_params(axis='y', colors=h1.get_color())
ax2.tick_params(axis='y', colors=h2.get_color())
ax3.tick_params(axis='y', colors=h3.get_color())
hl=[h1,h2,h3]
ax1.legend(hl, [h.get_label() for h in hl],loc='best')
sz=100; weights = np.repeat(1.0, sz) / sz
idx=actVel[:-1]>0
arg=np.argsort(actPos[idx])
p1=actPos[idx][arg];c1=iqMeas[idx][arg]
c1 = np.convolve(c1, weights, 'same')
idx=actVel[:-1]<0
arg=np.argsort(actPos[idx])
p2=actPos[idx][arg];c2=iqMeas[idx][arg]
c2 = np.convolve(c2, weights, 'same')
fig, ax1 = plt.subplots()
hl=[]
hl+=ax1 .plot(actPos,iqMeas,'y-',label='raw')
hl+=ax1 .plot(p1,c1,'g-',label='vel>0')
hl+=ax1 .plot(p2,c2,'r-',label='vel<0')
#cAll=c1;pAll=p1
#cAll=cAll-cAll.mean()
#p=np.arange(500,28000,100) #np.arange(500,28000,128)
#c=np.interp(p,pAll,cAll)
#hl+=ax1.plot(p,c,'b.',label='vel<0')
#FfricLut=np.array([p,c]).T
Ffric=np.array([c1.mean(),c2.mean()])
print 'Avg current forward:',Ffric[0],'Avg current backward:',Ffric[1]
#print 'FfricLut',FfricLut
#print '//positions '+'%g,%g,%g'%tuple(p[0:3]),'...%g,%g,%g'%tuple(p[-3:])
#print 'float lutCur[%d]={'%len(p)
#for i in range(len(p)):
# print '%g,'%(c[i]),
#print '};'
import scipy.io
#fn='/home/zamofing_t/afs/ESB-MX/data/friction.mat'
#scipy.io.savemat(fn,mdict={'Ffric':Ffric,'FfricLut':FfricLut})
#print '\n\nuncomment line above to saved to matlab file',fn
#ax.xaxis.set_label_text(channels[1])
#ax1.yaxis.set_label_text('current in bits: '+channels[4])
ax1.legend(loc='best')
plt.show(block=False)
def check_fast_stage(self,file='/tmp/gather.npz'):
if os.path.isfile(file):
f=np.load(file)
data=f['data']
meta=f['meta'].item()
meta['file']=file
else:
#self.init_stage();time sleep
phase=False
motor=2
gpascii = self.comm.gpascii
gt = self.gather
gt.set_phasemode(phase)
address=('Motor[2].ActPos','Motor[8].ActPos')
gt.set_address(*address)
gt.set_property(Period=1)
tSrv=gpascii.servo_period
tSrv=2E-4 # seconds
phOsv = gpascii.get_variable('sys.PhaseOverServoPeriod', float)
subs={'prgId':999,'mot':motor,'num':100,'phase':'Phase' if phase else ''}
#the servoloop is called 2 times per servo cycle ?!?
#don't know why, but this is the reason why the value L10 is incremented by 0.5
prog = '''
&1
open prog {prgId}
P1=Motor[{mot}].DesPos-Motor[{mot}].HomePos
Q2=10
Gather.{phase}Enable=2
Q1={num}
while (Q1>0)
{{
jog{mot}=(P1-1000)
dwell 10
jog{mot}=(P1)
dwell 10
Q1=Q1-1
}}
dwell 10
Gather.{phase}Enable=0
close
&1
b{prgId}r
'''.format(**subs)
gpascii.send_line(prog)
res=gpascii.sync()
res=res.replace(GpasciiChannel.ACK+'\r\n','')
print(res)
t=time.time()
gt.wait_stopped()
print('time %f'%(time.time()-t))
self.data=data=gt.upload()
meta={'motor':motor,'date':time.asctime(),'ts':tSrv if not phase else tSrv*phOsv,'address':address}
np.savez_compressed(file, data=data, meta=meta)
meta['file'] = file
tSrv=meta['ts']
t=tSrv*np.arange(data.shape[0])
data=data-data[0,:]
plt.plot(t,data[:,0],t,data[:,1])
plt.figure()
plt.plot(t,data[:,0]-data[:,1])
plt.show()
def run(self,mode):
#plt.ion()
self.homed=0
if mode&1: # full recording current step
plt.close('all')
for mot in (1, 2):
fn=os.path.join(self.baseDir, 'curr_step%d.npz' % mot)
self.init_stage(mot,fn)
plt.close('all')
self.bode_current(motor=mot, magMove=1000, magPhase=500, dwell=10, file=fn)
self.homed&=~mot
plt.show(block=False)
save_figs(fn)
f=np.load(fn)
fn=fn[:-3]+'mat'
import scipy.io
scipy.io.savemat(fn, mdict=f)
print('save to matlab file:'+fn)
fn=os.path.join(self.baseDir, 'curr_step_ol_%d.npz' % mot)
doSet=not os.path.isfile(fn)
if doSet:
gpascii = self.comm.gpascii
IpfGain=gpascii.get_variable('Motor[%d].IpfGain'%mot,float)
IpbGain=gpascii.get_variable('Motor[%d].IpbGain'%mot,float)
IiGain =gpascii.get_variable('Motor[%d].IiGain' %mot,float)
gpascii.set_variable('Motor[%d].IpfGain'%mot,1)
gpascii.set_variable('Motor[%d].IpbGain'%mot,-1)
gpascii.set_variable('Motor[%d].IiGain' %mot,0)
plt.close('all')
mag=3000
self.bode_current(motor=mot, magMove=mag, magPhase=500, dwell=10, file=fn)
if doSet:
gpascii = self.comm.gpascii
gpascii.set_variable('Motor[%d].IpfGain'%mot,IpfGain)
gpascii.set_variable('Motor[%d].IpbGain'%mot,IpbGain)
gpascii.set_variable('Motor[%d].IiGain' %mot,IiGain)
self.homed&=~mot
ax,=plt.figure(1).axes
ax.set_ylim(0,200)
ax.set_title('step of 0->3000 with current in open loop')
plt.show(block=False)
save_figs(fn)
if mode&2:
motLst = (1, 2) # (2,)#
#recType:
# IA 0 IqCmd,ActPos (for plant transfer function)
# DA 1 DesPos,ActPos (for regulation transfer function)
# all 2 DesPos,ActPos,IqCmd,IqMeas,IqVolts (for current, plant and regulation transfer function)
#>>>>> IqCmd->ActPos transfer function (plant) using closed loop for low frequencies
for mot in motLst:
fn = os.path.join(self.baseDir, 'chirp_IA_%da.npz' % mot)
self.init_stage(mot,fn)
plt.close('all')
self.custom_chirp(motor=mot, minFrq=1, maxFrq=15, amp=1000, tSec=15, recType=0,mode=1, file=fn)
self.homed &= ~mot
plt.show(block=False)
save_figs(fn)
#>>>>> IqCmd->ActPos transfer function (plant) using open loop for high frequencies
for ext,amp,minFrq,maxFrq,tSec in (('b', 10, 10, 100*1.5, 30),
('c', 50, 100, 300*1.5, 30),
('d', 50, 300, 1000*1.5, 10),
('e', 100, 1000, 2000, 10)):
for mot in motLst:
fn = os.path.join(self.baseDir, 'chirp_IA_%d%s.npz' % (mot,ext))
self.init_stage(mot,fn)
plt.close('all')
self.custom_chirp(motor=mot, minFrq=minFrq, maxFrq=maxFrq, amp=amp, tSec=tSec, recType=0,mode=0, file=fn)
self.homed&=~mot
plt.show(block=False)
save_figs(fn)
if mode&4:
plt.close('all')
motLst = (1, 2) #(2,)#
#>>>>> desPos->actPos transfer function (regulation) using closed loop
#motor1: 0dB at 20.4 Hz
#motor2: 0dB at 11.3 Hz
#1000um ampl. 15Hz ca. 2A current
#100um ampl. 15Hz ca. 200mA current
#100um at 11.3 Hz needs 100mA ->2A at sqrt(20)*11.3 =50Hz
#20um at 11.3 Hz needs 20mA ->2A at sqrt(100)*11.3 =113Hz
# 5um at 11.3 Hz needs 5mA ->2A at sqrt(2000/)*11.3 =226Hz
# 1um at 11.3 Hz needs 1mA ->2A at sqrt(2000)*11.3 =505Hz
#n times freq, -> n^2 current
for ext,amp,minFrq,maxFrq,tSec in (('a', 100, 1, 30*1.5, 10),
('b', 20, 30, 75*1.5, 15),
('c', 5, 75, 150*1.5, 5),
('d', 1, 150, 750, 5)):
for mot in motLst:
fn = os.path.join(self.baseDir, 'chirp_DA_%d%s.npz' % (mot,ext))
self.init_stage(mot,fn)
plt.close('all')
self.custom_chirp(motor=mot, minFrq=minFrq, maxFrq=maxFrq, amp=amp, tSec=tSec, recType=1,mode=1, file=fn)
self.homed&=~mot
plt.show(block=False)
save_figs(fn)
if mode & 8:
#>>>>> all data for different transfer function
#using closed loop with 1/w^2 lower amplitude
for ext,amp,minFrq,maxFrq,tSec,crpMd,mot in (('a', 800, 10, 250, 20, 2, 1),
('a', 800, 10, 250, 20, 2, 2),
('b', 5, 10, 220, 20, 1, 1),
('b', 5, 10, 220, 20, 1, 2),
):
fn = os.path.join(self.baseDir, 'chirp_all_%d%s.npz' % (mot,ext))
self.init_stage(mot,fn)
plt.close('all')
self.custom_chirp(motor=mot, minFrq=minFrq, maxFrq=maxFrq, amp=amp, tSec=tSec, recType=2,mode=crpMd, file=fn)
self.homed&=~mot
plt.show(block=False)
save_figs(fn)
if mode&16: #record/plot friction
for mot in (1, 2):
plt.close('all')
fn=os.path.join(self.baseDir, 'friction%d.npz' % mot)
self.init_stage(mot,fn)
self.friction(motor=mot, file=fn);
save_figs(fn)
if mode&32: #plot the full bode recording
plt.close('all')
self.bode_full_plot(mot=1,base=self.baseDir)
self.bode_full_plot(mot=2,base=self.baseDir)
plt.show(block=False)
save_figs(os.path.join(self.baseDir,'bode_full_plot'))
if mode&64: #plot the full bode recording with an approximation model
plt.close('all')
self.bode_model_plot(mot=1)
self.bode_model_plot(mot=2)
plt.show(block=False)
save_figs(os.path.join(self.baseDir,'bode_model_plot'))
if mode&128: # plot all raw acquired data files
plt.close('all')
import glob
for fn in glob.glob(os.path.join(self.baseDir,'*.npz')):
print(fn)
fh = np.load(fn)
meta = fh['meta'].item()
data = fh['data']
plt.close('all')
bm=25
if fn.find('all')>0:
bm+=32 #amplitude linear (not dB)
self.bode_plot(data, mode=bm, kwargs=meta)
plt.show(block=False)
save_figs(fn)
#wait_key()
raw_input('press return')
if mode&256: #custom plots
plt.close('all')
G1=signal.lti([1/8.8], [2.4E-3/8.8,1]) # num denum
print('rise time %e s'%(2.4E-3/8.8))
bode(G1)
t,y=G1.step()
fig=plt.figure();ax=fig.add_subplot(1,1,1)
ax.plot(t,y)
#current loop 2nd order approx
#identification with matlab: k=1, w0=8725, d=0.75
dc=.75 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
wc=8725. # rad/sec
#...but phase lag seems to have earlier effect -> reduce wc. Probably because of the slower outer servo loop
wc*=.5 # rad/sec
Tc=1/wc
num=np.poly1d([1.])
den=np.poly1d([Tc**2,2*Tc*dc,1])
mdl=signal.lti(num, den) # num denum
bode(mdl)
t,y=mdl.step()
ax.plot(t,y)
ax.grid(True)
plt.show(block=False)
raw_input('press return')
plt.close('all')
fn=os.path.join(self.baseDir,'chirp_all_1a.npz')
fh=np.load(fn)
data=fh['data']
meta=fh['meta'].item()
meta['file']=fn
# 2 DesPos,ActPos,IqCmd,IqMeas,IqVolts (for current, plant and regulation transfer function)
for xy in ((2, 3), (2, 4)):
self.bode_plot(data, xy=xy, mode=25+32, kwargs=meta)
fig=plt.figure(2)
fig.axes[0].set_ylim(.9, 1.1)
fig.axes[1].set_ylim(-40,10)
fig.suptitle('tf: iqCmd->iqMeas', fontsize=16)
fig=plt.figure(4)
fig.axes[0].set_ylim(15,30)
fig.axes[1].set_ylim(-20,20)
fig.suptitle('tf: iqCmd->iqVolts', fontsize=16)
plt.show(block=False)
save_figs(os.path.join(self.baseDir,'iqCmd_TF'))
if mode&512: #generater code
#before this can be done, the observer controller has to be designed with matlab:
#s.a.ESB_MX/matlab/Readme.md
#clear;
#clear global;
#close all;
#[mot1,mot2]=identifyFxFyStage();
#[pb]=simFxFyStage(mot1);
#[ssc]=StateSpaceControlDesign(mot1);
#[pb]=simFxFyStage(mot2);
#[ssc]=StateSpaceControlDesign(mot2);
#after this go to: python/usr_code and call make to build the controller
#to activate the controller checkout: PBTools/pbtools/usr_servo_phase
base=os.path.dirname(__file__)
(hdr1,prog1)=self.usr_servo_gen_code('/tmp/ssc1.mat')
(hdr2,prog2)=self.usr_servo_gen_code('/tmp/ssc2.mat')
fn_ct=os.path.join(base,'usr_code/usrcode_template.c')
fn_ht=os.path.join(base,'usr_code/usrcode_template.h')
fnc=os.path.join(base,'usr_code/usrcode.c')
fnh=os.path.join(base,'usr_code/usrcode.h')
s=open(fn_ht).read()
s=s.replace('<usr_header>',hdr1+'\n\n'+hdr2)
fh=open(fnh,'w')
fh.write(s)
fh.close()
print(fnh+' generated.')
s=open(fn_ct).read()
s=s.replace('<usr_code>',prog1+'\n\n'+prog2)
fh=open(fnc,'w')
fh.write(s)
fh.close()
print(fnc+' generated.')
print('now compile it looking at PBTools/pbtools/usr_servo_phase/usrServoSample')
print('done')
if mode&~512: # show and block only if not code generation
plt.show(block=False)
raw_input('press return')
def save_figs(fn):
figures = [manager.canvas.figure
for manager in mpl._pylab_helpers.Gcf.get_all_fig_managers()]
#print(figures)
fnBase=fn.rsplit('.')[0]
(a,b)=os.path.split(fnBase)
fnBase=os.path.join(a,'img',b)
for i, figure in enumerate(figures):
#figure.savefig('figure%d.png' % i)
fn=fnBase+'%d.png' % i
fn=fnBase+'%d.eps' % i
figure.savefig(fn)
#figure.savefig(fn.rsplit('.')[0]+'%d.eps' % i)
def bode(mdlLst,w=1000):
if not type(mdlLst) in (tuple,list):
mdlLst=((mdlLst,''),)
fig = plt.figure()
ax1 = fig.add_subplot(2, 1, 1)
ax2 = fig.add_subplot(2, 1, 2, sharex = ax1)
#f=w/(2*np.pi)
for mdl,lbl in mdlLst:
w_,mag,phase = signal.bode(mdl,w)
f_=w_/(2*np.pi)
ax1.semilogx(f_, mag, '-') # Bode magnitude plot
ax2.semilogx(f_,phase,'-',label=lbl) # Bode phase plot
ax1.yaxis.set_label_text('dB ampl')
ax2.yaxis.set_label_text('phase')
ax2.xaxis.set_label_text('frequency [Hz]')
ax1.grid(True)
ax2.grid(True)
if len(mdlLst)>3:
ax2.legend(loc='best',ncol=2,prop={'size': 10})
else:
ax2.legend(loc='best')
plt.show(block=False)
ax1.set_xlim(f_[0], f_[-1])
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
(h, t)=os.path.split(sys.argv[0]);cmd='\n '+(t if len(h)>3 else sys.argv[0])+' '
exampleCmd=('-n',
'-v15'
)
epilog=__doc__+'''
Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
parser=ArgumentParser(epilog=epilog,formatter_class=RawDescriptionHelpFormatter)
parser.add_argument('--host', help='hostname', metavar='HOST', default='SAR-CPPM-EXPMX1')
parser.add_argument('--mode', '-m', type=int, help='modes (see below)', default=1)
parser.add_argument('--dir', help='dir', default='MXTuning')
args=parser.parse_args()
#plt.ion()
#args.host='MOTTEST-CPPM-CRM0573'
#args.host=None
if args.host is None:
comm=gt=None
else:
comm = PPComm(host=args.host)
gt = Gather(comm)
tune=MXTuning(comm,gt)
tune.baseDir=args.dir
assert(os.path.exists(tune.baseDir))
tune.run(args.mode)
#------------------ Main Code ----------------------------------
#ssh_test()'/tmp/usrcode.c'
ret=parse_args()
exit(ret)
#enable plc1
#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 1 --mot 1 --dir tmp
#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 1 --mot 2 --dir tmp
#-> at low frequencied the speed is too high and encoder looses steps
#enable plc1
#AFTER each chirp measurement do enable plc1 again!
#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 2 --mot 1 --dir tmp
#./PBTuning.py --host SAR-CPPM-EXPMX1 --mode 2 --mot 2 --dir tmp
#./PBTuning.py --host SAR-CPPM-EXPMX1 --plot tmp
Reference in New Issue View Git Blame Copy Permalink