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

working so far. have to test helicalscan also on the real system

Browse Source
This commit is contained in:
zamofing_t
2018-01-04 16:04:30 +01:00
parent 57b6ff3a2c
commit e58047f2b0
8 changed files with 483 additions and 146 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
PBinspect.pbi → PBInspect.pbi
View File

113
PBInspect1.pbi Normal file
View File

@@ -0,0 +1,113 @@
[
[
"MotorListCtrl",
[
1,
2,
3,
4,
5,
6,
7,
8,
9,
10,
11,
12,
13,
14,
15,
16
],
"name=p;caption=motor pos;state=67377148;dir=4;layer=0;row=0;pos=1;prop=100000;bestw=200;besth=350;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=-1;floaty=-1;floatw=-1;floath=-1;notebookid=-1;transparent=255"
],
[
"WatchListCtrl",
[
[
"Motor[1].idCmd",
"lambda v: '%.8g'%float(v)"
],
[ "Motor[1].iqCmd", "lambda v: '%.8g'%float(v)" ],
[ "Motor[1].idCmd", "lambda v: '%.8g'%float(v)" ],
[ "Motor[2].iqCmd", "lambda v: '%.8g'%float(v)" ],
[ "Motor[2].idCmd", "lambda v: '%.8g'%float(v)" ],
[ "Motor[3].iqCmd", "lambda v: '%.8g'%float(v)" ],
[ "Motor[3].idCmd", "lambda v: '%.8g'%float(v)" ],
[ "Motor[4].iqCmd", "lambda v: '%.8g'%float(v)" ],
[ "Motor[4].idCmd", "lambda v: '%.8g'%float(v)" ],
[ "Motor[5].iqCmd", "lambda v: '%.8g'%float(v)" ],
[ "Motor[5].idCmd", "lambda v: '%.8g'%float(v)" ],
[ "Motor[6].iqCmd", "lambda v: '%.8g'%float(v)" ],
[ "Motor[6].idCmd", "lambda v: '%.8g'%float(v)" ],
[ "Motor[7].iqCmd", "lambda v: '%.8g'%float(v)" ],
[ "Motor[7].idCmd", "lambda v: '%.8g'%float(v)" ],
[ "Motor[8].iqCmd", "lambda v: '%.8g'%float(v)" ],
[ "Motor[8].idCmd", "lambda v: '%.8g'%float(v)" ],
[ "P2000", null ],
[ "P3000", null ]
],
"name=w;caption=watch;state=67377148;dir=4;layer=0;row=1;pos=0;prop=100000;bestw=200;besth=369;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=221;floaty=858;floatw=208;floath=377;notebookid=-1;transparent=255"
],
[
"StatusMotorListCtrl",
[
3
],
"name=m3;caption=motor3;state=67377148;dir=4;layer=0;row=5;pos=0;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=694;floaty=941;floatw=208;floath=677;notebookid=-1;transparent=255"
],
[
"StatusGblListCtrl",
[],
"name=g;caption=global;state=67377148;dir=4;layer=0;row=0;pos=0;prop=100000;bestw=200;besth=369;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=2058;floaty=457;floatw=208;floath=377;notebookid=-1;transparent=255"
],
[
"StatusMotorListCtrl",
[
2
],
"name=m2;caption=motor2;state=67377148;dir=4;layer=0;row=4;pos=1;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=2588;floaty=946;floatw=208;floath=677;notebookid=-1;transparent=255"
],
[
"StatusMotorListCtrl",
[
4
],
"name=m4;caption=motor4;state=67377148;dir=4;layer=0;row=6;pos=0;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=905;floaty=792;floatw=208;floath=677;notebookid=-1;transparent=255"
],
[
"StatusMotorListCtrl",
[
5
],
"name=m5;caption=motor5;state=67377148;dir=4;layer=0;row=6;pos=1;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=1139;floaty=836;floatw=208;floath=677;notebookid=-1;transparent=255"
],
[
"StatusMotorListCtrl",
[
7
],
"name=m7;caption=motor7;state=67377148;dir=4;layer=0;row=7;pos=1;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=1353;floaty=657;floatw=208;floath=677;notebookid=-1;transparent=255"
],
[
"StatusMotorListCtrl",
[
6
],
"name=m6;caption=motor6;state=67377148;dir=4;layer=0;row=7;pos=0;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=1276;floaty=771;floatw=208;floath=677;notebookid=-1;transparent=255"
],
[
"StatusCoordListCtrl",
[
0
],
"name=c0;caption=coord0;state=67377148;dir=4;layer=0;row=2;pos=0;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=457;floaty=774;floatw=208;floath=677;notebookid=-1;transparent=255"
],
[
"StatusMotorListCtrl",
[
1
],
"name=m1;caption=motor1;state=67377148;dir=4;layer=0;row=4;pos=0;prop=100000;bestw=200;besth=669;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=669;floaty=892;floatw=208;floath=677;notebookid=-1;transparent=255"
]
]

12
PBinspect2.pbi → PBInspect2.pbi
View File

