diff --git a/site_ansto/instrument/bilby/config/anticollider/anticollider.tcl b/site_ansto/instrument/bilby/config/anticollider/anticollider.tcl index 756835f3..bc3c30af 100644 --- a/site_ansto/instrument/bilby/config/anticollider/anticollider.tcl +++ b/site_ansto/instrument/bilby/config/anticollider/anticollider.tcl @@ -1,6 +1,6 @@ # Author: Ferdi Franceschini (ffr@ansto.gov.au) -source $cfPath(anticollider)/anticollider_common.tcl +fileeval $cfPath(anticollider)/anticollider_common.tcl namespace eval ::anticollider { set guide_posit 1 @@ -12,15 +12,103 @@ namespace eval ::anticollider { 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 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::prog $acrule - clientput $acrule + lappend ::anticollider::veto_rules $acrule set acrule [subst {forbid {-inf inf} for $col when $ap in {$ap_limit inf} }] - lappend ::anticollider::prog $acrule - clientput $acrule + 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 + + 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 @@ -32,3 +120,4 @@ proc ::anticollider::enable {args} { } } +::anticollider::loadscript acscript.txt