Merging release 2.0 branch with CVS trunk

r2601 | ffr | 2008-05-30 10:26:57 +1000 (Fri, 30 May 2008) | 2 lines

site_ansto/instrument/config/anticollider/anticollider_common.tcl

@@ -0,0 +1,167 @@
+# $Revision: 1.2 $
+# $Date: 2008-05-30 00:26:54 $
+# Author: Ferdi Franceschini (ffr@ansto.gov.au)
+# Last revision by: $Author: ffr $
+
+# TODO Handle sequencing when simultaneously moving multiple axes
+# TODO Handle functional dependencies (just write your own tcl script)
+#      or get the generic acscript to call a user proc
+# FIXME The anticollider module does not report the error messages from the
+# anticollision script, we currently get around this by using broadcast.
+
+AntiCollisionInstall
+namespace eval anticollider {
+  variable veto_region
+}
+array unset ::anticollider::veto_region 
+array set ::anticollider::veto_region ""
+
+##
+# @brief Load an anticollider script
+proc ::anticollider::loadscript {args} {
+  variable prog
+  set prog ""
+
+  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 prog $line
+  }
+}
+
+##
+# @brief Compile compile an anticollider declaration into a veto region table
+# for the anticollider script.
+#
+# @param vprog, 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 {vprog} {
+  variable veto_region
+  array unset veto_region 
+  set lnum 1
+
+  foreach line $vprog {
+    array unset vp
+    array set vp $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]
+    } 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 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::acscript {args} {
+  variable veto_region
+
+    foreach {regmot target} $args {
+      foreach row  $veto_region($regmot) {
+      if { [lindex $row 1] == "@and"} {
+        set forbid [lindex $row 0]
+        set no_veto 0
+        foreach {mot range} [lindex $row 2] {
+          set pos [SplitReply [$mot]]
+          foreach {lower upper} $range {}
+          if {$pos < $lower || $pos > $upper} {
+            set no_veto 1
+            break
+          }
+        }
+        if {$no_veto} {
+          continue
+        } else {
+          foreach {min max} $forbid {}
+          if {$min <= $target && $target <= $max} {
+            broadcast "ERROR:The range ($forbid) is forbidden for $regmot when [lindex $row 2]"
+            error "ERROR:The range ($forbid) is forbidden for $regmot when [lindex $row 2]"
+          }
+        }
+      } else {
+        foreach {forbidden_range obstmot obstrange} $row {
+          if {$obstmot == "@any"} {
+            if {$obstrange == "@all"} {
+              foreach {min max} [join $forbidden_range] {
+                if {$min <= $target && $target <= $max} {
+                  broadcast "ERROR: $regmot target ($target) is in the forbidden region ($forbidden_range)"
+                  error "ERROR: $regmot target ($target) is in the forbidden region ($forbidden_range)"
+                }
+              }
+            } else {
+              broadcast "ERROR: veto table must use @all with @any"
+              error "ERROR: veto table must use @all with @any"
+            }
+          } else {
+            if {$obstrange == "@all"} {
+              broadcast "ERROR: veto table must use @any with @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} {
+                      broadcast "ERROR:The range $min to $max is forbidden for $regmot when $obstmot is in this region ($obstrange)"
+                      error "ERROR:The range $min to $max is forbidden for $regmot when $obstmot is in this region ($obstrange)"
+
+                    }
+                  }
+                }
+              }
+            }
+          }
+        }
+      }
+      }
+      anticollision add 0 $regmot $target
+    }
+}
+
+##
+# @brief Generate anticollider veto_region and register motors with anticollider
+proc ::anticollider::init {} {
+  variable evp
+  variable veto_region
+
+  if [ catch {
+    ::anticollider::genveto $::anticollider::prog
+    foreach motor [array names veto_region] {
+      anticollision register $motor
+    }
+  } message ] {
+    if {$::errorCode=="NONE"} {return $message}
+    return -code error $message
+  }
+}
+
+publish ::anticollider::acscript user
+anticollision script ::anticollider::acscript