using new optimized control parameters
This commit is contained in:
@@ -594,7 +594,7 @@ EXPORT_SYMBOL(obsvr_servo_ctrl_{motid});'''.format(motid=motid)
|
||||
self.homed=0
|
||||
if mode&1: # full recording current step
|
||||
plt.close('all')
|
||||
for mot in (1, 2):
|
||||
for mot in (1, 2):#, 3):
|
||||
fn=os.path.join(self.baseDir, 'curr_step%d.npz' % mot)
|
||||
self.init_stage(mot,fn)
|
||||
plt.close('all')
|
||||
@@ -634,7 +634,7 @@ EXPORT_SYMBOL(obsvr_servo_ctrl_{motid});'''.format(motid=motid)
|
||||
save_figs(fn)
|
||||
|
||||
if mode&2:
|
||||
motLst = (1, 2) # (2,)#
|
||||
motLst = (1, 2) # (3,)#
|
||||
#recType:
|
||||
# IA 0 IqCmd,ActPos (for plant transfer function)
|
||||
# DA 1 DesPos,ActPos (for regulation transfer function)
|
||||
@@ -667,7 +667,7 @@ EXPORT_SYMBOL(obsvr_servo_ctrl_{motid});'''.format(motid=motid)
|
||||
|
||||
if mode&4:
|
||||
plt.close('all')
|
||||
motLst = (1, 2) #(2,)#
|
||||
motLst = (1, 2) #(3,)#
|
||||
#>>>>> desPos->actPos transfer function (regulation) using closed loop
|
||||
#motor1: 0dB at 20.4 Hz
|
||||
#motor2: 0dB at 11.3 Hz
|
||||
@@ -709,7 +709,7 @@ EXPORT_SYMBOL(obsvr_servo_ctrl_{motid});'''.format(motid=motid)
|
||||
save_figs(fn)
|
||||
|
||||
if mode&16: #record/plot friction
|
||||
for mot in (1, 2):
|
||||
for mot in (1, 2):#(3,)#
|
||||
plt.close('all')
|
||||
fn=os.path.join(self.baseDir, 'friction%d.npz' % mot)
|
||||
self.init_stage(mot,fn)
|
||||
@@ -721,6 +721,7 @@ EXPORT_SYMBOL(obsvr_servo_ctrl_{motid});'''.format(motid=motid)
|
||||
plt.close('all')
|
||||
self.bode_full_plot(mot=1,base=self.baseDir)
|
||||
self.bode_full_plot(mot=2,base=self.baseDir)
|
||||
#self.bode_full_plot(mot=3,base=self.baseDir)
|
||||
plt.show(block=False)
|
||||
save_figs(os.path.join(self.baseDir,'bode_full_plot'))
|
||||
|
||||
|
||||
@@ -1,489 +0,0 @@
|
||||
#!/usr/bin/env python
|
||||
# *-----------------------------------------------------------------------*
|
||||
# | |
|
||||
# | Copyright (c) 2016 by Paul Scherrer Institute (http://www.psi.ch) |
|
||||
# | |
|
||||
# | Author Thierry Zamofing (thierry.zamofing@psi.ch) |
|
||||
# *-----------------------------------------------------------------------*
|
||||
'''
|
||||
shape an optimal path with given points
|
||||
|
||||
verbose bits:
|
||||
1 basic info
|
||||
4 list program
|
||||
4 upload progress
|
||||
8 plot gather path
|
||||
|
||||
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
|
||||
'''
|
||||
|
||||
import os, sys, json
|
||||
import numpy as np
|
||||
import matplotlib as mpl
|
||||
import matplotlib.pyplot as plt
|
||||
import subprocess as sprc
|
||||
import telnetlib, re
|
||||
from utilities import *
|
||||
|
||||
os.environ['PATH']='/home/zamofing_t/scripts/:'+os.environ['PATH']
|
||||
|
||||
class MoveRecord:
|
||||
def __init__(self,args):
|
||||
fn='/tmp/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_2()']}
|
||||
cfg = {"sequencer": ['prog_3(host="SAROP11-CPPM-MOT6871",acq_per=10)', 'plot_2()']}
|
||||
self.cfg=dotdict(cfg)
|
||||
self.args=args
|
||||
|
||||
def run(self):
|
||||
print('args='+str(self.args))
|
||||
print('cfg='+str(self.cfg))
|
||||
try:
|
||||
sequencer= self.cfg.pop('sequencer')
|
||||
except KeyError:
|
||||
print('no command sequence to execute')
|
||||
else:
|
||||
for cmd in sequencer:
|
||||
print '>'*5+' '+cmd+' '+'<'*5
|
||||
eval('self.' + cmd)
|
||||
|
||||
def prg_prolog(self):
|
||||
prg=self.prg
|
||||
gather=self.gather
|
||||
channels=self.channels
|
||||
prg.append('Gather.Enable=0')
|
||||
prg.append('Gather.Items=%d'%len(channels))
|
||||
for k,v in gather.iteritems():
|
||||
prg.append('Gather.%s=%d'%(k,v))
|
||||
for i,c in enumerate(channels):
|
||||
prg.append('Gather.Addr[%d]=%s.a'%(i,c))
|
||||
prg.append('open prog %d'%(self.prgId))
|
||||
|
||||
def prg_epilog(self):
|
||||
fnPrg=self.fnPrg
|
||||
fnNpz=self.fnNpz
|
||||
host=self.host
|
||||
prg=self.prg
|
||||
|
||||
prg.append('close')
|
||||
prg.append('&1\nb%dr\n'%self.prgId)
|
||||
if self.args.verbose & 4:
|
||||
for ln in prg:
|
||||
print(ln)
|
||||
|
||||
if fnPrg is not None:
|
||||
fh=open(fnPrg,'w')
|
||||
fh.write('\n'.join(prg))
|
||||
fh.close()
|
||||
if host is not None:
|
||||
cmd ='gpasciiCommander --host '+host+' '+ fnPrg
|
||||
print cmd
|
||||
p = sprc.Popen(cmd, shell=True)#, stdout=sprc.PIPE, stderr=sprc.STDOUT)
|
||||
#res=p.stdout.readlines(); print res
|
||||
retval = p.wait()
|
||||
#gather -u /var/ftp/gather/out.txt
|
||||
cmd ='PBGatherPlot -m24 -v7 --host '+host
|
||||
print cmd
|
||||
p = sprc.Popen(cmd, shell=True)#, stdout=sprc.PIPE, stderr=sprc.STDOUT)
|
||||
retval = p.wait()
|
||||
|
||||
meta = self.meta
|
||||
channels = self.channels
|
||||
fnLoc='/tmp/gather.txt'
|
||||
self.rec=rec=np.genfromtxt(fnLoc, delimiter=' ')
|
||||
# rec=Motor[1].ActPos,Motor[2].ActPos,Motor[3].ActPos,Motor[1].DesPos,Motor[2].DesPos,Motor[3].DesPos
|
||||
# res=rot.ActPos,y.ActPos,x.ActPos,rot.DesPos,y.DesPos,x.DesPos
|
||||
# idx 0 1 2 3 4 5
|
||||
if fnNpz:
|
||||
np.savez_compressed(fnNpz, rec=rec, prg=self.prg, channels=self.channels, meta=meta)
|
||||
|
||||
def prog_1(self,prgId=2,host=None,acq_per=1):
|
||||
'''
|
||||
kwargs:
|
||||
acq_per : acquire period: acquire data all acq_per servo loops (default=1)
|
||||
prgId : used program number
|
||||
file : used filename to save if None, not saved
|
||||
host : host to send and execute program, if None, nothing is executed
|
||||
'''
|
||||
ServoPeriod= .2 #0.2ms Sys.ServoPeriod is dependent of !common() macro
|
||||
|
||||
self.prgId=prgId
|
||||
self.file=file
|
||||
self.host=host
|
||||
self.gather={"MaxSamples":1000000, "Period":acq_per}
|
||||
self.channels=channels=["Motor[3].ActPos","Motor[3].DesPos","Motor[3].PhasePos","Motor[3].idMeas","Motor[3].iqMeas"]
|
||||
self.meta=meta = {'timebase': ServoPeriod*self.gather['Period']}
|
||||
|
||||
self.prg=prg=[]
|
||||
#prg.append('#1..3$')
|
||||
#prg.append('#1..3j/')
|
||||
|
||||
self.prg_prolog()
|
||||
prg=self.prg
|
||||
|
||||
prg.append(' linear abs')
|
||||
prg.append('jog3=300')
|
||||
prg.append('dwell 100')
|
||||
prg.append('Gather.Enable=2')
|
||||
#for spd in (5,10,20,30,40):
|
||||
#for spd in (10,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=28500')
|
||||
prg.append('dwell 100')
|
||||
prg.append('jog3=300')
|
||||
prg.append('dwell 100')
|
||||
prg.append('Gather.Enable=0')
|
||||
|
||||
self.prg_epilog()
|
||||
rec=self.rec
|
||||
meta['HomePos']=rec[0,1]-300 #rec[0,1]= first DesPos, 300 um is the start position of the motion
|
||||
print meta
|
||||
if self.fnNpz:
|
||||
np.savez_compressed(self.fnNpz, rec=rec, prg=prg, channels=channels, meta=meta)
|
||||
|
||||
#!mx-stage()
|
||||
# &1 #1->0
|
||||
# &1 #2->0
|
||||
# #3$
|
||||
# an anschlag (kabel) bewegen
|
||||
# #3hmz
|
||||
# Motor[3].PhasePos=1200
|
||||
# #3j/
|
||||
# &1p
|
||||
|
||||
|
||||
@staticmethod
|
||||
def onclick(event):
|
||||
print 'button=%s, x=%d, y=%d, xdata=%f, ydata=%f'%(
|
||||
event.button, event.x, event.y, event.xdata, event.ydata)
|
||||
obj=event.canvas.figure.obj
|
||||
|
||||
|
||||
def plot_1(self):
|
||||
try:
|
||||
rec=self.rec
|
||||
prg=self.prg
|
||||
channels=self.channels
|
||||
meta=self.meta
|
||||
except AttributeError:
|
||||
print 'load file %s'%self.fnNpz
|
||||
fh=np.load(self.fnNpz)
|
||||
rec=fh['rec']
|
||||
prg=list(fh['prg'])
|
||||
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();
|
||||
fig.obj=self; cid=fig.canvas.mpl_connect('button_press_event', self.onclick)
|
||||
fig.canvas.set_window_title('position')
|
||||
ax = fig.add_subplot(1,1,1)
|
||||
hl=[]
|
||||
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')
|
||||
legend = ax.legend(loc='upper right', shadow=True)
|
||||
|
||||
fig=plt.figure();
|
||||
fig.obj=self; cid=fig.canvas.mpl_connect('button_press_event', self.onclick)
|
||||
fig.canvas.set_window_title('current')
|
||||
ax = fig.add_subplot(1,1,1)
|
||||
hl=[]
|
||||
sz=100; weights = np.repeat(1.0, sz) / sz
|
||||
v = np.convolve(rec[:,3], weights, 'same')
|
||||
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])
|
||||
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()
|
||||
|
||||
fig=plt.figure();
|
||||
fig.obj=self; cid=fig.canvas.mpl_connect('button_press_event', self.onclick)
|
||||
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')
|
||||
|
||||
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)
|
||||
|
||||
plt.show()
|
||||
|
||||
def prog_2(self,prgId=2,host=None,acq_per=1):
|
||||
'''
|
||||
kwargs:
|
||||
acq_per : acquire period: acquire data all acq_per servo loops (default=1)
|
||||
prgId : used program number
|
||||
file : used filename to save if None, not saved
|
||||
host : host to send and execute program, if None, nothing is executed
|
||||
'''
|
||||
ServoPeriod= .2 #0.2ms Sys.ServoPeriod is dependent of !common() macro
|
||||
|
||||
self.prgId=prgId
|
||||
self.file=file
|
||||
self.host=host
|
||||
self.gather={"MaxSamples":1000000, "Period":acq_per}
|
||||
self.channels=["Motor[3].ActPos","Motor[3].DesPos","Motor[3].PhasePos","Motor[3].idMeas","Motor[3].iqMeas"]
|
||||
self.meta=meta = {'timebase': ServoPeriod*self.gather['Period']}
|
||||
|
||||
self.prg=prg=[]
|
||||
#prg.append('#1..3$')
|
||||
#prg.append('#1..3j/')
|
||||
|
||||
self.prg_prolog()
|
||||
prg=self.prg
|
||||
|
||||
prg.append(' linear abs')
|
||||
prg.append('Motor[3].JogTs=0')
|
||||
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 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 1000')
|
||||
prg.append('Gather.Enable=0')
|
||||
|
||||
self.prg_epilog()
|
||||
# &1 #1->0
|
||||
# &1 #2->0
|
||||
# #3$
|
||||
# an anschlag (kabel) bewegen
|
||||
# #3hmz
|
||||
# Motor[3].PhasePos=1200 # or for motor 2: Motor[2].PhasePos=300
|
||||
# #3j/
|
||||
# &1p
|
||||
|
||||
|
||||
#folgende parameter setzen: Motor[3].HomePos
|
||||
meta['HomePos']=-223.8 #Motor[3].HomePos
|
||||
meta['PhasePos']=1200 #Motor[3].PhasePos
|
||||
|
||||
|
||||
def plot_2(self):
|
||||
try:
|
||||
rec=self.rec
|
||||
prg=self.prg
|
||||
channels=self.channels
|
||||
meta=self.meta
|
||||
except AttributeError:
|
||||
fh=np.load(self.fnNpz)
|
||||
rec=fh['rec']
|
||||
prg=list(fh['prg'])
|
||||
channels=list(fh['channels'])
|
||||
meta=fh['meta'].item()
|
||||
|
||||
time=np.arange(0,rec.shape[0])*meta['timebase']
|
||||
|
||||
fig=plt.figure();
|
||||
fig.obj=self; cid=fig.canvas.mpl_connect('button_press_event', self.onclick)
|
||||
fig.canvas.set_window_title('position')
|
||||
ax = fig.add_subplot(1,1,1)
|
||||
hl=[]
|
||||
p_avg=rec[:,1].mean();
|
||||
p=rec[:,0]-p_avg
|
||||
hl+=ax.plot(time,p,'r-',label=channels[0])
|
||||
#d=np.diff(p)*100
|
||||
#hl+=ax.plot(time[1:],d,'r-',label='vel of '+channels[0])
|
||||
#a=np.diff(d)*100
|
||||
#hl+=ax.plot(time[2:],a,'r-',label='acc of '+channels[0])
|
||||
|
||||
p=rec[:,1]-p_avg
|
||||
hl+=ax.plot(time,p,'g-',label=channels[1])
|
||||
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')
|
||||
ax.yaxis.set_label_text('um')
|
||||
legend = ax.legend(loc='upper right', shadow=True)
|
||||
|
||||
fig=plt.figure();
|
||||
fig.obj=self; cid=fig.canvas.mpl_connect('button_press_event', self.onclick)
|
||||
fig.canvas.set_window_title('current')
|
||||
ax = fig.add_subplot(1,1,1)
|
||||
hl=[]
|
||||
sz=100; weights = np.repeat(1.0, sz) / sz
|
||||
v = np.convolve(rec[:,3], weights, 'same')
|
||||
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()
|
||||
|
||||
|
||||
def prog_3(self,prgId=2,host=None,acq_per=1):
|
||||
'''
|
||||
kwargs:
|
||||
acq_per : acquire period: acquire data all acq_per servo loops (default=1)
|
||||
prgId : used program number
|
||||
file : used filename to save if None, not saved
|
||||
host : host to send and execute program, if None, nothing is executed
|
||||
'''
|
||||
ServoPeriod= .2 #0.2ms Sys.ServoPeriod is dependent of !common() macro
|
||||
|
||||
self.prgId=prgId
|
||||
self.file=file
|
||||
self.host=host
|
||||
self.gather={"MaxSamples":1000000, "Period":acq_per}
|
||||
self.channels=channels=["Motor[3].ActPos","Motor[3].DesPos","Motor[3].PhasePos","Motor[3].idMeas","Motor[3].iqMeas"]
|
||||
self.meta=meta = {'timebase': ServoPeriod*self.gather['Period']}
|
||||
|
||||
self.prg=prg=[]
|
||||
#prg.append('#1..3$')
|
||||
#prg.append('#1..3j/')
|
||||
|
||||
self.prg_prolog()
|
||||
prg=self.prg
|
||||
|
||||
fh=open('/tmp/shapepath.prg','r')
|
||||
lines=fh.read().splitlines()
|
||||
fh.close()
|
||||
prg.extend(lines[11:-3])
|
||||
self.prg_epilog()
|
||||
#print '\n'.join(prg)
|
||||
rec=self.rec
|
||||
lines[12]
|
||||
x0=float(re.match('X(\S+)\sY\S+',lines[12]).group(1))
|
||||
meta['HomePos']=rec[0,1]-x0 #rec[0,1]= first DesPos is x0, the start position of the motion
|
||||
print meta
|
||||
if self.fnNpz:
|
||||
np.savez_compressed(self.fnNpz, rec=rec, prg=prg, channels=channels, meta=meta)
|
||||
|
||||
|
||||
# &1 #1->0
|
||||
# &1 #2->0
|
||||
# #3$
|
||||
# an anschlag (kabel) bewegen
|
||||
# #3hmz
|
||||
# Motor[3].PhasePos=1200 # or for motor 2: Motor[2].PhasePos=300
|
||||
# #3j/
|
||||
# &1p
|
||||
|
||||
|
||||
#folgende parameter setzen: Motor[3].HomePos
|
||||
meta['HomePos']=-223.8 #Motor[3].HomePos
|
||||
meta['PhasePos']=1200 #Motor[3].PhasePos
|
||||
|
||||
|
||||
if __name__=='__main__':
|
||||
from optparse import OptionParser, IndentedHelpFormatter
|
||||
class MyFormatter(IndentedHelpFormatter):
|
||||
'helper class for formating the OptionParser'
|
||||
|
||||
def __init__(self):
|
||||
IndentedHelpFormatter.__init__(self)
|
||||
|
||||
def format_epilog(self, epilog):
|
||||
if epilog:
|
||||
return epilog
|
||||
else:
|
||||
return ""
|
||||
|
||||
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=('--host=PPMAC1391 -m 63 --cfg gather.cfg',
|
||||
'samplePowerBrick.cfg',
|
||||
'-n stackCheck1.cfg',
|
||||
'--host=PPMACZT84 stackCheck1.cfg',
|
||||
'--host=PPMACZT84 stackCheck1.cfg -v15',
|
||||
)
|
||||
epilog=__doc__+'''
|
||||
Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
|
||||
|
||||
fmt=MyFormatter()
|
||||
parser=OptionParser(epilog=epilog, formatter=fmt)
|
||||
|
||||
parser.add_option('-v', '--verbose', type="int", dest='verbose', help='verbosity bits (see below)', default=0)
|
||||
(args, other)=parser.parse_args()
|
||||
args.other=other
|
||||
|
||||
mr=MoveRecord(args)
|
||||
mr.run()
|
||||
#------------------ Main Code ----------------------------------
|
||||
#ssh_test()
|
||||
ret=parse_args()
|
||||
exit(ret)
|
||||
@@ -16,6 +16,7 @@ ift: inverse fourier transformation
|
||||
import numpy as np
|
||||
import matplotlib as mpl
|
||||
import matplotlib.pyplot as plt
|
||||
import os
|
||||
|
||||
np.set_printoptions(precision=3, suppress=True)
|
||||
|
||||
@@ -63,11 +64,15 @@ def gen_pvt(p,v,t,ts):
|
||||
|
||||
return pvt
|
||||
|
||||
|
||||
(a,b)=os.path.split(__file__)
|
||||
fnBase=os.path.abspath(os.path.join(a,'../MXfastStageDoc'))
|
||||
|
||||
#derivate_fft_test()
|
||||
w=40. # ms step between samples
|
||||
ts=.2 # sampling time
|
||||
n=int(w/ts)# servo cycle between samples
|
||||
k=32 #number of unique samples
|
||||
k=320 #number of unique samples
|
||||
|
||||
t = np.arange(0, w*(k+1), w) #time array of trajectory
|
||||
|
||||
@@ -80,9 +85,12 @@ p=np.random.random(k+1)*4. #position array of trajectory
|
||||
|
||||
|
||||
p[-1]=p[0] # put the first position at the end
|
||||
#p=np.array([1,-1]*16+[1,]);
|
||||
#p+=3
|
||||
|
||||
tt = np.arange(t[0],t[-1], ts) #time array of servo cycles
|
||||
ax=plt.figure().add_subplot(1, 1, 1)
|
||||
fig=plt.figure()
|
||||
ax=fig.add_subplot(1, 1, 1)
|
||||
ax.xaxis.set_ticks(t)
|
||||
markerline, stemlines, baseline = ax.stem(t, p, '-')
|
||||
|
||||
@@ -91,14 +99,8 @@ markerline, stemlines, baseline = ax.stem(t, p, '-')
|
||||
p_iftf=np.fft.fft(p[:-1])
|
||||
ft=np.hstack((p_iftf[:k/2],np.zeros((n-1)*k),p_iftf[k/2:]))
|
||||
pp_ift=np.fft.ifft(ft)*n
|
||||
|
||||
ax.plot(tt,pp_ift,'-b',label='ift')
|
||||
|
||||
#plt.figure()
|
||||
#ax=plt.gca()
|
||||
#ax.xaxis.set_ticks(x)
|
||||
#markerline, stemlines, baseline = ax.stem(x, y, '-')
|
||||
|
||||
### PVT move ###
|
||||
p2=np.hstack((p[-2],p,p[1]))
|
||||
v=(p2[2:]-p2[:-2])/(w*2)
|
||||
@@ -110,15 +112,16 @@ ax.plot(tt,pp_pvt,'-g',label='pvt')
|
||||
v*=0
|
||||
pp_p0t=gen_pvt(p,v,t,ts)
|
||||
ax.plot(tt,pp_p0t,'-r',label='p0t')
|
||||
ax.set_xlim(0,400)
|
||||
|
||||
|
||||
### PVT with ift velocity move -> PFT ###
|
||||
f=np.fft.fftfreq(k, d=1./k)
|
||||
p_pftf=np.fft.fft(p[:-1])
|
||||
p_pftfd=p_pftf*f*1j # differentiate in fourier
|
||||
print (p_pftfd)
|
||||
#print (p_pftfd)
|
||||
p_pftd=np.fft.ifft(p_pftfd)
|
||||
print (p_pftd)
|
||||
#print (p_pftd)
|
||||
|
||||
p_pftd=np.hstack((p_pftd,p_pftd[0]))
|
||||
#ax2=plt.figure().add_subplot(1,1,1)
|
||||
@@ -128,8 +131,12 @@ v=p_pftd.real/(k*2*np.pi)
|
||||
pp_pft=gen_pvt(p,v,t,ts)
|
||||
ax.plot(tt,pp_pft,'-c',label='pft')
|
||||
|
||||
ax.xaxis.set_label_text('time(ms)')
|
||||
ax.yaxis.set_label_text('position')
|
||||
|
||||
ax.legend(loc='best')
|
||||
plt.show(block=False)
|
||||
fig.savefig(os.path.join(fnBase,'traj1.eps'))
|
||||
|
||||
|
||||
### frequency plots ###
|
||||
@@ -146,22 +153,54 @@ pp_pftf=np.fft.rfft(pp_pft)/(2*n)
|
||||
f=np.fft.rfftfreq(pp_ift.shape[0], d=ts*1E-3)
|
||||
f=f[1:] #remove dc value frequency
|
||||
|
||||
mag=abs(pp_iftf[1:])#; mag=20*np.log10(abs(mag))
|
||||
def FreqSpecDb(spec,w=20.):
|
||||
#spec = abs(spec)
|
||||
spec = np.convolve(spec, np.ones(w) / w, 'same');
|
||||
spec = 20 * np.log10(spec)
|
||||
return spec
|
||||
|
||||
def FreqSpec(spec,w=20.):
|
||||
#spec = abs(spec)
|
||||
spec = np.convolve(spec, np.ones(w) / w, 'same');
|
||||
return spec
|
||||
|
||||
mag=FreqSpecDb(abs(pp_iftf[1:]))
|
||||
ax.semilogx(f,mag,'-b',label='ift') # Bode magnitude plot
|
||||
mag=abs(pp_pvtf[1:])#; mag=20*np.log10(abs(mag))
|
||||
mag=FreqSpecDb(abs(pp_pvtf[1:]))
|
||||
ax.semilogx(f,mag,'-g',label='pvt') # Bode magnitude plot
|
||||
mag=abs(pp_p0tf[1:])#; mag=20*np.log10(abs(mag))
|
||||
mag=FreqSpecDb(abs(pp_p0tf[1:]))
|
||||
ax.semilogx(f,mag,'-r',label='p0t') # Bode magnitude plot
|
||||
mag=abs(pp_pftf[1:])#; mag=20*np.log10(abs(mag))
|
||||
mag=FreqSpecDb(abs(pp_pftf[1:]))
|
||||
ax.semilogx(f,mag,'-c',label='pft') # Bode magnitude plot
|
||||
#ax.yaxis.set_label_text('dB ampl')
|
||||
ax.yaxis.set_label_text('ampl')
|
||||
ax.yaxis.set_label_text('dB ampl')
|
||||
#ax.yaxis.set_label_text('ampl')
|
||||
ax.xaxis.set_label_text('frequency [Hz]')
|
||||
plt.grid(True)
|
||||
|
||||
ax.legend(loc='best')
|
||||
ax.set_xlim(1,100)
|
||||
plt.show(block=False)
|
||||
fig.savefig(os.path.join(fnBase,'traj2.eps'))
|
||||
|
||||
fig=plt.figure()
|
||||
ax=fig.add_subplot(1,1,1)#ax=plt.gca()
|
||||
magift=abs(pp_iftf[1:])
|
||||
mag=FreqSpec(abs(pp_pvtf[1:])-magift)
|
||||
ax.semilogx(f,mag,'-g',label='pvt-ift') # Bode magnitude plot
|
||||
mag=FreqSpec(abs(pp_p0tf[1:])-magift)
|
||||
ax.semilogx(f,mag,'-r',label='p0t-ift') # Bode magnitude plot
|
||||
mag=FreqSpec(abs(pp_pftf[1:])-magift)
|
||||
ax.semilogx(f,mag,'-c',label='pft-ift') # Bode magnitude plot
|
||||
|
||||
#ax.yaxis.set_label_text('dB ampl')
|
||||
ax.yaxis.set_label_text('ampl')
|
||||
ax.xaxis.set_label_text('frequency [Hz]')
|
||||
ax.set_xlim(1,100)
|
||||
plt.grid(True)
|
||||
ax.legend(loc='best')
|
||||
|
||||
plt.show(block=False)
|
||||
fig.savefig(os.path.join(fnBase,'traj3.eps'))
|
||||
plt.show()
|
||||
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user