129 lines
4.9 KiB
Tcl
129 lines
4.9 KiB
Tcl
# Author: Ferdi Franceschini (ffr@ansto.gov.au)
|
|
|
|
fileeval $cfPath(anticollider)/anticollider_common.tcl
|
|
|
|
namespace eval ::anticollider {
|
|
set guide_posit 1
|
|
set aperture_posit 2
|
|
set apD10_posit 1
|
|
set coltol 0.5
|
|
set aptol 0.1
|
|
|
|
for {set n 1} {$n < 8} {incr n} {
|
|
foreach {col ap} "col$n ap$n" {
|
|
set col_limit [expr [SplitReply [$col posit2unit $aperture_posit]] - $coltol]
|
|
set ap_limit [expr [SplitReply [$ap posit2unit $apD10_posit]] - $aptol]
|
|
set acrule [subst {forbid {-inf inf} for $ap when $col in {-inf $col_limit} }]
|
|
lappend ::anticollider::veto_rules $acrule
|
|
set acrule [subst {forbid {-inf inf} for $col when $ap in {$ap_limit inf} }]
|
|
lappend ::anticollider::veto_rules $acrule
|
|
anticollision register $col
|
|
anticollision register $ap
|
|
}
|
|
}
|
|
|
|
proc ::anticollider::load_acrules {} {
|
|
variable proximity_rules
|
|
# Proximity rules have the following form,
|
|
# m1 m2 mdist
|
|
# This means that the position of m2 is always >= m1 and that a drive or
|
|
# run command which tries to drive them closer than mdist will be rejected.
|
|
# TODO Abort driving motors registered with the anticollider if one of them
|
|
# fails during the drive.
|
|
# TESTING WITH SLITS, uncomment this block and the foreach block to test with slits
|
|
# set proximity_rules {
|
|
# ss1l ss1r 10
|
|
# }
|
|
# TODO UNCOMMENT FOLLOWING TO ENABLE ANTICOLLISION FOR DETECTOR CARRIAGES
|
|
# # Detector carriage proximity rules.
|
|
# # det1 = curtain detector
|
|
# # det2 = main detector which is at higher y-pos then det1
|
|
# set proximity_rules {
|
|
# det1 det2 2000
|
|
# }
|
|
# foreach {m1 m2 minsep} $proximity_rules {
|
|
# anticollision register $m1
|
|
# anticollision register $m2
|
|
# clientput "::anticollider::proximity_rule: $m1 $m2 minsep = $minsep"
|
|
# }
|
|
}
|
|
|
|
# @brief Don't allow two axes to get closer than a minimum separation given in the 'proximity_rules' list.
|
|
# Assumes that both objects are on the same open track
|
|
# Both axes are required to move at the same speed with the same accel and decel values.
|
|
# NOTE: Doesn't check that the speeds and accel set by the Galil match what is requested.
|
|
# If axes are within the minimum distance then you can only drive them
|
|
# apart when the targets are greater than the minimum allowed separation.
|
|
proc proximity_script {args} {
|
|
set catch_status [ catch {
|
|
variable proximity_rules
|
|
|
|
if {[info exists proximity_rules] == 0} {
|
|
return
|
|
} elseif {[len $proximity_rules] == 0} {
|
|
return
|
|
}
|
|
foreach {m1 m2 mdist} $proximity_rules {
|
|
set final($m1) [SplitReply [$m1]]
|
|
set final($m2) [SplitReply [$m2]]
|
|
}
|
|
foreach {mot val} $args {
|
|
set final($mot) $val
|
|
}
|
|
foreach {mot val} $args {
|
|
set rulenum 1
|
|
foreach {m1 m2 mdist} $proximity_rules {
|
|
if {$m1 != $mot && $m2 != $mot} {
|
|
continue
|
|
}
|
|
if {$mdist <=0} {
|
|
error "Minimum separation for $m1 and $m2 must be > 0 in proximity_rule $rulenum"
|
|
}
|
|
set speed($m1) [SplitReply [$m1 speed]]
|
|
set accel($m1) [SplitReply [$m1 accel]]
|
|
set decel($m1) [SplitReply [$m1 decel]]
|
|
set pos($m1) [SplitReply [$m1]]
|
|
|
|
set speed($m2) [SplitReply [$m2 speed]]
|
|
set accel($m2) [SplitReply [$m2 accel]]
|
|
set decel($m2) [SplitReply [$m2 decel]]
|
|
set pos($m2) [SplitReply [$m2]]
|
|
# NOTE Speed and accel tests assume that the Galil actually sets the
|
|
# speeds and accelerations to the same expected steps after taking gear
|
|
# ratios, transmission, (something else?) into account on both axes
|
|
# when we drive the motors.
|
|
if {$speed($m1) != $speed($m2)} {
|
|
error "$m1 and $m2 speeds must be equal to ensure they won't collide"
|
|
}
|
|
if {$accel($m1) != $accel($m2)} {
|
|
error "$m1 and $m2 accelerations must be equal to ensure they won't collide"
|
|
}
|
|
if {$decel($m1) != $decel($m2)} {
|
|
error "$m1 and $m2 decelerations must be equal to ensure they won't collide"
|
|
}
|
|
if {$pos($m2) - $pos($m1) < 0} {
|
|
error "Rule($rulenum): $m1 $m2 $mdist seems to be invalid. It assumes that $m2 pos is always >= $m1 pos but $m1 is at $pos($m1) and $m2 is at $pos($m2)"
|
|
}
|
|
if {$final($m2) - $final($m1) < $mdist} {
|
|
error "$m1 and $m2 will violate rule:$rulenum, the minimum allowed separation is positive $mdist"
|
|
}
|
|
incr rulenum
|
|
}
|
|
}
|
|
} message ]
|
|
handle_exception $catch_status $message
|
|
}
|
|
lappend ::anticollider::scripts ::anticollider::proximity_script
|
|
}
|
|
|
|
# NOTE: This is called with a list of motorname target pairs
|
|
proc ::anticollider::enable {args} {
|
|
if {[SplitReply [::anticollider::protect_detector]] == "false"} {
|
|
return "false"
|
|
} else {
|
|
return "true"
|
|
}
|
|
}
|
|
|
|
::anticollider::loadscript acscript.txt
|