From b15014bfd5673d7178ff0bd5fd9c53da9429e800 Mon Sep 17 00:00:00 2001 From: Ferdi Franceschini Date: Tue, 23 Apr 2013 10:26:28 +1000 Subject: [PATCH] The mchs and schs motors can now be driven via run and drive command wrappers. Running mchs and schs sets the motor gain parameters. Uses new ANSTO tclmotor implementation. --- .../pelican/config/chopper/fermimot.tcl | 450 +++++++++++++----- 1 file changed, 338 insertions(+), 112 deletions(-) diff --git a/site_ansto/instrument/pelican/config/chopper/fermimot.tcl b/site_ansto/instrument/pelican/config/chopper/fermimot.tcl index fa133b0e..b0ce85f8 100644 --- a/site_ansto/instrument/pelican/config/chopper/fermimot.tcl +++ b/site_ansto/instrument/pelican/config/chopper/fermimot.tcl @@ -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 -