529 lines
19 KiB
Tcl
Executable File
529 lines
19 KiB
Tcl
Executable File
# 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::mvp {
|
|
# Environment controllers should have at least the following nodes
|
|
# /envcont/setpoint
|
|
# /envcont/sensor/value
|
|
proc debug_log {args} {
|
|
set fd [open "../log/mvp.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"
|
|
set dev [SplitReply [hgetprop $tc_root device]]
|
|
sct send "$dev$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 setDirn {tc_root nextState cmd} {
|
|
debug_log "setDirn cmd=$cmd [sct target] sct=[sct] $tc_root"
|
|
set data [string tolower [sct target]]
|
|
if {"$data" != "[sct oldval]"} {
|
|
sct oldval "$data"
|
|
sct update "$data"
|
|
sct utime readtime
|
|
}
|
|
debug_log "setDirn $cmd$data"
|
|
return $nextState
|
|
}
|
|
|
|
proc setPosn {tc_root nextState cmd} {
|
|
debug_log "setPosn cmd=$cmd [sct target] sct=[sct] $tc_root"
|
|
set data [string tolower [sct target]]
|
|
if {$data >= "1" || $data <= "6"} {
|
|
if {"$data" != "[sct oldval]"} {
|
|
sct oldval "$data"
|
|
sct update "$data"
|
|
sct utime readtime
|
|
}
|
|
}
|
|
debug_log "setPosn $data"
|
|
return $nextState
|
|
}
|
|
|
|
proc setCmnd {tc_root nextState cmd} {
|
|
debug_log "setCmnd cmd=$cmd [sct target] sct=[sct] $tc_root"
|
|
set dev [SplitReply [hgetprop $tc_root device]]
|
|
if {[string range [sct target] 0 1] == "1"} {
|
|
set data [sct target]
|
|
} else {
|
|
set data "$dev[string toupper [sct target]]"
|
|
}
|
|
sct send "$data"
|
|
debug_log "setCmnd $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 "\025"]} {
|
|
sct geterror "Error NAK: $data"
|
|
} else {
|
|
if {[string equal -nocase -length 1 $data "\006"]} {
|
|
set data [string range $data 1 end]
|
|
}
|
|
set cmnd [string toupper [sct target]]
|
|
debug_log "Command: \"$cmnd\" Response: \"$data\""
|
|
set data "$cmnd -> $data"
|
|
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]"
|
|
set err_msg ""
|
|
set data [expr {[sct target]}]
|
|
if { $data < 1 || $data > 6 } {
|
|
set err_msg "Invalid Position: $data"
|
|
}
|
|
if { $err_msg != "" } {
|
|
sct print "error:$err_msg"
|
|
debug_log "error:$err_msg"
|
|
return -code error "$err_msg"
|
|
}
|
|
if { [hval $tc_root/Control/Direction] == "cw" } {
|
|
set par "0"
|
|
} elseif { [hval $tc_root/Control/Direction] == "ccw" } {
|
|
set par "1"
|
|
} else {
|
|
#=IF(B2-A2<-2,6+B2-A2,IF(B2-A2<0,B2-A2,IF(B2-A2>3,B2-A2-6,B2-A2)))
|
|
if {"[sct oldval]" == "UNKNOWN"} {
|
|
sct oldval 1
|
|
}
|
|
if {($data - [sct oldval]) < -2} {
|
|
set par "0"
|
|
} elseif {($data - [sct oldval]) < 0} {
|
|
set par "1"
|
|
} elseif {($data - [sct oldval]) > 3} {
|
|
set par "1"
|
|
} else {
|
|
set par "0"
|
|
}
|
|
}
|
|
if {$data != [sct oldval]} {
|
|
sct oldval $data
|
|
sct update $data
|
|
sct utime readtime
|
|
debug_log "setPoint new data for $tc_root [sct] data=$data"
|
|
}
|
|
set dev [SplitReply [hgetprop $tc_root device]]
|
|
debug_log "setPoint send: ${dev}LP${par}${data}R"
|
|
sct send "${dev}LP${par}${data}R"
|
|
debug_log "status: busy"
|
|
hsetprop $tc_root/control/setpoint driving 1
|
|
} catch_message ]
|
|
if {$catch_status != 0} {
|
|
debug_log "error: $catch_message"
|
|
return -code error $catch_message
|
|
}
|
|
return $nextState
|
|
}
|
|
|
|
proc rdValue {tc_root} {
|
|
debug_log "rdValue tc_root=$tc_root sct=[sct] [lindex [split [sct] "/"] end]"
|
|
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 "\025"]} {
|
|
sct geterror "Error NAK: $data"
|
|
set nextState idle
|
|
} else {
|
|
if { [hpropexists [sct] geterror] } {
|
|
hdelprop [sct] geterror
|
|
}
|
|
if {[string equal -nocase -length 1 $data "\006"]} {
|
|
set data [string range $data 1 end]
|
|
}
|
|
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
|
|
}
|
|
if {[lindex [split [sct] "/"] end] == "Finished" && $data == "Y"} {
|
|
if {[SplitReply [hgetprop $tc_root/control/setpoint driving]] == 1} {
|
|
hsetprop $tc_root/control/setpoint driving 0
|
|
debug_log "status: idle"
|
|
}
|
|
}
|
|
}
|
|
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} {
|
|
debug_log "getState $tc_root $nextState sct=[sct]"
|
|
if {[ catch {
|
|
if { [hpropexists [sct] geterror] } {
|
|
hdelprop [sct] geterror
|
|
}
|
|
set my_state [sct my_state]
|
|
set my_substate [sct my_substate]
|
|
if {$my_state == "STATE_INIT"} {
|
|
if {$my_substate == 0} {
|
|
set my_cmd "1a"
|
|
} elseif {$my_substate == 1} {
|
|
set my_cmd "U"
|
|
} elseif {$my_substate == 2} {
|
|
set my_cmd "LXR"
|
|
} elseif {$my_substate == 3} {
|
|
set my_cmd "F"
|
|
} elseif {$my_substate == 4} {
|
|
set my_cmd "LP0[hval $tc_root/Control/Setpoint]R"
|
|
}
|
|
} elseif {$my_state == "STATE_IDLE"} {
|
|
set my_cmd "LQP"
|
|
} elseif {$my_state == "STATE_BUSY"} {
|
|
set my_cmd "F"
|
|
} elseif {$my_state == "STATE_ERROR"} {
|
|
set my_cmd "E2"
|
|
}
|
|
if {![string equal "$my_cmd" "1a"]} {
|
|
set dev [SplitReply [hgetprop $tc_root device]]
|
|
set my_cmd "${dev}${my_cmd}"
|
|
}
|
|
debug_log "getState sends: $my_cmd"
|
|
sct send "$my_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 MVP 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"
|
|
sct my_state "STATE_INIT"
|
|
sct my_substate 0
|
|
set nextState idle
|
|
} elseif {[string equal -nocase -length 1 $data "\025"]} {
|
|
sct geterror "Error NAK: $data"
|
|
set nextState idle
|
|
} else {
|
|
if {[string equal -nocase -length 1 $data "\006"]} {
|
|
set data [string range $data 1 end]
|
|
}
|
|
set my_state [sct my_state]
|
|
set my_substate [sct my_substate]
|
|
if { $my_state == "STATE_INIT" } {
|
|
if { $my_substate == 0 } {
|
|
if {[string length $data] == 0} {
|
|
sct my_substate 1
|
|
} elseif { [string equal -length 2 $data "1b"] } {
|
|
sct my_substate 1
|
|
}
|
|
} elseif { $my_substate == 1 } {
|
|
if { [string equal -length 2 $data "MV"] } {
|
|
sct my_substate 2
|
|
}
|
|
} elseif { $my_substate == 2 } {
|
|
sct my_substate 3
|
|
} elseif { $my_substate == 3 } {
|
|
if { [string equal -length 1 $data "Y"] } {
|
|
sct my_state "STATE_IDLE"
|
|
sct my_substate 0
|
|
if {[hval $tc_root/Control/Setpoint] != "UNKNOWN"} {
|
|
if {[hval $tc_root/Control/Setpoint] != [hval $tc_root/Display/Position]} {
|
|
if {[hval $tc_root/Control/Setpoint] >= 1 && [hval $tc_root/Control/Setpoint] <= 6} {
|
|
sct my_state "STATE_INIT"
|
|
sct my_substate 4
|
|
}
|
|
}
|
|
}
|
|
}
|
|
} elseif { $my_substate == 4 } {
|
|
sct my_substate 3
|
|
}
|
|
} elseif { $my_state == "STATE_IDLE" } {
|
|
if { "[SplitReply [hgetprop $tc_root/Control/Setpoint driving]]" } {
|
|
sct my_state "STATE_BUSY"
|
|
sct my_substate 0
|
|
}
|
|
} elseif { $my_state == "STATE_BUSY" } {
|
|
if {[string equal -nocase -length 1 $data "Y"]} {
|
|
hsetprop $tc_root/Control/Setpoint driving 0
|
|
sct my_state "STATE_IDLE"
|
|
sct my_substate 0
|
|
}
|
|
} elseif { $my_state == "STATE_ERROR" } {
|
|
sct my_state "STATE_IDLE"
|
|
sct my_substate 0
|
|
}
|
|
}
|
|
} catch_message ]} {
|
|
debug_log "rdState error: $catch_message"
|
|
}
|
|
if { "$nextState" == "" } {
|
|
set nextState "idle"
|
|
}
|
|
if {![string equal [sct oldval] [sct my_state]]} {
|
|
sct oldval [sct my_state]
|
|
sct update [sct my_state]
|
|
sct utime readtime
|
|
}
|
|
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 1
|
|
set hilimit 6
|
|
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 {
|
|
return idle
|
|
}
|
|
}
|
|
|
|
proc halt {tc_root} {
|
|
debug_log "halt $tc_root"
|
|
sct print "halt $tc_root"
|
|
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 nexus gumtree\
|
|
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
|
|
}
|
|
if {$nexus == 1} {
|
|
hsetprop $nodeName type part
|
|
hsetprop $nodeName klass $klass
|
|
hsetprop $nodeName privilege $permission
|
|
hsetprop $nodeName control true
|
|
hsetprop $nodeName data true
|
|
hsetprop $nodeName nxsave true
|
|
hsetprop $nodeName data true
|
|
}
|
|
if {$gumtree == 1} {
|
|
}
|
|
} catch_message ]
|
|
if {$catch_status != 0} {
|
|
debug_log "error: $catch_message"
|
|
return -code error "in createNode $catch_message"
|
|
}
|
|
return OK
|
|
}
|
|
|
|
proc mk_sct_mvp {sct_controller klass tempobj dev} {
|
|
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
|
|
hsetprop $scobj_hpath device "$dev"
|
|
|
|
set deviceCommand {\
|
|
Display Position 1 0 1 1 0 text internal {LQP} {rdValue} {} {} {}\
|
|
Display Angle 1 0 1 1 0 text internal {LQA} {rdValue} {} {} {}\
|
|
Display Hertz 1 0 1 1 0 text internal {LQF} {rdValue} {} {} {}\
|
|
Display Finished 1 0 1 1 0 text internal {F} {rdValue} {} {} {}\
|
|
Display Overload 1 0 1 1 0 text internal {G} {rdValue} {} {} {}\
|
|
Display Status 1 0 1 1 0 text internal {E1} {rdValue} {} {} {}\
|
|
Display Error 1 0 1 1 0 text internal {E2} {rdValue} {} {} {}\
|
|
Display Device 1 0 1 1 0 text internal {E3} {rdValue} {} {} {}\
|
|
Display Encoder 1 0 1 1 0 text internal {E4} {rdValue} {} {} {}\
|
|
Control Direction 0 1 1 1 0 text user {} {} {} {setDirn} {}\
|
|
Control SetPoint 0 1 1 1 1 text user {} {} {} {setPoint} {}\
|
|
Control Command 0 1 1 1 0 text user {} {} {} {setCmnd.chkWrite} {}\
|
|
}
|
|
|
|
hfactory $scobj_hpath/Display plain spy none
|
|
hfactory $scobj_hpath/Control plain spy none
|
|
|
|
foreach {cmdGroup varName readable writable nexus gumtree drivable dataType permission rdCmd rdFunc wrCmd wrFunc allowedValues} $deviceCommand {
|
|
createNode $scobj_hpath $sct_controller $cmdGroup $varName $readable $writable $nexus $gumtree $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
|
|
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"
|
|
|
|
# if {[SplitReply [environment_simulation]]=="false"} {
|
|
# }
|
|
debug_log "Registering node $scobj_hpath/device_state for poll at 1 seconds"
|
|
$sct_controller poll $scobj_hpath/device_state 1 halt read
|
|
ansto_makesctdrive ${tempobj}_driveable $scobj_hpath/control/setpoint $scobj_hpath/display/position $sct_controller
|
|
|
|
|
|
} catch_message ]
|
|
if {$catch_status != 0} {
|
|
return -code error $catch_message
|
|
}
|
|
}
|
|
namespace export mk_sct_mvp
|
|
}
|
|
|
|
##
|
|
# @brief Create an MVP Controller
|
|
#
|
|
# @param name, the name of the MVP
|
|
# @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_mvp {name IP port dev} {
|
|
set fd [open "../log/mvp.log" a]
|
|
if {[SplitReply [environment_simulation]]=="false"} {
|
|
makesctcontroller sct_mvp hamilton ${IP}:$port "\r"
|
|
}
|
|
puts $fd "::scobj::mvp::mk_sct_mvp sct_mvp environment $name"
|
|
::scobj::mvp::mk_sct_mvp sct_mvp environment $name $dev
|
|
close $fd
|
|
}
|
|
|
|
clientput "file evaluation of sct_mvp.tcl"
|
|
set fd [open "../log/mvp.log" w]
|
|
puts $fd "file evaluation of sct_mvp.tcl"
|
|
close $fd
|
|
|
|
namespace import ::scobj::mvp::*
|
|
|
|
#add_mvp mvp 192.168.56.101 6000
|