add Epson robot pick and place driver
r3017 | dcl | 2010-08-06 15:11:17 +1000 (Fri, 06 Aug 2010) | 1 line
This commit is contained in:
Douglas Clowes
464
site_ansto/instrument/config/robots/sct_pickandplace.tcl
Normal file
464
site_ansto/instrument/config/robots/sct_pickandplace.tcl
Normal file
@@ -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
|
||||
@@ -107,6 +107,7 @@ plc $cfParent/plc\
|
||||
commands $cfParent/commands\
|
||||
beamline $cfParent/beamline\
|
||||
goniometer $cfParent/goniometer\
|
||||
robots $cfParent/robots\
|
||||
]
|
||||
|
||||
ServerOption LogFileBaseName $sicsroot/log/serverlog
|
||||
|
||||
Reference in New Issue
Block a user