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
357
site_ansto/instrument/tas/config/tasmad/sicscommon/deltatau.tcl
Normal file
357
site_ansto/instrument/tas/config/tasmad/sicscommon/deltatau.tcl
Normal file
@@ -0,0 +1,357 @@
|
||||
#---------------------------------------------------------------
|
||||
# These are the scripts for the delta-tau PMAC motor
|
||||
# controller.
|
||||
#
|
||||
# !!!!!!!!! Script Chains !!!!!!!!!!!
|
||||
# -- For reading parameters:
|
||||
# sendpmacread code -- pmacreadreply
|
||||
# -- For setting standard parameters
|
||||
# sendpmacwrite code -- pmacreadreply
|
||||
# -- For reading limits
|
||||
# sendpmaclim -- readpmaclim
|
||||
# -- For reading the status
|
||||
# pmacsendaxer --- pmacrcvaxerr -- pmacrcvpos -- pmacrcvstat
|
||||
# This means we check for an axis error first, then update the position,
|
||||
# then check the axis status itself.
|
||||
# -- For setting the position
|
||||
# pmacsendhardpos -- pmacrcvhardpos -- pmacrcvhardax
|
||||
# This means, we send the positioning command, read the reply and read the
|
||||
# axisstatus until the axis has started
|
||||
#
|
||||
# copyright: see file COPYRIGHT
|
||||
#
|
||||
# Mark Koennecke, December 2008, March 2009
|
||||
#---------------------------------------------------------------
|
||||
proc translatePMACError {key} {
|
||||
set pmacerr(ERR001) "Command not allowed while executing"
|
||||
set pmacerr(ERR002) "Password error"
|
||||
set pmacerr(ERR003) "Unrecognized command"
|
||||
set pmacerr(ERR004) "Illegal character"
|
||||
set pmacerr(ERR005) "Command not allowed"
|
||||
set pmacerr(ERR006) "No room in buffer for command"
|
||||
set pmacerr(ERR007) "Buffer already in use"
|
||||
set pmacerr(ERR008) "MACRO auxiliary communication error"
|
||||
set pmacerr(ERR009) "Bad program in MCU"
|
||||
set pmacerr(ERR010) "Both HW limits set"
|
||||
set pmacerr(ERR011) "Previous move did not complete"
|
||||
set pmacerr(ERR012) "A motor is open looped"
|
||||
set pmacerr(ERR013) "A motor is not activated"
|
||||
set pmacerr(ERR014) "No motors"
|
||||
set pmacerr(ERR015) "No valid program in MCU"
|
||||
set pmacerr(ERR016) "Bad program in MCU"
|
||||
set pmacerr(ERR017) "Trying to resume after H or Q"
|
||||
set pmacerr(ERR018) "Invalid operation during move"
|
||||
set pmacerr(ERR019) "Illegal position change command during move"
|
||||
return $pmacerr($key)
|
||||
}
|
||||
#------------------------------------------------------------------
|
||||
proc translateAxisError {key} {
|
||||
switch [string trim $key] {
|
||||
0 {return "no error"}
|
||||
1 { return "limit violation"}
|
||||
2 -
|
||||
3 -
|
||||
4 { return "jog error"}
|
||||
5 {return "command not allowed"}
|
||||
6 {return "watchdog triggered"}
|
||||
7 {return "current limit reached"}
|
||||
8 -
|
||||
9 {return "Air cushion error"}
|
||||
10 {return "MCU lim reached"}
|
||||
11 {return "following error triggered"}
|
||||
12 {return "EMERGENCY STOP ACTIVATED"}
|
||||
13 {return "Driver electronics error"}
|
||||
default { return "Unknown axis error $key"}
|
||||
}
|
||||
}
|
||||
#---------------------------------------------------------------------
|
||||
proc evaluateAxisStatus {key} {
|
||||
#----- Tcl does not like negative numbers as keys.
|
||||
if {$key < 0} {
|
||||
set key [expr 50 + abs($key)]
|
||||
}
|
||||
switch $key {
|
||||
0 -
|
||||
14 {return idle}
|
||||
1 -
|
||||
2 -
|
||||
3 -
|
||||
4 -
|
||||
5 -
|
||||
6 -
|
||||
7 -
|
||||
8 -
|
||||
9 -
|
||||
10 -
|
||||
56 -
|
||||
11 {return run}
|
||||
55 {error "Axis is deactivated"}
|
||||
54 {error "emergency stop activated, please release"}
|
||||
53 {error "Axis inhibited"}
|
||||
51 -
|
||||
52 {error "Incoming command is blocked"}
|
||||
}
|
||||
}
|
||||
#-----------------------------------------------------------------------
|
||||
proc checkpmacresult {} {
|
||||
set data [sct result]
|
||||
if {[string first ASCERR $data] >= 0} {
|
||||
error $data
|
||||
}
|
||||
if {[string first ERR $data] >= 0} {
|
||||
error [translatePMACError $data]
|
||||
}
|
||||
return [string trim $data]
|
||||
}
|
||||
#------------------------------------------------------------------------
|
||||
proc sendpmacread {code} {
|
||||
sct send $code
|
||||
return pmacreadreply
|
||||
}
|
||||
#------------------------------------------------------------------------
|
||||
proc pmacreadreply {} {
|
||||
set status [catch {checkpmacresult} data]
|
||||
if {$status != 0} {
|
||||
sct geterror $data
|
||||
} else {
|
||||
sct update $data
|
||||
}
|
||||
return idle
|
||||
}
|
||||
#----------------------------------------------------------------------
|
||||
proc sendpmaclim {code} {
|
||||
sct send $code
|
||||
return pmacreadlim
|
||||
}
|
||||
#-----------------------------------------------------------------------
|
||||
proc pmacreadlim {motname} {
|
||||
set status [catch {checkpmacresult} data]
|
||||
if {$status != 0} {
|
||||
sct geterror $data
|
||||
} else {
|
||||
set scale [hval /sics/${motname}/scale_factor]
|
||||
sct update [expr $data * $scale]
|
||||
}
|
||||
return idle
|
||||
}
|
||||
#------------------------------------------------------------------------
|
||||
proc sendpmacwrite {code} {
|
||||
set value [sct target]
|
||||
sct send "$code=$value"
|
||||
return pmacwritereply
|
||||
}
|
||||
#------------------------------------------------------------------------
|
||||
proc pmacwritereply {} {
|
||||
set status [catch {checkpmacresult} data]
|
||||
if {$status != 0} {
|
||||
sct geterror $data
|
||||
sct print "ERROR: $data"
|
||||
} else {
|
||||
set con [sct controller]
|
||||
$con queue [sct] read read
|
||||
}
|
||||
return idle
|
||||
}
|
||||
#-------------------------------------------------------------------------
|
||||
proc configurePMACPar {name par code sct} {
|
||||
set path /sics/$name/$par
|
||||
hsetprop $path read "sendpmacread $code"
|
||||
hsetprop $path pmacreadreply pmacreadreply
|
||||
$sct poll $path 30
|
||||
hsetprop $path write "sendpmacwrite $code"
|
||||
hsetprop $path pmacwritereply pmacwritereply
|
||||
$sct write $path
|
||||
}
|
||||
#-------------------------------------------------------------------------
|
||||
proc makePMACPar {name par code sct priv} {
|
||||
set path /sics/$name/$par
|
||||
hfactory $path plain $priv float
|
||||
configurePMACPar $name $par $code $sct
|
||||
}
|
||||
#========================== status functions =============================
|
||||
proc pmacsendaxerr {num} {
|
||||
sct send "P${num}01"
|
||||
return rcvaxerr
|
||||
}
|
||||
#------------------------------------------------------------------------
|
||||
proc pmacrcvaxerr {motname num} {
|
||||
set status [catch {checkpmacresult} data]
|
||||
if {$status != 0} {
|
||||
clientput "ERROR: $motname : $data"
|
||||
sct update error
|
||||
sct geterror $data
|
||||
return idle
|
||||
}
|
||||
hupdate /sics/$motname/axiserror $data
|
||||
if {$data != 0 } {
|
||||
set err [translateAxisError $data]
|
||||
if {[string first following $err] >= 0} {
|
||||
clientput "WARNING: $motname : $err"
|
||||
sct update poserror
|
||||
} else {
|
||||
clientput "ERROR: $motname : $err"
|
||||
sct update error
|
||||
}
|
||||
return idle
|
||||
}
|
||||
hupdate /sics/$motname/axiserror $data
|
||||
sct send "Q${num}10"
|
||||
return rcvpos
|
||||
}
|
||||
#------------------------------------------------------------------------
|
||||
proc pmacrcvpos {motname num} {
|
||||
set status [catch {checkpmacresult} data]
|
||||
if {$status != 0} {
|
||||
clientput "ERROR: $motname : $data"
|
||||
sct geterror $data
|
||||
sct update error
|
||||
return idle
|
||||
}
|
||||
hupdate /sics/$motname/hardposition $data
|
||||
sct send "P${num}00"
|
||||
return rcvstat
|
||||
}
|
||||
#------------------------------------------------------------------------
|
||||
proc pmacrcvstat {motname num sct} {
|
||||
set status [catch {checkpmacresult} data]
|
||||
if {$status != 0} {
|
||||
clientput "ERROR: $motname : $data"
|
||||
sct update error
|
||||
return idle
|
||||
}
|
||||
set status [catch {evaluateAxisStatus $data} msg]
|
||||
if {$status != 0} {
|
||||
sct update error
|
||||
} else {
|
||||
sct update $msg
|
||||
switch $msg {
|
||||
idle {
|
||||
# force an update of the motor position
|
||||
$sct queue /sics/$motname/hardposition progress read
|
||||
}
|
||||
run {
|
||||
# force an update of ourselves, while running
|
||||
$sct queue /sics/$motname/status progress read
|
||||
}
|
||||
}
|
||||
}
|
||||
return idle
|
||||
}
|
||||
#-------------------------------------------------------------------------
|
||||
proc configurePMACStatus {motname num sct} {
|
||||
set path /sics/$motname/status
|
||||
hsetprop $path read "pmacsendaxerr $num"
|
||||
hsetprop $path rcvaxerr "pmacrcvaxerr $motname $num"
|
||||
hsetprop $path rcvpos "pmacrcvpos $motname $num"
|
||||
hsetprop $path rcvstat "pmacrcvstat $motname $num $sct"
|
||||
$sct poll $path 60
|
||||
}
|
||||
#======================= setting hard position ===========================
|
||||
proc pmacsendhardpos {motname num} {
|
||||
hupdate /sics/$motname/status run
|
||||
set value [sct target]
|
||||
sct send [format "P%2.2d23=0 Q%2.2d01=%12.4f M%2.2d=1" $num $num $value $num]
|
||||
return rcvhardpos
|
||||
}
|
||||
#-------------------------------------------------------------------------
|
||||
proc pmacrcvhardpos {num} {
|
||||
set status [catch {checkpmacresult} data]
|
||||
if {$status != 0} {
|
||||
clientput "ERROR: $data"
|
||||
sct seterror $data
|
||||
return idle
|
||||
}
|
||||
sct send "P${num}00"
|
||||
return rcvhardax
|
||||
}
|
||||
#------------------------------------------------------------------------
|
||||
proc pmacrcvhardax {motname num sct} {
|
||||
set status [catch {checkpmacresult} data]
|
||||
if {$status != 0} {
|
||||
clientput "ERROR: $motname : $data"
|
||||
sct seterror $data
|
||||
return idle
|
||||
}
|
||||
set status [catch {evaluateAxisStatus $data} msg]
|
||||
if {$status != 0} {
|
||||
clientput "ERROR: $motname : $msg"
|
||||
sct seterror $msg
|
||||
return idle
|
||||
}
|
||||
switch $msg {
|
||||
idle {
|
||||
sct send "P${num}00"
|
||||
return rcvhardax
|
||||
}
|
||||
run {
|
||||
$sct queue /sics/$motname/status progress read
|
||||
return idle
|
||||
}
|
||||
}
|
||||
}
|
||||
#------------------------------------------------------------------------
|
||||
proc configurePMAChardwrite {motname num sct} {
|
||||
set path /sics/$motname/hardposition
|
||||
hsetprop $path write "pmacsendhardpos $motname $num"
|
||||
hsetprop $path rcvhardpos "pmacrcvhardpos $num"
|
||||
hsetprop $path rcvhardax "pmacrcvhardax $motname $num $sct"
|
||||
}
|
||||
#======================= Halt =============================================
|
||||
proc pmacHalt {sct num} {
|
||||
$sct send "M${num}=8" halt
|
||||
return OK
|
||||
}
|
||||
#==================== Reference Run =======================================
|
||||
proc pmacrefrun {motorname sct num} {
|
||||
set path /sics/${motorname}/status
|
||||
$sct send "M${num}=9"
|
||||
hupdate /sics/${motorname}/status run
|
||||
set motstat run
|
||||
wait 3
|
||||
while {[string compare $motstat run] == 0} {
|
||||
$sct queue $path progress read
|
||||
wait 1
|
||||
set motstat [string trim [hval $path]]
|
||||
}
|
||||
return "Done"
|
||||
}
|
||||
#--------------------------------------------------------------------------
|
||||
proc MakeDeltaTau {name sct num} {
|
||||
MakeSecMotor $name
|
||||
hsetprop /sics/${name}/hardupperlim read "sendpmaclim I${num}13"
|
||||
hsetprop /sics/${name}/hardupperlim pmacreadlim "pmacreadlim $name"
|
||||
$sct poll /sics/${name}/hardupperlim 180
|
||||
hsetprop /sics/${name}/hardlowerlim read "sendpmaclim I${num}14"
|
||||
hsetprop /sics/${name}/hardlowerlim pmacreadlim "pmacreadlim $name"
|
||||
$sct poll /sics/${name}/hardlowerlim 180
|
||||
|
||||
# configurePMACPar $name hardlowerlim "Q${num}09" $sct
|
||||
# configurePMACPar $name hardupperlim "Q${num}08" $sct
|
||||
|
||||
configurePMACPar $name hardposition "Q${num}10" $sct
|
||||
configurePMAChardwrite $name $num $sct
|
||||
hfactory /sics/$name/numinmcu plain internal int
|
||||
hupdate /sics/$name/numinmcu ${num}
|
||||
makePMACPar $name enable "M${num}14" $sct mugger
|
||||
makePMACPar $name scale_factor "Q${num}00" $sct mugger
|
||||
makePMACPar $name maxspeed "Q${num}03" $sct mugger
|
||||
makePMACPar $name commandspeed "Q${num}04" $sct mugger
|
||||
makePMACPar $name maxaccel "Q${num}05" $sct mugger
|
||||
makePMACPar $name commandedaccel "Q${num}06" $sct mugger
|
||||
makePMACPar $name offset "Q${num}07" $sct mugger
|
||||
makePMACPar $name axisstatus "P${num}00" $sct internal
|
||||
makePMACPar $name axiserror "P${num}01" $sct internal
|
||||
makePMACPar $name poshwlimitactive "M${num}21" $sct internal
|
||||
makePMACPar $name neghwlimitactive "M${num}22" $sct internal
|
||||
makePMACPar $name liftaircushion "M${num}96" $sct internal
|
||||
configurePMACStatus $name $num $sct
|
||||
$name makescriptfunc halt "pmacHalt $sct $num" user
|
||||
$name makescriptfunc refrun "pmacrefrun $name $sct $num" user
|
||||
set parlist [list scale_factor hardposition maxspeed \
|
||||
commandspeed maxaccel offset axisstatus axiserror status poshwlimitactive \
|
||||
neghwlimitactive liftaircushion hardlowerlim hardupperlim]
|
||||
# $sct send [format "M%2.2d14=1" $num]
|
||||
foreach par $parlist {
|
||||
$sct queue /sics/$name/$par progress read
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user