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

full gathering data of closed and open loop.

Browse Source 
ready for further analysis and tuning
This commit is contained in:
zamofing_t
2018-09-28 09:57:41 +02:00
parent d589cf8aed
commit eefd545289
4 changed files with 351 additions and 709 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
Readme.md
View File

@@ -1046,9 +1046,45 @@ cpx ;linear abs; X-13.68 Y-8.03 Z-1483.1 B0.35
z=((1.73,.93,2.129),(2.13,.03,1.103))):
&1;cpx abs linear;jog1=557;jog3=0;jog4=468;jog5=1730
&1;cpx abs linear;jog1=557;jog3=0;jog4=-468;jog5=-1730
&1;cpx abs linear;jog1=-8;jog3=240000;jog4=2129;jog5=1.73
&1;cpx abs linear;jog1=-8;jog3=240000;jog4=-351;jog5=-1103
caQtDM -macro "NAME=ESB-MX-CAM,CAMNAME=ESB-MX-CAM" /sf/controls/config/qt/Camera/CameraMiniView
#1,3,4,5p
point 1 0,120,240 deg
575.5 0 -241.5 -1401.3
575.5 120000 96.7 -1401.7
575.5 240000 -53.8 -1802.4
point 2 0,120,240 deg
175.5 0 -162.3 -1802.5
175.5 120000 -293.2 -1303.7
175.5 240000 246.4 -1402.25
#1j=575.5; #3j=0 ; #4j=-241.5; #5j=-1401.3
#1j=575.5; #3j=120000; #4j= 96.7; #5j=-1401.7
#1j=575.5; #3j=240000; #4j= -53.8; #5j=-1802.4
#1j=175.5; #3j=0 ; #4j=-162.3; #5j=-1802.5
#1j=175.5; #3j=120000; #4j=-293.2; #5j=-1303.7
#1j=175.5; #3j=240000; #4j= 246.4; #5j=-1402.25
pixel center 1110 1090
#1j=575.5; #3j=0 ; #4j=-241.5; #5j=-1401.3
cpx pmatch
-1:fwd_inp(0) -241.5 -1401.3 0 575.5
-1:fwd_res -88.9829 308.514 0 575.5
cpx ;linear abs; X0 Z0 Y575 B0
cpx ;linear abs; X0 Z0 Y575 B100000
TODO: CLEANUP CODE OF HELICALSCAN, REMOVE LOGGING (comment out)

303
python/MXTuning.py Executable file
View File

