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

friction compensating user servo

Browse Source
This commit is contained in:
zamofing_t
2017-02-01 15:21:56 +01:00
parent c2910090f3
commit 3fafe53c39
14 changed files with 841 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files

137
python/move_record.py
View File

@@ -14,37 +14,9 @@ verbose bits:
4 upload progress
8 plot gather path
#config file example:
{
"points": [
[100,523],[635,632],[756,213],
"sequencer":[
'gen_grid_points(w=10,h=10,pitch=100,rnd=.2)',
'sort_points()',
'gen_prog(file="'+fn+'.prg")',
'plot_gather("'+fn+'.npz")']
}
Sequencer functions are:
- generate points (if not in the 'points' configuration)
gen_rand_points(self,n=107,scale=1000)
gen_grid_points(w=10,h=10,pitch=100,rnd=.2)
- sorting points:
sort_points(self)
- generate/download/execute motion progran, upload trace of motors (gather data)
gen_prog(self,prgId=2,file=None,host=None)
if host=None nothing will be downloaded/executed and trace of motors will not be uploaded
if file=None the program will not be saved and nothing will be executed
gen_prog modes:
-1 jog a 10mm square
0 linear motion
1 pvt motion
2 spline motion
- plot gathered data
plot_gather("'+fn+'.npz")
this makes only sence, if motion has been executed and data can be gathered from the powerbrick
prog_1(),plot_1(): position dependent friction(=current) Move multiple times with different speeds
generate friction lut and save to friction.mat
prog_2(),plot_2(): plot position speed acceleration. Move multiple times with different accelerations
Acquired time is:MaxSamples*Period*.2
'''
@@ -65,8 +37,8 @@ class MoveRecord:
self.fnPrg=fn+'.prg'
self.fnNpz=fn+'.npz'
#cfg = {"sequencer": ['prog_1(host="SAROP11-CPPM-MOT6871",acq_per=10)', 'plot_1()']}
cfg = {"sequencer": ['plot_1()']}
#cfg = {"sequencer": ['prog_2(host="SAROP11-CPPM-MOT6871",acq_per=10)', 'plot_2()']}
#cfg = {"sequencer": ['plot_1()']}
cfg = {"sequencer": ['prog_2(host="SAROP11-CPPM-MOT6871",acq_per=10)', 'plot_2()']}
#cfg = {"sequencer": ['plot_2()']}
self.cfg=dotdict(cfg)
self.args=args
@@ -158,19 +130,22 @@ class MoveRecord:
prg=self.prg
prg.append(' linear abs')
prg.append('jog3=1000')
prg.append('jog3=100')
prg.append('dwell 100')
prg.append('Gather.Enable=2')
#for spd in (5,10,20,30,40):
for spd in (20,20,20,20,20,20,20,20,20,20):
for spd in (10,10,10,10,20,20,20,20,20,20):
prg.append('Motor[3].JogSpeed=%d'%spd)
prg.append('jog3=27000')
#prg.append('jog3=27000')
prg.append('jog3=28500')
prg.append('dwell 100')
prg.append('jog3=1000')
prg.append('jog3=100')
prg.append('dwell 100')
prg.append('Gather.Enable=0')
meta['HomePos']=-4885.72 #set the value of Motor[3].HomePos
self.prg_epilog()
#!mx-stage()
# &1 #1->0
# &1 #2->0
# #3$
@@ -181,11 +156,6 @@ class MoveRecord:
# &1p
#folgende parameter setzen: Motor[3].HomePos
meta['HomePos']=-223.8 #Motor[3].HomePos
meta['PhasePos']=1200 #Motor[3].PhasePos
@staticmethod
def onclick(event):
print 'button=%s, x=%d, y=%d, xdata=%f, ydata=%f'%(
@@ -206,6 +176,11 @@ class MoveRecord:
channels=list(fh['channels'])
meta=fh['meta'].item()
actPos=rec[:,0]-meta['HomePos']
desPos=rec[:,1]-meta['HomePos']
desVel=np.diff(desPos)
time=np.arange(0,rec.shape[0])*meta['timebase']
fig=plt.figure();
@@ -213,8 +188,8 @@ class MoveRecord:
fig.canvas.set_window_title('position')
ax = fig.add_subplot(1,1,1)
hl=[]
hl+=ax.plot(time,rec[:,0],'r-',label=channels[0])
hl+=ax.plot(time,rec[:,1],'g-',label=channels[1])
hl+=ax.plot(time,actPos,'r-',label=channels[0])
hl+=ax.plot(time,desPos,'g-',label=channels[1])
hl+=ax.plot(time,rec[:,2],'b-',label=channels[2])
ax.xaxis.set_label_text('ms')
ax.yaxis.set_label_text('um')
@@ -237,14 +212,55 @@ class MoveRecord:
fig=plt.figure();
fig.obj=self; cid=fig.canvas.mpl_connect('button_press_event', self.onclick)
fig.canvas.set_window_title('overlay')
fig.canvas.set_window_title('friction(=current) at position current')
ax = fig.add_subplot(1,1,1)
hl=[]
sz=10; weights = np.repeat(1.0, sz) / sz
curr = np.convolve(rec[:,4], weights, 'same')
v = np.convolve(rec[:,4], weights, 'same')
hl+=ax.plot(rec[:,1],v,'r-',label=channels[4])
ax.xaxis.set_label_text(channels[1])
hl+=ax.plot(desPos,curr,'y-',label=channels[4])
sz=100; weights = np.repeat(1.0, sz) / sz
idx=desVel[:-1]>0
arg=np.argsort(desPos[idx])
p1=desPos[idx][arg];c1=curr[idx][arg]
c1 = np.convolve(c1, weights, 'same')
idx=desVel[:-1]<0
arg=np.argsort(desPos[idx])
p2=desPos[idx][arg];c2=curr[idx][arg]
c2 = np.convolve(c2, weights, 'same')
hl+=ax.plot(p1,c1,'g-',label=channels[4]+' vel>0')
hl+=ax.plot(p2,c2,'r-',label=channels[4]+' lut')
#hl+=ax.plot(p1,c1-c1.mean(),'g-',label=channels[4]+' vel>0')
#hl+=ax.plot(p2,c2-c2.mean(),'r-',label=channels[4]+' vel<0')
cAll=c1;pAll=p1
cAll=cAll-cAll.mean()
#p=p[::100];c=c[::100] # poor selection of points
p=np.arange(500,28000,100) #np.arange(500,28000,128)
c=np.interp(p,pAll,cAll)
hl+=ax.plot(p,c,'b.',label=channels[4]+' 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])
ax.yaxis.set_label_text('current in bits: '+channels[4])
legend = ax.legend(loc='upper right', shadow=True)
@@ -276,17 +292,20 @@ class MoveRecord:
prg.append(' linear abs')
prg.append('Motor[3].JogTs=0')
prg.append('Motor[3].JogSpeed=40')
prg.append('Motor[3].JogSpeed=10')
prg.append('Motor[3].JogTa=-2.5')
prg.append('jog3=10000')
prg.append('dwell 100')
prg.append('Gather.Enable=2')
for acc in (-2.5,-1.25,-.5,-.25):
prg.append('Motor[3].JogTa=%g'%acc)
for spd in (5,10,20,30,40):
prg.append('Motor[3].JogSpeed=%d'%spd)
#for acc in (-2.5,-1.25,-.5,-.25):
# prg.append('Motor[3].JogTa=%g'%acc)
prg.append('jog3=17000')
prg.append('dwell 100')
prg.append('jog3=10000')
prg.append('dwell 100')
prg.append('dwell 1000')
prg.append('Gather.Enable=0')
self.prg_epilog()
@@ -295,7 +314,7 @@ class MoveRecord:
# #3$
# an anschlag (kabel) bewegen
# #3hmz
# Motor[3].PhasePos=1200
# Motor[3].PhasePos=1200 # or for motor 2: Motor[2].PhasePos=300
# #3j/
# &1p
@@ -335,10 +354,10 @@ class MoveRecord:
p=rec[:,1]-p_avg
hl+=ax.plot(time,p,'g-',label=channels[1])
d=np.diff(p)*100
hl+=ax.plot(time[1:],d,'g-',label='vel of '+channels[0])
a=np.diff(d)*100
hl+=ax.plot(time[2:],a,'g-',label='acc of '+channels[0])
d=np.diff(p)*50
hl+=ax.plot(time[1:],d,'b-',label='vel of '+channels[0])
a=np.diff(d)*50
hl+=ax.plot(time[2:],a,'m-',label='acc of '+channels[0])
#hl+=ax.plot(time,rec[:,2],'b-',label=channels[2])
ax.xaxis.set_label_text('ms')
@@ -355,10 +374,14 @@ class MoveRecord:
hl+=ax.plot(time,v,'r-',label=channels[3])
v = np.convolve(rec[:, 4], weights, 'same')
hl+=ax.plot(time,v,'g-',label=channels[4])
hl+=ax.plot(time,p/100.,'y-',label=channels[1])
hl+=ax.plot(time,(rec[:,0]-p_avg)/100.,'g-',label=channels[0])
hl+=ax.plot(time,(rec[:,0]-rec[:,1]),'c-',label='PosError')
ax.xaxis.set_label_text('ms')
ax.yaxis.set_label_text('current in bits')
legend = ax.legend(loc='upper right', shadow=True)
print 'abs average ',channels[4], np.abs(rec[:, 4]).mean()
print 'abs average pos Error',np.abs(rec[:,0]-rec[:,1]).mean()
plt.show()