@@ -52,6 +52,18 @@
[ [
"Motor[1].ServoOut", "Motor[1].ServoOut",
null null
],
[
"P1000",
null
],
[
"Gather.Samples",
null
],
[
"Gather.MaxLines",
null
] ]
], ],
"name=w;caption=watch;state=67377148;dir=4;layer=0;row=1;pos=0;prop=100000;bestw=200;besth=369;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=221;floaty=858;floatw=208;floath=377;notebookid=-1;transparent=255" "name=w;caption=watch;state=67377148;dir=4;layer=0;row=1;pos=0;prop=100000;bestw=200;besth=369;minw=-1;minh=-1;maxw=-1;maxh=-1;floatx=221;floaty=858;floatw=208;floath=377;notebookid=-1;transparent=255"

102
Readme.md
View File

@@ -153,58 +153,71 @@ HelicalScan
----------- -----------
``` ```
PPMAC=MOTTEST-CPPM-CRM0485 PPMAC=MOTTEST-CPPM-CRM0485
gpasciiCommander --host $PPMAC mx-stage_sim.cfg -i
Coord[1].SegMoveTime=.05 cd /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX
will calculate all 0.05 sec the inverse kinematic. PBInspect --host $PPMAC --cfg PBInspect2.pbi
default Coord[1].SegMoveTime=0, calculates inv kin. only at endpoints
cd /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/cfg
gpasciiCommander --host $PPMAC sim_8_motors.cfg -i
ssh root@$PPMAC sendgetsends -1 ssh root@$PPMAC sendgetsends -1
send 1"SampleMessage\n" cpx send 1"SampleMessage\n"
execute helicalscan.py cd /home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python
./helicalscan.py
```
SCRATCH fwd/inv kinematic stuff
------- -----------------------
```
Coord[1].SegMoveTime=.05, will calculate all 0.05 sec the inverse kinematic.
default Coord[1].SegMoveTime=0, calculates inv kin. only at endpoints
&1p ->this will trigger:forward kinematic &1p ->this will trigger:forward kinematic
cpx pmatch ->this will trigger:forward kinematic cpx pmatch ->this will trigger:forward kinematic
cpx ;linear rel; X0Y0Z0B0 ->this will trigger: inverse
cpx ;linear abs; X0Y0Z0B0 ->this will trigger: inverse test_coord_trf() (w is in mdeg) -> Output:
input : cx:0.2 cz:0.3 w:5729.58 fy:0.4
fwd_trf: dx:-1.14768 dz:-19.0991 w:5729.58 fy:0.4
inv_trf: cx:0.2 cz:0.3 w:5729.58 fy:0.4
#7j=0.1 //cx input : dx:0.2 dz:0.3 w:5729.58 fy:0.4
#8j=0.2 //cy inv_trf: cx:1.54768 cz:19.6991 w:5729.58 fy:0.4
#1j=0.3 //w fwd_trf: dx:0.2 dz:0.3 w:5729.58 fy:0.4
#2j=0.4 //fx
(cx,cz,w,fy) (2.6164986454377614, 17.758728111340563, 0.1, 4.3) #4j=0.2 //cx
(dx,dz,w,y) (0.2, 0.3, 0.1, 4.3) #5j=0.3 //cz
#3j=5729.58 //w
#7j=2.6164986454377614 #1j=0.4 //fy
#8j=17.758728111340563
#1j=5729.578 // 0.1*1000.*360./(2*pi)
#2j=4.3
&1p &1p
B0.1 X0.2 Y4.3 Z0.3 forward kinematic 0.199997 0.300003 5729.58 0.399994
cpx ;linear abs; X0.2 Z0.3, B0.1 Y4.3 forward result -1.14769 -19.0991 5729.58 0.399994
Motor[1,27,8].JogSpeed=8 cpx ;linear abs; X.2Y.4Z.3B5729.58
inverse kinematic 0.2 0.3 5729.58 0.4
inverse result 1.54579 19.7026 5729.58 0.4
#1,2,7,8j=10000 input : cx:10 cz:20 w:171887 fy:40
fwd_trf: dx:28.9419 dz:-5.52304 w:171887 fy:40
cpx ;linear abs; X0.2 Z0.3 B0.1 Y00000 inv_trf: cx:10 cz:20 w:171887 fy:40
cpx ;linear abs; X0.2 Z0.3 B0.1 Y10000 #4j=10 //cx
#5j=20 //cz
input : dx:0.2 dz:0.3 w:5.72958 fy:3.3 #3j=171887 //w
inv_trf: cx:2.34244 cz:18.2563 w:5.72958 fy:3.3 #1j=40 //fy
fwd_trf: dx:0.2 dz:0.3 w:5.72958 fy:3.3
#7j=2.34244;#8j=18.2563;#1j=5729.58;#2j=3.3
&1p &1p
forward kinematic 10 20 171887 40
forward result 28.942 -5.52296 171887 40
input : dx:10 dz:20 w:171887 fy:40
inv_trf: cx:-8.94193 cz:45.523 w:171887 fy:40
fwd_trf: dx:10 dz:20 w:171887 fy:40
cpx ;linear abs; X10 Z20 B171887 Y40
inverse kinematic 10 20 171887 40
inverse result -8.94198 45.523 171887 40
``` ```
Motors final setup 20.12.17 Motors final setup 20.12.17
@@ -242,6 +255,7 @@ SAR-CPPM-EXPMX3: biss:n timing:y
PPMAC=MOTTEST-CPPM-CRM0485 PPMAC=MOTTEST-CPPM-CRM0485
ssh root@$PPMAC ssh root@$PPMAC
cd /ioc/SAR-CPPM-EXPMX1;iocsh startup.script cd /ioc/SAR-CPPM-EXPMX1;iocsh startup.script
sendgetsends -1
ssh x06mx-cons-1 ssh x06mx-cons-1
@@ -252,25 +266,3 @@ export CAQTDM_DISPLAY_PATH=/net/slsfs-crtl/export/sf/common/config/qt/:/net/slsf
caqtdm -macro "P=SAR-EXPMX" ESB_MX_exp.ui caqtdm -macro "P=SAR-EXPMX" ESB_MX_exp.ui
``` ```
SCRATCH
--------
PPMAC=SAR-CPPM-EXPMX1
gpasciiCommander --host $PPMAC -i
$$$***
!common()
!SAR-EXPMX1()
PPMAC=SAR-CPPM-EXPMX1
PBInspect --host $PPMAC -i
Coord[1].SegMoveTime=.05
will calculate all 0.05 sec the inverse kinematic.
default Coord[1].SegMoveTime=0, calculates inv kin. only at endpoints
ssh root@$PPMAC sendgetsends -1
send 1"SampleMessage\n"