@@ -0,0 +1,303 @@
#!/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:
1 full recording current step
2 full recording open loop
3 full recording closed loop
4 plot the full bode recording
5 plot the full bode recording with an approximation model
6 plot all raw acquired data files
-> 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
from utilities import *
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
from pbtools.misc.gather import Gather
from pbtools.misc.tuning import Tuning
class MXTuning(Tuning):
tuneDir='/opt/ppmac/tune/'
def __init__(self,comm,gather):
Tuning.__init__(self,comm,gather)
def init_stage(self):
comm=self.comm
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)
#Coord[%d].ProgActive if a running proc
def bode_model_plot(self, mot,base):
self.bode_full_plot(mot,base)
fig=plt.gcf()
if mot==1:
db_mag1=17.3 #dB
mag1=10**(db_mag1/20)
f1=6.5 #Hz
w1=f1*2*np.pi #rad/sec
T1=1/w1
d1=.7 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
num1=np.poly1d([mag1])
den1 = np.poly1d([T1**2,2*T1*d1,1])
#first resonance frequency
f2=np.array([197,199])
d2=np.array([.02,.02])#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])
mdl= signal.lti(num2, den2) #num denum
#bode(mdl)
#current loop 2nd order approx
f4=900.
d4=1 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
w4=f4*2*np.pi #rad/sec
T4=1/w4
num4 = np.poly1d([1.])
den4 = np.poly1d([T4**2,2*T4*d4,1])
#mdl= signal.lti(num4, den4) #num denum
#bode(mdl)
num=num1*num2*num4#*num3
den=den1*den2*den4#*den3
mdl= signal.lti(num, den) #num denum
print num,den
print mdl
elif mot==2:
# basic 1/s^2 system with damping an d resonance
db_mag1=17.3 #dB
mag1=10**(db_mag1/20)
f1=4.5 #Hz
w1=f1*2*np.pi #rad/sec
d1=.3 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
T1=1/w1
num1 = np.poly1d([mag1])
den1 = np.poly1d([T1**2,2*T1*d1,1])
#first resonance frequency
f2=np.array([57.8,61.8])
d2=np.array([.05,.055])#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])
mdl= signal.lti(num2, den2) #num denum
#bode(mdl)
#second resonance frequency
f3=np.array([138,151])
d3=np.array([.04,.03])#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])
#mdl= signal.lti(num3, den3) #num denum
#bode(mdl)
#second resonance frequency
f4=np.array([410,417])
d4=np.array([.015,.02])#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])
#mdl= signal.lti(num3, den3) #num denum
#bode(mdl)
f5=np.array([228,230])
d5=np.array([.03,.03])#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
fc=900.
dc=1 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
wc=fc*2*np.pi #rad/sec
Tc=1/wc
numc = np.poly1d([1.])
denc = np.poly1d([Tc**2,2*Tc*dc,1])
#mdl= signal.lti(num4, den4) #num denum
#bode(mdl)
num=num1*num2*num3*num4*num5*numc
den=den1*den2*den3*den4*den5*denc
mdl= signal.lti(num, den) #num denum
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=2) # Bode magnitude plot
ax=fig.axes[1]
ax.semilogx(f, phase,'-k',lw=2) # 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 bode(mdl):
w,mag,phase = signal.bode(mdl,1000)
f=w/(2*np.pi)
fig = plt.figure()
ax = fig.add_subplot(2, 1, 1)
ax.semilogx(f,mag,'-') # Bode magnitude plot
ax.yaxis.set_label_text('dB ampl')
plt.grid(True)
ax = fig.add_subplot(2, 1, 2)
ax.semilogx(f,phase,'-') # Bode magnitude plot
ax.yaxis.set_label_text('phase')
ax.xaxis.set_label_text('frequency [Hz]')
plt.grid(True)
#plt.show()
if __name__=='__main__':
from argparse import ArgumentParser,RawDescriptionHelpFormatter
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=None)
args=parser.parse_args()
#plt.ion()
comm = PPComm(host=args.host)
gt = Gather(comm)
tune=MXTuning(comm,gt)
base='MXTuning'
if args.dir is not None:
base=args.dir
assert(os.path.exists(base))
#base=os.path.join(base,args.dir)
#if not os.path.exists(base):
# os.mkdir(base)
mode=args.mode
#plt.ion()
if mode==1: # full recording current step
homed=False
for mot in (1, 2):
fn=os.path.join(base, 'curr_step%d.npz' % mot)
if not os.path.isfile(fn) and not homed: tune.init_stage();homed=True
fig=plt.figure(mot)
tune.bode_current(motor=mot, magMove=1000, magPhase=500, dwell=10, file=fn,fig=fig)
plt.show(block=False)
elif mode==2: # full recording open loop
for mot in (1, 2):
fsin=os.path.join(base, 'sine_ol_mot%d.npz' % (mot))
fcrp=os.path.join(base, 'chirp_ol_mot%d' % (mot))
if not os.path.isfile(fsin): tune.init_stage();plt.close('all')
tune.bode_sine(openloop=True, file=fsin, motor=mot)
plt.show(block=False)
for ext,amp,minFrq,maxFrq,tSec in (('a', 10, 10, 300,30),
('b', 50,100, 500,30),
('c', 50,300,1500,10),
('d',100,300,2000,10)):
f=fcrp+ext+'.npz'
if not os.path.isfile(f): tune.init_stage();plt.close('all')
tune.bode_chirp(openloop=True, file=f, motor=mot, amp=amp, minFrq=minFrq, maxFrq=maxFrq, tSec=tSec)
plt.show(block=False)
elif mode==3: # full recording closed loop
for mot in (1,2):
#fsin=os.path.join(base, 'sine_cl_mot%d.npz' % (mot))
fcrp=os.path.join(base, 'chirp_cl_mot%d' % (mot))
#if not os.path.isfile(fsin): tune.init_stage();plt.close('all')
#tune.bode_sine(openloop=False, file=fsin, motor=mot)
#the generated program is prog 999 (only during acquisition)
# Gather.MaxLines=174762 -> ca 15 sec. is max.
#with higher frequency and amplitudes often is in DacLimit !
for ext,amp,minFrq,maxFrq,tSec in (('a',100, 1, 200,15),
('b', 20,10, 300,5),
('c', 5,10, 300,5),
('d', 1,10, 600,5),
):
f=fcrp+ext+'.npz'
if not os.path.isfile(f): tune.init_stage();plt.close('all')
tune.bode_chirp(openloop=False, file=f, motor=mot, amp=amp, minFrq=minFrq, maxFrq=maxFrq, tSec=tSec)
plt.show(block=False)
elif mode==4: #plot the full bode recording
tune.bode_full_plot(mot=1,base=base)
tune.bode_full_plot(mot=2,base=base)
elif mode==5: #plot the full bode recording with an approximation model
tune.bode_model_plot(mot=1,base=base)
tune.bode_model_plot(mot=2,base=base)
elif mode == 6: # plot all raw acquired data files
# display bode plots
for fn in args.plot:
if os.path.basename(fn).startswith('sine_ol_mot'):
tune.bode_sine(openloop=True, file=fn)
if os.path.basename(fn).startswith('chirp_ol_mot'):
tune.bode_chirp(openloop=True, file=fn)
if os.path.basename(fn).startswith('sine_cl_mot'):
tune.bode_sine(openloop=False, file=fn)
if os.path.basename(fn).startswith('chirp_cl_mot'):
tune.bode_chirp(openloop=False, file=fn)
print 'done'
plt.show()
#------------------ Main Code ----------------------------------
#ssh_test()
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

