From 2954216d2dbb37a13c177f67a42beef6c662599e Mon Sep 17 00:00:00 2001 From: Douglas Clowes Date: Mon, 12 Dec 2011 16:17:39 +1100 Subject: [PATCH] A driver for the Cybaman goniometer r3317 | dcl | 2011-12-12 16:17:39 +1100 (Mon, 12 Dec 2011) | 1 line --- .../config/environment/sct_cybaman.tcl | 1118 +++++++++++++++++ 1 file changed, 1118 insertions(+) create mode 100644 site_ansto/instrument/config/environment/sct_cybaman.tcl diff --git a/site_ansto/instrument/config/environment/sct_cybaman.tcl b/site_ansto/instrument/config/environment/sct_cybaman.tcl new file mode 100644 index 00000000..2e848e47 --- /dev/null +++ b/site_ansto/instrument/config/environment/sct_cybaman.tcl @@ -0,0 +1,1118 @@ +# Define procs in ::scobj::xxx namespace +# MakeSICSObj $obj SCT_ +# The MakeSICSObj cmd adds a /sics/$obj node. NOTE the /sics node is not browsable. +# This is a Cybaman goniometer - derived from Keithley 2700 driver +# Axis A (lower) is p103 and act_pos_1 [-180,180] +# Axis B (middle) is p102 and act_pos_2 [-180,180] +# Axis A (upper) is p101 and act_pos_3 [0,360] (may read 0 at 360.0000) +# +namespace eval ::scobj::cybaman { +# Environment controllers should have at least the following nodes +# /envcont/setpoint +# /envcont/sensor/value + proc debug_log {args} { + set fd [open "/tmp/cybaman.log" a] + puts $fd "[clock format [clock seconds] -format "%T"] $args" + close $fd + } + + proc basename {node} { + set point [string last "/" $node] + if { $point < 0 } { + return $node + } else { + incr point + return "[string range $node $point end]" + } + } + proc pathname {node} { + set point [string last "/" $node] + if { $point < 0 } { + return "" + } else { + incr point -1 + return "[string range $node 0 $point]" + } + return "[join [lrange [split $node '/'] 0 end-1] '/']" + } + + proc splitxml {str} { + set my_list [list] + set idx 0 + while {$idx < [string length $str]} { + set p1 [string first "<" $str $idx] + if {$p1 > $idx} { + # copy text + lappend my_list [string range $str $idx [expr {$p1 - 1}]] + } + set p2 [string first ">" $str $p1] + if {$p2 > $p1} { + # copy tag + lappend my_list [string range $str $p1 $p2] + } + set idx [expr $p2 + 1] + } + debug_log "splitxml: $my_list" + return $my_list + } + + proc xml2list {str} { + set new_list [list] + set my_list [splitxml $str] + set idx 0 + while {$idx < ([llength $my_list] - 2)} { + if {[string index [lindex $my_list $idx] 0] == "<" && [string index [lindex $my_list $idx] end] == ">"} { + if {[string index [lindex $my_list $idx] 1] == "/" || [string index [lindex $my_list $idx] end-1] == "/"} { + set idx [expr {$idx + 1}] + continue + } + if {[string index [lindex $my_list [expr {$idx + 1}]] 0] == "<" || [string index [lindex $my_list [expr {$idx + 1}]] end] == ">"} { + set idx [expr {$idx + 1}] + continue + } + if {[string index [lindex $my_list [expr {$idx + 2}]] 0] != "<" || [string index [lindex $my_list [expr {$idx + 2}]] end] != ">"} { + set idx [expr {$idx + 1}] + continue + } + if {[string index [lindex $my_list [expr {$idx + 2}]] 1] != "/" || [string index [lindex $my_list [expr {$idx + 2}]] end-1] == "/"} { + set idx [expr {$idx + 1}] + continue + } + set name "[string range [lindex $my_list $idx] 1 end-1]" + if { [string index $name 0] == "." } { + set name [string range "$name" 1 end] + } + lappend new_list "$name" + set value "[lindex $my_list [expr {$idx + 1}]]" + lappend new_list "$value" + set idx [expr {$idx + 3}] + } + } + debug_log "xml2list: $new_list" + return $new_list + } + + proc rdV { tc_root nextState cmd } { + set channel [basename [pathname [sct]]] + set value [hval [pathname [pathname [sct]]]/Display/act_pos_$channel] + if {$value != [sct oldval]} { + sct oldval $value + sct update $value + sct utime readtime + } + return "idle" + } + proc rdN { tc_root } { + # should never get called + return "idle" + } + +# issue a command to read a register and expect a value response + proc getValue {tc_root nextState cmd} { + debug_log "getValue sct=[sct] root=$tc_root nextState=$nextState cmd=$cmd" + set line ".$cmd" + debug_log "getValue send: $line" + sct send "$line" + return $nextState + } + + proc rdParm {tc_root} { + debug_log "rdParm tc_root=$tc_root sct=[sct]" + set data [sct result] + if {[ catch { + debug_log "rdParm $tc_root [sct] result=$data" + } catch_message ]} { + debug_log "rdParm $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 + } + debug_log "rdParm Read: $data" + if {$data != [sct oldval]} { + sct oldval $data + sct update $data + sct utime readtime + debug_log "rdParm new data for $tc_root [sct] result=$data" + } + } + return idle + } + + proc rdText {tc_root} { + debug_log "rdText tc_root=$tc_root sct=[sct]" + set data [sct result] + if {[ catch { + debug_log "rdText $tc_root [sct] result=$data" + } catch_message ]} { + debug_log "rdText $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 + } + debug_log "rdText Read: $data" + if {$data != [sct oldval]} { + sct oldval $data + sct update $data + sct utime readtime + debug_log "rdText new data for $tc_root [sct] result=$data" + } + } + return idle + } + + proc rdAngle {tc_root} { + debug_log "rdAngle tc_root=$tc_root sct=[sct] result=[sct result]" + if { [hpropexists [sct] geterror] } { + hdelprop [sct] geterror + } + if {[string equal -nocase -length 7 [sct result] "ASCERR:"]} { + sct geterror "[sct result]" + return idle + } + if {[ catch { + set my_list [xml2list [sct result]] + debug_log "rdAngle my_list=$my_list" + set name [lindex $my_list 0] + set data [lindex $my_list 1] + if {[string equal -nocase $name "err"]} { + sct geterror "Error - report: $data" + return idle + } + debug_log "rdAngle $tc_root [sct] result=$data" + } catch_message ]} { + debug_log "rdAngle $tc_root failure" + sct geterror "Error - catch: $catch_message" + return idle + } + set data [expr {round($data * 0.1) * 0.001}] + if {$data != [sct oldval]} { + sct oldval $data + sct update $data + sct utime readtime + debug_log "rdAngle new data for $tc_root [sct] result=$data" + } + return idle + } + + proc rdValue {tc_root} { + debug_log "rdValue tc_root=$tc_root sct=[sct] result=[sct result]" + if { [hpropexists [sct] geterror] } { + hdelprop [sct] geterror + } + if {[string equal -nocase -length 7 [sct result] "ASCERR:"]} { + sct geterror "[sct result]" + return idle + } + if {[ catch { + set my_list [xml2list [sct result]] + debug_log "rdValue my_list=$my_list" + set name [lindex $my_list 0] + set data [lindex $my_list 1] + if {[string equal -nocase $name "err"]} { + sct geterror "Error - report: $data" + return idle + } + debug_log "rdValue data=$data" + debug_log "rdValue $tc_root [sct] result=$data" + } catch_message ]} { + debug_log "rdValue $tc_root failure" + sct geterror "Error - catch: $catch_message" + return idle + } + if {$data != [sct oldval]} { + sct oldval $data + sct update $data + sct utime readtime + debug_log "rdValue new data for $tc_root [sct] result=$data" + } + return idle + } + + proc rdData {tc_root} { + debug_log "rdData tc_root=$tc_root sct=[sct]" + set data [sct result] + if {[ catch { + debug_log "rdData $tc_root [sct] result=$data" + } catch_message ]} { + debug_log "rdData $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 + } + set flds [split "$data" ","] + scan [lindex $flds 0] "%f%s" value units + debug_log "rdData Read: $value $units from $data" + if {$value != [sct oldval]} { + sct oldval $value + sct update $value + sct utime readtime + sct units $units + debug_log "rdData new data for $tc_root [sct] result=$value" + } + } + return idle + } + + proc wrParm {tc_root nextState cmd} { + debug_log "wrParm root=$tc_root sct=[sct] cmd=$cmd target=[sct target]" + set par "[sct target]" + sct send "$cmd $par" + debug_log "wrParm send: $cmd $par;*IDN?" + if {$par != [sct oldval]} { + sct oldval $par + sct update $par + sct utime readtime + debug_log "wrParm new data for $tc_root [sct] result=$par" + } + return $nextState + } + + proc wrText {tc_root nextState cmd} { + debug_log "wrText root=$tc_root sct=[sct] cmd=$cmd target=[sct target]" + set par "[sct target]" + sct send "$cmd \"$par\";*IDN?" + debug_log "wrText send: $cmd \"$par\"" + if {$par != [sct oldval]} { + sct oldval $par + sct update $par + sct utime readtime + debug_log "wrText new data for $tc_root [sct] result=$par" + } + return $nextState + } + + proc chkTarget {par} { + set target {} + if {[hpropexists [sct] values]} { + set values [split [SplitReply [sct values]] ","] + foreach value $values { + #sct print "Testing $par against $value" + set lo_hi [split $value ":"] + if {[llength $lo_hi] == 2} { + if {[lindex $lo_hi 0] <= $par && [lindex $lo_hi 1] >= $par} { + #sct print "Success $par between [lindex $lo_hi 0] and [lindex $lo_hi 1]" + set target "$par" + break + } + } else { + if {[string toupper "$par"] == [string toupper "$value"]} { + #sct print "Success $par matches $value" + set target "$par" + break + } + } + } + if {"$target" == ""} { + #sct print "Failure $par no matches in $values" + return -code error "Invalid value: \"$par\"" + } + } else { + set target "$par" + } + return $target + } + +# issue a command with a value in the target property of the variable + proc wrAngle {tc_root nextState cmd} { + debug_log "wrAngle root=$tc_root sct=[sct] cmd=$cmd target=[sct target]" + set par "[sct target]" + set data "[expr {round(10000 * [sct target])}]" + set target [chkTarget "$par"] + if {"$target" == ""} { + return -code error "Invalid value: \"$par\"" + } + set line ".$cmd$data" + debug_log "wrAngle send: $line" + sct send "$line" + #sct print "$line" + if {$par != [sct oldval]} { + sct oldval $par + sct update $par + sct utime readtime + debug_log "wrAngle new data for $tc_root [sct] result=$par is $data" + } + return $nextState + } + + proc wrValue {tc_root nextState cmd} { + debug_log "wrValue root=$tc_root sct=[sct] cmd=$cmd target=[sct target]" + set par "[sct target]" + set target [chkTarget "$par"] + if {"$target" == ""} { + return -code error "Invalid value: \"$par\"" + } + set line ".$cmd$par" + debug_log "wrValue send: $line" + sct send "$line" + #sct print "$line" + if {$par != [sct oldval]} { + sct oldval $par + sct update $par + sct utime readtime + debug_log "wrValue new data for $tc_root [sct] result=$par" + } + return $nextState + } + + proc wrFunc {tc_root nextState cmd} { + debug_log "wrFunc root=$tc_root sct=[sct] cmd=$cmd target=[sct target]" + set par "[sct target]" + if {[hpropexists [sct] values]} { + set target {} + set values [split [SplitReply [hgetprop [sct] values]] ","] + foreach value $values { + #sct print "Testing $par against $value" + if {[string toupper "$par"] == [string toupper "$value"]} { + set target "$par" + break + } + } + if {"$target" == ""} { + return -code error "Invalid value: \"$par\"" + } + } + sct send "$cmd '$par';*IDN?" + debug_log "wrFunc send: $cmd '$par'" + if {$par != [sct oldval]} { + sct oldval $par + sct update $par + sct utime readtime + debug_log "wrFunc new data for $tc_root [sct] result=$par" + } + return $nextState + } + + proc wrNode {tc_root cmd level} { + set catch_status [ catch { + set space [string repeat " " $level] + set val "" + set catch_status [ catch { + set val [hval $tc_root] + } catch_message ] + if {"$val" == ""} { + set line "$tc_root ([hinfo $tc_root])" + } else { + set line "$tc_root ([hinfo $tc_root]) = $val" + } + sct print "$space* $line" + if {"[string tolower "$cmd"]" == "-prop"} { + set props [hlistprop $tc_root] + #sct print "<<$props>>" + foreach prop $props { + #sct print "prop: $prop" + set flds [split $prop "="] + #sct print "flds: $flds" + if {[llength $flds] > 1} { + set fld0 [lindex $flds 0] + #sct print "fld0: $fld0" + if {[hpropexists $tc_root $fld0]} { + sct print "$space - [hgetprop $tc_root $fld0]" + } + } + } + } + } catch_message ] + foreach node [hlist $tc_root] { + wrNode $tc_root/$node "$cmd" [expr {$level + 1}] + } + } + + proc wrTree {tc_root nextState cmd} { + debug_log "wrTree root=$tc_root sct=[sct] cmd=$cmd target=[sct target]" + sct print "$tc_root" + wrNode $tc_root "[sct target]" 1 + return idle + } + + 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 { $err_msg != "" } { + #sct print "error:$err_msg" + debug_log "error:$err_msg" + return -code error "$err_msg" + } + + set par "[sct target]" + if {[sct writestatus] == "start"} { + # Called by drive adapter + hset $tc_root/status "busy" + hsetprop $tc_root/setpoint driving 1 + } + 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" + hset $tc_root/drive_state "START" + } catch_message ] + if {$catch_status != 0} { + hsetprop $tc_root/setpoint driving 0 + return -code error $catch_message + } + #sct print "setPoint: [hget $tc_root/drive_state]" + return $nextState + } + + proc setPt {tc_root nextState cmd} { + debug_log "setPt: tc_root=$tc_root sct=[sct] target=[sct target]" + #sct print "setPt: tc_root=$tc_root sct=[sct] target=[sct target]" + set channel [string toupper [basename [pathname [sct]]]] + if { "$channel" == "A" } { + hset $tc_root/Display/set_pos_A [sct target] + sct update [sct target] + sct utime readtime + } elseif { "$channel" == "B" } { + hset $tc_root/Display/set_pos_B [sct target] + sct update [sct target] + sct utime readtime + } elseif { "$channel" == "C" } { + hset $tc_root/Display/set_pos_C [sct target] + sct update [sct target] + sct utime readtime + } else { + return -code error "Bad channel" + } + # TODO - drive + sct driving 1 + return "idle" + } + + proc currentState { tc_root } { + debug_log "currentState $tc_root" + set current_state 0 + set new_state "UNKNOWN" + set is_ready 1 + set catch_status [catch { + foreach v { mi_emergency vise_stop on_home_ok on_auto on_program on_cycleon } { + if { [hpropexists $tc_root/display/$v geterror] } { + debug_log "currentState: $v has geterror = \"[hgetprop $tc_root/display/$v geterror]\"" + set is_ready 0 + } + if { [hval $tc_root/display/$v] == 0 || [hval $tc_root/display/$v] == 1 } { + set $v [hval $tc_root/display/$v] + debug_log "currentState: $v has good value = \"[hval $tc_root/display/$v] [set $v]\"" + } else { + debug_log "currentState: $v has bad value = \"[hval $tc_root/display/$v]\"" + set is_ready 0 + } + } + } catch_message ] + if { $catch_status != 0 } { + debug_log "currentState: catch message = \"$catch_message\"" + return 0 + } + if { $is_ready == 0 } { + debug_log "currentState: not ready" + return 0 + } + set catch_status [catch { + if { $mi_emergency == 0 } { + # HARD_ESTOP + set new_state "HARD_ESTOP - Release hardware E-STOP" + set current_state 7 + } elseif { $vise_stop == 0 } { + # SOFT_ESTOP + set new_state "SOFT_ESTOP - Click Software E-STOP" + set current_state 8 + } elseif { $on_home_ok == 0 } { + if { $on_auto == 0 } { + # NEEDS_HOMING + set new_state "NEEDS_HOMING - Click START button" + if { $on_cycleon == 1 } { + set new_state "IS_HOMING - Please wait" + } + set current_state 2 + } else { + # INCONSISTENT_STATE + set new_state "INCONSISTENT_STATE - Set MANUAL console and press START" + set current_state 9 + } + } elseif { $on_auto == 0 } { + # MANUAL + set new_state "MANUAL_STATE - Set AUTO console" + set current_state 3 + } elseif { $on_program == 0 } { + if { $on_cycleon == 0 } { + # NOT_RUNNING + set new_state "NOT_RUNNING - Click START" + set current_state 4 + } else { + # NEEDS_RESET + set new_state "NEEDS_RESET - Dunno" + set current_state 5 + } + } else { + if { $on_cycleon == 0 } { + # READY_IDLE + set new_state "READY_IDLE - Ready to Run" + set current_state 1 + } else { + # RUNNING + set new_state "RUNNING - Busy moving, please wait" + set current_state 6 + } + } + } catch_message ] + if { $catch_status != 0 } { + debug_log "currentState: catch message1 = \"$catch_message\"" + } + debug_log "currentState: current state = $current_state ($new_state)" + return [list $current_state $new_state] + } + +# 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} { + set my_state "[SplitReply [hgetprop $tc_root/device_state my_state]]" + debug_log "getState $tc_root $nextState sct=[sct], my_state=$my_state" + if {[ catch { + if { [hpropexists [sct] geterror] } { + hdelprop [sct] geterror + } + # Prepare a request to read all of the STATE variables at once + set request "" + set comma "" + foreach v { mi_emergency vise_stop on_home_ok on_auto on_program on_cycleon opmode } { + set request "${request}${comma}.$v" + set comma "," + } + set request "$request" + if {$my_state == "STATE_INIT"} { + } elseif {$my_state == "STATE_IDLE"} { + } elseif {$my_state == "STATE_BUSY"} { + } else { + } + debug_log "getState end $tc_root state=$my_state" + } catch_message ]} { + debug_log "getState error: $catch_message" + } + debug_log "getState sends: \"$request\"" + sct send $request + 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 cybaman state and error messages. + proc rdState {tc_root} { + set my_state "[SplitReply [hgetprop $tc_root/device_state my_state]]" + debug_log "rdState $tc_root sct=[sct] state=$my_state response=\"[sct result]\"" + set nextState {} + if {[ catch { + if { [hpropexists [sct] geterror] } { + hdelprop [sct] geterror + } + 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 { + # parse XML and set state variables + if {[ catch { + set my_list [xml2list [sct result]] + debug_log "rdState my_list=$my_list" + set name [lindex $my_list 0] + set data [lindex $my_list 1] + if {[string equal -nocase $name "err"]} { + sct geterror "Error - report: $data" + return -code error "[sct geterror]" + } + foreach { name value } $my_list { + if { "[hval $tc_root/Display/$name]" != "$value" } { + debug_log "rdState: change $tc_root/Display/$name from [hval $tc_root/Display/$name] to $value" + hset $tc_root/Display/$name $value + } + } + } catch_message ]} { + debug_log "rdState $tc_root failure" + sct geterror "Error - catch: $catch_message" + return -code error "[sct geterror]" + } + # determine the current device state + set current_state [currentState $tc_root] + set catch_status [catch { + set new_state "[lindex $current_state 1]" + set current_state "[lindex $current_state 0]" + if { "[sct]" == "$tc_root/device_state" } { + if {"$new_state" != "[sct oldval]" } { + set old_state "[sct oldval]" + sct oldval "$new_state" + sct update "$new_state" + sct utime readtime + debug_log "rdState new state for $tc_root [sct] old=\"$old_state\" new=\"$new_state\"" + } + } + } catch_message ] + if { $catch_status != 0 } { + debug_log "rdState: catch message2 = \"$catch_message\"" + } + debug_log "rdState $tc_root state=$my_state, response=\"[sct result]\", current_state=$current_state" + # handle events based on the driver state + if {$my_state == "STATE_INIT"} { + if { "$current_state" == 1 } { + hsetprop $tc_root/device_state my_state "STATE_IDLE" + hset $tc_root/drive_state "IDLE" + hset $tc_root/display/set_pos_A [hval $tc_root/display/act_pos_A] + hset $tc_root/display/set_pos_B [hval $tc_root/display/act_pos_B] + hset $tc_root/display/set_pos_C [hval $tc_root/display/act_pos_C] + hsetprop $tc_root/A/Setpoint driving 0 + hsetprop $tc_root/B/Setpoint driving 0 + hsetprop $tc_root/C/Setpoint driving 0 + } + set nextState "idle" + } elseif {$my_state == "STATE_IDLE"} { + set nextState "idle" + if { "$current_state" == 1 } { + if { [hgetpropval $tc_root/A/Setpoint driving] || [hgetpropval $tc_root/B/Setpoint driving] || [hgetpropval $tc_root/C/Setpoint driving] } { + hset $tc_root/drive_state "DRIVING" + debug_log "drive_state: [sct] DRIVING" + hset $tc_root/Display/visstart 1 + hsetprop $tc_root/device_state my_state "STATE_BUSY" + set nextState "read" + } + } else { + hsetprop $tc_root/device_state my_state "STATE_INIT" + set nextState "read" + } + } elseif {$my_state == "STATE_BUSY"} { + if { "$current_state" == 6 } { + # still moving + } elseif { "$current_state" == 1 } { + if { [hval $tc_root/display/act_pos_A] == [hval $tc_root/display/set_pos_A] && + [hval $tc_root/display/act_pos_B] == [hval $tc_root/display/set_pos_B] && + [hval $tc_root/display/act_pos_C] == [hval $tc_root/display/set_pos_C] } { + hsetprop $tc_root/device_state my_state "STATE_IDLE" + hsetprop $tc_root/A/Setpoint driving 0 + hsetprop $tc_root/B/Setpoint driving 0 + hsetprop $tc_root/C/Setpoint driving 0 + debug_log "drive_state: [sct] IDLE" + hset $tc_root/drive_state "IDLE" + } + set nextState "idle" + } else { + hsetprop $tc_root/device_state my_state "STATE_INIT" + set nextState "read" + if { "$current_state" == 8 } { + if { [hval $tc_root/drive_state] == "HALT"} { + debug_log "device_state: [sct] HALT ESTOP" + #hset $tc_root/display/vise_stop 1 + } else { + debug_log "device_state: [sct] HARD E-STOP - HWFAULT" + set nextState "hwfault" + } + } elseif { "$current_state" == 7 } { + if { [hval $tc_root/drive_state] == "HALT"} { + debug_log "device_state: [sct] HALT START" + #hset $tc_root/display/visstart 1 + } else { + debug_log "device_state: [sct] SOFT E-STOP - HWFAULT" + set nextState "hwfault" + } + } + } + } + } + if { $nextState == "" } { + set nextState "idle" + } + } catch_message ]} { + debug_log "rdState error: $catch_message" + } + set old_state $my_state + set new_state "[SplitReply [hgetprop $tc_root/device_state my_state]]" + if {"$new_state" != "$old_state"} { + debug_log "rdState change $old_state to $new_state" + } + if { "$nextState" == "" } { + set nextState "idle" + } + debug_log "rdState returns: $nextState" + return $nextState +#TODO merge this lot in the right place + set old_state $my_state + set new_state "[SplitReply [hgetprop $tc_root/device_state my_state]]" + if {"$new_state" != "$old_state"} { + debug_log "getState change $old_state to $new_state" + } + } + + proc noResponse { tc_root } { + if { [hpropexists [sct] result] } { + debug_log "noResponse: sct=[sct], response=\"[sct result]\"" + #sct print "...response=\"[sct result]\"" + } else { + debug_log "noResponse: sct=[sct], response=None" + #sct print "...response=None" + } + return idle + } + proc wrtCheck {wcmd args} { + debug_log "wrtCheck: sct=[sct], response=[sct result]" + return idle + } + +# check that a target is within allowable limits + proc checklimits {tc_root} { + set catch_status [ catch { + set target [chkTarget [sct target]] + if { "$target" == "" } { + error "setpoint violates limits" + } + } catch_message ] + if {$catch_status != 0} { + return -code error $catch_message + } + return OK + } + +# Check that the sensor is reading within tolerance of the setpoint. +# Return 1 or 0 if it is or is not, respectively. + proc checktol {tc_root currtime timecheck} { +debug_log "checktol $tc_root $currtime $timecheck" + return 1 + } + +## +# @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} { + debug_log "drivestatus: enter sct=[sct], tc_root=$tc_root" + set catch_status [ catch { + set current_state "[currentState $tc_root]" + set new_state "[lindex $current_state 1]" + set current_state "[lindex $current_state 0]" + debug_log "drivestatus: current_state = $current_state {$new_state}" + } catch_message ] + if {$catch_status != 0} { + debug_log "drivestatus: current_state = UNKNOWN ERROR" + set current_state 0 + } + if { "$current_state" == "8" } { + debug_log "drivestatus: [sct] state = $current_state (HARD ESTOP = hwfault)" + return "hwfault" + } + if { "$current_state" == "7" } { + debug_log "drivestatus: [sct] state = $current_state (SOFT ESTOP = hwfault)" + return "hwfault" + } + if {[sct driving]} { + return "busy" + } else { + debug_log "drivestatus: [sct] idle" + return "idle" + } + } + + proc halt {tc_root} { +debug_log "halt $tc_root" +sct print "halt $tc_root" + hset $tc_root/drive_state "HALT" + hset $tc_root/display/vise_stop 0 + 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 wrValue() +# @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 ::scobj::cybaman + 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} { + # the node is readable so set it up to be polled using the rdFunc + # rdFunc is getValueFunc.rdValueFunc with both explicit functions + # or rdValueFunc where "getValue" is the implied getValueFunc + set parts [split "$rdFunc" "."] + if { [llength $parts] == 2 } { + set func_name [lindex $parts 0] + set next_state [lindex $parts 1] + } else { + set func_name "getValue" + set next_state [lindex $parts 0] + } + hsetprop $nodeName read ${ns}::$func_name $scobj_hpath $next_state $rdCmd + hsetprop $nodeName $next_state ${ns}::$next_state $scobj_hpath + # set the poll rate as a period in seconds + # TODO allow directly settable value in seconds + set poll_period 5 + if { $readable >= 0 && $readable <= 300 } { + set poll_period [expr {int($readable)}] + } + debug_log 1 "Registering node $nodeName for poll at $poll_period seconds" + $sct_controller poll $nodeName $poll_period + } + if {$writable == 1} { + # the node is writable so set it up to invoke a callback when written + # rdFunc is putValueFunc.chkWriteFunc with both explicit functions + # or putValueFunc where "noResponse" is the implied chkWriteFunc + set parts [split "$wrFunc" "."] + if { [llength $parts] == 2 } { + set func_name [lindex $parts 0] + set next_state [lindex $parts 1] + } else { + set func_name [lindex $parts 0] + set next_state "noResponse" + } + hsetprop $nodeName write ${ns}::$func_name $scobj_hpath $next_state $wrCmd + hsetprop $nodeName $next_state ${ns}::$next_state $scobj_hpath + hsetprop $nodeName writestatus UNKNOWN + debug_log 1 "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 driving 0 + hsetprop $nodeName checklimits ${ns}::checklimits $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 createDriveable { name axis scobj_hpath getNodeName setNodeName chkLimits chkStatus doHalt sct_controller } { + set ns "[namespace current]" + hsetprop $scobj_hpath/$setNodeName checklimits ${ns}::$chkLimits $scobj_hpath + hsetprop $scobj_hpath/$setNodeName checkstatus ${ns}::$chkStatus $scobj_hpath + hsetprop $scobj_hpath/$setNodeName halt ${ns}::$doHalt $scobj_hpath + ::scobj::hinitprops $name/$axis [basename $setNodeName] + if {[SplitReply [environment_simulation]]=="false"} { + ansto_makesctdrive ${name}_${axis}_driveable $scobj_hpath/$setNodeName $scobj_hpath/$getNodeName $sct_controller + } + } + + proc mk_sct_cybaman {sct_controller klass tempobj tol} { + set catch_status [ catch { +# set ns ::scobj::cybaman + set ns "[namespace current]" + + MakeSICSObj $tempobj SCT_OBJECT + sicslist setatt $tempobj klass $klass + sicslist setatt $tempobj long_name $tempobj + + set scobj_hpath /sics/$tempobj + # Group Node R W D type perm rdCmd rdFunc wrCmd wrFunc allowed + set deviceCommand {\ + Display mi_emergency 0 1 0 int user {mi_emergency} {rdValue} {mi_emergency} {wrValue} {}\ + Display on_home_ok 0 0 0 int user {on_home_ok} {rdValue} {} {} {}\ + Display on_auto 0 1 0 int user {on_auto} {rdValue} {on_auto} {wrValue} {}\ + Display on_program 0 0 0 int user {on_program} {rdValue} {} {} {}\ + Display on_cycleon 0 0 0 int user {on_cycleon} {rdValue} {} {} {}\ + Display act_pos_C 1 0 0 float user {act_pos_1} {rdAngle} {} {} {}\ + Display act_pos_B 1 0 0 float user {act_pos_2} {rdAngle} {} {} {}\ + Display act_pos_A 1 0 0 float user {act_pos_3} {rdAngle} {} {} {}\ + Display opmode 0 0 0 int user {opmode} {rdValue} {} {} {}\ + Display prognumber 1 0 0 int user {mainProgNumber} {rdValue} {} {} {}\ + Display reset 1 1 0 int user {visin_ctrlR} {rdValue} {visin_ctrlR} {wrValue} {}\ + Display set_pos_A 1 1 0 float user {p101} {rdAngle} {p101} {wrAngle} {0:360}\ + Display set_pos_B 1 1 0 float user {p102} {rdAngle} {p102} {wrAngle} {-180:180}\ + Display set_pos_C 1 1 0 float user {p103} {rdAngle} {p103} {wrAngle} {-180:180}\ + Display feed_rate 1 1 0 int user {p104} {rdValue} {p104} {wrValue} {0:99999}\ + Display dwell_time 1 1 0 int user {p105} {rdValue} {p105} {wrValue} {}\ + Display visauto 1 1 0 int user {visauto} {rdValue} {visauto} {wrValue} {}\ + Display visstart 1 1 0 int user {visstart} {rdValue} {visstart} {wrValue} {}\ + Display visstop 1 1 0 int user {visstop} {rdValue} {visstop} {wrValue} {}\ + Display vise_stop 0 1 0 int user {vise_stop} {rdValue} {vise_stop} {wrValue} {}\ + Display Value 1 0 0 int internal {mainProgNumber} {rdValue} {} {} {}\ + {} Setpoint 0 1 1 int user {} {} {} {setPoint} {}\ + A Setpoint 0 1 1 float user {} {} {} {setPt} {-180:180}\ + A Value 1 0 0 float user {} {rdV.rdN} {} {} {}\ + B Setpoint 0 1 1 float user {} {} {} {setPt} {-180:180}\ + B Value 1 0 0 float user {} {rdV.rdN} {} {} {}\ + C Setpoint 0 1 1 float user {} {} {} {setPt} {-180:180}\ + C Value 1 0 0 float user {} {rdV.rdN} {} {} {}\ + Display Tree 0 1 0 text user {} {} {} {wrTree} {}\ + } + + hfactory $scobj_hpath/Control plain spy none + hfactory $scobj_hpath/Display plain spy none + hfactory $scobj_hpath/A plain spy none + hfactory $scobj_hpath/B plain spy none + hfactory $scobj_hpath/C 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 + } + + hfactory $scobj_hpath/status plain spy text + hset $scobj_hpath/status "idle" + hsetprop $scobj_hpath/status values busy,idle + + hfactory $scobj_hpath/device_state plain spy text + hsetprop $scobj_hpath/device_state read ${ns}::getState $scobj_hpath rdState "X" + hsetprop $scobj_hpath/device_state rdState ${ns}::rdState $scobj_hpath + hsetprop $scobj_hpath/device_state oldval "UNKNOWN" + hsetprop $scobj_hpath/device_state my_state "STATE_INIT" + hsetprop $scobj_hpath/device_state my_substate 0 + hsetprop $scobj_hpath/device_state my_status "UNKNOWN" + hsetprop $scobj_hpath/device_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/device_lasterror plain user text + hset $scobj_hpath/device_lasterror "" + + hsetprop $scobj_hpath/setpoint tolerance $tol + + if {[SplitReply [environment_simulation]]=="false"} { + $sct_controller poll $scobj_hpath/device_state 0.5 halt read + } + + hsetprop $scobj_hpath type part + foreach snsr {Display} { + 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 mm1_${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/display/value $sct_controller + } + createDriveable ${tempobj} A $scobj_hpath A/Value A/Setpoint checklimits drivestatus halt $sct_controller + createDriveable ${tempobj} B $scobj_hpath B/Value B/Setpoint checklimits drivestatus halt $sct_controller + createDriveable ${tempobj} C $scobj_hpath C/Value C/Setpoint checklimits drivestatus halt $sct_controller + + } catch_message ] + if {$catch_status != 0} { + return -code error $catch_message + } + } + namespace export mk_sct_cybaman +} + +## +# @brief Create a Cybaman +# +# @param name, the name of the goniometer (eg gon1) +# @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_cybaman {name IP port terminator {_tol 1.0}} { + set fd [open "/tmp/cybaman.log" a] + if {[SplitReply [environment_simulation]]=="false"} { + puts $fd "makesctcontroller sct_cybaman std ${IP}:$port" + makesctcontroller sct_cybaman std ${IP}:$port $terminator + } + puts $fd "mk_sct_cybaman sct_cybaman environment $name $_tol" + mk_sct_cybaman sct_cybaman environment $name $_tol + close $fd +} + +puts stdout "file evaluation of sct_cybaman.tcl" +set fd [open "/tmp/cybaman.log" w] +puts $fd "file evaluation of sct_cybaman.tcl" +close $fd + +namespace import ::scobj::cybaman::*