118
cfg/MX1_home.cfg
View File

@@ -5,12 +5,11 @@ statusHoming=0
//statusHoming: //statusHoming:
// 1: M1 phased // 1: M1 phased
// 2: M2 phased // 2: M2 phased
// 4: M1 homed // 4: M1 pos limit found
// 8: M2 homed // 8: M2 pos limit found
// 16: M1 neg limit // 16: M1 homed
// 32: M2 neg limit // 32: M2 homed
// 64: homing done // 256: homing failed
// 128: homing failed
open plc PLC_Homing open plc PLC_Homing
@@ -33,46 +32,95 @@ open plc PLC_Homing
Motor[1].PhaseFindingStep=1 Motor[1].PhaseFindingStep=1
Motor[2].PhaseFindingStep=1 Motor[2].PhaseFindingStep=1
timer = Sys.RunTime + 1;while (Sys.RunTime < timer){} //wait 1 sec while(Motor[1].PhaseFindingStep>0 || Motor[2].PhaseFindingStep>0){}
if (Motor[1].PhaseFound==1)
Motor[1].HomeVel=2
Motor[2].HomeVel=2
home1
home2
timer = Sys.RunTime + 1;while (Sys.RunTime < timer){} //wait 1 sec
statusHoming|=1
while(1)
{ {
if(Motor[1].FeFatal && (statusHoming&2)==0) statusHoming|=1
{ cmd "#1j/"
statusHoming=statusHoming|2 send 1"phasing Y_Achse ok\n"
Motor[1].HomeVel=-Motor[1].HomeVel }
home1 else
} {
if(Motor[2].HomeInProgress==0 && (statusHoming&4)==0) send 1"phasing Y Achsde error!\n"
{ goto 0 //ERROR_END
statusHoming=statusHoming|4 }
Motor[2].HomeVel=-Motor[2].HomeVel if (Motor[2].PhaseFound==1)
home2 {
} statusHoming|=2
if(Motor[1].HomeInProgress==0 && Motor[1].HomeInProgress==0) cmd "#2j/"
{ send 1"phasing X_Achse ok\n"
statusHoming=statusHoming|8 }
break else
} {
send 1"phasing X Achse error!\n"
statusHoming|=256
goto 0 //ERROR_END
} }
Motor[1].PhasePos=560 // 555 581 593 558
Motor[2].PhasePos=1540 //1549 1531 1543 1537
//move until FeWarn
Motor[1].JogSpeed=6
Motor[2].JogSpeed=6
timer = Sys.RunTime + .5;while (Sys.RunTime < timer){} //wait .5 sec
jog+1
jog+2
while(1)
{
if(Motor[1].FeWarn && (statusHoming&4)==0)
{
cmd "#1j/"
send 1"Y_Achse +limit\n"
statusHoming|=4
}
if(Motor[2].FeWarn && (statusHoming&8)==0)
{
cmd "#2j/"
send 1"X_Achse +limit\n"
statusHoming|=8
}
if((statusHoming&12)==12)
break
}
timer = Sys.RunTime + .5;while (Sys.RunTime < timer){} //wait .5 sec
Motor[1].HomeVel=-3
Motor[2].HomeVel=-3
home1
home2
timer = Sys.RunTime + .5;while (Sys.RunTime < timer){} //wait .5 sec
while(Motor[1].HomeInProgress || Motor[2].HomeInProgress){}
if(Motor[1].HomeComplete)
statusHoming=statusHoming|16
else
{
statusHoming|=256
send 1"Y_Achse home failed\n"
}
if(Motor[2].HomeComplete)
statusHoming=statusHoming|32
else
{
statusHoming|=256
send 1"X_Achse home failed\n"
}
//Motor[1].PhasePos=560 // 555 581 593 558
//Motor[2].PhasePos=1540 //1549 1531 1543 1537
goto 1
N0: //ERROR_END
statusHoming|=256
N1: //ENDPLC
Motor[1].MaxDac=L10 Motor[1].MaxDac=L10
Motor[1].FatalFeLimit=L11 Motor[1].FatalFeLimit=L11
Motor[1].JogSpeed=L12 Motor[1].JogSpeed=L12
Motor[2].MaxDac=L20 Motor[2].MaxDac=L20
Motor[2].FatalFeLimit=L21 Motor[2].FatalFeLimit=L21
Motor[2].JogSpeed=L22 Motor[2].JogSpeed=L22
send 1"homeing done\n"
disable plc PLC_Homing disable plc PLC_Homing
close close