697
python/PBTuning.py
View File

@@ -1,697 +0,0 @@
#!/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 deltatau:
For now it does 10 step move and uploads the data
other available tuning progs on the powerbrick are:
analyzerautotunemove
autotunecalc
autotunemove
chirpmove
currentautotunecalc
currentstep
filtercalculation
openloopchirp +
openloopsine +
openlooptestmove
othertrajectory
parabolicmove
randommove
sinesweep +
sinusoidal +
stepmove
user_gantry_crosscoupled.h
usertrajectory
Modes:
1: sine bode open loop
2: chirp bode open loop
3: sine bode closed loop
4: chirp bode closed loop
5: full bode open loop record of both motors (including init_stage
10: bode of current step -> does not work because gathering phase data not implemented
-> 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
TODO:
use openloopsine to create a bode diagram of the 'strecke'
'''
import os, sys, json, time
import numpy as np
import matplotlib as mpl
#mpl.use('GTKAgg')
import matplotlib.pyplot as plt
import subprocess as sprc
import telnetlib
from scipy.signal.waveforms import chirp
from scipy import signal
from scipy import interpolate
from scipy import stats
from utilities import *
class PBTuning:
tuneDir='/opt/ppmac/tune/'
def __init__(self,args):
self.args=args
def do_command(self,cmd,*param):
args=self.args
host=args.host
cmd=('ssh','root@'+host, self.tuneDir+cmd)+tuple(map(str,param))
print(' '.join(cmd))
p=sprc.Popen(cmd, shell=False, stdin=sprc.PIPE, stdout=sprc.PIPE, stderr=sprc.PIPE)
res=p.wait()
print(res)
print(p.stderr.read())
print(p.stdout.read())
PBGatherPlot='/home/zamofing_t/scripts/PBGatherPlot'
try:
fnLoc=self.fnLoc
except AttributeError:
fnLoc = '/tmp/gather.txt'
cmd=(PBGatherPlot,'-m24','-v255','--host',host,'--dat',fnLoc)
#p = sprc.Popen(cmd, shell=False, stdin=sprc.PIPE, stdout=sprc.PIPE, stderr=sprc.PIPE)
p = sprc.Popen(cmd, shell=False)
retval = p.wait()
print(p.stderr.read())
#print(p.stdout.read())
#self.meta = {'timebase': ServoPeriod*gather['Period']}
self.data = np.genfromtxt(fnLoc, delimiter=' ')
return self.data
def init_stage(self):
args=self.args
host=args.host
gpasciiCommander='/home/zamofing_t/scripts/gpasciiCommander'
cmd=(gpasciiCommander ,'--host',host,'--cmd','enable plc1')
p = sprc.Popen(cmd, shell=False, stdin=sprc.PIPE, stdout=sprc.PIPE, stderr=sprc.PIPE)
retval = p.wait()
print(p.stderr.read());print(p.stdout.read())
time.sleep(10)
#cmd=(gpasciiCommander ,'--host',host,'--cmd','#1..2j=10000')
#p = sprc.Popen(cmd, shell=False, stdin=sprc.PIPE, stdout=sprc.PIPE, stderr=sprc.PIPE)
#retval = p.wait()
#print(p.stderr.read());print(p.stdout.read())
#time.sleep(5)
def phase_amp(self,frq,rep):
try:
ax1=self.ax1
ax2=self.ax2
ax1.clear()
ax2.clear()
except AttributeError:
fig = plt.figure(); self.ax1=ax1 = fig.add_subplot(1, 1, 1)
fig = plt.figure(); self.ax2=ax2 = fig.add_subplot(1, 1, 1)
n = self.data.shape[0]
w=np.hamming(n)
res=np.ndarray((2,2))
col=('b-','g-')
for i in (0,1):
data=self.data[:,i]
avg=data.mean(0)
#data=data-data[0]
data=data-avg
data*=w
ax1.plot(data, col[i])
f = np.fft.fft(data)
ax2.plot(np.absolute(f) , col[i])
idx = int(rep)
bias = np.absolute(f[0] / n)
phase = np.angle(f[idx])
#ampl = np.absolute(f[idx]) * 2 / n
ampl = np.absolute(f[idx]) * 2 / w.sum()
res[i,:]=(ampl,phase)
plt.pause(.05)
return (res[1,0]/res[0,0],res[1,1]-res[0,1])
def bode_sine(self,openloop=True,motor=1,minFrq=1,maxFrq=300,numFrq=150,amp=10,file='/tmp/bode.npz'):
if os.path.isfile(file):
f=np.load(file)
bode=f['bode']
meta=f['meta'].item()
meta['file']=file
else:
#motor 1 maximum: 13750
#amp= percentage of maximum amplitude
fig = plt.figure()
ax = fig.add_subplot(1, 1, 1)
frqLst=np.logspace(np.log10(minFrq),np.log10(maxFrq),numFrq)
n=len(frqLst)
#frqLst=(10,15,20,25,30)
bode=np.ndarray((n,3))
bode[:, 0]=frqLst
#for i in range(n):
for i in range(n-1,-1,-1):
frq=frqLst[i]
t=1
rep=max(1,frq*t)
self.fnLoc='/tmp/gather%d.txt'%i
if openloop:
data=self.do_command('openloopsine',motor,amp,frq,rep,0)
else:
data=self.do_command('sinusoidal',motor,amp,frq,rep,0)
data=data[:,(1,2)]
ax.clear()
avg=data.mean(0)
print(avg)
ax.plot(data[:, 0]-avg[0] , 'b-', label='input')
ax.plot(data[:, 1]-avg[1], 'g-', label='output')
plt.pause(.05)
bode[i,1:]=self.phase_amp(frq, rep)
print('frq %g ampl %g phase %g'%tuple(bode[i,:]))
plt.draw_all()
meta={'motor':motor,'date':time.asctime()}
np.savez_compressed(file, bode=bode, meta=meta)
meta['file']=file
self.bode_sine_plot(bode, meta)
def bode_sine_plot(self,bode,meta):
frq=bode[:,0]
db_mag=20*np.log10(bode[:,1])
phase=np.degrees(np.unwrap(bode[:,2]))# numpy.unwrap(p, discont=3.141592653589793, axis=-1)
if phase[0]>0: phase-=360
fig = plt.figure()
fig.canvas.set_window_title(os.path.basename(meta['file']))
ax = fig.add_subplot(2, 1, 1)
ax.semilogx(frq, db_mag,'.-') # Bode magnitude plot
ax.xaxis.set_label_text('dB Mag.')
plt.grid(True)
#ax.loglog(frqLst, bode[:,0],'.-') # Bode magnitude plot
ax = fig.add_subplot(2, 1, 2)
ax.semilogx(frq, phase,'.-') # Bode phase plot
ax.xaxis.set_label_text('phase')
plt.grid(True)
def bode_chirp(self,openloop=True,motor=1,minFrq=10,maxFrq=150,tSec=30.,amp=10,mode=1,file='/tmp/gather.npz'):
#amp= percentage of maximum amplitud
if os.path.isfile(file):
f=np.load(file)
data=f['data']
#try:
meta=f['meta'].item()
#except KeyError:
# meta={'motor': motor, 'date': time.asctime(), 'minFrq': 10, 'maxFrq': 150, 'tSec': 30, 'amp': amp}
# np.savez_compressed(file, data=data, meta=meta)
meta['file']=file
else:
mode=1
#Sys.ServoPeriod=0.2ms = 5kHz -> therefore factor 2000 for the time second->servoCnt
#don't know why the frequency must be scaled...
if openloop:
data=self.do_command('openloopchirp',motor,amp,minFrq/2.,maxFrq/2.,tSec*2000,mode,0)
else:
data=self.do_command('sinesweep', motor, amp, minFrq/2., maxFrq/2., tSec*2000, mode, 0)
data = data[:, (1, 2)]
meta={'motor':motor,'date':time.asctime(),'minFrq':minFrq,'maxFrq':maxFrq,'tSec':tSec,'amp':amp}
np.savez_compressed(file, data=data, meta=meta)
meta['file'] = file
self.bode_chirp_plot(data, meta,openloop)
def bode_chirp_plot(self, data, meta,openloop):
tSec=meta['tSec']
minFrq=meta['minFrq']
maxFrq=meta['maxFrq']
#n=25000
#t=np.linspace(0, tSec, 2*n+1, endpoint=True)
#c=chirp(t, f0=minFrq, f1=maxFrq, t1=tSec, phi=-90, method='logarithmic')
#plt.figure()
#plt.clf()
#plt.subplot(2, 1, 1)
#plt.plot(t, c)
#tstr="Logarithmic Chirp, f(0)=%g, f(%g)=%g"%(minFrq, tSec, maxFrq)
#plt.title(tstr)
#plt.subplot(2, 1, 2)
#plt.plot(t, minFrq*(maxFrq/minFrq)**(t/tSec), 'r')
## yscale('log')
#plt.grid(True)
#plt.ylabel('Frequency (Hz)')
#plt.xlabel('time (sec)')
if openloop:
n=1000
d=np.concatenate((np.ones(n-1)*data[0, 1],data[:, 1]))
d=np.convolve(d,np.ones(n),'valid')/n
data[:, 1]-=d
# 5..150 Hz
# 15 sec
c = data[:, 0]
o = data[:, 1]
o -= o[0]
else:
data=data-data[0,:]
c = data[:,0]
o=data[:,1]
t = np.linspace(0, tSec, data.shape[0], endpoint=True)
n=data.shape[0]/2
fig=plt.figure()
fig.canvas.set_window_title(os.path.basename(meta['file']+' raw'))
plt.plot(t, c)
plt.plot(t, o)
#bode_plot(o)
#plt.show()
#n samples
#t1 seconds
#ts=t1/(2*n)#samplingperiode =t[1]-t[0]
#w_k=2*pi*k/T
#w=2*np.pi*np.arange(n+1)/t1*(2*n)
f=np.arange(n+1)/tSec #Hz
#w*=2.*np.pi #rad/sec
fig=plt.figure()
fig.canvas.set_window_title(os.path.basename(meta['file']+' bode'))
ftX=np.fft.rfft(c)
ftY=np.fft.rfft(o)
i=int(minFrq*tSec); j=int(maxFrq*tSec); #print(w[i],w[j])
f=f[i:j+1]
ftX=ftX[i:j+1]
ftY=ftY[i:j+1]
ft=ftY/ftX
phase=np.angle(ft)
phase=np.degrees(np.unwrap(phase))
#magDb=10*np.log10(np.abs(ft)) #in decibel
mag=np.abs(ftY)/np.abs(ftX)
magDb=20*np.log10(mag) #in decibel (20=10*2: factor 2 because rfft only half)
#magDb=np.abs(ftY)/np.abs(ftX)
ax=plt.subplot(2,1,1)
ax.semilogx(f,magDb,'b') # Bode magnitude plot
#ax.plot(w,magDb) # Bode magnitude plot
ax.axvline(minFrq,c='k');ax.axvline(maxFrq,c='k')
ax.grid(True)
ax=plt.subplot(2,1,2)
ax.semilogx(f,phase,'b') # Bode phase plot
ax.yaxis.set_label_text('Amplitude [dB]')
#ax.set_ylim(phase[i],phase[j])
ax.grid(True)
ax.xaxis.set_label_text('Frequency [Hz]')
ax.yaxis.set_label_text('Phase [degree]')
#plt.axvline(x=0.22058956)
#plt.show()
#meta={'motor':motor,'date':time.asctime()}
#np.savez_compressed(file, bode=bode, meta=meta)
def bode_full_plot(self, mot,base):
import glob
fn='sine_ol_mot%d.npz'%mot
file=os.path.join(base,fn)
f=np.load(file)
bode=f['bode']
meta=f['meta'].item()
frq=[bode[:,0],]
mag=[bode[:,1],]
phase=[np.unwrap(bode[:,2]),]
#for fn in ('chirp_ol_mot%da.npz','chirp_ol_mot%db.npz','chirp_ol_mot%dc.npz','chirp_ol_mot%dd.npz'):
# fn=fn%mot
# file=os.path.join(base,fn)
for file in sorted(glob.glob(os.path.join(base,'chirp_ol_mot%d*'%mot))):
print(os.path.basename(file))
f=np.load(file)
data=f['data']
meta=f['meta'].item()
tSec=float(meta['tSec'])
minFrq=meta['minFrq']
maxFrq=meta['maxFrq']
amp=meta['amp']
n=1000
d=np.concatenate((np.ones(n-1)*data[0, 1],data[:, 1]))
d=np.convolve(d,np.ones(n),'valid')/n
data[:, 1]-=d
# 5..150 Hz
# 15 sec
c = data[:, 0]
o = data[:, 1]
o -= o[0]
n=data.shape[0]/2
ftX=np.fft.rfft(c)
ftY=np.fft.rfft(o)
i=int(minFrq*tSec); j=int(maxFrq*tSec); #print(w[i],w[j])
frq_=np.arange(n+1)/tSec #Hz
frq_=frq_[i:j+1]
ftX=ftX[i:j+1]
ftY=ftY[i:j+1]
ft=ftY/ftX
frq.append(frq_)
phase_=np.unwrap(np.angle(ft))
phase.append(phase_)
mag.append(np.abs(ftY)/np.abs(ftX))
numFrq=1000
fFrq= np.logspace(np.log10(frq[0][0]), np.log10(frq[-1][-1]),numFrq)
fdb_mag = np.zeros(fFrq.shape)
fdeg_phase = np.zeros(fFrq.shape)
fig = plt.figure()
fig.canvas.set_window_title('full bode of motor %d'%mot)
ax1 = fig.add_subplot(2, 1, 1)
ax2 = fig.add_subplot(2, 1, 2)
plt.title('bode of motor %d'%mot)
for i in range(len(frq)):
db_mag = 20 * np.log10(mag[i])
deg_phase = np.degrees(phase[i]) # numpy.unwrap(p, discont=3.141592653589793, axis=-1)
if deg_phase[0]>0:
deg_phase-=360
ax1.semilogx(frq[i], db_mag,'-') # Bode magnitude plot
ax2.semilogx(frq[i], deg_phase, '-') # ,zorder=i) # Bode phase plot
#fill the final magnitude and phase
if i==0:
f=interpolate.interp1d(frq[i], db_mag,bounds_error=False)
fdb_mag=f(fFrq)
f=interpolate.interp1d(frq[i], deg_phase,bounds_error=False)
fdeg_phase=f(fFrq)
else:
print((frq[i][0],frq[i][-1]))
s=stats.binned_statistic(frq[i], db_mag,'mean',fFrq)[0]
b=~np.isnan(s); fdb_mag[:-1][b]=s[b] #[:-2][b] because the statistics has one less entry than the count of bins
s=stats.binned_statistic(frq[i], deg_phase,'mean',fFrq)[0]
b=~np.isnan(s); fdeg_phase[:-1][b]=s[b]
pass
ax1.semilogx(fFrq, fdb_mag,'y')
ax2.semilogx(fFrq, fdeg_phase, 'y')
#export bode plot fot matlab analysis
fn = os.path.join(base,'full_bode_mot%d.mat'%mot)
import scipy.io
scipy.io.savemat(fn, mdict={'db_mag':fdb_mag,'deg_phase':fdeg_phase})
#scipy.io.savemat('/home/zamofing_t/afs/ESB-MX/data/' + fn + '.mat', mdict=f)
ax1.yaxis.set_label_text('dB ampl')
ax1.set_xlim(1,2000)
ax1.grid(True)
ax2.yaxis.set_label_text('phase')
ax2.xaxis.set_label_text('frequency [Hz]')
ax2.set_xlim(1,2000)
ax2.set_ylim(-360,0)
ax2.grid(True)
pass
def bode_model_plot(self, mot,base):
self.bode_full_plot(mot,base)
fig=plt.gcf()
if mot==1:
db_mag1=17.3 #dB
mag1=10**(db_mag1/20)
f1=6.5 #Hz
w1=f1*2*np.pi #rad/sec
T1=1/w1
d1=.7 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
num1=np.poly1d([mag1])
den1 = np.poly1d([T1**2,2*T1*d1,1])
#first resonance frequency
f2=np.array([197,199])
d2=np.array([.02,.02])#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])
mdl= signal.lti(num2, den2) #num denum
#bode(mdl)
#current loop 2nd order approx
f4=900.
d4=1 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
w4=f4*2*np.pi #rad/sec
T4=1/w4
num4 = np.poly1d([1.])
den4 = np.poly1d([T4**2,2*T4*d4,1])
#mdl= signal.lti(num4, den4) #num denum
#bode(mdl)
num=num1*num2*num4#*num3
den=den1*den2*den4#*den3
mdl= signal.lti(num, den) #num denum
print num,den
print mdl
elif mot==2:
# basic 1/s^2 system with damping an d resonance
db_mag1=17.3 #dB
mag1=10**(db_mag1/20)
f1=4.5 #Hz
w1=f1*2*np.pi #rad/sec
d1=.3 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
T1=1/w1
num1 = np.poly1d([mag1])
den1 = np.poly1d([T1**2,2*T1*d1,1])
#first resonance frequency
f2=np.array([57.8,61.8])
d2=np.array([.05,.055])#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])
mdl= signal.lti(num2, den2) #num denum
#bode(mdl)
#second resonance frequency
f3=np.array([138,151])
d3=np.array([.04,.03])#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])
#mdl= signal.lti(num3, den3) #num denum
#bode(mdl)
#second resonance frequency
f4=np.array([410,417])
d4=np.array([.015,.02])#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])
#mdl= signal.lti(num3, den3) #num denum
#bode(mdl)
f5=np.array([228,230])
d5=np.array([.03,.03])#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
fc=900.
dc=1 # daempfung =1 -> keine resonanz -> den1= np.poly1d([T1,1])**2
wc=fc*2*np.pi #rad/sec
Tc=1/wc
numc = np.poly1d([1.])
denc = np.poly1d([Tc**2,2*Tc*dc,1])
#mdl= signal.lti(num4, den4) #num denum
#bode(mdl)
num=num1*num2*num3*num4*num5*numc
den=den1*den2*den3*den4*den5*denc
mdl= signal.lti(num, den) #num denum
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=2) # Bode magnitude plot
ax=fig.axes[1]
ax.semilogx(f, phase,'-k',lw=2) # 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 bode_current(self,openloop=True,motor=1,magMove=1000,magPhase=500,dwell=10,file='/tmp/bode.npz'):
#currentstep 2 1000 500 10
#magPhase: set this current to move the stage at a stable position: vslue in bits
#magMove: set this current to measure the current transition: value in bits
#dwell: measurement time in ms.the time the current is set
# Amplifier specs (Power Brick LV User Manual.pdf p.19)
# 5A_rms continous current
# 15A_rms peak current
# 14 bit ADC resolution
# 2us PWM deadBand
# 33.85A Maximum ADC Current (corresponds to a DAC Value 32737 ==2^15)
data = self.do_command('currentstep', motor, magMove, magPhase, dwell)
def bode(mdl):
w,mag,phase = signal.bode(mdl,1000)
f=w/(2*np.pi)
fig = plt.figure()
ax = fig.add_subplot(2, 1, 1)
ax.semilogx(f,mag,'-') # Bode magnitude plot
ax.yaxis.set_label_text('dB ampl')
plt.grid(True)
ax = fig.add_subplot(2, 1, 2)
ax.semilogx(f,phase,'-') # Bode magnitude plot
ax.yaxis.set_label_text('phase')
ax.xaxis.set_label_text('frequency [Hz]')
plt.grid(True)
#plt.show()
if __name__=='__main__':
from argparse import ArgumentParser,RawDescriptionHelpFormatter
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('--plot', nargs='*')
parser.add_argument('--host', help='hostname', metavar='HOST')
parser.add_argument('--verbose', '-v', type=int, help='verbosity bits (see below)', default=0)
parser.add_argument('--dryrun', '-n', action='store_true', help='dryrun to stdout')
parser.add_argument('--mode', '-m', type=int, help='modes (see below)', default=1)
parser.add_argument('--mot', type=int, help='motor', default=1)
parser.add_argument('--dir', help='dir', default='')
args=parser.parse_args()
#plt.ion()
tune=PBTuning(args)
#data=self.do_command('openloopsine',motor,amp,frq,rep,0)
# #data=self.do_command('openloopchirp',motor,amp,minFrq,maxFrq,tSec*1000,mode,0)
#THIS IS A TEST TO DO 5 Hz during 10 seconds
#frq=5
#frq2=frq*1.1
#ts=10 #seconds
#5 kHz Servo
#ds=tune.do_command('openloopsine', 2, 20, frq, frq*ts, 0)
#dc=tune.do_command('openloopchirp', 2, 20, frq/2., frq2/2., ts*2000, 1, 0) #THIS IS NOT 10 Seconds!!!
#dc=tune.do_command('openloopchirp', 2, 10, 5, 6, 10*1000, 1, 0) #THIS IS NOT 10 Seconds!!!
#ds=ds-ds[0,:]
#dc=dc-dc[0,:]
#plt.figure()
#plt.plot(ds[:,0],'b')
#plt.plot(ds[:,1],'g')
#plt.figure()
#plt.plot(dc[:,0],'b')
#plt.plot(dc[:,1],'g')
#plt.show()
#return
base='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/PBTuning/'
if args.dir is not None:
base=os.path.join(base,args.dir)
if not os.path.exists(base):
os.mkdir(base)
#file=os.path.join(base, 'sine_ol_mot_tst.npz')
#tune.bode_sine(openloop=True,file=file,motor=2, minFrq=20,maxFrq=200,numFrq=30)
#file=os.path.join(base, 'chirp_ol_mot_tst.npz')
#tune.bode_chirp(openloop=True, file=file, motor=2, minFrq=20,maxFrq=200,tSec=30.)
#plt.show()
#return
if args.plot:
#display bode plots
if args.plot[0]=='full':
tune.bode_full_plot(mot=1,base=base)
tune.bode_full_plot(mot=2,base=base)
plt.show()
return
if args.plot[0]=='model':
tune.bode_model_plot(mot=1,base=base)
tune.bode_model_plot(mot=2,base=base)
plt.show()
return
for fn in args.plot:
if os.path.basename(fn).startswith('sine_ol_mot'):
tune.bode_sine(openloop=True,file=fn)
if os.path.basename(fn).startswith('chirp_ol_mot'):
tune.bode_chirp(openloop=True,file=fn)
if os.path.basename(fn).startswith('sine_cl_mot'):
tune.bode_sine(openloop=False, file=fn)
if os.path.basename(fn).startswith('chirp_cl_mot'):
tune.bode_chirp(openloop=False, file=fn)
plt.show()
return
mode=args.mode
mot=args.mot
if mode==1:
file=os.path.join(base,'sine_ol_mot%d.npz'%(mot))
#def bode_sine(self, openloop=True, motor=1, minFrq=1, maxFrq=300, numFrq=150, amp=10, file='/tmp/bode.npz'):
tune.bode_sine(openloop=True,file=file,motor=mot)
elif mode==2:
file=os.path.join(base,'chirp_ol_mot%d.npz'%(mot))
#def bode_chirp(self,openloop=True,motor=1,minFrq=10,maxFrq=150,tSec=30.,amp=10,mode=1,file='/tmp/gather.npz'):
#tune.bode_chirp(openloop=True,file=file,motor=mot, minFrq=10, maxFrq=300, tSec=30)
tune.bode_chirp(openloop=True,file=file[:-4]+'a.npz',motor=mot, minFrq=10, maxFrq=300, tSec=30)
tune.bode_chirp(openloop=True,file=file[:-4]+'b.npz',motor=mot,amp=50,minFrq=100,maxFrq=500, tSec=30)
tune.bode_chirp(openloop=True,file=file[:-4]+'c.npz',motor=mot,amp=50,minFrq=300,maxFrq=1500,tSec=10)
tune.bode_chirp(openloop=True,file=file[:-4]+'d.npz',motor=mot,amp=100,minFrq=300,maxFrq=2000,tSec=10)
elif mode==3:
file=os.path.join(base,'sine_cl_mot%d.npz'%(mot))
tune.bode_sine(openloop=False,file=file,motor=mot)
elif mode==4:
file=os.path.join(base,'chirp_cl_mot%d.npz'%(mot))
tune.bode_chirp(openloop=False,file=file,motor=mot)
elif mode==5: #full recording open loop
for mot in (1,2):
tune.init_stage()
file=os.path.join(base,'sine_ol_mot%d.npz'%(mot))
tune.bode_sine(openloop=True,file=file,motor=mot)
file=os.path.join(base,'chirp_ol_mot%d.npz'%(mot))
tune.init_stage()
tune.bode_chirp(openloop=True,file=file[:-4]+'a.npz',motor=mot, minFrq=10, maxFrq=300, tSec=30)
tune.init_stage()
tune.bode_chirp(openloop=True,file=file[:-4]+'b.npz',motor=mot,amp=50,minFrq=100,maxFrq=500, tSec=30)
tune.init_stage()
tune.bode_chirp(openloop=True,file=file[:-4]+'c.npz',motor=mot,amp=50,minFrq=300,maxFrq=1500,tSec=10)
tune.init_stage()
tune.bode_chirp(openloop=True,file=file[:-4]+'d.npz',motor=mot,amp=100,minFrq=300,maxFrq=2000,tSec=10)
elif mode==10:
#for mot in (1,2):
tune.bode_current(motor=mot, magMove=1000, magPhase=500, dwell=10, file='/tmp/curr_step%d.npz'%mot)
print 'done'
plt.show()
#------------------ Main Code ----------------------------------
#ssh_test()
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

20
python/helicalscan.py
View File

@@ -762,7 +762,7 @@ open forward
DX=qCX-p0_x
DZ=qCZ-p0_z
Y=qFY
send 1"fwd_res %f %f %f %f\\n",DX,DZ,W,Y
//send 1"fwd_res %f %f %f %f\\n",DX,DZ,W,Y
//P1001+=1
D0=$000001c2; //B=$2 X=$40 Y=$80 Z=$100 hex(2+int('40',16)+int('80',16)+int('100',16)) -> 0x1c2
close
@@ -783,10 +783,10 @@ open inverse
define(p_x='L16', p_y='L17', p_z='L18')
define(sclY='L19')
define(scl='L20')
if(D0>0)
send 1"inv_inp(%f) %f:%f %f:%f %f:%f %f:%f\\n",D0,DX,vDX,DZ,vDZ,W,vW,Y,vY
else
send 1"inv_inp(%f) %f %f %f %f\\n",D0,DX,DZ,W,Y''')
//if(D0>0)
// send 1"inv_inp(%f) %f:%f %f:%f %f:%f %f:%f\\n",D0,DX,vDX,DZ,vDZ,W,vW,Y,vY
//else
// send 1"inv_inp(%f) %f %f %f %f\\n",D0,DX,DZ,W,Y''')
for i in range(2):
# https://stackoverflow.com/questions/3471999/how-do-i-merge-two-lists-into-a-single-list
l = [j for i in zip((i,) * param.shape[1], list(param[i])) for j in i]
@@ -821,10 +821,10 @@ open inverse
vqW=vW//+((p1_x-p0_x)/(p1_y-p0_y)*vY)*p_z+((p1_z-p0_z)/(p1_y-p0_y)*vY*p_x
''')
prg.append(" vqW=vqW*%g"%(1000./d2r)) #scale from rad to 1000*deg
prg.append(''' send 1"inv_res %f:%f %f:%f %f:%f %f:%f\\n",qCX,vqCX,qCZ,vqCZ,qW,vqW,qFY,vqFY
prg.append('''// send 1"inv_res %f:%f %f:%f %f:%f %f:%f\\n",qCX,vqCX,qCZ,vqCZ,qW,vqW,qFY,vqFY
}
else
send 1"inv_res %f %f %f %f\\n",qCX,qCZ,qW,qFY
//else
// send 1"inv_res %f %f %f %f\\n",qCX,qCZ,qW,qFY
//P1002+=1
close
''')
@@ -1193,8 +1193,8 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
#hs.gen_prog(mode=0,cntHor=3,cntVert=10,hRng=(-5,5),wRng=(0,120000))
#hs.gen_prog(mode=0,cntHor=3,cntVert=25,hRng=(-5,5),wRng=(0,120000))
#hs.gen_prog(mode=1,cntHor=3,cntVert=25,hRng=(-5,5),wRng=(0,120000),smt=0,pt2pt_time=300)
#hs.gen_prog(mode=1,cntHor=5,cntVert=25,hRng=(-100,100),wRng=(0,120000),smt=0,pt2pt_time=300)
hs.gen_prog(mode=1,cntHor=5,cntVert=25,hRng=(-100,100),wRng=(0,120000),smt=0,pt2pt_time=40)
hs.gen_prog(mode=1,cntHor=5,cntVert=25,hRng=(-100,100),wRng=(0,120000),smt=0,pt2pt_time=300)
#hs.gen_prog(mode=1,cntHor=5,cntVert=25,hRng=(-100,100),wRng=(0,120000),smt=0,pt2pt_time=40)
#hs.gen_prog(mode=1,cntHor=3,cntVert=20,hRng=(-5,5),wRng=(0,1200),smt=0,pt2pt_time=200)
#hs.gen_prog(mode=1, cntHor=2, cntVert=2, wRng=(0, 360000), smt=0)
#hs.gen_prog(mode=1)