233 lines
6.8 KiB
Tcl
233 lines
6.8 KiB
Tcl
# Author: Ferdi Franceschini (ffr@ansto.gov.au)
|
|
|
|
# TODO Handle sequencing when simultaneously moving multiple axes
|
|
|
|
AntiCollisionInstall
|
|
namespace eval anticollider {
|
|
variable veto_region
|
|
}
|
|
array unset ::anticollider::veto_region
|
|
array set ::anticollider::veto_region ""
|
|
|
|
# Don't show 'acscript' call on error. This is done by the anticollider
|
|
# module.
|
|
proc handle_acscript_exception {status message args} {
|
|
switch $status {
|
|
0 {
|
|
# TCL_OK, This is raised when you just drop out of the
|
|
# bottom of a 'catch' command.
|
|
return -code ok
|
|
}
|
|
1 {
|
|
# TCL_ERROR
|
|
return -code error "$message: $args"
|
|
}
|
|
2 {
|
|
# TCL_RETURN
|
|
return -code return "$message"
|
|
}
|
|
3 {
|
|
# TCL_BREAK
|
|
return -code break
|
|
}
|
|
4 {
|
|
# TCL_CONTINUE
|
|
return -code continue
|
|
}
|
|
default {
|
|
# Propogate user defined return codes with message
|
|
return -code $status "$message"
|
|
}
|
|
}
|
|
}
|
|
##
|
|
# @brief Load an anticollider script
|
|
proc ::anticollider::loadscript {args} {
|
|
variable veto_rules
|
|
|
|
catch {
|
|
set fh [open $::cfPath(anticollider)/[lindex $args 0] RDONLY ]
|
|
while {[gets $fh line] >= 0} {
|
|
# Skip empty lines and comments
|
|
if [regexp {^\s*$|^ *#} $line] {
|
|
continue
|
|
}
|
|
lappend veto_rules $line
|
|
}
|
|
}
|
|
catch {
|
|
close $fh
|
|
}
|
|
}
|
|
|
|
##
|
|
# @brief Compile compile an anticollider declaration into a veto region table
|
|
# for the anticollider script.
|
|
#
|
|
# @param veto_rules, an anticollider declaration as a list of quote enclosed lines.
|
|
# @return Generates the ::anticollider::veto_region lookup table.
|
|
#
|
|
# Example\n
|
|
# forbid {160 167} for stth when mtth in {87 88}\n
|
|
# forbid { {0 15} {20 25} } for stth when mtth in { {80 90} {139.5 140.5} }\n
|
|
# for pcx forbid { {80 130} {-inf 10} }\n
|
|
# when mom in {0 45} forbid {{0 15} {345 360}} for pcr\n
|
|
# for sphi forbid { {0 5} {10 15} } when schi in { {5 10} {15 20} }\n
|
|
# forbid {-inf 5} when mtth in {0 10} for sphi\n
|
|
# forbid {0 10} for samx whenall { samrot in {0 5} samy in {0 15} }\n
|
|
proc ::anticollider::genveto {veto_rules} {
|
|
variable veto_region
|
|
array unset veto_region
|
|
set lnum 1
|
|
|
|
foreach line $veto_rules {
|
|
array unset vp
|
|
array set vp $line
|
|
clientput "::anticollider::veto_rule: $line"
|
|
|
|
if [info exists vp(whenall)] {
|
|
foreach {mot in range} $vp(whenall) {
|
|
# if {[llength [join $range]] != 2} {
|
|
# error "ERROR: $range is not a valid range for $mot. Line $lnum of the veto list"
|
|
# }
|
|
lappend condlist $mot $range
|
|
}
|
|
lappend veto_region($vp(for)) [list $vp(forbid) @and $condlist]
|
|
unset condlist
|
|
} elseif [info exists vp(when)] {
|
|
lappend veto_region($vp(for)) [list $vp(forbid) $vp(when) $vp(in)]
|
|
} else {
|
|
lappend veto_region($vp(for)) [list $vp(forbid) @any @all]
|
|
}
|
|
incr lnum
|
|
}
|
|
}
|
|
|
|
##
|
|
# @brief Instrument specific configurations should re-implement this if they want
|
|
# to override the anticollision detection on some conditions
|
|
#
|
|
# @param args list of motorname target pairs
|
|
# @return "true" (default) enables anticollision detection
|
|
# @return "false" disables anticollision detection
|
|
proc ::anticollider::enable {args} {
|
|
return "true"
|
|
}
|
|
##
|
|
# @brief Generic anti-collision script for simple collision avoidance.
|
|
#
|
|
# WARNING: This does not handle sequencing. Only run one motor at a time.
|
|
# This script requires that an ::anticollider::veto_region has been generated
|
|
# by the ::anticollider::genveto procedure.
|
|
#
|
|
# The ::anticollider::veto_region is a hash indexed by the names of the motors
|
|
# which have been registered with the anticollision module.
|
|
proc ::anticollider::veto_region_acscript {args} {
|
|
variable veto_region
|
|
|
|
set catch_status [ catch {
|
|
foreach {regmot target} $args {
|
|
if { ! [info exists veto_region($regmot)] } {
|
|
continue
|
|
}
|
|
foreach row $veto_region($regmot) {
|
|
if { [lindex $row 1] == "@and"} {
|
|
set forbid [lindex $row 0]
|
|
set veto 1
|
|
foreach {mot range} [lindex $row 2] {
|
|
set pos [SplitReply [$mot]]
|
|
foreach {lower upper} [join $range] {
|
|
if {$pos < $lower || $pos > $upper} {
|
|
set veto 0
|
|
break
|
|
}
|
|
}
|
|
}
|
|
if {!$veto} {
|
|
continue
|
|
} else {
|
|
foreach {min max} $forbid {}
|
|
if {$min <= $target && $target <= $max} {
|
|
foreach {mot range} [lindex $row 2] {
|
|
lappend msg [list $mot in $range]
|
|
}
|
|
error "ERROR:The range ($forbid) is forbidden for $regmot when [join $msg { and }]"
|
|
}
|
|
}
|
|
} else {
|
|
foreach {forbidden_range obstmot obstrange} $row {
|
|
if {$obstmot == "@any"} {
|
|
if {$obstrange == "@all"} {
|
|
foreach {min max} [join $forbidden_range] {
|
|
if {$min <= $target && $target <= $max} {
|
|
error "ERROR: $regmot target ($target) is in the forbidden region ($forbidden_range)"
|
|
}
|
|
}
|
|
} else {
|
|
error "ERROR: veto table must use @all with @any"
|
|
}
|
|
} else {
|
|
if {$obstrange == "@all"} {
|
|
error "ERROR: veto table must use @any with @all"
|
|
} else {
|
|
foreach {lower upper} [join $obstrange] {
|
|
set pos [SplitReply [$obstmot]]
|
|
if {$lower <= $pos && $pos <= $upper} {
|
|
foreach {min max} [join $forbidden_range] {
|
|
if {$min <= $target && $target <= $max} {
|
|
error "ERROR:The range $min to $max is forbidden for $regmot when $obstmot is in this region ($obstrange)"
|
|
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
} message ]
|
|
handle_exception $catch_status $message
|
|
}
|
|
|
|
##
|
|
# @brief Generate anticollider veto_region and register motors with anticollider
|
|
proc ::anticollider::init {} {
|
|
variable evp
|
|
variable veto_region
|
|
|
|
set catch_status [ catch {
|
|
if { ![info exists ::anticollider::veto_rules] } {
|
|
return
|
|
}
|
|
clientput Load anticollider rules:
|
|
if { [info procs ::anticollider::load_acrules] == "::anticollider::load_acrules" } {
|
|
::anticollider::load_acrules
|
|
}
|
|
::anticollider::genveto $::anticollider::veto_rules
|
|
foreach motor [array names veto_region] {
|
|
anticollision register $motor
|
|
}
|
|
} message ]
|
|
handle_exception $catch_status $message
|
|
}
|
|
|
|
lappend ::anticollider::scripts ::anticollider::veto_region_acscript
|
|
proc ::anticollider::acscript {args} {
|
|
set catch_status [ catch {
|
|
foreach {regmot target} $args {
|
|
anticollision add 0 $regmot $target
|
|
}
|
|
if {[::anticollider::enable $args] == "true"} {
|
|
foreach script $::anticollider::scripts {
|
|
$script {*}$args
|
|
}
|
|
}
|
|
} message ]
|
|
handle_acscript_exception $catch_status $message
|
|
}
|
|
|
|
publish ::anticollider::acscript user
|
|
anticollision script ::anticollider::acscript
|