33
cfg/sim_8_motors.cfg
View File

@@ -1,32 +1,6 @@
//simulated stage without real motors needed //simulated 8 motors without needing real ones
$$$*** $$$***
!common() !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
//use some step and direction lines to setup simulate motors //use some step and direction lines to setup simulate motors
// /afs/psi.ch/user/h/humar_t/public/Modules/XMI/cfg/xmi.cfg // /afs/psi.ch/user/h/humar_t/public/Modules/XMI/cfg/xmi.cfg
@@ -63,5 +37,6 @@ Motor[7].pLimits=0;Motor[7].AmpFaultLevel=0;Motor[7].pAmpEnable=0;Motor[7].pAmpF
!motor(mot=8,dirCur=0,contCur=100,peakCur=1000,timeAtPeak=1,JogSpeed=8.,numPhase=3,invDir=True) !motor(mot=8,dirCur=0,contCur=100,peakCur=1000,timeAtPeak=1,JogSpeed=8.,numPhase=3,invDir=True)
Motor[8].pLimits=0;Motor[8].AmpFaultLevel=0;Motor[8].pAmpEnable=0;Motor[8].pAmpFault=0 Motor[8].pLimits=0;Motor[8].AmpFaultLevel=0;Motor[8].pAmpEnable=0;Motor[8].pAmpFault=0
Motor[3].JogSpeed=1000 # used for joging
!coordTrf() Motor[3].MaxSpeed=1000 # used for motion program
#1..8j/

247
python/helicalscan.py
View File

