helicalscan wip
This commit is contained in:
23
Readme.md
23
Readme.md
@@ -184,3 +184,26 @@ cd /net/slsfs-crtl/export/sf/ioc/modules/ESB_MX/zamofing_t/R3.14.12/
|
||||
export EPICS_CA_ADDR_LIST="129.129.109.255"
|
||||
export EPICS_CA_ADDR_LIST="129.129.126.255"
|
||||
caQtDm -macro 'P=SAR-ESB_MX' ESB_MX_exp
|
||||
|
||||
|
||||
|
||||
PPMAC=MOTTEST-CPPM-CRM0485
|
||||
ssh root@$PPMAC sendgetsends -1
|
||||
send 1"SampleMessage\n"
|
||||
&1p ->this will trigger:forward kinematic
|
||||
cpx pmatch ->this will trigger:forward kinematic
|
||||
|
||||
|
||||
cpx ;linear abs; X0Y0Z0B0 ->this will trigger: inverse
|
||||
|
||||
cpx ;linear abs; X1.4678795645244058 Y18.549693638496166, B0.0 Z2.3
|
||||
|
||||
#7j=1.4678795645244058
|
||||
#8j=18.549693638496166
|
||||
#1j=0.0
|
||||
#2j=2.3
|
||||
|
||||
#7j=0 //cx
|
||||
#8j=0 //cy
|
||||
#1j=0.0 //w
|
||||
#2j=2.3 //fx
|
||||
|
||||
47
cfg/coordTrf.cfg
Normal file
47
cfg/coordTrf.cfg
Normal file
@@ -0,0 +1,47 @@
|
||||
|
||||
&1
|
||||
//#1-> 0.00001X+ 0.00001Y + A
|
||||
//#2-> +1. X + .5Y + 0.01A
|
||||
//#3-> + .5X +1. Y + 0.01A
|
||||
|
||||
#1-> A
|
||||
#2-> X
|
||||
#3-> Y
|
||||
|
||||
Coord[1].AltFeedRate=0
|
||||
Coord[1].Tm=1 //1ms time
|
||||
Coord[1].Ta=1
|
||||
Coord[1].Td=1
|
||||
Coord[1].Ts=0
|
||||
|
||||
//Parker: Continous Force 4N -> assume 1kg load -> acceleration=a=F/m=4m/s^2
|
||||
//do not use time to accelerate but acceleration
|
||||
//4m/s^2=4um/(ms)^2 Motor units=um -> JogTa= -1ms^2/4mu -2.5-> 4m/s^2 -25->0.4m/s^2
|
||||
Motor[2].JogTs=0
|
||||
Motor[2].JogTa=-2.5
|
||||
Motor[3].JogTs=0
|
||||
Motor[3].JogTa=-2.5
|
||||
|
||||
|
||||
Motor[1].MaxSpeed=360
|
||||
Motor[2].MaxSpeed=50
|
||||
Motor[3].MaxSpeed=50
|
||||
|
||||
open prog 1
|
||||
//this uses jogspeed
|
||||
rapid abs
|
||||
X(10000) Y(0) A(0)
|
||||
X(0) Y(10000) A(0)
|
||||
X(0) Y(0) A(36000)
|
||||
X(0) Y(0) A(0)
|
||||
close
|
||||
|
||||
open prog 2
|
||||
//this uses Coord[1].Tm and limits with MaxSpeed
|
||||
linear abs
|
||||
X(10000) Y(0) A(0)
|
||||
X(0) Y(10000) A(0)
|
||||
X(0) Y(0) A(0)
|
||||
X(0) Y(0) A(36000)
|
||||
X(0) Y(0) A(0)
|
||||
close
|
||||
@@ -7,52 +7,4 @@
|
||||
|
||||
!torqueCtrl()
|
||||
!init()
|
||||
|
||||
|
||||
&1
|
||||
//#1-> 0.00001X+ 0.00001Y + A
|
||||
//#2-> +1. X + .5Y + 0.01A
|
||||
//#3-> + .5X +1. Y + 0.01A
|
||||
|
||||
#1-> A
|
||||
#2-> X
|
||||
#3-> Y
|
||||
|
||||
Coord[1].AltFeedRate=0
|
||||
Coord[1].Tm=1 //1ms time
|
||||
Coord[1].Ta=1
|
||||
Coord[1].Td=1
|
||||
Coord[1].Ts=0
|
||||
|
||||
//Parker: Continous Force 4N -> assume 1kg load -> acceleration=a=F/m=4m/s^2
|
||||
//do not use time to accelerate but acceleration
|
||||
//4m/s^2=4um/(ms)^2 Motor units=um -> JogTa= -1ms^2/4mu -2.5-> 4m/s^2 -25->0.4m/s^2
|
||||
Motor[2].JogTs=0
|
||||
Motor[2].JogTa=-2.5
|
||||
Motor[3].JogTs=0
|
||||
Motor[3].JogTa=-2.5
|
||||
|
||||
|
||||
Motor[1].MaxSpeed=360
|
||||
Motor[2].MaxSpeed=50
|
||||
Motor[3].MaxSpeed=50
|
||||
|
||||
open prog 1
|
||||
//this uses jogspeed
|
||||
rapid abs
|
||||
X(10000) Y(0) A(0)
|
||||
X(0) Y(10000) A(0)
|
||||
X(0) Y(0) A(36000)
|
||||
X(0) Y(0) A(0)
|
||||
close
|
||||
|
||||
open prog 2
|
||||
//this uses Coord[1].Tm and limits with MaxSpeed
|
||||
linear abs
|
||||
X(10000) Y(0) A(0)
|
||||
X(0) Y(10000) A(0)
|
||||
X(0) Y(0) A(0)
|
||||
X(0) Y(0) A(36000)
|
||||
X(0) Y(0) A(0)
|
||||
close
|
||||
|
||||
!coordTrf()
|
||||
66
cfg/mx-stage_sim.cfg
Normal file
66
cfg/mx-stage_sim.cfg
Normal file
@@ -0,0 +1,66 @@
|
||||
//simulated stage without real motors needed
|
||||
$$$***
|
||||
!common()
|
||||
//!common(PhaseFreq=20000,PhasePerServo=4)
|
||||
//!common(PhaseFreq=20000,PhasePerServo=1)
|
||||
//!common(PhaseFreq=40000)
|
||||
|
||||
//Mot 1: Rotation stage LS Mecapion MDM-DC06DNC0H 32 poles = 1 rev = 16*2048=32768 phase_step
|
||||
//Enc 1: Rotation stage LS Mecapion 1 rev = 1048576 enc_steps
|
||||
|
||||
//Mot 2: Stage Y Parker MX80L D11 25mm one pole cycle = 13mm = 2048 phase_step
|
||||
//Enc 2: Stage Y Parker MX80L D11 inc_enc 20nm one pole cycle = 13mm = 650000 enc_step (20nm/enc_step)
|
||||
|
||||
//Mot 3: Stage X Parker MX80L D11 25mm one pole cycle = 13mm = 2048 phase_step
|
||||
//Enc 3: Stage X Parker MX80L D11 inc_enc 20nm one pole cycle = 13mm = 650000 enc_step (20nm/enc_step)
|
||||
|
||||
//Mot/Enc 4: camera base plate X
|
||||
// OBSOLETE: Enc 4: Interferometer 1
|
||||
|
||||
//Mot/Enc 5: camera base plate Y
|
||||
// OBSOLETE Enc 5: Interferometer 2
|
||||
|
||||
//Mot 6: Backlight 2.3A
|
||||
|
||||
//Mot 7: Stada Stepper: 670mA 200 poles 1 rev = 100*2048 phase_step (2 stepper motor)
|
||||
//Enc 7: Renishaw absolute BiSS
|
||||
|
||||
//Mot 8: Stada Stepper: 670mA 200 poles 1 rev = 100*2048 phase_step (2 stepper motor)
|
||||
//Enc 8: Renishaw absolute BiSS
|
||||
|
||||
!encoder_sim(enc=1,tbl=1,mot=1)
|
||||
!motor(mot=1,dirCur=0,contCur=100,peakCur=100,timeAtPeak=1,JogSpeed=8.,numPhase=3,invDir=True)
|
||||
Motor[1].pLimits=0
|
||||
//Motor[1].pDac=PowerBrick[0].MacroOutA[0][0].a
|
||||
//Motor[1].pAdc=PowerBrick[0].MacroInA[0][1].a
|
||||
|
||||
!encoder_sim(enc=2,tbl=2,mot=2)
|
||||
!motor(mot=2,dirCur=0,contCur=100,peakCur=1000,timeAtPeak=1,JogSpeed=8.,numPhase=3,invDir=True)
|
||||
Motor[2].pLimits=0
|
||||
|
||||
!encoder_sim(enc=3,tbl=3,mot=3)
|
||||
!motor(mot=3,dirCur=0,contCur=100,peakCur=1000,timeAtPeak=1,JogSpeed=8.,numPhase=3,invDir=True)
|
||||
Motor[3].pLimits=0
|
||||
|
||||
!encoder_sim(enc=4,tbl=4,mot=4)
|
||||
!motor(mot=4,dirCur=0,contCur=100,peakCur=1000,timeAtPeak=1,JogSpeed=8.,numPhase=3,invDir=True)
|
||||
Motor[4].pLimits=0
|
||||
|
||||
!encoder_sim(enc=5,tbl=5,mot=5)
|
||||
!motor(mot=5,dirCur=0,contCur=100,peakCur=1000,timeAtPeak=1,JogSpeed=8.,numPhase=3,invDir=True)
|
||||
Motor[5].pLimits=0
|
||||
|
||||
!encoder_sim(enc=6,tbl=6,mot=6)
|
||||
!motor(mot=6,dirCur=0,contCur=100,peakCur=1000,timeAtPeak=1,JogSpeed=8.,numPhase=3,invDir=True)
|
||||
Motor[6].pLimits=0
|
||||
|
||||
!encoder_sim(enc=7,tbl=7,mot=7)
|
||||
!motor(mot=7,dirCur=0,contCur=100,peakCur=1000,timeAtPeak=1,JogSpeed=8.,numPhase=3,invDir=True)
|
||||
Motor[7].pLimits=0
|
||||
|
||||
!encoder_sim(enc=8,tbl=8,mot=8)
|
||||
!motor(mot=8,dirCur=0,contCur=100,peakCur=1000,timeAtPeak=1,JogSpeed=8.,numPhase=3,invDir=True)
|
||||
Motor[8].pLimits=0
|
||||
|
||||
|
||||
!coordTrf()
|
||||
@@ -7,7 +7,27 @@
|
||||
# *-----------------------------------------------------------------------*
|
||||
'''
|
||||
tools to setup and execute a helical scan of a cristal
|
||||
#THIS IS JUST TESTING CODE TO SOLVE FINDING THE ROTATION CENTER
|
||||
|
||||
motors CX CZ RY FY
|
||||
7 8 1 2
|
||||
Mot 1: Rotation stage LS Mecapion MDM-DC06DNC0H 32 poles = 1 rev = 16*2048=32768 phase_step
|
||||
Mot 2: Stage Y Parker MX80L D11 25mm one pole cycle = 13mm = 2048 phase_step
|
||||
Mot 3: Stage X Parker MX80L D11 25mm one pole cycle = 13mm = 2048 phase_step
|
||||
Mot/Enc 4: camera base plate X
|
||||
Mot/Enc 5: camera base plate Y
|
||||
Mot 6: Backlight 2.3A
|
||||
Mot 7: Stada Stepper: 670mA 200 poles 1 rev = 100*2048 phase_step (2 stepper motor)
|
||||
Mot 8: Stada Stepper: 670mA 200 poles 1 rev = 100*2048 phase_step (2 stepper motor)
|
||||
|
||||
verbose bits:
|
||||
#1 basic info
|
||||
#2 plot sorting steps
|
||||
4 list program
|
||||
#4 upload progress
|
||||
#8 plot gather path
|
||||
|
||||
|
||||
|
||||
'''
|
||||
|
||||
import os, sys, json
|
||||
@@ -17,7 +37,7 @@ import matplotlib.pyplot as plt
|
||||
import mpl_toolkits.mplot3d as plt3d
|
||||
import matplotlib.animation as anim
|
||||
from matplotlib.widgets import Slider
|
||||
|
||||
import subprocess as sprc
|
||||
from utilities import *
|
||||
|
||||
d2r=2*np.pi/360
|
||||
@@ -129,12 +149,12 @@ class HelicalScan:
|
||||
def test_coord_trf(self):
|
||||
self.calcParam()
|
||||
param = self.param
|
||||
y, w, dx, dz = (4.3, .1, 0.2, 0.3)
|
||||
print 'input : fy:%.3g dx:%.3g dz:%.3g w:%.3g' % (y, dx, dz, w / d2r)
|
||||
(fy, w, cx, cz) = self.inv_transform(y, w, dx, dz)
|
||||
print 'inv_trf: fy:%.3g cx:%.3g cz:%.3g w:%.3g' % (fy, cx, cz, w / d2r)
|
||||
(y, w, dx, dz) = self.fwd_transform(fy, w, cx, cz)
|
||||
print 'fwd_trf: fy:%.3g dx:%.3g dz:%.3g w:%.3g' % (y, dx, dz, w / d2r)
|
||||
dx, dz, w, y, = (0.2,0.3,0.1,4.3)
|
||||
print 'input : dx:%.3g dz:%.3g w:%.3g fy:%.3g' % (dx,dz,w/d2r,y)
|
||||
(cx,cz,w,fy) = self.inv_transform(dx,dz,w,y)
|
||||
print 'inv_trf: cx:%.3g cz:%.3g w:%.3g fy:%.3g' % (cx,cz,w/d2r,fy)
|
||||
(dx, dz,w,y) = self.fwd_transform(cx,cz,w,fy)
|
||||
print 'fwd_trf: dx:%.3g dz:%.3g w:%.3g fy:%.3g' % (dx,dz,w/d2r,y)
|
||||
|
||||
# plt.ion()
|
||||
# fig = plt.figure()
|
||||
@@ -145,8 +165,7 @@ class HelicalScan:
|
||||
# def my_anim_func3(self,idx):
|
||||
# self.hCrist,pt=self.pltCrist(cx=0,ty=0,cz=0,w=10*idx*d2r,h=self.hCrist)
|
||||
|
||||
|
||||
def interactive_fy_cx_cz_w(self):
|
||||
def interactive_cx_cz_w_fy(self):
|
||||
fig = plt.figure()
|
||||
self.manip=False#True#False
|
||||
self.ax=ax=plt3d.Axes3D(fig,[0.02, 0.15, 0.96, 0.83])
|
||||
@@ -158,24 +177,24 @@ class HelicalScan:
|
||||
ctr=param[:,0:3].mean(0)[::-1]
|
||||
self.axSetCenter(ctr,10)
|
||||
|
||||
axFy = plt.axes([0.1, 0.01, 0.8, 0.02])
|
||||
axCx = plt.axes([0.1, 0.04, 0.8, 0.02])
|
||||
axCz = plt.axes([0.1, 0.07, 0.8, 0.02])
|
||||
axW = plt.axes([0.1, 0.10, 0.8, 0.02])
|
||||
axCx=plt.axes([0.1, 0.01, 0.8, 0.02])
|
||||
axCz=plt.axes([0.1, 0.04, 0.8, 0.02])
|
||||
axW =plt.axes([0.1, 0.07, 0.8, 0.02])
|
||||
axFy=plt.axes([0.1, 0.10, 0.8, 0.02])
|
||||
if self.manip:
|
||||
lz=ax.get_xlim()
|
||||
lx=ax.get_ylim()
|
||||
ly=ax.get_zlim()
|
||||
else:
|
||||
lx = ly=lz=[-5,5]
|
||||
self.sldFy = sFy = Slider(axFy, 'fy', ly[0], ly[1], valinit=(ly[0]+ly[1])/2.)
|
||||
self.sldCx = sCx = Slider(axCx, 'cx', lx[0], lx[1], valinit=(lx[0]+lx[1])/2.)
|
||||
self.sldCz = sCz = Slider(axCz, 'cz', lz[0], lz[1], valinit=(lz[0]+lz[1])/2.)
|
||||
self.sldW =sW = Slider(axW, 'ang', -180., 180.0, valinit=0)
|
||||
sFy.on_changed(self.update_fy_cx_cz_w)
|
||||
sCx.on_changed(self.update_fy_cx_cz_w)
|
||||
sCz.on_changed(self.update_fy_cx_cz_w)
|
||||
sW.on_changed(self.update_fy_cx_cz_w)
|
||||
self.sldCx=sCx=Slider(axCx, 'cx', lx[0], lx[1], valinit=(lx[0]+lx[1])/2.)
|
||||
self.sldCz=sCz=Slider(axCz, 'cz', lz[0], lz[1], valinit=(lz[0]+lz[1])/2.)
|
||||
self.sldW =sW =Slider(axW, 'ang', -180., 180.0, valinit=0)
|
||||
self.sldFy=sFy=Slider(axFy, 'fy', ly[0], ly[1], valinit=(ly[0]+ly[1])/2.)
|
||||
sCx.on_changed(self.update_cx_cz_w_fy)
|
||||
sCz.on_changed(self.update_cx_cz_w_fy)
|
||||
sW.on_changed(self.update_cx_cz_w_fy)
|
||||
sFy.on_changed(self.update_cx_cz_w_fy)
|
||||
|
||||
hCrist,pt=self.pltCrist()
|
||||
|
||||
@@ -186,18 +205,19 @@ class HelicalScan:
|
||||
(z_i, y_i, x_i, r_i, phi_i)=param[i]
|
||||
p[i,0]=x_i+r_i*np.sin(phi_i) # x= x_i+r_i*cos(phi_i+w)+cx
|
||||
p[i,1]=y_i # y= y_i
|
||||
p[i,2] =z_i+r_i*np.cos(phi_i) # z= z_i+r_i*sin(phi_i*w)
|
||||
p[i,2]=z_i+r_i*np.cos(phi_i) # z= z_i+r_i*sin(phi_i*w)
|
||||
print p
|
||||
ofs=(p[1]+p[0])/2. # = center of the cristal
|
||||
|
||||
m=Trf.trans(*ofs); self.hOrig=self.pltOrig(m)
|
||||
plt.show()
|
||||
|
||||
def update_fy_cx_cz_w(self,val):
|
||||
fy = self.sldFy.val
|
||||
|
||||
def update_cx_cz_w_fy(self,val):
|
||||
cx = self.sldCx.val
|
||||
cz = self.sldCz.val
|
||||
w = self.sldW.val
|
||||
w = self.sldW.val
|
||||
fy = self.sldFy.val
|
||||
if self.manip:
|
||||
param = self.param
|
||||
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
|
||||
@@ -218,7 +238,8 @@ class HelicalScan:
|
||||
#l.set_ydata(amp * np.sin(2 * np.pi * freq * t))
|
||||
self.fig.canvas.draw_idle()
|
||||
|
||||
def interactive_y_dx_dz_w(self):
|
||||
|
||||
def interactive_dx_dz_w_y(self):
|
||||
fig = plt.figure()
|
||||
self.ax=ax=plt3d.Axes3D(fig,[0.02, 0.15, 0.96, 0.83])
|
||||
ax.set_xlabel('Z');ax.set_ylabel('X');ax.set_zlabel('Y')
|
||||
@@ -230,21 +251,21 @@ class HelicalScan:
|
||||
ctr=(0,0,0)
|
||||
self.axSetCenter(ctr,10)
|
||||
|
||||
axY = plt.axes([0.1, 0.01, 0.8, 0.02])
|
||||
axDx = plt.axes([0.1, 0.04, 0.8, 0.02])
|
||||
axDz = plt.axes([0.1, 0.07, 0.8, 0.02])
|
||||
axW = plt.axes([0.1, 0.10, 0.8, 0.02])
|
||||
axDx=plt.axes([0.1, 0.01, 0.8, 0.02])
|
||||
axDz=plt.axes([0.1, 0.04, 0.8, 0.02])
|
||||
axW =plt.axes([0.1, 0.07, 0.8, 0.02])
|
||||
axY =plt.axes([0.1, 0.10, 0.8, 0.02])
|
||||
|
||||
lx=[-1,1];ly=[0,1];lz=[-1,1]
|
||||
ly = param[:,1]
|
||||
self.sldY = sY = Slider(axY, 'y', ly[0], ly[1], valinit=(ly[0]+ly[1])/2.)
|
||||
self.sldDx = sDx = Slider(axDx, 'dx', lx[0], lx[1], valinit=(lx[0]+lx[1])/2.)
|
||||
self.sldDz = sDz = Slider(axDz, 'dz', lz[0], lz[1], valinit=(lz[0]+lz[1])/2.)
|
||||
self.sldW =sW = Slider(axW, 'ang', -180., 180.0, valinit=0)
|
||||
sY.on_changed(self.update_y_dx_dz_w)
|
||||
sDx.on_changed(self.update_y_dx_dz_w)
|
||||
sDz.on_changed(self.update_y_dx_dz_w)
|
||||
sW.on_changed(self.update_y_dx_dz_w)
|
||||
self.sldDx=sDx=Slider(axDx, 'dx', lx[0], lx[1], valinit=(lx[0]+lx[1])/2.)
|
||||
self.sldDz=sDz=Slider(axDz, 'dz', lz[0], lz[1], valinit=(lz[0]+lz[1])/2.)
|
||||
self.sldW =sW =Slider(axW, 'ang', -180., 180.0, valinit=0)
|
||||
self.sldY =sY =Slider(axY, 'y', ly[0], ly[1], valinit=(ly[0]+ly[1])/2.)
|
||||
sDx.on_changed(self.update_dx_dz_w_y)
|
||||
sDz.on_changed(self.update_dx_dz_w_y)
|
||||
sW.on_changed(self.update_dx_dz_w_y)
|
||||
sY.on_changed(self.update_dx_dz_w_y)
|
||||
|
||||
|
||||
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
|
||||
@@ -263,21 +284,23 @@ class HelicalScan:
|
||||
self.hCrist=hCrist;self.fig=fig
|
||||
plt.show()
|
||||
|
||||
def update_y_dx_dz_w(self,val):
|
||||
y = self.sldY.val
|
||||
|
||||
def update_dx_dz_w_y(self,val):
|
||||
dx = self.sldDx.val
|
||||
dz = self.sldDz.val
|
||||
w = self.sldW.val
|
||||
y = self.sldY.val
|
||||
w=w*d2r
|
||||
(fy, w, cx, cz)=self.inv_transform(y,w,dx,dz)
|
||||
|
||||
(cx,cz,w,fy)=self.inv_transform(dx,dz,w,y)
|
||||
#print (cx,cz,w,fy)
|
||||
self.hCrist,pt=self.pltCrist(-fy,-cx,-cz,w,self.hCrist)
|
||||
self.fig.canvas.draw_idle()
|
||||
|
||||
def fwd_transform(self,fy,w,cx,cz):
|
||||
|
||||
def fwd_transform(self,cx,cz,w,fy):
|
||||
#cx,cy: coarse stage
|
||||
#input: fy,w,cx,cz
|
||||
#output: y,w,dx,dz
|
||||
#input: cx,cz,w,fy
|
||||
#output: dx,dz,w,y
|
||||
# param[i]=(z_i, y_i, x_i, r_i,phi_i)
|
||||
|
||||
param=self.param
|
||||
@@ -297,12 +320,13 @@ class HelicalScan:
|
||||
dx=cx-v[0]
|
||||
dz=cz-v[2]
|
||||
y=v[1]
|
||||
res=(y,w,dx,dz)
|
||||
res=(dx,dz,w,y)
|
||||
return res
|
||||
|
||||
def inv_transform(self,y,w,dx=0,dz=0):
|
||||
#input: y,w,dx,dz
|
||||
#output: y,w,cx,cz
|
||||
|
||||
def inv_transform(self,dx,dz,w,y):
|
||||
#input: dx,dz,w,y
|
||||
#output: cx,cz,w,fy
|
||||
#dx,dy: deviation from cristal center line
|
||||
param=self.param
|
||||
p=np.ndarray((param.shape[0], 3))
|
||||
@@ -321,7 +345,7 @@ class HelicalScan:
|
||||
cx=dx+v[0]
|
||||
cz=dz+v[2]
|
||||
fy=v[1]
|
||||
res=(fy,w,cx,cz)
|
||||
res=(cx,cz,w,fy)
|
||||
return res
|
||||
|
||||
|
||||
@@ -366,6 +390,7 @@ class HelicalScan:
|
||||
hb.set_3d_properties((o[1], o[1] + b[1]))
|
||||
return h
|
||||
|
||||
|
||||
def pltCrist(self,fy=0,cx=0,cz=0,w=0,h=None):
|
||||
#h are the handles
|
||||
if h is None:
|
||||
@@ -414,6 +439,7 @@ class HelicalScan:
|
||||
#h+=(hs,hp[0])
|
||||
return (h,pt)
|
||||
|
||||
|
||||
@staticmethod
|
||||
def meas_rot_ctr(y,per=1):
|
||||
# find the amplitude bias and phase of an equidistant sampled sinus
|
||||
@@ -436,11 +462,252 @@ class HelicalScan:
|
||||
ax.set_ylim(v[0]-l2, v[0]+l2);
|
||||
ax.set_zlim(v[1]-l2, v[1]+l2)
|
||||
|
||||
def my_anim_func(idx,hs,horig):
|
||||
print('anim')
|
||||
a=idx*.01*2*np.pi
|
||||
m=Trf.rotY(a)
|
||||
hs.pltOrig(m,horig)
|
||||
|
||||
def gen_coord_trf_code(self,file=None,host=None):
|
||||
param=self.param
|
||||
prg = []
|
||||
prg.append('''
|
||||
// Set the motors as inverse kinematic axes in CS 1
|
||||
//motors CX CZ RY FY
|
||||
// 7 8 1 2
|
||||
|
||||
&1
|
||||
#7->I
|
||||
#8->I
|
||||
#1->I
|
||||
#2->I
|
||||
|
||||
open forward
|
||||
define(qCX='L7', qCZ='L8', qW='L1', qFY='L2')
|
||||
define(DX='C6', DZ='C8', W='C1', Y='C7')
|
||||
//coord X Z B Y
|
||||
define(p0_x='L10', p0_y='L11', p0_z='L12')
|
||||
define(p1_x='L13', p1_y='L14', p1_z='L15')
|
||||
define(f='L16')
|
||||
|
||||
send 1"forward kinematic\\n"''')
|
||||
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]
|
||||
prg.append(" define(z_%i=%g, y_%i=%g, x_%i=%g, r_%i=%g, phi_%i=%g)"%tuple(l))
|
||||
prg.append('''
|
||||
p0_x=x_0+r_0*sin(phi_0+W)
|
||||
p1_x=x_1+r_1*sin(phi_1+W)
|
||||
p0_y=y_0
|
||||
p1_y=y_1
|
||||
p0_z=z_0+r_0*cos(phi_0+W)
|
||||
p1_z=z_1+r_1*cos(phi_0+W)
|
||||
|
||||
f=(qFY-y_0)/(y_1-y_0)
|
||||
p0_x=p0_x+f*(p1_x-p0_x)
|
||||
p0_y=p0_y+f*(p1_y-p0_y)
|
||||
p0_z=p0_z+f*(p1_z-p0_z)
|
||||
DX=qCX-p0_x
|
||||
DZ=qCZ-p0_z
|
||||
Y=qFY
|
||||
W=qW
|
||||
|
||||
D0=$000001c2; //B=$2 X=$40 Y=$80 Z=$100 hex(2+int('40',16)+int('80',16)+int('100',16)) -> 0x1c2
|
||||
close
|
||||
''')
|
||||
|
||||
prg.append('''
|
||||
open inverse
|
||||
define(DX='C6', DZ='C8', W='C1', Y='C7')
|
||||
//coord X Z B Y
|
||||
//D0 is set to $000001c2
|
||||
define(qCX='L7', qCZ='L8', qW='L1', qFY='L2')
|
||||
define(p0_x='L10', p0_y='L11', p0_z='L12')
|
||||
define(p1_x='L13', p1_y='L14', p1_z='L15')
|
||||
define(f='L16')
|
||||
|
||||
send 1"inverse kinematic\\n"''')
|
||||
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]
|
||||
prg.append(" define(z_%i=%g, y_%i=%g, x_%i=%g, r_%i=%g, phi_%i=%g)" % tuple(l))
|
||||
prg.append('''
|
||||
p0_x=x_0+r_0*sin(phi_0+W)
|
||||
p1_x=x_1+r_1*sin(phi_1+W)
|
||||
p0_y=y_0
|
||||
p1_y=y_1
|
||||
p0_z=z_0+r_0*cos(phi_0+W)
|
||||
p1_z=z_1+r_1*cos(phi_0+W)
|
||||
|
||||
f=(qFY-y_0)/(y_1-y_0)
|
||||
p0_x=p0_x+f*(p1_x-p0_x)
|
||||
p0_y=p0_y+f*(p1_y-p0_y)
|
||||
p0_z=p0_z+f*(p1_z-p0_z)
|
||||
qCX=DX+p0_x
|
||||
qCZ=DZ+p0_z
|
||||
qFY=Y
|
||||
qW=W
|
||||
|
||||
close
|
||||
''')
|
||||
|
||||
if self.args.verbose & 4:
|
||||
for ln in prg:
|
||||
print(ln)
|
||||
if file is not None:
|
||||
fh = open(file, 'w')
|
||||
fh.write('\n'.join(prg))
|
||||
fh.close()
|
||||
if host is not None:
|
||||
cmd = '/home/zamofing_t/scripts/gpasciiCommander --host ' + host + ' ' + file
|
||||
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
|
||||
|
||||
def gen_prog(self,prgId=2,file=None,host=None,mode=0,**kwargs):
|
||||
'''
|
||||
kwargs:
|
||||
acq_per : acquire period: acquire data all acq_per servo loops (default=1)
|
||||
pt2pt_time : time to move from one point to the next point
|
||||
'''
|
||||
prg=[]
|
||||
acq_per=kwargs.get('acq_per',1)
|
||||
gather={"MaxSamples":1000000, "Period":acq_per}
|
||||
#Sys.ServoPeriod is dependent of !common() macro
|
||||
ServoPeriod= .2 #0.2ms
|
||||
#ServoPeriod = .05
|
||||
self.meta = {'timebase': ServoPeriod*gather['Period']}
|
||||
#channels=["Motor[1].ActPos","Motor[2].ActPos","Motor[3].ActPos"]
|
||||
channels=["Motor[7].ActPos","Motor[8].ActPos","Motor[1].ActPos","Motor[2].ActPos"]
|
||||
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'%(prgId))
|
||||
# this uses Coord[1].Tm and limits with MaxSpeed
|
||||
if mode==-1: #### jog a 10mm square
|
||||
pos=self.points
|
||||
prg.append(' linear abs')
|
||||
prg.append('X(%g) Y(%g)' % tuple(pos[0, :]))
|
||||
prg.append('dwell 10')
|
||||
prg.append('Gather.Enable=2')
|
||||
prg.append('jog2:10000')
|
||||
prg.append('dwell 100')
|
||||
prg.append('jog3:10000')
|
||||
prg.append('dwell 100')
|
||||
prg.append('jog2:-10000')
|
||||
prg.append('dwell 100')
|
||||
prg.append('jog3:-10000')
|
||||
prg.append('dwell 100')
|
||||
prg.append('Gather.Enable=0')
|
||||
elif mode==0: #### linear motion
|
||||
pos=self.points
|
||||
prg.append(' linear abs')
|
||||
prg.append('X(%g) Y(%g)' % tuple(pos[0, :]))
|
||||
prg.append('dwell 10')
|
||||
prg.append('Gather.Enable=2')
|
||||
prg.append(' linear abs')
|
||||
for idx in range(pos.shape[0]):
|
||||
prg.append('X%g Y%g'%tuple(pos[idx,:]))
|
||||
prg.append('dwell 100')
|
||||
prg.append('Gather.Enable=0')
|
||||
elif mode==1: #### pvt motion
|
||||
try:
|
||||
pt2pt_time=kwargs['pt2pt_time'] #how many ms to move to next point (pt2pt_time)
|
||||
except KeyError:
|
||||
print('missing pt2pt_time, use default=100ms')
|
||||
pt2pt_time=100.
|
||||
try:
|
||||
cnt=kwargs['cnt'] #move path multiple times
|
||||
except KeyError:
|
||||
cnt=1
|
||||
|
||||
try:
|
||||
pt=self.ptsCorr
|
||||
except AttributeError:
|
||||
pt=self.points
|
||||
vel=pt[2:,:]-pt[:-2,:]
|
||||
#pv is an array of posx posy velx vely
|
||||
pv=np.ndarray(shape=(pt.shape[0]+2,4),dtype=pt.dtype)
|
||||
pv[:]=np.NaN
|
||||
#pv[ 0,(0,1)]=2*pt[0,:]-pt[1,:]
|
||||
pv[ 0,(0,1)]=pt[0,:]
|
||||
pv[ 1:-1,(0,1)]=pt
|
||||
#pv[ -1,(0,1)]=2*pt[-1,:]-pt[-2,:]
|
||||
pv[ -1,(0,1)]=pt[-1,:]
|
||||
pv[(0,0,-1,-1),(2,3,2,3)]=0
|
||||
dist=pv[2:,(0,1)] - pv[:-2,(0,1)]
|
||||
pv[ 1:-1,(2,3)] = 1000.*dist/(2.*pt2pt_time)
|
||||
|
||||
prg.append(' linear abs')
|
||||
prg.append('X%g Y%g' % tuple(pv[0, (0,1)]))
|
||||
prg.append('dwell 10')
|
||||
prg.append('Gather.Enable=2')
|
||||
if cnt>1:
|
||||
prg.append('P100=%d'%cnt)
|
||||
prg.append('N100:')
|
||||
prg.append(' pvt%g abs'%pt2pt_time) #100ms to next position
|
||||
for idx in range(1,pv.shape[0]):
|
||||
prg.append('X%g:%g Y%g:%g'%tuple(pv[idx,(0,2,1,3)]))
|
||||
prg.append('X%g Y%g' % tuple(pv[-1, (0,1)]))
|
||||
if cnt>1:
|
||||
prg.append('dwell 10')
|
||||
prg.append('P100=P100-1')
|
||||
prg.append('if(P100>0)')
|
||||
prg.append('{')
|
||||
prg.append(' linear abs')
|
||||
prg.append('X%g Y%g' % tuple(pv[0, (0,1)]))
|
||||
prg.append('dwell 100')
|
||||
prg.append('goto 100')
|
||||
prg.append('}')
|
||||
else:
|
||||
prg.append('dwell 1000')
|
||||
prg.append('Gather.Enable=0')
|
||||
elif mode==2: #### spline motion
|
||||
try:
|
||||
pt2pt_time=kwargs['pt2pt_time'] #how many ms to move to next point (pt2pt_time)
|
||||
except KeyError:
|
||||
print('missing pt2pt_time, use default=100ms')
|
||||
pt2pt_time=100.
|
||||
pos=self.points
|
||||
pcor=np.ndarray(pos.shape,dtype=pos.dtype);pcor[:]=np.NaN
|
||||
pcor[(0,-1),:]=pos[(0,-1),:]
|
||||
pcor[1:-1,:]=(-pos[0:-2,:]+8*pos[1:-1,:]-pos[2:,:])/6.
|
||||
#pcor=pos
|
||||
prg.append(' linear abs')
|
||||
prg.append('X(%g) Y(%g)' % tuple(pcor[0, :]))
|
||||
prg.append('dwell 10')
|
||||
prg.append('Gather.Enable=2')
|
||||
prg.append(' spline%g abs'%pt2pt_time) #100ms to next position
|
||||
for idx in range(pcor.shape[0]):
|
||||
prg.append('X%g Y%g'%tuple(pcor[idx,:]))
|
||||
prg.append('dwell 100')
|
||||
prg.append('Gather.Enable=0')
|
||||
|
||||
prg.append('close')
|
||||
prg.append('&1\nb%dr\n'%prgId)
|
||||
if self.args.verbose & 4:
|
||||
for ln in prg:
|
||||
print(ln)
|
||||
|
||||
if file is not None:
|
||||
fh=open(file,'w')
|
||||
fh.write('\n'.join(prg))
|
||||
fh.close()
|
||||
if host is not None:
|
||||
cmd ='gpasciiCommander --host '+host+' '+ file
|
||||
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()
|
||||
self.prg=prg
|
||||
|
||||
|
||||
if __name__=='__main__':
|
||||
from optparse import OptionParser, IndentedHelpFormatter
|
||||
@@ -479,11 +746,13 @@ Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '
|
||||
|
||||
hs=HelicalScan(args)
|
||||
#hs.sequencer()
|
||||
hs.args.verbose=255;hs.calcParam();hs.gen_coord_trf_code('/tmp/helicalscan.cfg','MOTTEST-CPPM-CRM0485')
|
||||
#return
|
||||
hs.test_find_rot_ctr()
|
||||
hs.test_find_rot_ctr(n=5. ,per=1.,bias=2.31,ampl=4.12,phi=24.6)
|
||||
hs.test_coord_trf()
|
||||
hs.interactive_fy_cx_cz_w()
|
||||
hs.interactive_y_dx_dz_w()
|
||||
hs.interactive_cx_cz_w_fy()
|
||||
hs.interactive_dx_dz_w_y()
|
||||
|
||||
|
||||
#------------------ Main Code ----------------------------------
|
||||
|
||||
@@ -133,15 +133,15 @@
|
||||
borderopacity="1.0"
|
||||
inkscape:pageopacity="0.0"
|
||||
inkscape:pageshadow="2"
|
||||
inkscape:zoom="1.9573107"
|
||||
inkscape:cx="306.39127"
|
||||
inkscape:cy="814.96274"
|
||||
inkscape:zoom="1.8926215"
|
||||
inkscape:cx="369.55834"
|
||||
inkscape:cy="749.80396"
|
||||
inkscape:document-units="px"
|
||||
inkscape:current-layer="layer1"
|
||||
showgrid="false"
|
||||
inkscape:window-width="1920"
|
||||
inkscape:window-width="1871"
|
||||
inkscape:window-height="1176"
|
||||
inkscape:window-x="1920"
|
||||
inkscape:window-x="49"
|
||||
inkscape:window-y="24"
|
||||
inkscape:window-maximized="1"
|
||||
inkscape:snap-bbox="false"
|
||||
@@ -199,8 +199,8 @@
|
||||
id="path4159"
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:0.70889032;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
|
||||
d="m 137.25568,374.13424 441.76765,0"
|
||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:0.75307697;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
|
||||
d="m 80.466601,374.13424 498.556729,0"
|
||||
id="path4157-6"
|
||||
inkscape:connector-curvature="0" />
|
||||
<path
|
||||
@@ -281,9 +281,9 @@
|
||||
sodipodi:cy="373.53488"
|
||||
sodipodi:rx="20.900398"
|
||||
sodipodi:ry="5.2250996"
|
||||
sodipodi:start="1.2174288"
|
||||
sodipodi:end="0.78462883"
|
||||
d="m -415.49223,378.43714 a 20.900398,5.2250996 0 0 1 -25.94606,-2.57525 20.900398,5.2250996 0 0 1 7.43885,-6.72668 20.900398,5.2250996 0 0 1 27.55269,1.12243 20.900398,5.2250996 0 0 1 -1.48808,6.9691"
|
||||
sodipodi:start="0.47984631"
|
||||
sodipodi:end="3.2616514"
|
||||
d="m -404.18498,375.94701 a 20.900398,5.2250996 0 0 1 -24.71557,2.57967 20.900398,5.2250996 0 0 1 -14.5744,-5.61761"
|
||||
transform="scale(-1,1)"
|
||||
sodipodi:open="true" />
|
||||
<path
|
||||
@@ -294,9 +294,9 @@
|
||||
sodipodi:cy="192.17082"
|
||||
sodipodi:rx="18.581232"
|
||||
sodipodi:ry="4.645308"
|
||||
sodipodi:start="0.58303123"
|
||||
sodipodi:end="2.0657742"
|
||||
d="m 437.03781,194.72833 a 18.581232,4.645308 0 0 1 -24.33788,1.53027"
|
||||
sodipodi:start="6.0151975"
|
||||
sodipodi:end="1.0471386"
|
||||
d="m 439.44423,190.94078 a 18.581232,4.645308 0 0 1 -8.62642,5.25286"
|
||||
sodipodi:open="true" />
|
||||
<text
|
||||
xml:space="preserve"
|
||||
@@ -353,14 +353,14 @@
|
||||
<text
|
||||
xml:space="preserve"
|
||||
style="font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;font-size:15px;line-height:125%;font-family:MathJax_Caligraphic;-inkscape-font-specification:MathJax_Caligraphic;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
|
||||
x="113.81639"
|
||||
y="375.64291"
|
||||
x="65.620399"
|
||||
y="373.21445"
|
||||
id="text5206"
|
||||
sodipodi:linespacing="125%"><tspan
|
||||
sodipodi:role="line"
|
||||
id="tspan5208"
|
||||
x="113.81639"
|
||||
y="375.64291">y<tspan
|
||||
x="65.620399"
|
||||
y="373.21445">y<tspan
|
||||
style="font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;font-size:64.99999762%;font-family:MathJax_Size1;-inkscape-font-specification:MathJax_Size1;baseline-shift:sub"
|
||||
id="tspan5210">s</tspan></tspan></text>
|
||||
<text
|
||||
@@ -420,24 +420,24 @@
|
||||
d="m 85.856928,445.65589 0,-261.31492"
|
||||
style="fill:none;fill-rule:evenodd;stroke:#000000;stroke-width:0.54521042;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
|
||||
<circle
|
||||
style="opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:1;stroke-linecap:round;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
|
||||
style="opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:1.40831339;stroke-linecap:round;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
|
||||
id="path4230"
|
||||
cx="85.865181"
|
||||
cx="85.856926"
|
||||
cy="192.17082"
|
||||
r="0.56510383" />
|
||||
r="0.7958433" />
|
||||
<circle
|
||||
cy="433.67297"
|
||||
cx="85.856926"
|
||||
cx="85.891953"
|
||||
id="circle4232"
|
||||
style="opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:1.40831339;stroke-linecap:round;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
|
||||
r="0.7958433"
|
||||
inkscape:transform-center-x="-4.2929873"
|
||||
inkscape:transform-center-y="-0.39627575" />
|
||||
style="opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:1.87775123;stroke-linecap:round;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
|
||||
inkscape:transform-center-x="-5.723983"
|
||||
inkscape:transform-center-y="-0.52836768"
|
||||
r="1.0611243" />
|
||||
<circle
|
||||
style="opacity:1;fill:#000000;fill-opacity:1;stroke:#000000;stroke-width:1.40831339;stroke-linecap:round;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
|
||||
id="circle4234"
|
||||
cx="145.47098"
|
||||
cy="374.18094"
|
||||
cx="85.856926"
|
||||
cy="374.07587"
|
||||
r="0.7958433" />
|
||||
<text
|
||||
sodipodi:linespacing="125%"
|
||||
|
||||
|
Before Width: | Height: | Size: 44 KiB After Width: | Height: | Size: 44 KiB |
Reference in New Issue
Block a user