From e87cc117b8b86f58e250dc01f4f3b8d466c34fa4 Mon Sep 17 00:00:00 2001
From: Douglas Clowes <dcl@ansto.gov.au>
Date: Fri, 6 Aug 2010 15:11:17 +1000
Subject: [PATCH] add Epson robot pick and place driver r3017 | dcl |
 2010-08-06 15:11:17 +1000 (Fri, 06 Aug 2010) | 1 line

---
 .../config/robots/sct_pickandplace.tcl        | 464 ++++++++++++++++++
 site_ansto/instrument/server_config.tcl       |   1 +
 2 files changed, 465 insertions(+)
 create mode 100644 site_ansto/instrument/config/robots/sct_pickandplace.tcl

diff --git a/site_ansto/instrument/config/robots/sct_pickandplace.tcl b/site_ansto/instrument/config/robots/sct_pickandplace.tcl
new file mode 100644
index 00000000..dc0d4e9f
--- /dev/null
+++ b/site_ansto/instrument/config/robots/sct_pickandplace.tcl
@@ -0,0 +1,464 @@
+# Define procs in ::scobj::xxx namespace
+# MakeSICSObj $obj SCT_<class>
+#  The MakeSICSObj cmd adds a /sics/$obj node.  NOTE the /sics node is not browsable.
+
+
+namespace eval ::scobj::robot_pp {
+# Environment controllers should have at least the following nodes
+# /envcont/setpoint
+# /envcont/sensor/value
+  proc debug_log {args} {
+    set fd [open "/tmp/robot_pp.log" a]
+    puts $fd "[clock format [clock seconds] -format "%T"] $args"
+    close $fd
+  }
+
+# issue a command to read a register and expect a value response
+  proc getValue {tc_root nextState cmd} {
+debug_log "getValue $cmd sct=[sct] root=$tc_root nextState=$nextState"
+    sct send "$cmd"
+    return $nextState
+  }
+
+# issue a command with a value in the target property of the variable
+  proc setValue {tc_root nextState cmd} {
+debug_log "setValue cmd=$cmd sct=[sct] $tc_root"
+    set par "[sct target]"
+    sct send "$cmd$par"
+debug_log "setValue $cmd$par"
+    return $nextState
+  }
+
+  proc setNam {tc_root nextState cmd} {
+    set data [string toupper [sct target]]
+    if {$data != [sct oldval]} {
+      sct oldval $data
+      sct update $data
+      sct utime readtime
+debug_log "setNam new data for $tc_root [sct] result=$data"
+    }
+    return $nextState
+  }
+
+  proc chkWrite {tc_root} {
+    set data [sct result]
+debug_log "chkWrite resp=$data sct=[sct] tc_root=$tc_root"
+    if {[string equal -nocase -length 7 $data "ASCERR:"]} {
+      sct geterror "$data"
+    } elseif {[string equal -nocase -length 1 $data "?"]} {
+      sct geterror "Error: $data"
+    } else {
+      set data [sct target]
+      if {$data != [sct oldval]} {
+        sct oldval $data
+        sct update $data
+        sct utime readtime
+debug_log "chkWrite new data for $tc_root [sct] result=$data"
+      }
+    }
+    return idle
+  }
+
+  proc setPoint {tc_root nextState cmd} {
+    set catch_status [ catch {
+debug_log "setPoint $tc_root $nextState $cmd sct=[sct]"
+debug_log "setPoint: sct=[sct] target=[sct target] writestatus=[sct writestatus]"
+    sct print "setPoint: sct=[sct] target=[sct target] writestatus=[sct writestatus]"
+    set err_msg ""
+    if { [hval $tc_root/Control/Pallet_Nam] != "A" && [hval $tc_root/Control/Pallet_Nam] != "B" } {
+      set err_msg "Invalid Pallet_ Num: [hval $tc_root/Control/Pallet_Nam]"
+    } elseif { [hval $tc_root/Control/Pallet_Idx] < 1 || [hval $tc_root/Control/Pallet_Idx] > 50 } {
+      set err_msg "Invalid Pallet_Idx: [hval $tc_root/Control/Pallet_Idx]"
+    } 
+    if { $err_msg != "" } {
+      sct print "error:$err_msg"
+      debug_log "error:$err_msg"
+      return -code error "$err_msg"
+    }
+    set par "[sct target]"
+    if {$par != [sct oldval]} {
+      sct oldval $par
+      sct update $par
+      sct utime readtime
+debug_log "setPoint new data for $tc_root [sct] result=$par"
+    }
+    hset $tc_root/status "busy"
+    sct print "status: busy"
+    if { [sct target] == 1 } {
+      if { [string toupper [hval $tc_root/Control/Target_Loc]] == "BEAM" } {
+        set cmd  "SampToBeam [hval $tc_root/Control/Pallet_Nam] [hval $tc_root/Control/Pallet_Idx]"
+debug_log "sct send $cmd"
+        sct send "$cmd"
+      } elseif { [string toupper [hval $tc_root/Control/Target_Loc]] == "VACUUM" } {
+        set cmd  "SampToVac [hval $tc_root/Control/Pallet_Nam] [hval $tc_root/Control/Pallet_Idx]"
+debug_log "sct send $cmd"
+        sct send "$cmd"
+      } else {
+        set err_msg "Invalid Target_Loc: [hval $tc_root/Control/Target_Loc]"
+        sct print "error:$err_msg"
+        debug_log "error:$err_msg"
+        return -code error "$err_msg"
+      }
+    } elseif { [sct target] == 0 } {
+      if { [string toupper [hval $tc_root/Control/Target_Loc]] == "BEAM" } {
+        set cmd  "SampRtn [hval $tc_root/Control/Pallet_Nam] [hval $tc_root/Control/Pallet_Idx]"
+debug_log "sct send $cmd"
+        sct send "$cmd"
+      } elseif { [string toupper [hval $tc_root/Control/Target_Loc]] == "VACUUM" } {
+        set cmd  "VacRtn [hval $tc_root/Control/Pallet_Nam] [hval $tc_root/Control/Pallet_Idx]"
+debug_log "sct send $cmd"
+        sct send "$cmd"
+      } else {
+        set err_msg "Invalid Target_Loc: [hval $tc_root/Control/Target_Loc]"
+        sct print "error:$err_msg"
+        debug_log "error:$err_msg"
+        return -code error "$err_msg"
+      }
+    } else {
+        set err_msg "Invalid DRIVE target: [sct target]"
+        sct print "error:$err_msg"
+        debug_log "error:$err_msg"
+        return -code error "$err_msg"
+    }
+    hsetprop $tc_root/setpoint driving 1
+    } catch_message ]
+    if {$catch_status != 0} {
+      return -code error $catch_message
+    }
+    sct print "setPoint: [hget $tc_root/drive_state]"
+    return $nextState
+  }
+
+  proc rdValue {tc_root} {
+debug_log "rdValue tc_root=$tc_root sct=[sct]"
+    set data [sct result]
+    if {[ catch {
+debug_log "rdValue $tc_root [sct] result=$data"
+    } catch_message ]} {
+debug_log "rdValue $tc_root failure"
+    }
+    if {[string equal -nocase -length 7 $data "ASCERR:"]} {
+      sct geterror "$data"
+      set nextState idle
+    } elseif {[string equal -nocase -length 1 $data "?"]} {
+      sct geterror "Error: $data"
+      set nextState idle
+    } else {
+      if { [hpropexists [sct] geterror] } {
+        hdelprop [sct] geterror
+      }
+      if {$data != [sct oldval]} {
+debug_log "rdValue new data for $tc_root [sct] was=[sct oldval] now=$data"
+        sct oldval $data
+        sct update $data
+        sct utime readtime
+      }
+    }
+    return idle
+  }
+
+# This is the command phase of the state machine that drives the controller.
+# For each state, it sends the appropriate command to get values from, or set
+# values in the controller in a sequence intended to transition the controller
+# between states.
+  proc getState {tc_root nextState cmd} {
+debug_log "getState $tc_root $nextState $cmd sct=[sct]"
+  if {[ catch {
+    if { [hpropexists [sct] geterror] } {
+      hdelprop [sct] geterror
+    }
+debug_log "getState sends: $cmd"
+    sct send "$cmd"
+  } catch_message ]} {
+debug_log "getState error: $catch_message"
+  }
+debug_log "getState returns: $nextState"
+    return $nextState
+  }
+
+# This is the response phase of the state machine that drives the controller.
+# For each state, it reads the appropriate command response from the controller
+# and, based on the response and internal variables performs a sequence
+# intended to transition the controller between states.
+
+  ##
+  # @brief Reads the current robot state and error messages.
+  proc rdState {tc_root} {
+    debug_log "rdState $tc_root sct=[sct] response=\"[sct result]\""
+    set nextState {}
+    if {[ catch {
+      set data "[sct result]"
+      if {[string equal -nocase -length 7 $data "ASCERR:"]} {
+        sct geterror "$data"
+        set nextState idle
+      } elseif {[string equal -nocase -length 1 $data "?"]} {
+        sct geterror "Error: $data"
+        set nextState idle
+      } else {
+        set my_driving [SplitReply [hgetprop $tc_root/setpoint driving]]
+        if { $my_driving } {
+          if { [string toupper [sct result]] == "IDLE" } {
+            hsetprop $tc_root/setpoint driving 0
+            hset     $tc_root/sensor/value [hval $tc_root/setpoint]
+          }
+        }
+      }
+    } catch_message ]} {
+    debug_log "rdState error: $catch_message"
+    }
+    if { "$nextState" == "" } {
+      set nextState "idle"
+    }
+    debug_log "rdState returns: $nextState"
+    return $nextState
+  }
+
+  proc noResponse {} {
+    return idle
+  }
+  proc wrtValue {wcmd args} {
+  }
+
+# check that a target is within allowable limits
+  proc check {tc_root} {
+    set setpoint [sct target]
+    set lolimit [hval $tc_root/lowerlimit]
+    set hilimit [hval $tc_root/upperlimit]
+    if {$setpoint < $lolimit || $setpoint > $hilimit} {
+      error "setpoint violates limits"
+    }
+    return OK
+  }
+
+##
+# @brief Implement the checkstatus command for the drivable interface
+#
+# NOTE: The drive adapter initially sets the writestatus to "start" and will
+# only call this when writestatus!="start"
+  proc drivestatus {tc_root} {
+    if {[sct driving]} {
+      return busy
+    } else {
+      sct print "drivestatus: idle"
+      return idle
+    }
+  }
+
+  proc halt {tc_root} {
+debug_log "halt $tc_root"
+sct print "halt $tc_root"
+    set my_driving [SplitReply [hgetprop $tc_root/setpoint driving]]
+    hset $tc_root/drive_state "HALT"
+    if { $my_driving } {
+      hsetprop $tc_root/robot_state my_state "HALT"
+      return read
+    }
+    return idle
+  }
+
+##
+# @brief createNode() creates a node for the given nodename with the properties and virtual
+# function names provided
+# @param scobj_hpath    string variable holding the path to the object's base node in sics (/sample/tc1)
+# @param sct_controller name of the scriptcontext object (typically sct_xxx_yyy)
+# @param cmdGroup       subdirectory (below /sample/tc*/) in which the node is to be created
+# @param varName        name of the actual node typically representing one device command
+# @param readable       set to 1 if the node represents a query command, 0 if it is not
+# @param writable       set to 1 if the node represents a request for a change in settings sent to the device
+# @param drivable       if set to 1 it prepares the node to provide a drivable interface
+# @param dataType       data type of the node, must be one of none, int, float, text
+# @param permission     defines what user group may read/write to this node (is one of spy, user, manager)
+# @param rdCmd          actual device query command to be sent to the device
+# @param rdFunc         nextState Function to be called after the getValue function, typically rdValue()
+# @param wrCmd          actual device write command to be sent to the device
+# @param wrFunc         Function to be called to send the wrCmd to the device, typically setValue()
+# @param allowedValues  allowed values for the node data - does not permit other
+# @param klass          Nexus class name (?)
+# @return               OK
+proc createNode {scobj_hpath sct_controller cmdGroup varName readable writable\
+  drivable dataType permission rdCmd rdFunc wrCmd\
+  wrFunc allowedValues klass} {
+
+  set catch_status [ catch {
+    set ns "[namespace current]"
+    set nodeName "$scobj_hpath/$cmdGroup/$varName"
+    if {1 > [string length $cmdGroup]} {
+      set nodeName "$scobj_hpath/$varName"
+    }
+debug_log "Creating node $nodeName"
+    hfactory $nodeName plain $permission $dataType
+    if {$readable > 0} {
+      hsetprop $nodeName read ${ns}::getValue $scobj_hpath $rdFunc $rdCmd
+      hsetprop $nodeName $rdFunc ${ns}::$rdFunc $scobj_hpath
+      set poll_period 30
+      if { $readable >= 0 && $readable <= 9 } {
+        set poll_period [lindex [list 0 1 2 3 4 5 10 15 20 30] $readable]
+      }
+debug_log "Registering node $nodeName for poll at $poll_period seconds"
+      $sct_controller poll $nodeName $poll_period
+    }
+    if {$writable == 1 && "$wrFunc" != ""} {
+      set pos [string first "." "$wrFunc"]
+      if { $pos > 0 } {
+        set parts [split "$wrFunc" "."]
+        if { [llength $parts] >= 2 } {
+          set func_name [lindex $parts 0]
+          set next_state [lindex $parts 1]
+          hsetprop $nodeName write ${ns}::$func_name $scobj_hpath $next_state $wrCmd
+          hsetprop $nodeName $next_state ${ns}::$next_state $scobj_hpath
+        }
+      } else {
+        hsetprop $nodeName write ${ns}::$wrFunc $scobj_hpath noResponse $wrCmd
+        hsetprop $nodeName noResponse ${ns}::noResponse
+      }
+      hsetprop $nodeName writestatus UNKNOWN
+debug_log "Registering node $nodeName for write callback"
+      $sct_controller write $nodeName
+    }
+    switch -exact $dataType {
+      "none"  { }
+      "int"   { hsetprop $nodeName oldval -1 }
+      "float" { hsetprop $nodeName oldval -1.0 }
+      default { hsetprop $nodeName oldval UNKNOWN }
+    }
+    if {1 < [string length $allowedValues]} {
+      hsetprop $nodeName values $allowedValues
+    }
+    #        Drive adapter interface
+    if {$drivable == 1} {
+      hsetprop $nodeName check ${ns}::check $scobj_hpath
+      hsetprop $nodeName driving 0
+      hsetprop $nodeName checklimits ${ns}::check $scobj_hpath
+      hsetprop $nodeName checkstatus ${ns}::drivestatus $scobj_hpath
+      hsetprop $nodeName halt ${ns}::halt $scobj_hpath
+    }
+  } message ]
+  if {$catch_status != 0} {
+    return -code error "in createNode $message"
+  }
+  return OK
+}
+
+  proc mk_sct_robot_pp {sct_controller klass tempobj} {
+    set catch_status [ catch {
+      set ns "[namespace current]"
+
+      MakeSICSObj $tempobj SCT_OBJECT
+      sicslist setatt $tempobj klass $klass
+      sicslist setatt $tempobj long_name $tempobj
+
+      set scobj_hpath /sics/$tempobj
+
+      set deviceCommand {\
+        {}       setpoint        0 1 1 float user     {}         {rdValue}      {}                 {setPoint.chkWrite} {}\
+        sensor   value           0 0 0 float internal {}         {rdValue}      {}                 {}         {}\
+        Display  State           1 0 0 text  internal {STATE}    {rdValue}      {}                 {}         {}\
+        Display  Status          1 0 0 text  internal {STATUS}   {rdValue}      {}                 {}         {}\
+        Display  Location        1 0 0 text  internal {WHERE}    {rdValue}      {}                 {}         {}\
+        Control  Pallet_Nam      0 1 0 text  user     {}         {}             {}                 {setNam}   {A,B}\
+        Control  Pallet_Idx      0 1 0 int   user     {}         {}             {}                 {}         {}\
+        Control  Target_Loc      0 1 0 Text  user     {}         {}             {}                 {setNam}   {Beam,Vacuum}\
+      }
+
+      hfactory $scobj_hpath/sensor plain spy none
+      hfactory $scobj_hpath/Display plain spy none
+      hfactory $scobj_hpath/Control plain spy none
+
+      foreach {cmdGroup varName readable writable drivable dataType permission rdCmd rdFunc wrCmd wrFunc allowedValues} $deviceCommand {
+        createNode $scobj_hpath $sct_controller $cmdGroup $varName $readable $writable $drivable $dataType $permission $rdCmd $rdFunc $wrCmd $wrFunc $allowedValues $klass
+      }
+
+      hsetprop $scobj_hpath/sensor/value lowerlimit -12
+      hsetprop $scobj_hpath/sensor/value upperlimit 12
+      hsetprop $scobj_hpath/sensor/value units "T"
+
+      hfactory $scobj_hpath/status plain spy text
+      hset     $scobj_hpath/status "idle"
+      hsetprop $scobj_hpath/status values busy,idle
+
+      hfactory $scobj_hpath/robot_state plain spy text
+      hsetprop $scobj_hpath/robot_state read ${ns}::getState $scobj_hpath rdState "STATE"
+      hsetprop $scobj_hpath/robot_state rdState ${ns}::rdState $scobj_hpath
+      hsetprop $scobj_hpath/robot_state oldval "UNKNOWN"
+      hsetprop $scobj_hpath/robot_state my_state "UNKNOWN"
+      hsetprop $scobj_hpath/robot_state my_status "UNKNOWN"
+      hsetprop $scobj_hpath/robot_state my_version "UNKNOWN"
+
+      hfactory $scobj_hpath/drive_state plain mugger text
+      hset     $scobj_hpath/drive_state "UNKNOWN"
+
+      hfactory $scobj_hpath/remote_ctrl plain spy text
+      hset     $scobj_hpath/remote_ctrl "UNKNOWN"
+
+      hfactory $scobj_hpath/robot_lasterror plain user text
+      hset     $scobj_hpath/robot_lasterror ""
+
+      hfactory $scobj_hpath/lowerlimit plain mugger float
+      hsetprop $scobj_hpath/lowerlimit units "T"
+      hset     $scobj_hpath/lowerlimit -11
+
+      hfactory $scobj_hpath/upperlimit plain mugger float
+      hsetprop $scobj_hpath/upperlimit units "T"
+      hset     $scobj_hpath/upperlimit 11
+
+      if {[SplitReply [environment_simulation]]=="false"} {
+        $sct_controller poll $scobj_hpath/robot_state 1 halt read
+      }
+
+      hsetprop $scobj_hpath type part
+      foreach snsr {sensor} {
+        foreach {rootpath hpath klass priv} "\
+          $scobj_hpath $snsr         NXsensor  spy\
+          $scobj_hpath $snsr/value   sensor    user\
+        " {
+          hsetprop $rootpath/$hpath klass $klass
+          hsetprop $rootpath/$hpath privilege $priv
+          hsetprop $rootpath/$hpath control true
+          hsetprop $rootpath/$hpath data true
+          hsetprop $rootpath/$hpath nxsave true
+        }
+        hsetprop $scobj_hpath/$snsr type part
+        hsetprop $scobj_hpath/$snsr/value nxalias tc1_${snsr}_value
+        hsetprop $scobj_hpath/$snsr/value mutable true
+        hsetprop $scobj_hpath/$snsr/value sdsinfo ::nexus::scobj::sdsinfo
+      }
+      hsetprop $scobj_hpath privilege spy
+      ::scobj::hinitprops $tempobj setpoint
+      if {[SplitReply [environment_simulation]]=="false"} {
+        ansto_makesctdrive ${tempobj}_driveable $scobj_hpath/setpoint $scobj_hpath/sensor/value $sct_controller
+      }
+    } catch_message ]
+    if {$catch_status != 0} {
+      return -code error $catch_message
+    }
+  }
+  namespace export mk_sct_robot_pp
+}
+
+##
+# @brief Create a Pick and Place Robot Controller
+#
+# @param name, the name of the robot (eg robbie)
+# @param IP, the IP address of  the device, this can be a hostname, (eg ca1-kowari)
+# @param port, the IP protocol port number of the device
+proc add_robot {name IP port } {
+  set fd [open "/tmp/robot_pp.log" a]
+  if {[SplitReply [environment_simulation]]=="false"} {
+    puts $fd "makesctcontroller sct_robot_pp std ${IP}:$port"
+    makesctcontroller sct_robot_pp std ${IP}:$port [binary format cc 0x0D 0x0A] 5.0
+  }
+  puts $fd "mk_sct_robot_pp sct_robot environment $name"
+  mk_sct_robot_pp sct_robot_pp environment $name
+  close $fd
+}
+
+puts stdout "file evaluation of sct_pickandplace.tcl"
+set fd [open "/tmp/robot_pp.log" w]
+puts $fd "file evaluation of sct_pickandplace.tcl"
+close $fd
+
+namespace import ::scobj::robot_pp::*
+
+#add_robot robbie 137.157.201.213 502 5
+#add_robot robbie localhost 30509
+add_robot robbie 137.157.201.26 6000
diff --git a/site_ansto/instrument/server_config.tcl b/site_ansto/instrument/server_config.tcl
index f4cd450b..6237ed80 100644
--- a/site_ansto/instrument/server_config.tcl
+++ b/site_ansto/instrument/server_config.tcl
@@ -107,6 +107,7 @@ plc           $cfParent/plc\
 commands      $cfParent/commands\
 beamline      $cfParent/beamline\
 goniometer    $cfParent/goniometer\
+robots        $cfParent/robots\
 ]
 
 ServerOption LogFileBaseName $sicsroot/log/serverlog