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

wip

Browse Source
This commit is contained in:
zamofing_t
2019-02-12 16:52:55 +01:00
parent 3412e33595
commit 5a05b7355e
8 changed files with 306 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files

201
python/MXTuning.py
View File

@@ -13,11 +13,12 @@ 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: plot the full bode recording
bit 5=32: plot the full bode recording with an approximation model
bit 6=64: plot all raw acquired data files
bit 7=128: custom plots
bit 8=256: generate observer code (after files generated with matlab)
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
@@ -281,10 +282,10 @@ class MXTuning(Tuning):
#[ssc]=StateSpaceControlDesign(mot1);
mat=scipy.io.loadmat(fn)
motid=int(re.search('(\d)\.mat',fn).group(1))
A=mat['Aoz']
B=mat['Boz']
C=mat['Coz']
D=mat['Doz']
A=mat['ozA']
B=mat['ozB']
C=mat['ozC']
D=mat['ozD']
V=mat['V']
u=('DesPos','IqMeas','IqVolts','ActPos')
y=('obsvOut',)
@@ -348,6 +349,136 @@ class MXTuning(Tuning):
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)
@@ -433,6 +564,31 @@ EXPORT_SYMBOL(obsvr_servo_ctrl_{motid});'''.format(motid=motid)
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:
@@ -508,21 +664,30 @@ EXPORT_SYMBOL(obsvr_servo_ctrl_{motid});'''.format(motid=motid)
plt.show(block=False)
save_figs(fn)
if mode&16: #plot the full bode recording
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&32: #plot the full bode recording with an approximation model
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&64: # plot all raw acquired data files
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')):
@@ -541,7 +706,7 @@ EXPORT_SYMBOL(obsvr_servo_ctrl_{motid});'''.format(motid=motid)
raw_input('press return')
if mode&128: #custom plots
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))
@@ -588,7 +753,7 @@ EXPORT_SYMBOL(obsvr_servo_ctrl_{motid});'''.format(motid=motid)
save_figs(os.path.join(self.baseDir,'iqCmd_TF'))
if mode&256: #generater code
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;
@@ -622,8 +787,10 @@ EXPORT_SYMBOL(obsvr_servo_ctrl_{motid});'''.format(motid=motid)
print(fnc+' generated.')
print('now compile it looking at PBTools/pbtools/usr_servo_phase/usrServoSample')
print('done')
plt.show(block=False)
raw_input('press return')
if mode&~512: # show and block only if not code generation
plt.show(block=False)
raw_input('press return')
def save_figs(fn):
@@ -701,7 +868,7 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
#plt.ion()
#args.host='MOTTEST-CPPM-CRM0573'
args.host=None
#args.host=None
if args.host is None:
comm=gt=None
else: