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

helicalscan commissioned at ESB_MX!

Browse Source
This commit is contained in:
zamofing_t
2018-09-26 16:43:34 +02:00
parent 6f5278ec5c
commit d589cf8aed
3 changed files with 207 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
Readme.md
View File

@@ -1014,4 +1014,41 @@ Gate3[1].Chan[0].UserFlag -> is set to 0 when event triggered (mapped to output
``` ```
Testing Helical Coord Trf
-------------------------
```
B0.3504637004371034 X-13.68414496427224 Y-8.029999999998836 Z-1483.096457761122
&1p ->this will trigger:forward kinematic
cpx pmatch ->this will trigger:forward kinematic
cpx ;linear rel; X0Y0Z0B0
cpx ;linear abs; X-13.68 Y-8.03 Z-1483.1 B0.35
->this will trigger: inverse
&1;cpx abs linear;jog1=0;jog2=0;jog3=0;jog4=0;jog5=0
-1:fwd_inp(0) 0.45 -1103.7 0.350464 -7.98
-1:fwd_res -13.5538 -1481 0.350464 -7.98
//motors CX CZ RY FY
// 4 5 3 1
&1;cpx abs linear;jog1=0;jog2=0;jog3=0;jog4=0;jog5=0
def calcParam(self,x=((.468,-.627,-.523),(.357,-1.349,.351)),
y=(.557,-.008),
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=-8;jog3=240000;jog4=2129;jog5=1.73
caQtDM -macro "NAME=ESB-MX-CAM,CAMNAME=ESB-MX-CAM" /sf/controls/config/qt/Camera/CameraMiniView

123
python/PBTuning.py
View File

@@ -34,7 +34,10 @@ Modes:
3: sine bode closed loop 3: sine bode closed loop
4: chirp bode closed loop 4: chirp bode closed loop
5: full bode open loop record of both motors (including init_stage 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: TODO:
use openloopsine to create a bode diagram of the 'strecke' use openloopsine to create a bode diagram of the 'strecke'
''' '''
@@ -48,6 +51,8 @@ import subprocess as sprc
import telnetlib import telnetlib
from scipy.signal.waveforms import chirp from scipy.signal.waveforms import chirp
from scipy import signal from scipy import signal
from scipy import interpolate
from scipy import stats
from utilities import * from utilities import *
class PBTuning: class PBTuning:
@@ -71,8 +76,9 @@ class PBTuning:
fnLoc=self.fnLoc fnLoc=self.fnLoc
except AttributeError: except AttributeError:
fnLoc = '/tmp/gather.txt' fnLoc = '/tmp/gather.txt'
cmd=(PBGatherPlot,'-m24','-v0','--host',host,'--dat',fnLoc) 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, stdin=sprc.PIPE, stdout=sprc.PIPE, stderr=sprc.PIPE)
p = sprc.Popen(cmd, shell=False)
retval = p.wait() retval = p.wait()
print(p.stderr.read()) print(p.stderr.read())
#print(p.stdout.read()) #print(p.stdout.read())
@@ -312,11 +318,10 @@ class PBTuning:
f=np.load(file) f=np.load(file)
bode=f['bode'] bode=f['bode']
meta=f['meta'].item() meta=f['meta'].item()
frq=bode[:,0] frq=[bode[:,0],]
mag=bode[:,1] mag=[bode[:,1],]
phase=np.unwrap(bode[:,2]) phase=[np.unwrap(bode[:,2]),]
l=[0,len(frq)]
#for fn in ('chirp_ol_mot%da.npz','chirp_ol_mot%db.npz','chirp_ol_mot%dc.npz','chirp_ol_mot%dd.npz'): #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 # fn=fn%mot
# file=os.path.join(base,fn) # file=os.path.join(base,fn)
@@ -325,7 +330,7 @@ class PBTuning:
f=np.load(file) f=np.load(file)
data=f['data'] data=f['data']
meta=f['meta'].item() meta=f['meta'].item()
tSec=meta['tSec'] tSec=float(meta['tSec'])
minFrq=meta['minFrq'] minFrq=meta['minFrq']
maxFrq=meta['maxFrq'] maxFrq=meta['maxFrq']
amp=meta['amp'] amp=meta['amp']
@@ -342,42 +347,68 @@ class PBTuning:
ftX=np.fft.rfft(c) ftX=np.fft.rfft(c)
ftY=np.fft.rfft(o) ftY=np.fft.rfft(o)
i=int(minFrq*tSec); j=int(maxFrq*tSec); #print(w[i],w[j]) i=int(minFrq*tSec); j=int(maxFrq*tSec); #print(w[i],w[j])
f=np.arange(n+1)/tSec #Hz frq_=np.arange(n+1)/tSec #Hz
f=f[i:j+1] frq_=frq_[i:j+1]
ftX=ftX[i:j+1] ftX=ftX[i:j+1]
ftY=ftY[i:j+1] ftY=ftY[i:j+1]
ft=ftY/ftX ft=ftY/ftX
frq=np.concatenate((frq,f)) frq.append(frq_)
phase_=np.unwrap(np.angle(ft)) phase_=np.unwrap(np.angle(ft))
if phase_[0]>0: phase.append(phase_)
phase_-=2*np.pi mag.append(np.abs(ftY)/np.abs(ftX))
phase=np.concatenate((phase,phase_))
mag=np.concatenate((mag,(np.abs(ftY)/np.abs(ftX))))
l.append(len(frq))
numFrq=1000
db_mag=20*np.log10(mag) fFrq= np.logspace(np.log10(frq[0][0]), np.log10(frq[-1][-1]),numFrq)
phase=np.degrees(phase)# numpy.unwrap(p, discont=3.141592653589793, axis=-1) fdb_mag = np.zeros(fFrq.shape)
fdeg_phase = np.zeros(fFrq.shape)
fig = plt.figure() fig = plt.figure()
fig.canvas.set_window_title('full bode of motor %d'%mot) fig.canvas.set_window_title('full bode of motor %d'%mot)
ax = fig.add_subplot(2, 1, 1) ax1 = fig.add_subplot(2, 1, 1)
ax2 = fig.add_subplot(2, 1, 2)
plt.title('bode of motor %d'%mot) plt.title('bode of motor %d'%mot)
for i in range(len(l)-1): for i in range(len(frq)):
ax.semilogx(frq[l[i]:l[i+1]], db_mag[l[i]:l[i+1]],'-') # Bode magnitude plot db_mag = 20 * np.log10(mag[i])
ax.yaxis.set_label_text('dB ampl') deg_phase = np.degrees(phase[i]) # numpy.unwrap(p, discont=3.141592653589793, axis=-1)
ax.set_xlim(1,2000) if deg_phase[0]>0:
plt.grid(True) deg_phase-=360
#ax.loglog(frqLst, bode[:,0],'.-') # Bode magnitude plot
ax = fig.add_subplot(2, 1, 2)
for i in range(len(l)-1): ax1.semilogx(frq[i], db_mag,'-') # Bode magnitude plot
ax.semilogx(frq[l[i]:l[i+1]], phase[l[i]:l[i+1]],'-')#,zorder=i) # Bode phase plot ax2.semilogx(frq[i], deg_phase, '-') # ,zorder=i) # Bode phase plot
ax.yaxis.set_label_text('phase')
ax.xaxis.set_label_text('frequency [Hz]') #fill the final magnitude and phase
ax.set_xlim(1,2000) if i==0:
ax.set_ylim(-360,0) f=interpolate.interp1d(frq[i], db_mag,bounds_error=False)
plt.grid(True) 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): def bode_model_plot(self, mot,base):
self.bode_full_plot(mot,base) self.bode_full_plot(mot,base)
@@ -481,7 +512,7 @@ class PBTuning:
den=den1*den2*den3*den4*den5*denc den=den1*den2*den3*den4*den5*denc
mdl= signal.lti(num, den) #num denum mdl= signal.lti(num, den) #num denum
bode(mdl) bode(mdl)
w=np.logspace(0,3,1000)*2*np.pi w=np.logspace(0,np.log10(2000),1000)*2*np.pi
w,mag,phase = signal.bode(mdl,w) w,mag,phase = signal.bode(mdl,w)
f=w/(2*np.pi) f=w/(2*np.pi)
ax=fig.axes[0] ax=fig.axes[0]
@@ -490,8 +521,24 @@ class PBTuning:
ax.semilogx(f, phase,'-k',lw=2) # Bode phase plot 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')) # 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): def bode(mdl):
w,mag,phase = signal.bode(mdl) w,mag,phase = signal.bode(mdl,1000)
f=w/(2*np.pi) f=w/(2*np.pi)
fig = plt.figure() fig = plt.figure()
ax = fig.add_subplot(2, 1, 1) ax = fig.add_subplot(2, 1, 1)
@@ -626,6 +673,10 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
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]+'c.npz',motor=mot,amp=50,minFrq=300,maxFrq=1500,tSec=10)
tune.init_stage() tune.init_stage()
tune.bode_chirp(openloop=True,file=file[:-4]+'d.npz',motor=mot,amp=100,minFrq=300,maxFrq=2000,tSec=10) 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() plt.show()
#------------------ Main Code ---------------------------------- #------------------ Main Code ----------------------------------
#ssh_test() #ssh_test()

112
python/helicalscan.py
View File

@@ -470,9 +470,9 @@ class HelicalScan:
p=np.ndarray((param.shape[0], 3)) p=np.ndarray((param.shape[0], 3))
for i in range(2): for i in range(2):
(z_i, y_i, x_i, r_i, phi_i)=param[i] (z_i, y_i, x_i, r_i, phi_i)=param[i]
p[i,0]=x_i+r_i*np.sin(phi_i+w) # x= x_i+r_i*cos(phi_i+w)+cx p[i,0]=x_i+r_i*np.cos(phi_i+w) # x= x_i+r_i*cos(phi_i+w)+cx
p[i,1]=y_i # y= y_i p[i,1]=y_i # y= y_i
p[i,2]=z_i+r_i*np.cos(phi_i+w) # z= z_i+r_i*sin(phi_i*w) p[i,2]=z_i-r_i*np.sin(phi_i+w) # z= z_i+r_i*sin(phi_i*w)
v=p[1]-p[0] v=p[1]-p[0]
#for y = 0..1: #for y = 0..1:
#v=v*y #v=v*y
@@ -495,9 +495,9 @@ class HelicalScan:
p=np.ndarray((param.shape[0], 3)) p=np.ndarray((param.shape[0], 3))
for i in range(2): for i in range(2):
(z_i, y_i, x_i, r_i, phi_i)=param[i] (z_i, y_i, x_i, r_i, phi_i)=param[i]
p[i,0]=x_i+r_i*np.sin(phi_i+w) # x= x_i+r_i*cos(phi_i+w)+cx p[i,0]=x_i+r_i*np.cos(phi_i+w) # x= x_i+r_i*cos(phi_i+w)+cx
p[i,1]=y_i # y= y_i p[i,1]=y_i # y= y_i
p[i,2]=z_i+r_i*np.cos(phi_i+w) # z= z_i+r_i*sin(phi_i*w) p[i,2]=z_i-r_i*np.sin(phi_i+w) # z= z_i+r_i*sin(phi_i*w)
v=p[1]-p[0] v=p[1]-p[0]
#for y = 0..1: #for y = 0..1:
#v=v*y #v=v*y
@@ -512,11 +512,13 @@ class HelicalScan:
return res return res
def calcParam(self): def calcParamSim(self):
#simulated test values
n = 3.; n = 3.;
per = 1.; per = 1.;
w = 2 * np.pi * per / n * np.arange(n) w = 2 * np.pi * per / n * np.arange(n)
p = ((2.3, .71, 4.12, 10.6 * d2r),(6.2, .45, 3.2, 45.28 * d2r)) # (y, bias, ampl, phi) #p = ((2.3, .71, 4.12, 10.6 * d2r),(6.2, .45, 3.2, 45.28 * d2r)) # (y, bias, ampl, phi)
p = ((2.3, -100., 10, 10. * d2r),(6.2, 100., 10., -10. * d2r)) # (y, bias, ampl, phi)
self.param = param = np.ndarray((len(p), 5)) self.param = param = np.ndarray((len(p), 5))
z = 14.5 # fix z position z = 14.5 # fix z position
for i in range(2): for i in range(2):
@@ -531,6 +533,53 @@ class HelicalScan:
print param print param
def calcParam(self,x=((-241.,96.,-53.),(-162.,-293.,246.)),
y=(575.,175.),
z=((-1401.,-1401.,-1802.),(-1802.,-1303.,-1402.))):
# #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
#real measured values:
#y : 2x1 array : y position were the measurements were taken
#x : 3x2 array : 3 measurements at angle 0,120,240 for y[0] and y[1]
#z : 3x2 array : 3 measurements at angle 0,120,240 for y[0] and y[1]
# the z value is only used to find a rought bias of z
assert(len(y)==2)
n = float(len(x[0])) #number of angles
per = 1 #number of rotations
self.param = param = np.ndarray((2,5))
for i in range(len(y)):
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
param[i, 0] = HelicalScan.meas_rot_ctr(z[i])[0]
param[i, 1] = y[i]
param[i, 2:] = HelicalScan.meas_rot_ctr(x[i]) # (bias,ampl,phase)
(bias, ampl, phase) = param[i][2:]
#check correctness of center:
w = 2 * np.pi * per / n * np.arange(n)
x_ = ampl * np.cos(w + phase) + bias
print(x_)
(dx,dz,w,y_) = (0,0,0,y[0])
print 'input : dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r*1000.,y_)
(cx,cz,w,fy) = self.inv_transform(dx,dz,w,y_)
print 'inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r*1000.,fy)
(dx, dz, w, y_) = (0,0,0,y[1])
print 'input : dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx, dz, w / d2r * 1000., y_)
(cx, cz, w, fy) = self.inv_transform(dx, dz, w, y_)
print 'inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx, cz, w / d2r * 1000., fy)
print param
def pltOrig(self,m,h=None): def pltOrig(self,m,h=None):
ax=self.ax ax=self.ax
# m is a 4x4 matrix. the transformed matrix # m is a 4x4 matrix. the transformed matrix
@@ -646,7 +695,9 @@ class HelicalScan:
n=len(y) n=len(y)
f = np.fft.fft(y) f = np.fft.fft(y)
idx=int(per) idx=int(per)
bias=np.absolute(f[0]/n) #bias=np.absolute(f[0]/n)
assert(np.imag(f[0])==0.)
bias=np.real(f[0]/n)
phase=np.angle(f[idx]) phase=np.angle(f[idx])
ampl=np.absolute(f[idx])*2/n ampl=np.absolute(f[idx])*2/n
return (bias,ampl,phase) return (bias,ampl,phase)
@@ -689,7 +740,7 @@ open forward
define(p1_x='L13', p1_y='L14', p1_z='L15') define(p1_x='L13', p1_y='L14', p1_z='L15')
define(scale='L16') define(scale='L16')
//send 1"fwd_inp(%f) %f %f %f %f\\n",D0,qCX,qCZ,qW,qFY''') send 1"fwd_inp(%f) %f %f %f %f\\n",D0,qCX,qCZ,qW,qFY''')
for i in range(2): for i in range(2):
#https://stackoverflow.com/questions/3471999/how-do-i-merge-two-lists-into-a-single-list #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] l=[j for i in zip((i,) * param.shape[1], list(param[i])) for j in i]
@@ -697,12 +748,12 @@ open forward
prg.append(" W=qW") prg.append(" W=qW")
prg.append(" qW=qW*%g"%(d2r/1000.)) #scale from 1000*deg to rad prg.append(" qW=qW*%g"%(d2r/1000.)) #scale from 1000*deg to rad
prg.append(''' prg.append('''
p0_x=x_0+r_0*sin(phi_0+qW) p0_x=x_0+r_0*cos(phi_0+qW)
p1_x=x_1+r_1*sin(phi_1+qW) p1_x=x_1+r_1*cos(phi_1+qW)
p0_y=y_0 p0_y=y_0
p1_y=y_1 p1_y=y_1
p0_z=z_0+r_0*cos(phi_0+qW) p0_z=z_0-r_0*sin(phi_0+qW)
p1_z=z_1+r_1*cos(phi_1+qW) p1_z=z_1-r_1*sin(phi_1+qW)
scale=(qFY-(y_0))/(y_1-(y_0)) scale=(qFY-(y_0))/(y_1-(y_0))
p0_x=p0_x+scale*(p1_x-p0_x) p0_x=p0_x+scale*(p1_x-p0_x)
@@ -711,7 +762,7 @@ open forward
DX=qCX-p0_x DX=qCX-p0_x
DZ=qCZ-p0_z DZ=qCZ-p0_z
Y=qFY 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 //P1001+=1
D0=$000001c2; //B=$2 X=$40 Y=$80 Z=$100 hex(2+int('40',16)+int('80',16)+int('100',16)) -> 0x1c2 D0=$000001c2; //B=$2 X=$40 Y=$80 Z=$100 hex(2+int('40',16)+int('80',16)+int('100',16)) -> 0x1c2
close close
@@ -732,10 +783,10 @@ open inverse
define(p_x='L16', p_y='L17', p_z='L18') define(p_x='L16', p_y='L17', p_z='L18')
define(sclY='L19') define(sclY='L19')
define(scl='L20') define(scl='L20')
//if(D0>0) 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 send 1"inv_inp(%f) %f:%f %f:%f %f:%f %f:%f\\n",D0,DX,vDX,DZ,vDZ,W,vW,Y,vY
//else else
// send 1"inv_inp(%f) %f %f %f %f\\n",D0,DX,DZ,W,Y''') send 1"inv_inp(%f) %f %f %f %f\\n",D0,DX,DZ,W,Y''')
for i in range(2): for i in range(2):
# https://stackoverflow.com/questions/3471999/how-do-i-merge-two-lists-into-a-single-list # 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] l = [j for i in zip((i,) * param.shape[1], list(param[i])) for j in i]
@@ -744,12 +795,12 @@ open inverse
prg.append(" W=W*%g"%(d2r/1000.)) #scale from 1000*deg to rad prg.append(" W=W*%g"%(d2r/1000.)) #scale from 1000*deg to rad
prg.append(''' prg.append('''
p0_x=x_0+r_0*sin(phi_0+W) p0_x=x_0+r_0*cos(phi_0+W)
p1_x=x_1+r_1*sin(phi_1+W) p1_x=x_1+r_1*cos(phi_1+W)
p0_y=y_0 p0_y=y_0
p1_y=y_1 p1_y=y_1
p0_z=z_0+r_0*cos(phi_0+W) p0_z=z_0-r_0*sin(phi_0+W)
p1_z=z_1+r_1*cos(phi_1+W) p1_z=z_1-r_1*sin(phi_1+W)
sclY=(Y-(y_0))/(y_1-(y_0)) sclY=(Y-(y_0))/(y_1-(y_0))
p_x=p0_x+sclY*(p1_x-p0_x) p_x=p0_x+sclY*(p1_x-p0_x)
@@ -770,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 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(" 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 else
// send 1"inv_res %f %f %f %f\\n",qCX,qCZ,qW,qFY send 1"inv_res %f %f %f %f\\n",qCX,qCZ,qW,qFY
//P1002+=1 //P1002+=1
close close
''') ''')
@@ -1096,12 +1147,13 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
#hs.test_find_rot_ctr(n=5. ,per=1.,bias=2.31,ampl=4.12,phi=24.6) #hs.test_find_rot_ctr(n=5. ,per=1.,bias=2.31,ampl=4.12,phi=24.6)
hs.calcParam() hs.calcParam()
#hs.calcParamSim()
#hs.param[0]=(15,2,0,3,0)#(z_i, y_i, x_i, r_i,phi_i) #hs.param[0]=(15,2,0,3,0)#(z_i, y_i, x_i, r_i,phi_i)
#hs.param[1]=(15,4,0,3,0)#(z_i, y_i, x_i, r_i,phi_i) #hs.param[1]=(15,4,0,3,0)#(z_i, y_i, x_i, r_i,phi_i)
hs.param[0]=(-100, 100,0,50,0)#(z_i, y_i, x_i, r_i,phi_i) #hs.param[0]=(-100, 100,0,50,0)#(z_i, y_i, x_i, r_i,phi_i)
hs.param[1]=(-100,-100,0,70,0)#(z_i, y_i, x_i, r_i,phi_i) #hs.param[1]=(-100,-100,0,70,0)#(z_i, y_i, x_i, r_i,phi_i)
hs.test_coord_trf() #hs.test_coord_trf()
#hs.interactive_cx_cz_w_fy() #hs.interactive_cx_cz_w_fy()
#hs.interactive_dx_dz_w_y() #hs.interactive_dx_dz_w_y()
@@ -1109,7 +1161,7 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
#0:1 config simulated motors #0:1 config simulated motors
#1:2 config real motors #1:2 config real motors
#2:4 config coord trf #2:4 config coord trf
mode=6#5#4#0 mode=4#5#4#0
os.chdir(os.path.join(os.path.dirname(__file__),'../cfg')) os.chdir(os.path.join(os.path.dirname(__file__),'../cfg'))
if mode&1: if mode&1:
hs.download(file='sim_8_motors.cfg') hs.download(file='sim_8_motors.cfg')
@@ -1140,7 +1192,9 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
#hs.gen_prog(mode=1,cntHor=3,cntVert=6,hRng=(-5,5),wRng=(00,120000),smt=0,pt2pt_time=10) #hs.gen_prog(mode=1,cntHor=3,cntVert=6,hRng=(-5,5),wRng=(00,120000),smt=0,pt2pt_time=10)
#hs.gen_prog(mode=0,cntHor=3,cntVert=10,hRng=(-5,5),wRng=(0,120000)) #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=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=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=3,cntVert=20,hRng=(-5,5),wRng=(0,1200),smt=0,pt2pt_time=200) #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, cntHor=2, cntVert=2, wRng=(0, 360000), smt=0)
#hs.gen_prog(mode=1) #hs.gen_prog(mode=1)