move folder tasmad
r3086 | jgn | 2011-03-29 15:32:07 +1100 (Tue, 29 Mar 2011) | 1 line
This commit is contained in:
Jing Chen
committed by
Douglas Clowes
Douglas Clowes
parent
e92dce3948
commit
66e3096b24
488
site_ansto/instrument/tas/config/tasmad/sicscommon/el734.tcl
Normal file
488
site_ansto/instrument/tas/config/tasmad/sicscommon/el734.tcl
Normal file
@@ -0,0 +1,488 @@
|
||||
#--------------------------------------------------------
|
||||
# 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
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user