123 lines
3.6 KiB
Tcl
123 lines
3.6 KiB
Tcl
|
|
# Drive the 3 Monochromator blades as a "bunch"
|
|
#
|
|
# Author: Jing Chen, jgn@ansto.gov.au
|
|
#
|
|
# Date: 11/11/2011
|
|
|
|
# get a specified parameter value of a motor
|
|
proc getSetting {mot field} {
|
|
|
|
switch $field {
|
|
"position" {
|
|
set tmpStr1 [$mot]
|
|
set tmpStr2 [split $tmpStr1 "="]
|
|
array set tmpstr3 $tmpStr2
|
|
set ind "$mot "
|
|
return $tmpstr3($ind)
|
|
}
|
|
"softlowerlim" {
|
|
set tmpStr1 [$mot softlowerlim]
|
|
set tmpStr2 [split $tmpStr1 "="]
|
|
array set tmpstr3 $tmpStr2
|
|
set ind "$mot.softlowerlim "
|
|
return $tmpstr3($ind)
|
|
}
|
|
"softupperlim" {
|
|
set tmpStr1 [$mot softupperlim]
|
|
set tmpStr2 [split $tmpStr1 "="]
|
|
array set tmpstr3 $tmpStr2
|
|
set ind "$mot.softupperlim "
|
|
return $tmpstr3($ind)
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
# DriveMono: drive the 3 Monochromators to translate as a "bunch"
|
|
proc DriveMono {mot focus} {
|
|
variable currAngleMoma
|
|
variable currAngleMomb
|
|
variable currAngleMomc
|
|
|
|
variable mraPosition
|
|
variable mraSoftlowerlim
|
|
variable mraSoftupperlim
|
|
variable mrbPosition
|
|
variable mrbSoftlowerlim
|
|
variable mrbSoftupperlim
|
|
variable mrcPosition
|
|
variable mrcSoftlowerlim
|
|
variable mrcSoftupperlim
|
|
|
|
variable translateDis
|
|
|
|
variable moveFlag
|
|
|
|
# set moveFlag to 1 to move the three motors
|
|
set moveFlag 1
|
|
|
|
# Record current angle positions of the three motors
|
|
set currAngleMoma [getSetting moma position]
|
|
set currAngleMomb [getSetting momb position]
|
|
set currAngleMomc [getSetting momc position]
|
|
|
|
# Get current focus positions of the three motors mra, mrb and mrc
|
|
set mraPosition [getSetting mra position]
|
|
set mrbPosition [getSetting mrb position]
|
|
set mrcPosition [getSetting mrc position]
|
|
|
|
# Get soft lower/upper limits of mra, mrb and mrc
|
|
set mraSoftlowerlim [getSetting mra softlowerlim]
|
|
set mraSoftupperlim [getSetting mra softupperlim]
|
|
set mrbSoftlowerlim [getSetting mrb softlowerlim]
|
|
set mrbSoftupperlim [getSetting mrb softupperlim]
|
|
set mrcSoftlowerlim [getSetting mrc softlowerlim]
|
|
set mrcSoftupperlim [getSetting mrc softupperlim]
|
|
|
|
# Calculate moving distance
|
|
set translateDis [expr $focus - [getSetting $mot position]]
|
|
|
|
# Determine if the movement is within the ranges of the three motors
|
|
if {[expr $mraPosition + $translateDis] > $mraSoftupperlim ||
|
|
[expr $mraPosition + $translateDis] < $mraSoftlowerlim} {
|
|
broadcast "Error: the movement is out of the range limit of motor mra"
|
|
set moveFlag 0
|
|
}
|
|
|
|
if {[expr $mrbPosition + $translateDis] > $mrbSoftupperlim ||
|
|
[expr $mrbPosition + $translateDis] < $mrbSoftlowerlim} {
|
|
broadcast "Error: the movement is out of the range limit of motor mrb"
|
|
set moveFlag 0
|
|
}
|
|
|
|
if {[expr $mrcPosition + $translateDis] > $mrcSoftupperlim ||
|
|
[expr $mrcPosition + $translateDis] < $mrcSoftlowerlim} {
|
|
broadcast "Error: the movement is out of the range limit of motor mrc"
|
|
set moveFlag 0
|
|
}
|
|
|
|
# move the three motors
|
|
if {$moveFlag == 1} {
|
|
# Unlock the three motors
|
|
mra fixed -1
|
|
mrb fixed -1
|
|
mrc fixed -1
|
|
|
|
drive moma $currAngleMomb momc $currAngleMomb
|
|
drive mra [expr $mraPosition + $translateDis] mrb [expr $mrbPosition + $translateDis] mrc [expr $mrcPosition + $translateDis]
|
|
drive moma $currAngleMoma momc $currAngleMomc
|
|
|
|
# Lock the three motors again
|
|
mra fixed 1
|
|
mrb fixed 1
|
|
mrc fixed 1
|
|
}
|
|
}
|
|
|
|
|
|
|
|
publish DriveMono user
|
|
|
|
|