linse/sics
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

The mchs and schs motors can now be driven via run and drive command

Browse Source 
wrappers.
Running mchs and schs sets the motor gain parameters.
Uses new ANSTO tclmotor implementation.
This commit is contained in:
Ferdi Franceschini
2013-04-23 10:26:28 +10:00
parent bda409da04
commit b15014bfd5
1 changed files with 338 additions and 112 deletions
Download Patch File Download Diff File Expand all files Collapse all files

450
site_ansto/instrument/pelican/config/chopper/fermimot.tcl
View File

@@ -1,6 +1,57 @@
# Table Source Document: 998-0234-004 Rev A.doc
# Speed (RPM) {veto {KpCW KpCCW} {KiCW KiCCW} {KthCW KthCCW}
# NOTE: You can use the motor direction register value to lookup the gains because motdir=0 is CW and motdir=1 is CCW
# TEST
global SCode Ecode
array set SCode {
OKOK 1
HWIdle 2
HWBusy 3
HWFault 4
HWPosFault 5
HWCrash 6
NOMEMORY 7
HWNoBeam 8
HWPause 9
HWWarn 10
HWRedo 11
}
array set ECode {
MOTREDO -1
MOTFAIL 0
MOTOK 1
}
proc handle_exception {status message args} {
switch $status {
0 {
# TCL_OK, This is raised when you just drop out of the
# bottom of a 'catch' command.
return -code ok
}
1 {
# TCL_ERROR
return -code error "([info level -1]) $message: $args"
}
2 {
# TCL_RETURN
return -code return "$message"
}
3 {
# TCL_BREAK
return -code break
}
4 {
# TCL_CONTINUE
return -code continue
}
default {
# Propogate user defined return codes with message
return -code $status "$message"
}
}
}
namespace eval ::chopper {
variable ch1_gains
variable ch2_gains
@@ -45,6 +96,7 @@ namespace eval ::chopper {
3000 { 2750 { 5 5 } { 5 5 } { 2 2 } }
}
set i 0
foreach mult {
1.0/12 1.0/11 1.0/10 1.0/9 1.0/8
1.0/7 1.0/6 1.0/5 1.0/4 1.0/3
@@ -58,7 +110,8 @@ namespace eval ::chopper {
21 22 23 24 25
26 27 28 29 30
} {
lappend speedMult [expr double($mult)]
set speedMult($i) [expr double($mult)]
incr i
}
##
@@ -106,7 +159,7 @@ namespace eval ::chopper {
}
}
proc getVetoWin {ch speed} {
proc findGainIndex {ch speed} {
variable ch1_gains
variable ch2_gains
variable ch3_gains
@@ -121,12 +174,24 @@ namespace eval ::chopper {
2 { set gainTable ch2_gains }
3 { set gainTable ch3_gains }
}
foreach sp [lsort -integer -decreasing [array names $gainTable]] {
if {$sp <= $speed} {
return [lindex [set ${gainTable}($sp)] 0]
set speeds [lsort -integer [array names $gainTable]]
set lower 0
set upper [expr [llength $speeds] - 1]
while {1} {
if {[expr $upper - $lower] == 1} {
if { $speed < [lindex $speeds $upper] } {
return "$gainTable [lindex $speeds $lower]"
} else {
return "$gainTable [lindex $speeds $upper]"
}
}
set mid [expr int(($lower + $upper)/2.0)]
if {$speed < [lindex $speeds $mid]} {
set upper $mid
} else {
set lower $mid
}
}
error "Failed when looking up veto window for speed $speed"
} message ]
handle_exception $catch_status $message
}
@@ -164,18 +229,6 @@ namespace eval ::chopper {
}
##
# setSpeed
# ch: Chopper number
# refFreq: The reference frequency in Hz
# dir: Chopper direction CW or CCW
# speed: The chopper speed in RPM
proc setSpeed {ch refFreq dir speed} {
set catch_status [ catch {
Echeck_SpeedMult $refFreq $speed
} message ]
handle_exception $catch_status $message
}
#TODO Define a run function which looks up parameters from hdb tree and calls the setSpeed function
# pDriv->GetPosition = GetTclPos;
# pDriv->RunTo = TclRun;
@@ -195,9 +248,120 @@ namespace eval ::chopper {
return $rSpeed
}
proc findSpeedMult {sprat} {
variable speedMult
set lower 0
set upper [expr [array size speedMult] - 1]
while {1} {
if {[expr ($upper - $lower)] == 1} {
if { [ expr ($sprat - $speedMult($lower)) ] <= [expr ($speedMult($upper) - $sprat)] } {
return $speedMult($lower)
} else {
return $speedMult($upper)
}
}
set mid [expr int( ($upper + $lower) / 2.0)]
if { $sprat < $speedMult($mid) } {
set upper $mid
} else {
set lower $mid
}
}
}
proc listAllowedSpeeds {ref_period_50ns} {
variable speedMult
set ref_speed_rpm [ expr { 60.0 / ($ref_period_50ns * 50e-9) } ]
set len [array size speedMult]
for {set i 0} {$i < $len} {incr i} {
lappend speeds [format "%.2f" [expr $ref_speed_rpm * $speedMult($i)]]
}
return $speeds
}
proc get_refSpeedRPM {hpath node addr name} {
set ref_period_50ns [ hval $hpath/$node/ref_period ]
set ref_speed_rpm [ expr { 60.0 / ($ref_period_50ns * 50e-9) } ]
return $ref_speed_rpm
}
##
# Implement speed and phase motor objects
proc SGetPos {hpath node addr name} {
# Return nearest allowed speed to target speed
proc abspermittedSpeed {hpath node addr name target} {
set ref_period_50ns [ hval $hpath/$node/ref_period ]
# set ref_period_50ns 333333.33333334
set ref_speed_rpm [ expr { 60.0 / ($ref_period_50ns * 50e-9) } ]
set speed [expr abs($target)]
set sprat [expr {$speed / $ref_speed_rpm}]
set mult [findSpeedMult $sprat]
set allowed_speed [expr $mult * $ref_speed_rpm]
return $allowed_speed
}
proc setSpeed {hpath node addr name target} {
global SCode
variable CW
variable CCW
variable ch1_gains
variable ch2_gains
variable ch3_gains
if {$target >= 0} {
set dir $CW
} else {
set dir $CCW
}
set allowed_speed [abspermittedSpeed $hpath $node $addr $name $target]
foreach {gainTable index} [findGainIndex $addr $allowed_speed] {}
set row [array get $gainTable $index]
set vetowin [lindex $row 1 0]
set propGain [lindex $row 1 1 $dir]
set intGain [lindex $row 1 2 $dir]
set phGain [lindex $row 1 3 $dir]
hset $hpath/$node/control/set_vetowin50 $vetowin
hset $hpath/$node/control/set_prop_gain $propGain
hset $hpath/$node/control/set_int_gain $intGain
hset $hpath/$node/control/set_phase_gain $phGain
hset $hpath/$node/control/set_rotspeed $allowed_speed
return $SCode(OKOK)
}
proc get_setMode {hpath node addr name {target ""}} {
global SCode
if {$target == ""} {
set mode [hval $hpath/$node/mode]
return $mode
} else {
hset $hpath/$node/control/set_mode $target
return $target
}
}
proc get_setRefDelay {hpath node addr name {target_ ""}} {
global SCode
if {$target == ""} {
set delay [hval $hpath/$node/ref_delay]
return $delay
} else {
hset $hpath/$node/control/set_ref_delay $target
return $target
}
}
proc get_setSettleTime {hpath node addr name {settle ""}} {
global SCode
if {$settle == ""} {
return [hgetpropval $hpath/$node settletime]
} else {
hsetprop $hpath/$node settletime $settle
return $settle
}
}
proc imot_SGetpos {hpath node addr name} {
global SCode
variable CW
variable CCW
@@ -205,157 +369,219 @@ namespace eval ::chopper {
set chpath ${hpath}/$node/rotation_speed
set speed [hval $chpath]
broadcast [info level 0]: chpath=$chpath, speed = $speed
if {[hval $hpath/$node/motdir] == $POSDIR} {
return $speed
return $speed
} else {
return -$speed
}
}
proc SRun {hpath node addr name target} {
proc imot_SRun {hpath node addr name target} {
global SCode
variable CW
variable CCW
# broadcast [info level 0]
if {$target >= 0} {
set dir $CW
} else {
set dir $CCW
}
set speed [expr abs($target)]
getVetoWin $addr $speed
broadcast TODO: hset $hpath/$node/control/set_rotspeed $speed
# hset $hpath/$node/control/set_rotspeed $speed
setSpeed $hpath $node $addr $name $target
hset $hpath/$node/control/start 1
hsetprop $hpath/$node/control timecheck -1
set readtime [hgetpropval $hpath read_time]
hsetprop $hpath lastupdate $readtime
return $SCode(OKOK)
}
proc SStatus {hpath node addr name} {
proc imot_SStatus {hpath node addr name} {
global SCode
return $SCode(HWIdle)
set catch_status [ catch {
if [hgetpropval $hpath/$node abort] {
hsetprop $hpath/$node abort 0
error "User requested stop. Aborting operation"
}
set readtime [hgetpropval $hpath read_time]
set lastupdate [hgetpropval $hpath lastupdate]
if {$readtime <= $lastupdate} {
return $SCode(HWBusy)
}
hsetprop $hpath lastupdate $readtime
set up_to_speed [hval $hpath/$node/system_status/up_to_speed]
set phase_locked [hval $hpath/$node/system_status/phase_locked]
set timecheck [hgetpropval $hpath/$node/control timecheck]
set timeout [hgetpropval $hpath/$node settletime]
if {[hval $hpath/$node/mode] == 0} {
# RPM mode
set locked $up_to_speed
} else {
# PHASE mode
set locked [expr $up_to_speed && $phase_locked]
}
if {$locked} {
if {$timecheck == -1} {
hsetprop $hpath/$node/control timecheck $readtime
return $SCode(HWBusy)
} elseif {[expr $readtime - $timecheck] > $timeout} {
return $SCode(HWIdle)
}
} else {
if {$timecheck != -1} {
hsetprop $hpath/$node/control timecheck $readtime
}
return $SCode(HWBusy)
}
} message ]
handle_exception $catch_status $message
}
proc SHalt {hpath node addr name} {
proc imot_SHalt {hpath node addr name} {
global SCode
hset $hpath/$node/control/stop 1
return $SCode(OKOK)
}
proc SGetError {hpath node addr name} {
proc imot_SGetError {hpath node addr name} {
global SCode
return "[info level 0]: TODO error message"
return [hgetpropval $hpath/$node errmsg]
}
proc SFixit {hpath node addr name icode fVal} {
global SCode
proc imot_SFixit {hpath node addr name icode fVal} {
global ECode
return $SCode(MOTOK)
return $ECode(MOTFAIL)
}
# Implementation of phase motor interface
proc PhGetPos {hpath node addr name} {
proc imot_PhGetPos {hpath node addr name} {
global SCode
return 0
set phacc [hval $hpath/$node/phase_acc]
return $phacc
}
proc PhRun {hpath node addr name target} {
proc imot_PhRun {hpath node addr name target} {
global SCode
hset $hpath/$node/control/set_ref_delay $target
hsetprop $hpath/$node/control timecheck -1
set readtime [hgetpropval $hpath read_time]
hsetprop $hpath lastupdate $readtime
return $SCode(OKOK)
}
proc PhStatus {hpath node addr name} {
proc imot_PhStatus {hpath node addr name} {
global SCode
return $SCode(HWIdle)
if [hgetpropval $hpath/$node abort] {
hsetprop $hpath/$node abort 0
error "User requested stop. Aborting operation"
}
set readtime [hgetpropval $hpath read_time]
set lastupdate [hgetpropval $hpath lastupdate]
if {$readtime <= $lastupdate} {
return $SCode(HWBusy)
}
hsetprop $hpath lastupdate $readtime
set phase_locked [hval $hpath/$node/system_status/phase_locked]
set timecheck [hgetpropval $hpath/$node/control timecheck]
set timeout [hgetpropval $hpath/$node settletime]
if {$phase_locked} {
if {$timecheck == -1} {
hsetprop $hpath/$node/control timecheck $readtime
return $SCode(HWBusy)
} elseif {[expr $readtime - $timecheck] > $timeout} {
return $SCode(HWIdle)
}
} else {
if {$timecheck != -1} {
hsetprop $hpath/$node/control timecheck $readtime
}
return $SCode(HWBusy)
}
}
proc PhHalt {hpath node addr name} {
proc imot_PhHalt {hpath node addr name} {
global SCode
hsetprop $hpath/$node abort 1
return $SCode(OKOK)
}
proc PhGetError {hpath node addr name} {
proc imot_PhGetError {hpath node addr name} {
global SCode
return "[info level 0]: TODO error message"
}
proc PhFixit {hpath node addr name icode fVal} {
proc imot_PhFixit {hpath node addr name icode fVal} {
global SCode
return $SCode(MOTOK)
}
}
# TEST
global SCode
array set SCode {
OKOK 1
HWIdle 2
HWBusy 3
HWFault 4
HWPosFault 5
HWCrash 6
NOMEMORY 7
HWNoBeam 8
HWPause 9
HWWarn 10
HWRedo 11
}
proc handle_exception {status message args} {
switch $status {
0 {
# TCL_OK, This is raised when you just drop out of the
# bottom of a 'catch' command.
return -code ok
##
# @brief Returns the permitted speed as a multiple of the reference period
# @target 0, Lists permitted speeds as multiples of the reference speed.
# speed, Returns nearest permitted speed to the given speed.
proc permSpd {hpath node addr name target} {
global SCode
set ref_period_50ns [ hval $hpath/$node/ref_period ]
if {$target == 0} {
return "[listAllowedSpeeds $ref_period_50ns]"
}
1 {
# TCL_ERROR
return -code error "([info level -1]) $message: $args"
}
2 {
# TCL_RETURN
return -code return "$message"
}
3 {
# TCL_BREAK
return -code break
}
4 {
# TCL_CONTINUE
return -code continue
}
default {
# Propogate user defined return codes with message
return -code $status "$message"
set allowed_speed [abspermittedSpeed $hpath $node $addr $name $target]
if {$target > 0} {
return $allowed_speed
} else {
return -$allowed_speed
}
}
proc start {hpath node addr name} {
global SCode
hset $hpath/$node/control/start 1
return $SCode(OKOK)
}
proc stop {hpath node addr name} {
global SCode
hset $hpath/$node/control/stop 1
hsetprop $hpath/$node abort 1
return $SCode(OKOK)
}
}
proc mkFSpeedMot {fmot hdbroot chnode addr lowlim uplim} {
Motor $fmot tclmot [params\
getpos "::chopper::SGetPos $hdbroot $chnode $addr"\
run "::chopper::SRun $hdbroot $chnode $addr"\
status "::chopper::SStatus $hdbroot $chnode $addr"\
halt "::chopper::SHalt $hdbroot $chnode $addr"\
geterror "::chopper::SGetError $hdbroot $chnode $addr"\
fixit "::chopper::SFixit $hdbroot $chnode $addr"
]
Motor $fmot tclmot [subst {\
getpos "::chopper::imot_SGetpos $hdbroot $chnode $addr"\
run "::chopper::imot_SRun $hdbroot $chnode $addr"\
status "::chopper::imot_SStatus $hdbroot $chnode $addr"\
halt "::chopper::imot_SHalt $hdbroot $chnode $addr"\
geterror "::chopper::imot_SGetError $hdbroot $chnode $addr"\
fixit "::chopper::imot_SFixit $hdbroot $chnode $addr"\
refspeed "::chopper::get_refSpeedRPM $hdbroot $chnode $addr"\
mode "::chopper::get_setMode $hdbroot $chnode $addr"\
refdelay "::chopper::get_setRefDelay $hdbroot $chnode $addr"\
settle "::chopper::get_setSettleTime $hdbroot $chnode $addr"\
setspeed "::chopper::setSpeed $hdbroot $chnode $addr"\
permspd "::chopper::permSpd $hdbroot $chnode $addr"\
start "::chopper::start $hdbroot $chnode $addr"\
stop "::chopper::stop $hdbroot $chnode $addr"\
}]
hsetprop $hdbroot/$chnode abort 0
$fmot movecount 5000
$fmot hardlowerlim $lowlim
$fmot hardupperlim $uplim
$fmot softlowerlim $lowlim
$fmot softupperlim $uplim
$fmot settle 30
$fmot precision 1
sicslist setatt $fmot units rpm
sicslist setatt $fmot klass fermi_chopper
sicslist setatt $fmot long_name $fmot
}
proc mkFPhaseMot {fmot hdbroot chnode addr lowlim uplim} {
Motor $fmot tclmot [params\
getpos "::chopper::PhGetPos $hdbroot $chnode $addr"\
run "::chopper::PhRun $hdbroot $chnode $addr"\
status "::chopper::PhStatus $hdbroot $chnode $addr"\
halt "::chopper::PhHalt $hdbroot $chnode $addr"\
geterror "::chopper::PhGetError $hdbroot $chnode $addr"\
fixit "::chopper::PhFixit $hdbroot $chnode $addr"
]
Motor $fmot tclmot [subst {\
getpos "::chopper::imot_PhGetPos $hdbroot $chnode $addr"\
run "::chopper::imot_PhRun $hdbroot $chnode $addr"\
status "::chopper::imot_PhStatus $hdbroot $chnode $addr"\
halt "::chopper::imot_PhHalt $hdbroot $chnode $addr"\
geterror "::chopper::imot_PhGetError $hdbroot $chnode $addr"\
fixit "::chopper::imot_PhFixit $hdbroot $chnode $addr"\
}]
$fmot hardlowerlim $lowlim
$fmot hardupperlim $uplim
$fmot softlowerlim $lowlim
@@ -376,6 +602,7 @@ proc mkFPhaseMot {fmot hdbroot chnode addr lowlim uplim} {
# @param maddr, Master chopper modbus address
# @param slave, Slave chopper hdb node name
# @param saddr, Slave chopper modbus address
#proc mkFermiMotors {mSpdmot sSpdmot master maddr slave saddr} {
proc mkFermiMotors {mSpdmot sSpdmot sPhmot master maddr slave saddr} {
set hdbPath [mkChoppers " $master $maddr $slave $saddr"]
# Create Master Chopper Speed motor object
@@ -385,4 +612,3 @@ proc mkFermiMotors {mSpdmot sSpdmot sPhmot master maddr slave saddr} {
}
mkFermiMotors mchs schs schp mch 1 sch 2