@@ -149,12 +149,23 @@ class HelicalScan:
def test_coord_trf(self): def test_coord_trf(self):
param = self.param param = self.param
dx, dz, w, y, = (0.2,0.3,0.1,3.3) cx, cz, w, fy, = (0.2,0.3,0.1,0.4)
print 'input : dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r,y) #cx, cz, w, fy, = (10.,20,3.,40)
(cx,cz,w,fy) = self.inv_transform(dx,dz,w,y) print 'input : cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r*1000.,fy)
print 'inv_trf: cx:%.6g cz:%.6g w:%.6g fy:%.6g' % (cx,cz,w/d2r,fy)
(dx,dz,w,y) = self.fwd_transform(cx,cz,w,fy) (dx,dz,w,y) = self.fwd_transform(cx,cz,w,fy)
print 'fwd_trf: dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r,y) print 'fwd_trf: 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.2,0.3,0.1,0.4)
#dx, dz, w, y, = (10.,20,3.,40)
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) = self.fwd_transform(cx,cz,w,fy)
print 'fwd_trf: dx:%.6g dz:%.6g w:%.6g fy:%.6g' % (dx,dz,w/d2r*1000.,y)
# plt.ion() # plt.ion()
# fig = plt.figure() # fig = plt.figure()
@@ -164,6 +175,32 @@ class HelicalScan:
# def my_anim_func3(self,idx): # def my_anim_func3(self,idx):
# self.hCrist,pt=self.pltCrist(cx=0,ty=0,cz=0,w=10*idx*d2r,h=self.hCrist) # self.hCrist,pt=self.pltCrist(cx=0,ty=0,cz=0,w=10*idx*d2r,h=self.hCrist)
def mpl_test(self):
plt.ion()
fig = plt.figure()
ax = fig.add_axes([.2, .05, .75, .90])
x=np.arange(0,2*np.pi,.1)
y=np.sin(x)
h=ax.plot(x,y)
# ax.set_position([.2,.05,.75,.90])
#ax2 = ax.twinx()
ax2 = fig.add_axes([.2, .05, .75, .90],sharex=ax)
ax2.set_position([.1,.05,.75,.90])
ax2.spines['top'].set_visible(False)
ax2.spines['bottom'].set_visible(False)
ax2.xaxis.set_visible(False)
ax2.patch.set_visible(False)
y2=y+.1*np.random.random(y.shape)
h+=ax2.plot(x,y2,'g')
ax2.set_position(ax.get_position())
ax2.set_ylabel('mylbl', color='r')
ax2.tick_params(axis='y', colors='b')
ax2.spines['left'].set_position(('axes',-.1))
plt.legend(handels=h)
pass
def show_vel(self): def show_vel(self):
rec=self.rec rec=self.rec
@@ -172,10 +209,62 @@ class HelicalScan:
y = np.diff(rec,axis=0) y = np.diff(rec,axis=0)
mx=y.max(0) mx=y.max(0)
mn=y.min(0) mn=y.min(0)
y=y/(mx-mn) scl=np.maximum(mx,-mn)
scl=np.where(scl==0, 1, scl) # replace 0 by 1
y/=scl
x = np.arange(0, y.shape[0]); x = np.arange(0, y.shape[0]);
x= x.reshape(-1,1).dot(np.ones((1,y.shape[1]))) x= x.reshape(-1,1).dot(np.ones((1,y.shape[1])))
plt.plot(x, y) lbl=('cx','cz','w','fy')
#plt.plot(x, y,label=lbl)
for i in range(y.shape[1]):
plt.plot(x[:,i], y[:,i],label=lbl[i])
plt.legend()
def show_pos(self):
rec=self.rec
y = rec
#plt.ion()
fig = plt.figure(figsize=(20,6))
c='bgrc'
lbl=('cx','cz','w','fy')
dx=.25/len(lbl)
for i in range(rec.shape[1]):
if i==0:
ax = fig.add_axes([.2, .05, .75, .90])
axl =[ax,]
else:
ax = fig.add_axes(axl[0].get_position(), sharex=ax)
ax.spines['top'].set_visible(False)
ax.spines['bottom'].set_visible(False)
ax.xaxis.set_visible(False)
ax.patch.set_visible(False)
ax.spines['left'].set_position(('axes', -dx*i))
axl.append(ax)
ax.set_ylabel(lbl[i], color=c[i])
ax.tick_params(axis='y', colors=c[i])
x = np.arange(0, y.shape[0]);
h=[]
for i in range(rec.shape[1]):
h+=axl[i].plot(x, y[:,i],c[i],label=lbl[i])
plt.legend(handles=h)
cid = fig.canvas.mpl_connect('motion_notify_event', self.onmove)
fig.obj=self
fig.data=y
@staticmethod
def onmove(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
data = event.canvas.figure.data
if(event.xdata):
idx=round(event.xdata)
msg='%d: cx:%.3f cz:%.3f w:%.1f fy:%.3f'%((idx,)+tuple(data[idx,:]))
#print msg
event.canvas.toolbar.set_message(msg)
def interactive_cx_cz_w_fy(self): def interactive_cx_cz_w_fy(self):
fig = plt.figure() fig = plt.figure()
@@ -578,21 +667,28 @@ class HelicalScan:
prg.append(''' prg.append('''
// Set the motors as inverse kinematic axes in CS 1 // Set the motors as inverse kinematic axes in CS 1
//motors CX CZ RY FY //motors CX CZ RY FY
// 7 8 1 2 // 4 5 3 1
&1 &1
#7->I a
#8->I #4->0
#5->0
#3->0
#1->0
#4->I
#5->I
#3->I
#1->I #1->I
#2->I
open forward open forward
define(qCX='L7', qCZ='L8', qW='L1', qFY='L2') define(qCX='L4', qCZ='L5', qW='L3', qFY='L1')
define(DX='C6', DZ='C8', W='C1', Y='C7') define(DX='C6', DZ='C8', W='C1', Y='C7')
//coord X Z B Y //coord X Z B Y
define(p0_x='L10', p0_y='L11', p0_z='L12') define(p0_x='L10', p0_y='L11', p0_z='L12')
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"forward kinematic %f %f %f %f\\n",qCX,qCZ,qW,qFY''') //send 1"forward kinematic %f %f %f %f\\n",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
@@ -626,7 +722,7 @@ open inverse
define(DX='C6', DZ='C8', W='C1', Y='C7') define(DX='C6', DZ='C8', W='C1', Y='C7')
//coord X Z B Y //coord X Z B Y
//D0 is set to $000001c2 //D0 is set to $000001c2
define(qCX='L7', qCZ='L8', qW='L1', qFY='L2') define(qCX='L4', qCZ='L5', qW='L3', qFY='L1')
define(p0_x='L10', p0_y='L11', p0_z='L12') define(p0_x='L10', p0_y='L11', p0_z='L12')
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')
@@ -638,6 +734,7 @@ open inverse
prg.append(" qW=W") prg.append(" qW=W")
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*sin(phi_0+W)
p1_x=x_1+r_1*sin(phi_1+W) p1_x=x_1+r_1*sin(phi_1+W)
p0_y=y_0 p0_y=y_0
@@ -645,7 +742,7 @@ open inverse
p0_z=z_0+r_0*cos(phi_0+W) p0_z=z_0+r_0*cos(phi_0+W)
p1_z=z_1+r_1*cos(phi_1+W) p1_z=z_1+r_1*cos(phi_1+W)
scale=(qFY-y_0)/(y_1-y_0) scale=(Y-y_0)/(y_1-y_0)
p0_x=p0_x+scale*(p1_x-p0_x) p0_x=p0_x+scale*(p1_x-p0_x)
p0_y=p0_y+scale*(p1_y-p0_y) p0_y=p0_y+scale*(p1_y-p0_y)
p0_z=p0_z+scale*(p1_z-p0_z) p0_z=p0_z+scale*(p1_z-p0_z)
@@ -654,10 +751,90 @@ open inverse
qFY=Y qFY=Y
//send 1"inverse result %f %f %f %f\\n",qCX,qCZ,qW,qFY //send 1"inverse result %f %f %f %f\\n",qCX,qCZ,qW,qFY
//P1002+=1 //P1002+=1
close close
''') ''')
# ****** TESTING CODE ******
#this is testcode to check linear coordinate transformation and
#simple (same as before) fwd and inv-kinematics
#When using pvt mode they do not behave the same:
#(Power PMAC Users Manual P648)
#Enabling Move Segmentation:
#If segmentation mode is disabled, a circle-mode move will execute as a linear-mode move
#directly from the start point to the end point. Segmentation mode must also be enabled to use the
#special lookahead buffer, tool-radius compensation, and kinematic-subroutine calculations.
#set ct to:
# 1 for simple linear transformation
# 2 simple linear fwd/inv kinematics
ct=0;
if ct==1:
#simple linear transformation
prg = []
prg.append('''
//motors CX CZ RY FY
// 4 5 3 1
&1
a
#4->0
#5->0
#3->0
#1->0
#4->X
#5->Z
#3->B
#1->Y
''')
elif ct==2:
#simple linear fwd/inv kinematics
prg = []
prg.append('''
// Set the motors as inverse kinematic axes in CS 1
//motors CX CZ RY FY
// 4 5 3 1
&1
a
#4->0
#5->0
#3->0
#1->0
#4->I
#5->I
#3->I
#1->I
open forward
define(qCX='L4', qCZ='L5', qW='L3', qFY='L1')
define(DX='C6', DZ='C8', W='C1', Y='C7')
//coord X Z B Y
send 1"forward D0: %f \\n",D0
if(D0>0) callsub 100
D0=$000001c2; //B=$2 X=$40 Y=$80 Z=$100 hex(2+int('40',16)+int('80',16)+int('100',16)) -> 0x1c2
N100:
DX=qCX
DZ=qCZ
W=qW
Y=qFY
send 1"forward result %f %f %f %f\\n",DX,DZ,W,Y
P1001+=1
close
open inverse
define(DX='C6', DZ='C8', W='C1', Y='C7')
//coord X Z B Y
//D0 is set to $000001c2
define(qCX='L4', qCZ='L5', qW='L3', qFY='L1')
qCX=DX
qCZ=DZ
qW=W
qFY=Y
send 1"inverse result %f %f %f %f\\n",qCX,qCZ,qW,qFY
P1002+=1
close
''')
if self.args.verbose & 4: if self.args.verbose & 4:
for ln in prg: for ln in prg:
print(ln) print(ln)
@@ -695,7 +872,7 @@ close
yRng : starting and ending height yRng : starting and ending height
''' '''
prg=[] prg=[]
acq_per=kwargs.get('acq_per',1) acq_per=kwargs.get('acq_per',10)
gather={"MaxSamples":1000000, "Period":acq_per} gather={"MaxSamples":1000000, "Period":acq_per}
#Sys.ServoPeriod is dependent of !common() macro #Sys.ServoPeriod is dependent of !common() macro
ServoPeriod= .2 #0.2ms ServoPeriod= .2 #0.2ms
@@ -703,7 +880,7 @@ close
self.meta = {'timebase': ServoPeriod*gather['Period']} self.meta = {'timebase': ServoPeriod*gather['Period']}
#channels=["Motor[1].ActPos","Motor[2].ActPos","Motor[3].ActPos"] #channels=["Motor[1].ActPos","Motor[2].ActPos","Motor[3].ActPos"]
# CX CZ W FY # CX CZ W FY
channels=["Motor[7].ActPos","Motor[8].ActPos","Motor[1].ActPos","Motor[2].ActPos"] channels=["Motor[4].ActPos","Motor[5].ActPos","Motor[3].ActPos","Motor[1].ActPos"]
prg.append('Gather.Enable=0') prg.append('Gather.Enable=0')
prg.append('Gather.Items=%d'%len(channels)) prg.append('Gather.Items=%d'%len(channels))
for k,v in gather.iteritems(): for k,v in gather.iteritems():
@@ -757,11 +934,16 @@ close
pt[:,2]= np.linspace(wRng[0],wRng[1],numPt) #w pt[:,2]= np.linspace(wRng[0],wRng[1],numPt) #w
pt[:,3]= np.repeat(np.linspace(yRng[0],yRng[1],cntVert),cntHor) #y pt[:,3]= np.repeat(np.linspace(yRng[0],yRng[1],cntVert),cntHor) #y
self.points=pt self.points=pt
prg.append(' Coord[1].SegMoveTime=.05') #to calculate every 0.05 ms the inverse kinematics #prg.append(' Coord[1].SegMoveTime=.05') #to calculate every 0.05 ms the inverse kinematics
prg.append(' nofrax') #needed to set all axis to use AltFeedRate
prg.append(' Coord[1].AltFeedRate=0') # allow maximum speed
prg.append(' Coord[1].SegMoveTime=0') #turn off segmented mode
prg.append(' linear abs') prg.append(' linear abs')
prg.append(' X%g Z%g B%g Y%g' % tuple(pt[0, :])) prg.append(' X%g Z%g B%g Y%g' % tuple(pt[0, :]))
prg.append(' dwell 100') prg.append(' dwell 100')
prg.append(' Gather.Enable=2') prg.append(' Gather.Enable=2')
prg.append(' Coord[1].AltFeedRate=0') # allow maximum speed
prg.append(' Coord[1].SegMoveTime=1') #to calculate every 1 ms the inverse kinematics
for i in range(pt.shape[0]): for i in range(pt.shape[0]):
prg.append(' X%g Z%g B%g Y%g' % tuple(pt[i, :])) prg.append(' X%g Z%g B%g Y%g' % tuple(pt[i, :]))
prg.append(' dwell 100') prg.append(' dwell 100')
@@ -776,7 +958,7 @@ close
hRng = kwargs.get('hRng', (-.2,.2)) hRng = kwargs.get('hRng', (-.2,.2))
wRng = kwargs.get('wRng', (0,360000)) wRng = kwargs.get('wRng', (0,360000))
yRng = kwargs.get('yRng', (2.3,6.2)) yRng = kwargs.get('yRng', (2.3,6.2))
pt2pt_time = kwargs.get('pt2pt_time ', 100) pt2pt_time = kwargs.get('pt2pt_time', 100)
numPt=cntVert*cntHor numPt=cntVert*cntHor
pt=np.zeros((numPt,4)) pt=np.zeros((numPt,4))
if cntHor>1: if cntHor>1:
@@ -798,6 +980,9 @@ close
dist=pv[2:,(0,1,2,3)] - pv[:-2,(0,1,2,3)] dist=pv[2:,(0,1,2,3)] - pv[:-2,(0,1,2,3)]
pv[ 1:-1,(4,5,6,7)] = 1000.*dist/(2.*pt2pt_time) pv[ 1:-1,(4,5,6,7)] = 1000.*dist/(2.*pt2pt_time)
prg.append(' nofrax') #needed to set all axis to use AltFeedRate
prg.append(' Coord[1].AltFeedRate=0') # allow maximum speed
prg.append(' Coord[1].SegMoveTime=0') #turn off segmented mode
prg.append(' linear abs') prg.append(' linear abs')
prg.append(' X%g Z%g B%g Y%g' % tuple(pv[0, (0,1,2,3)])) prg.append(' X%g Z%g B%g Y%g' % tuple(pv[0, (0,1,2,3)]))
prg.append(' dwell 10') prg.append(' dwell 10')
@@ -805,10 +990,14 @@ close
if cnt>1: if cnt>1:
prg.append(' P100=%d'%cnt) prg.append(' P100=%d'%cnt)
prg.append(' N100:') prg.append(' N100:')
prg.append(' Coord[1].AltFeedRate=0') # allow maximum speed
prg.append(' Coord[1].SegMoveTime=1') #to calculate every 1 ms the inverse kinematics
prg.append(' pvt%g abs'%pt2pt_time) #100ms to next position prg.append(' pvt%g abs'%pt2pt_time) #100ms to next position
for idx in range(1,pv.shape[0]): for idx in range(1,pv.shape[0]):
prg.append(' P2000=%d'%idx) #prg.append(' P2000=%d'%idx)
prg.append(' X%g:%g Z%g:%g B%g:%g Y%g:%g' % tuple(pv[idx, (0,4,1,5,2,6,3,7)])) prg.append(' X%g:%g Z%g:%g B%g:%g Y%g:%g' % tuple(pv[idx, (0,4,1,5,2,6,3,7)]))
#prg.append('Y%g:%g' % tuple(pv[idx, (5, 7)]))
#prg.append('B%g:%g' %(idx*1000,0))
if cnt>1: if cnt>1:
prg.append(' dwell 10') prg.append(' dwell 10')
prg.append(' P100=P100-1') prg.append(' P100=P100-1')
@@ -847,6 +1036,7 @@ close
ack=GpasciiCommunicator.gpascii_ack ack=GpasciiCommunicator.gpascii_ack
sys.stdout.write('wait execution...');sys.stdout.flush() sys.stdout.write('wait execution...');sys.stdout.flush()
while(True): while(True):
#Gather.MaxLines calculates maximum numbewr of gathering into memory
com.write('P1000\n') com.write('P1000\n')
val=com.read_until(ack) val=com.read_until(ack)
#print val #print val
@@ -938,20 +1128,27 @@ 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.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()
#SAR-CPPM-EXPMX1 MOTTEST-CPPM-CRM0485
hs.gen_coord_trf_code('/tmp/helicalscan.cfg','MOTTEST-CPPM-CRM0485') hs.gen_coord_trf_code('/tmp/helicalscan.cfg','MOTTEST-CPPM-CRM0485')
#hs.gen_prog(file='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=-1) #hs.gen_prog(file='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=-1)
#hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=0) #hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=0,cntHor=1,cntVert=3,wRng=(120000,120000))
#hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=0,cntHor=1) #hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=0,cntHor=1)
#hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=0)
#hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=0,cntHor=1,cntVert=5,wRng=(120000,120000))
#hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=1,cntHor=1,cntVert=5,wRng=(120000,120000),pt2pt_time=100)
#hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=1,cntHor=1,cntVert=5,wRng=(0,360000))
#hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=1) #hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=1)
#hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=1, hs.gen_prog(fnPrg='/tmp/prg.cfg',host='MOTTEST-CPPM-CRM0485',mode=1,
# pt2pt_time=100,cnt=1,cntVert=35,cntHor=7,hRng=(-.3,.3),wRng=(0,360000*3),yRng=(6.2,2.3)) pt2pt_time=100,cnt=1,cntVert=35,cntHor=7,hRng=(-.3,.3),wRng=(0,360000*3),yRng=(6.2,2.3))
hs.load_rec() hs.load_rec()
hs.show_pos();hs.show_vel()
hs.interactive_anim() hs.interactive_anim()
hs.show_vel(); plt.show() #hs.show_vel(); plt.show()
#hs.show_pos(); plt.show()
return return

4
python/shapepath.py
View File

@@ -85,7 +85,7 @@ class ShapePath:
#cfg = {"sequencer": ['gen_grid_points(w=2,h=2,pitch=10000,rnd=0)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=1000)', 'plot_gather("'+fn+'.npz")']} #cfg = {"sequencer": ['gen_grid_points(w=2,h=2,pitch=10000,rnd=0)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=1000)', 'plot_gather("'+fn+'.npz")']}
#cfg = {"sequencer": ['gen_grid_points(w=20,h=20,pitch=50,rnd=0.2)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=10,acq_per=10)', 'plot_gather("'+fn+'.npz")']} #cfg = {"sequencer": ['gen_grid_points(w=20,h=20,pitch=50,rnd=0.2)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=10,acq_per=10)', 'plot_gather("'+fn+'.npz")']}
#cfg = {"sequencer": ['gen_rand_points(n=400, scale=1000)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=10)', 'plot_gather("'+fn+'.npz")']} #cfg = {"sequencer": ['gen_rand_points(n=400, scale=1000)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=10)', 'plot_gather("'+fn+'.npz")']}
cfg = {"sequencer": ['gen_grid_points(w=5,h=5,pitch=100,rnd=0.4)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=10,cnt=1)', 'plot_gather("'+fn+'.npz")']} cfg = {"sequencer": ['gen_grid_points(w=10,h=10,pitch=100,rnd=0.4)', 'sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=10,cnt=1)', 'plot_gather("'+fn+'.npz")']}
#cfg = {"sequencer":['gen_rand_points(n=400, scale=1000)','sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=20,acq_per=10)','plot_gather("'+fn+'.npz")']} #cfg = {"sequencer":['gen_rand_points(n=400, scale=1000)','sort_points()','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=20,acq_per=10)','plot_gather("'+fn+'.npz")']}
#cfg = {"sequencer": ['gen_swissfel_points(scale=300)','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=100,acq_per=10)', 'plot_gather("'+fn+'.npz")']} #cfg = {"sequencer": ['gen_swissfel_points(scale=300)','gen_prog(file="'+fn+'.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=100,acq_per=10)', 'plot_gather("'+fn+'.npz")']}
#cfg = {"sequencer": ['opt_pts("'+fn+'.npz")','gen_prog(file="'+fn+'_corr.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=10,cnt=1)', 'plot_gather("'+fn+'_corr.npz")']} #cfg = {"sequencer": ['opt_pts("'+fn+'.npz")','gen_prog(file="'+fn+'_corr.prg",host="SAR-CPPM-EXPMX1",mode=1,pt2pt_time=10,cnt=1)', 'plot_gather("'+fn+'_corr.npz")']}
@@ -182,7 +182,7 @@ class ShapePath:
#ServoPeriod = .05 #ServoPeriod = .05
self.meta = {'timebase': ServoPeriod*gather['Period']} self.meta = {'timebase': ServoPeriod*gather['Period']}
#channels=["Motor[1].ActPos","Motor[2].ActPos","Motor[3].ActPos"] #channels=["Motor[1].ActPos","Motor[2].ActPos","Motor[3].ActPos"]
channels=["Motor[1].ActPos","Motor[2].ActPos","Motor[3].ActPos","Motor[1].DesPos","Motor[2].DesPos","Motor[3].DesPos"] channels=["Motor[3].ActPos","Motor[2].ActPos","Motor[1].ActPos","Motor[3].DesPos","Motor[2].DesPos","Motor[1].DesPos"]
prg.append('Gather.Enable=0') prg.append('Gather.Enable=0')
prg.append('Gather.Items=%d'%len(channels)) prg.append('Gather.Items=%d'%len(channels))
for k,v in gather.iteritems(): for k,v in gather.iteritems():