489 lines
15 KiB
Tcl
489 lines
15 KiB
Tcl
#--------------------------------------------------------
|
|
# This is a scriptcontext based driver for the EL734
|
|
# motor controller. This is part of an ongoing effort to
|
|
# expire older drivers and to consolidate on the new
|
|
# scriptcontext system.
|
|
#
|
|
# Scriptchains:
|
|
# Rather then having long script chains many of the
|
|
# intricacies of the EL734 are handled via a command
|
|
# processing state machine. See the docs below for
|
|
# details
|
|
#
|
|
# copyright: see file COPYRIGHT
|
|
#
|
|
# Mark Koennecke, February 2011
|
|
#--------------------------------------------------------
|
|
|
|
namespace eval el734 {}
|
|
|
|
#---------------------------------------------------------
|
|
# The EL734 is a a tricky thing. Some special conditions
|
|
# apply:
|
|
# - On emergency stop an *ES is sent. But only the second
|
|
# response of this kind is valid because there can be
|
|
# spurious *ES on the line even when the emergency stop
|
|
# has been released.
|
|
# - If someone fingers the EL734 or after startup it is in
|
|
# local mode. Then two commands have to be sent in order to
|
|
# make it go into remote mode before retrying the command.
|
|
# - In some echo modes of the controller it sends a echo
|
|
# of the command. This has to be ignored in order to get at
|
|
# the real problem
|
|
#
|
|
# In order to deal with all this, el734::command is implemented
|
|
# as a state machine which calls another script when a valid
|
|
# reponse has actually been found.
|
|
# The state of the current command processing
|
|
# is saved in a node property comstate. The actual command to send
|
|
# is in the property comstring. The script to call if we actually
|
|
# have a valid response is stored in the property comresponse
|
|
#---------------------------------------------------------------
|
|
proc el734::setcommand {command responsescript {motno 0}} {
|
|
sct comresponse $responsescript
|
|
sct comstate start
|
|
sct comstring $command
|
|
sct commotno $motno
|
|
return command
|
|
}
|
|
#---------------------------------------------------------------
|
|
# As implemented now this can go in an endless loop if switching
|
|
# to local fails repeatedly. TODO: test if this happens with the
|
|
# real device
|
|
#---------------------------------------------------------------
|
|
proc el734::command {} {
|
|
set state [sct comstate]
|
|
switch $state {
|
|
start {
|
|
set com [sct comstring]
|
|
sct send $com
|
|
sct comstate waitresponse
|
|
}
|
|
waitstart {
|
|
wait 1
|
|
sct comstate start
|
|
return [el734::command]
|
|
}
|
|
waitresponse {
|
|
set reply [sct result]
|
|
if {[string first "*ES" $reply] >= 0} {
|
|
set com [sct comstring]
|
|
sct send $com
|
|
sct comstate waitES
|
|
return command
|
|
}
|
|
if {[string first "?LOC" $reply] >= 0} {
|
|
sct send "RMT 1"
|
|
sct comstate waitrmt
|
|
return command
|
|
}
|
|
if {[string first "?BSY" $reply] >= 0} {
|
|
set mot [sct commotno]
|
|
if {$mot != 0} {
|
|
set com [format "S %d" $mot]
|
|
} else {
|
|
set com "S"
|
|
}
|
|
sct send $com
|
|
sct comstate waitstart
|
|
return command
|
|
}
|
|
set com [sct comstring]
|
|
set idx [string first $com $reply]
|
|
if {[string first $com $reply] >= 0} {
|
|
sct send @@NOSEND@@
|
|
sct comstate waitresponse
|
|
return command
|
|
}
|
|
set responsescript [sct comresponse]
|
|
return [eval $responsescript]
|
|
}
|
|
waitES {
|
|
set reply [sct result]
|
|
if {[string first "*ES" $reply] >= 0} {
|
|
clientput "Emergency STOP ENGAGED, release to continue"
|
|
error "Emergency Stop ENGAGED"
|
|
}
|
|
set responsescript [sct comresponse]
|
|
return [eval $responsescript]
|
|
}
|
|
waitrmt {
|
|
sct send "ECHO 0"
|
|
sct comstate start
|
|
}
|
|
}
|
|
return command
|
|
}
|
|
#-------------------------------------------------------------------
|
|
proc el734::checkerror {} {
|
|
set err(?ADR) "Bad address"
|
|
set err(?CMD) "Bad command"
|
|
set err(?PAR) "Bad parameter"
|
|
set err(?RNG) "Parameter out of range"
|
|
set err(?BSY) "Motor busy"
|
|
set err(*MS) "Bad step"
|
|
set err(*ES) "Emergency stop engaged"
|
|
|
|
set reply [string trim [sct result]]
|
|
set errlist [array names err]
|
|
foreach entry $errlist {
|
|
if {[string first $entry $reply] >= 0} {
|
|
error $err($entry)
|
|
}
|
|
}
|
|
return $reply
|
|
}
|
|
#========================== Position ===============================
|
|
proc el734::readpos {num} {
|
|
set com [format "u %d" $num]
|
|
return [el734::setcommand $com el734::posresponse]
|
|
}
|
|
#-------------------------------------------------------------------
|
|
proc el734::posresponse {} {
|
|
set stat [catch {checkerror} reply]
|
|
if {$stat == 0} {
|
|
sct update $reply
|
|
return idle
|
|
} else {
|
|
clientput $reply
|
|
return idle
|
|
}
|
|
}
|
|
#-------------------------------------------------------------------
|
|
proc el734::setpos {name num} {
|
|
set newpos [sct target]
|
|
set com [format "p %d %f" $num $newpos]
|
|
hupdate /sics/${name}/status run
|
|
hupdate /sics/${name}/oredmsr 3
|
|
hupdate /sics/${name}/runfault 0
|
|
hupdate /sics/${name}/posfault 0
|
|
return [el734::setcommand $com "el734::setposresponse $name"]
|
|
}
|
|
#-------------------------------------------------------------------
|
|
proc el734::setposresponse {name} {
|
|
set stat [catch {checkerror} reply]
|
|
if {$stat == 0} {
|
|
[sct controller] queue /sics/${name}/status progress read
|
|
return idle
|
|
} else {
|
|
clientput $reply
|
|
return idle
|
|
}
|
|
}
|
|
#===================== Limits =====================================
|
|
proc el734::getlim {name num} {
|
|
set com [format "H %d" $num]
|
|
return [el734::setcommand $com "el734::limresponse $name"]
|
|
}
|
|
#-----------------------------------------------------------------
|
|
proc el734::limresponse {name} {
|
|
set stat [catch {checkerror} reply]
|
|
if {$stat == 0} {
|
|
stscan $reply "%f %f" low high
|
|
hupdate /sics/${name}/hardlowerlim $low
|
|
hupdate /sics/${name}/hardupperlim $high
|
|
return idle
|
|
} else {
|
|
clientput $reply
|
|
return idle
|
|
}
|
|
}
|
|
#------------------------------------------------------------------
|
|
proc el734::setlim {controller name num low high} {
|
|
set com [format "H %d %f %f" $num $low $high]
|
|
$controller send $com
|
|
$controller queue /sics/${name}/hardlowerlim progress read
|
|
wait 1
|
|
return Done
|
|
}
|
|
#======================== status ================================
|
|
proc el734::decodemsr {name msr} {
|
|
set oredata(0x02) idle:none
|
|
set oredata(0x10) error:lowlim
|
|
set oredata(0x20) error:hilim
|
|
set oredata(0x80) posfault:runfault
|
|
set oredata(0x200) posfault:posfault
|
|
set oredata(0x1000) "error:air cushion"
|
|
set oredata(0x40) "error:bad step"
|
|
set oredata(0x100) error:positionfault
|
|
set oredata(0x400) error:positionfault
|
|
|
|
set msrdata(0x20) hilim
|
|
set msrdata(0x10) lowlim
|
|
set msrdata(0x1000) "air cushion"
|
|
set msrdata(0x40) "Bad step"
|
|
set msrdata(0x100) posfault
|
|
set msrdata(0x400) posfault
|
|
|
|
set oredmsr [hval /sics/${name}/oredmsr]
|
|
if {$msr == 0} {
|
|
#-------- FINISHED
|
|
set pos [hval /sics/${name}/posfault]
|
|
set run [hval /sics/${name}/runfault]
|
|
if {$pos > 0 || $run > 0} {
|
|
return posfault
|
|
}
|
|
|
|
set orlist [array names oredata]
|
|
foreach code $orlist {
|
|
if {$oredmsr & $code} {
|
|
set l [split $oredata($code) :]
|
|
set txt [lindex $l 1]
|
|
set ret [lindex $l 0]
|
|
hupdate /sics/${name}/lasterror $txt
|
|
if {[string compare $ret error] == 0} {
|
|
clientput "ERROR: $txt on motor $name"
|
|
}
|
|
return $ret
|
|
}
|
|
}
|
|
if {$oredmsr == 0} {
|
|
return idle
|
|
}
|
|
} else {
|
|
#------------ Still Running.........
|
|
set msrlist [array names msrdata]
|
|
foreach code $msrlist {
|
|
if {$msr & $code} {
|
|
clientput "ERROR: $msrdata($code) on motor $name"
|
|
return error
|
|
}
|
|
}
|
|
if {$msr & 0x80} {
|
|
set val [hval /sics/${name}/runfault]
|
|
incr val
|
|
hupdate /sics/${name}/runfault $val
|
|
}
|
|
if {$msr & 0x200} {
|
|
set val [hval /sics/${name}/posfault]
|
|
incr val
|
|
hupdate /sics/${name}/posfault $val
|
|
}
|
|
|
|
hupdate /sics/${name}/oredmsr [expr $oredmsr | $msr]
|
|
return run
|
|
}
|
|
}
|
|
#----------------------------------------------------------------
|
|
proc el734::readstatus {num name} {
|
|
set com [format "msr %d" $num]
|
|
return [el734::setcommand $com "el734::statresponse $name $num"]
|
|
}
|
|
#----------------------------------------------------------------
|
|
proc el734::statresponse {name num} {
|
|
set stat [catch {checkerror} reply]
|
|
if {$stat == 0} {
|
|
stscan $reply "%d" msr
|
|
set status [el734::decodemsr $name $msr]
|
|
sct update $status
|
|
switch $status {
|
|
run {
|
|
set con [sct controller]
|
|
$con queue /sics/${name}/hardposition progress read
|
|
$con queue /sics/${name}/status progress read
|
|
}
|
|
idle {
|
|
set com [format "u %d" $num]
|
|
return [el734::setcommand $com "el734::posstat $name" ]
|
|
}
|
|
}
|
|
return idle
|
|
} else {
|
|
clientput $reply
|
|
return idle
|
|
}
|
|
}
|
|
#----------------------------------------------------------------
|
|
proc el734::posstat {name} {
|
|
set stat [catch {checkerror} reply]
|
|
if {$stat == 0} {
|
|
hupdate /sics/${name}/hardposition $reply
|
|
return idle
|
|
} else {
|
|
clientput $reply
|
|
return idle
|
|
}
|
|
}
|
|
#========================== Halt =================================
|
|
proc el734::halt {controller no} {
|
|
set com [format "S %d" $no]
|
|
$controller send $com
|
|
return Done
|
|
}
|
|
#========================= Speed ================================
|
|
proc el734::readspeed {num} {
|
|
set com [format "J %d" $num]
|
|
return [el734::setcommand $com el734::speedresponse]
|
|
}
|
|
#-------------------------------------------------------------------
|
|
proc el734::speedresponse {} {
|
|
set stat [catch {checkerror} reply]
|
|
if {$stat == 0} {
|
|
sct update $reply
|
|
return idle
|
|
} else {
|
|
clientput $reply
|
|
return idle
|
|
}
|
|
}
|
|
#-------------------------------------------------------------------
|
|
proc el734::setspeed {name num} {
|
|
set newpos [sct target]
|
|
set com [format "J %d %d" $num $newpos]
|
|
return [el734::setcommand $com "el734::setspeedresponse $name $num"]
|
|
}
|
|
#-------------------------------------------------------------------
|
|
proc el734::setspeedresponse {name num} {
|
|
set stat [catch {checkerror} reply]
|
|
if {$stat == 0} {
|
|
return [el734::readspeed $num]
|
|
} else {
|
|
clientput $reply
|
|
return idle
|
|
}
|
|
}
|
|
#========================= refnull ================================
|
|
proc el734::readref {num} {
|
|
set com [format "V %d" $num]
|
|
return [el734::setcommand $com el734::refresponse]
|
|
}
|
|
#-------------------------------------------------------------------
|
|
proc el734::refresponse {} {
|
|
set stat [catch {checkerror} reply]
|
|
if {$stat == 0} {
|
|
sct update $reply
|
|
return idle
|
|
} else {
|
|
clientput $reply
|
|
return idle
|
|
}
|
|
}
|
|
#-------------------------------------------------------------------
|
|
proc el734::setref {name num} {
|
|
set newpos [sct target]
|
|
set com [format "V %d %d" $num $newpos]
|
|
return [el734::setcommand $com "el734::setrefresponse $name $num"]
|
|
}
|
|
#-------------------------------------------------------------------
|
|
proc el734::setrefresponse {name num} {
|
|
set stat [catch {checkerror} reply]
|
|
if {$stat == 0} {
|
|
return [el734::readref $num]
|
|
} else {
|
|
clientput $reply
|
|
return idle
|
|
}
|
|
}
|
|
#============================= SS =================================
|
|
proc el734::readss {num} {
|
|
set com [format "SS %d" $num]
|
|
sct send $com
|
|
return ssread
|
|
}
|
|
#-----------------------------------------------------------------
|
|
proc el734::ssread {} {
|
|
sct update [sct result]
|
|
return idle
|
|
}
|
|
#======================== setpos ================================
|
|
proc el734::forcepos {controller name num newpos} {
|
|
set com [format "U %d %f" $num $newpos]
|
|
$controller send $com
|
|
$controller queue /sics/${name}/hardposition progress read
|
|
wait 1
|
|
return Done
|
|
}
|
|
#======================= refrun ==================================
|
|
proc el734::refrun {controller name num} {
|
|
clientput "Starting reference run"
|
|
$controller send [format "R %d" $num]
|
|
$controller queue /sics/${name}/ss progress read
|
|
while {1} {
|
|
wait 2
|
|
set ss [hval /sics/${name}/ss]
|
|
if { [string first ?BSY $ss] < 0} {
|
|
break
|
|
}
|
|
set rupt [getint]
|
|
if { [string compare $rupt continue] != 0} {
|
|
error "Refererence run interrupted"
|
|
}
|
|
$controller queue /sics/${name}/ss progress read
|
|
}
|
|
$controller queue /sics/${name}/hardposition progress read
|
|
wait 2
|
|
return "Reference run Finished"
|
|
}
|
|
#================================================================
|
|
proc el734::reset {name} {
|
|
set x [hval /sics/${name}/hardlowerlim]
|
|
hupdate /sics/${name}/softlowerlim $x
|
|
set x [hval /sics/${name}/hardupperlim]
|
|
hupdate /sics/${name}/softupperlim $x
|
|
hupdate /sics/${name}/softzero 0
|
|
hupdate /sics/${name}/fixed -1
|
|
}
|
|
#========================= Make ==================================
|
|
proc el734::make {name no controller} {
|
|
MakeSecMotor $name
|
|
|
|
hfactory /sics/${name}/oredmsr plain internal int
|
|
hfactory /sics/${name}/runfault plain internal int
|
|
hfactory /sics/${name}/posfault plain internal int
|
|
hfactory /sics/${name}/lasterror plain internal text
|
|
|
|
hsetprop /sics/${name}/hardposition read el734::readpos $no
|
|
hsetprop /sics/${name}/hardposition command el734::command
|
|
|
|
hsetprop /sics/${name}/hardposition write el734::setpos $name $no
|
|
hsetprop /sics/${name}/hardposition command el734::command
|
|
$controller write /sics/${name}/hardposition
|
|
|
|
hsetprop /sics/${name}/hardlowerlim read el734::getlim $name $no
|
|
hsetprop /sics/${name}/hardlowerlim command el734::command
|
|
$controller poll /sics/${name}/hardlowerlim 120
|
|
|
|
hsetprop /sics/${name}/status read el734::readstatus $no $name
|
|
hsetprop /sics/${name}/status command el734::command
|
|
$controller poll /sics/${name}/status 40
|
|
|
|
hfactory /sics/${name}/speed plain user int
|
|
hsetprop /sics/${name}/speed read el734::readspeed $no
|
|
hsetprop /sics/${name}/speed command el734::command
|
|
$controller poll /sics/${name}/speed 120
|
|
|
|
hsetprop /sics/${name}/speed write el734::setspeed $name $no
|
|
hsetprop /sics/${name}/speed command el734::command
|
|
$controller write /sics/${name}/speed
|
|
|
|
$name makescriptfunc halt "el734::halt $controller $no" user
|
|
$name makescriptfunc reset "el734::reset $name" user
|
|
|
|
$name makescriptfunc sethardlim "el734::setlim $controller $name $no" mugger
|
|
hfactory /sics/${name}/sethardlim/low plain mugger float
|
|
hfactory /sics/${name}/sethardlim/high plain mugger float
|
|
|
|
hfactory /sics/${name}/motno plain internal int
|
|
hupdate /sics/${name}/motno $no
|
|
|
|
}
|
|
#-------------------------------------------------------------------------------
|
|
proc el734::addrefstuff {name no controller} {
|
|
hfactory /sics/${name}/refnull plain user int
|
|
hsetprop /sics/${name}/refnull read el734::readref $no
|
|
hsetprop /sics/${name}/refnull command el734::command
|
|
$controller poll /sics/${name}/refnull 300
|
|
|
|
hsetprop /sics/${name}/refnull write el734::setref $name $no
|
|
hsetprop /sics/${name}/refnull command el734::command
|
|
$controller write /sics/${name}/refnull
|
|
|
|
hfactory /sics/${name}/ss plain internal text
|
|
hsetprop /sics/${name}/ss read el734::readss $no
|
|
hsetprop /sics/${name}/ss ssread el734::ssread
|
|
$controller poll /sics/${name}/ss 300
|
|
|
|
$name makescriptfunc refrun "el734::refrun $controller $name $no" user
|
|
|
|
}
|