platypus sct_chopper.tcl
New scriptcontext chopper driver. r3141 | ffr | 2011-05-19 08:44:53 +1000 (Thu, 19 May 2011) | 2 lines
This commit is contained in:
Ferdi Franceschini
committed by
Douglas Clowes
Douglas Clowes
parent
dea8c2c1bb
commit
2e0729532a
@@ -0,0 +1,325 @@
|
||||
##
|
||||
# @file
|
||||
# Implements astrium disk_chopper driver for platypus
|
||||
|
||||
# Test by adding the following to barebones.tcl
|
||||
# InstallHdb
|
||||
# source config/chopper/sct_chopper.tcl
|
||||
# hfactory /chopper link disk_chopper
|
||||
|
||||
# The chopper doesn't close client connections
|
||||
# if the connection is broken. It only closes the connection
|
||||
# when a client logs off with "#SES#bye", NOTE bye must be lowercase.
|
||||
## State reports for each chopper are requested with the STATE n command
|
||||
## #SOS#STATE 1:
|
||||
## #SOS#ACCEPT CH= 1# State= Asynchron.#ASPEED= 1200#RSPEED= 1200#APHASE= 999.99#RPHASE= 0#AVETO = 0#DIR = CW#MONIT = ok#FLOWR = 2.9#WTEMP = 15.7#MTEMP = 19.2#MVIBR = 0.0#MVACU = 0.0036#DATE = 29/03/2011#TIME = 8:11:54 AM#
|
||||
|
||||
namespace eval ::scobj::chopper {
|
||||
variable UID
|
||||
variable PWD
|
||||
variable sim_mode
|
||||
variable paramindex
|
||||
variable paramtype
|
||||
variable pollrate 900
|
||||
|
||||
|
||||
set sim_mode [SplitReply [chopper_simulation]]
|
||||
foreach {
|
||||
param index type units } {
|
||||
state 0 text @none
|
||||
aspeed 1 float rpm
|
||||
rspeed 2 float rpm
|
||||
aphase 3 float degrees
|
||||
rphase 4 float degrees
|
||||
aveto 5 text @none
|
||||
dir 6 text @none
|
||||
monit 7 text @none
|
||||
flowr 8 float @none
|
||||
wtemp 9 float C
|
||||
mtemp 10 float C
|
||||
mvibr 11 float @none
|
||||
mvacu 12 float mbar
|
||||
date 13 text @none
|
||||
time 14 text @none
|
||||
} {
|
||||
set paramindex($param) $index
|
||||
set paramtype($param) $type
|
||||
set paramunits($param) $units
|
||||
}
|
||||
|
||||
MakeSICSObj disk_chopper SCT_OBJECT user int
|
||||
sicslist setatt disk_chopper klass NXdisk_chopper
|
||||
sicslist setatt disk_chopper long_name disk_chopper
|
||||
|
||||
proc sendUID {user} {
|
||||
sct send "user:$user"
|
||||
return RDPWDCHALLENGE
|
||||
}
|
||||
|
||||
proc rdPwdChallenge {} {
|
||||
set challenge [sct result]
|
||||
switch -glob -- $challenge {
|
||||
"#SES#Fill in your password*" { return SENDPWD }
|
||||
"#SES#You are not a valid user, try again*" {return SENDUID}
|
||||
default {return -code error "Unhandled reply to [info level 0]: $challenge"}
|
||||
}
|
||||
}
|
||||
proc sndPwd {pwd} {
|
||||
sct send "password:$pwd"
|
||||
return RDPWDACK
|
||||
}
|
||||
proc rdPwdAck {} {
|
||||
set ack [sct result]
|
||||
switch -glob -- $ack {
|
||||
"#SES#Hello*" {
|
||||
sct chopper 1
|
||||
return NXTCHOPPER
|
||||
}
|
||||
"#SES#Fill in your password*" { return SENDPWD }
|
||||
"#SES#You are not a valid user, try again*" {return SENDUID}
|
||||
default {return -code error "Unhandled reply to [info level 0]: $ack"}
|
||||
}
|
||||
}
|
||||
##
|
||||
# @brief Request a state report from the chopper
|
||||
proc sndStateReq {root} {
|
||||
set curChopper [sct chopper]
|
||||
if {$curChopper == 1} {
|
||||
hset $root/device_error ""
|
||||
sct update 0
|
||||
}
|
||||
sct send "#SOS#STATE [sct chopper]:"
|
||||
return RDSTATE
|
||||
}
|
||||
|
||||
##
|
||||
# @brief Read the current state report from the chopper.
|
||||
proc rdState {root} {
|
||||
variable paramindex
|
||||
|
||||
set catch_status [ catch {
|
||||
set staterep [sct result]
|
||||
set curChopper [sct chopper]
|
||||
|
||||
if {[string match {ASCERR:*} $staterep]} {
|
||||
sct chopper 1
|
||||
sct update -1
|
||||
hset $root/device_error $staterep
|
||||
error $staterep
|
||||
}
|
||||
if {[string match {*#SES#*} $staterep]} {
|
||||
sct chopper 1
|
||||
return SENDUID
|
||||
}
|
||||
if {[string match {#SOS#*} $staterep] == 0 } {
|
||||
sct chopper 1
|
||||
sct update -1
|
||||
hset $root/device_error $staterep
|
||||
error $staterep
|
||||
}
|
||||
set status [lrange [split $staterep "#"] 3 end-1]
|
||||
|
||||
if {$staterep != [sct _oldval]} {
|
||||
set state [lindex $status $paramindex(state) end]
|
||||
if {$state != [sct _oldstate]} {
|
||||
sct _oldstate $state
|
||||
}
|
||||
sct _oldval $staterep
|
||||
sct staterep $status
|
||||
foreach {param index} [array get paramindex] {
|
||||
set data [lindex [ split [lindex $status $paramindex($param)] = ] 1]
|
||||
if {$param == "time"} {
|
||||
# Convert time to 24 hour format
|
||||
set data [clock format [clock scan $data] -format %T]
|
||||
}
|
||||
set data [string trim $data]
|
||||
if {$param == "aspeed"} {
|
||||
hset $root/ch${curChopper}speed $data
|
||||
}
|
||||
if {$param == "rphase"} {
|
||||
hset $root/ch${curChopper}phase $data
|
||||
}
|
||||
hset $root/ch$curChopper/$param $data
|
||||
}
|
||||
sct utime readtime
|
||||
}
|
||||
|
||||
if {$curChopper >= 4} {
|
||||
sct chopper 1
|
||||
sct update 1
|
||||
return idle
|
||||
} else {
|
||||
incr curChopper
|
||||
sct chopper $curChopper
|
||||
return NXTCHOPPER
|
||||
}
|
||||
} message ]
|
||||
handle_exception $catch_status $message
|
||||
}
|
||||
|
||||
# Create chopper control
|
||||
set scobjNS ::scobj::chopper
|
||||
set chopperPath /sics/disk_chopper
|
||||
|
||||
hsetprop $chopperPath read ${scobjNS}::sndStateReq $chopperPath
|
||||
hsetprop $chopperPath NXTCHOPPER ${scobjNS}::sndStateReq $chopperPath
|
||||
hsetprop $chopperPath RDSTATE ${scobjNS}::rdState $chopperPath
|
||||
hsetprop $chopperPath chopper 1
|
||||
hsetprop $chopperPath chopper_ready "UNKNOWN"
|
||||
hsetprop $chopperPath SENDUID ${scobjNS}::sendUID $UID
|
||||
hsetprop $chopperPath RDPWDCHALLENGE ${scobjNS}::rdPwdChallenge
|
||||
hsetprop $chopperPath SENDPWD ${scobjNS}::sndPwd $PWD
|
||||
hsetprop $chopperPath RDPWDACK ${scobjNS}::rdPwdAck
|
||||
hsetprop $chopperPath _oldval "UNKNOWN"
|
||||
hsetprop $chopperPath _oldstate "UNKNOWN"
|
||||
|
||||
hfactory $chopperPath/device_error plain spy text
|
||||
hset $chopperPath/device_error ""
|
||||
|
||||
# Must be set by user
|
||||
hfactory $chopperPath/geometry plain spy none
|
||||
hfactory $chopperPath/geometry/position plain spy none
|
||||
hfactory $chopperPath/geometry/position/ChopperPosX plain user float
|
||||
hsetprop $chopperPath/geometry/position/ChopperPosX units "mm"
|
||||
hfactory $chopperPath/geometry/position/ChopperPosY plain user float
|
||||
hsetprop $chopperPath/geometry/position/ChopperPosY units "mm"
|
||||
hfactory $chopperPath/geometry/position/ChopperPosZ plain user float
|
||||
hsetprop $chopperPath/geometry/position/ChopperPosZ units "mm"
|
||||
hfactory $chopperPath/geometry/position/ChopperCoordScheme plain user text
|
||||
|
||||
# Setup nodes for state report parameters
|
||||
foreach ch {"ch1" "ch2" "ch3" "ch4"} {
|
||||
hfactory $chopperPath/$ch plain spy none
|
||||
foreach par [lsort [array names paramindex]] {
|
||||
hfactory $chopperPath/$ch/$par plain spy $paramtype($par)
|
||||
if {$paramunits($par) != "@none"} {
|
||||
hsetprop $chopperPath/$ch/$par units $paramunits($par)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
# ::scobj::hinitprops chopper
|
||||
::scobj::set_required_props $chopperPath
|
||||
hsetprop $chopperPath klass NXdisk_chopper
|
||||
hsetprop $chopperPath privilege spy
|
||||
hsetprop $chopperPath type part
|
||||
hsetprop $chopperPath control true
|
||||
hsetprop $chopperPath data true
|
||||
hsetprop $chopperPath nxsave true
|
||||
|
||||
|
||||
proc mkChopperDatPar {chopperPath datPar datSrce} {
|
||||
variable paramunits
|
||||
|
||||
hfactory $chopperPath/$datPar plain user float
|
||||
hsetprop $chopperPath/$datPar klass parameter
|
||||
hsetprop $chopperPath/$datPar control true
|
||||
hsetprop $chopperPath/$datPar data true
|
||||
hsetprop $chopperPath/$datPar nxsave true
|
||||
hsetprop $chopperPath/$datPar mutable true
|
||||
hsetprop $chopperPath/$datPar privilege user
|
||||
hsetprop $chopperPath/$datPar nxalias $datPar
|
||||
hsetprop $chopperPath/$datPar units $paramunits($datSrce)
|
||||
hsetprop $chopperPath/$datPar sdsinfo ::nexus::scobj::sdsinfo
|
||||
}
|
||||
foreach param {ch1speed ch2speed ch3speed ch4speed} {
|
||||
mkChopperDatPar $chopperPath $param "aspeed"
|
||||
}
|
||||
foreach param {ch1phase ch2phase ch3phase ch4phase} {
|
||||
mkChopperDatPar $chopperPath $param "rphase"
|
||||
}
|
||||
if {$sim_mode == "false"} {
|
||||
makesctcontroller sct_chopper astvelsel $chopper_IP:$chopper_port "\r" 10
|
||||
sct_chopper poll $chopperPath $pollrate
|
||||
sct_chopper queue $chopperPath progress read
|
||||
}
|
||||
}
|
||||
|
||||
namespace eval ::chopper {
|
||||
}
|
||||
|
||||
proc ::chopper::ready? {} {
|
||||
set chPath $::scobj::chopper::chopperPath
|
||||
|
||||
sct_chopper queue $chPath progress read
|
||||
wait 1
|
||||
set UDstate [hval $chPath]
|
||||
set retry 0
|
||||
while {$UDstate != 1} {
|
||||
if {$UDstate == -1} {
|
||||
if {$retry < 3} {
|
||||
incr retry
|
||||
} else {
|
||||
return "NOTREADY: Failed to getstatus update"
|
||||
}
|
||||
}
|
||||
wait 1
|
||||
set UDstate [hval $chPath]
|
||||
}
|
||||
foreach ch {"1" "2" "3" "4"} {
|
||||
set stVal [hval $chPath/ch$ch/monit]
|
||||
if {$stVal != "ok"} {
|
||||
return "NOTREADY: monitor $ch = $stVal"
|
||||
}
|
||||
set stVal [hval $chPath/ch$ch/state]
|
||||
switch $stVal {
|
||||
"Brake" {
|
||||
return "NOTREADY: Chopper $ch braking"
|
||||
}
|
||||
"Commutation" {
|
||||
return "NOTREADY: Calibrating"
|
||||
}
|
||||
"Inactive" {
|
||||
return "NOTREADY: Inactive, may need calibrating"
|
||||
}
|
||||
"E-Stop" {
|
||||
return "NOTREADY: State is EMERGENCY STOP"
|
||||
}
|
||||
}
|
||||
set aspeed [hval $chPath/ch$ch/aspeed]
|
||||
set rspeed [hval $chPath/ch$ch/rspeed]
|
||||
if {[expr abs(abs($aspeed) - abs($rspeed))] > 2} {
|
||||
return "NOTREADY: Current speed $aspeed != requested $rspeed for chopper $ch"
|
||||
}
|
||||
}
|
||||
foreach ch {"2" "3" "4"} {
|
||||
set stVal [hval $chPath/ch$ch/state]
|
||||
if { $stVal == "Asynchron."} {
|
||||
return "NOTREADY: Chopper $ch is set to Asynchronous"
|
||||
}
|
||||
set aphase [hval $chPath/ch$ch/aphase]
|
||||
if { $aphase == "999.99"} {
|
||||
return "NOTREADY: phase=$aphase on CH$ch, press 'Accept' on Astrium program"
|
||||
}
|
||||
set rphase [hval $chPath/ch$ch/rphase]
|
||||
if { [expr abs($aphase - $rphase)] > 0.2 } {
|
||||
return "NOTREADY: Current phase $aphase != requested phase $rphase for chopper $ch"
|
||||
}
|
||||
}
|
||||
return "READY"
|
||||
}
|
||||
|
||||
proc ::chopper::get_frequency {} {
|
||||
set chPath $::scobj::chopper::chopperPath
|
||||
|
||||
sct_chopper queue $chPath progress read
|
||||
wait 1
|
||||
set UDstate [hval $chPath]
|
||||
set retry 0
|
||||
while {$UDstate != 1} {
|
||||
if {$UDstate == -1} {
|
||||
if {$retry < 3} {
|
||||
incr retry
|
||||
} else {
|
||||
return "NOTREADY: Failed to getstatus update"
|
||||
}
|
||||
}
|
||||
wait 1
|
||||
set UDstate [hval $chPath]
|
||||
}
|
||||
set speed [hval $chPath/ch1speed]
|
||||
return [expr $speed/60.0]
|
||||
}
|
||||
|
||||
publish ::chopper::get_frequency user
|
||||
publish ::chopper::ready? user
|
||||
Reference in New Issue
Block a user