357 lines
11 KiB
Tcl
357 lines
11 KiB
Tcl
#---------------------------------------------------------------
|
|
# 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 -
|
|
11 {return run}
|
|
56 {error "Controller aborted"}
|
|
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: $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: $err"
|
|
sct update poserror
|
|
} else {
|
|
clientput "ERROR: $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: $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: $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: $data"
|
|
sct seterror $data
|
|
return idle
|
|
}
|
|
set status [catch {evaluateAxisStatus $data} msg]
|
|
if {$status != 0} {
|
|
clientput "ERROR: $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 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=0" $num]
|
|
foreach par $parlist {
|
|
$sct queue /sics/$name/$par progress read
|
|
}
|
|
}
|