diff --git a/site_ansto/instrument/pelican/config/chopper/fermimot.tcl b/site_ansto/instrument/pelican/config/chopper/fermimot.tcl new file mode 100644 index 00000000..b76040e4 --- /dev/null +++ b/site_ansto/instrument/pelican/config/chopper/fermimot.tcl @@ -0,0 +1,388 @@ +# 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 +namespace eval ::chopper { + variable ch1_gains + variable ch2_gains + variable ch3_gains + variable speedMult + variable CW 0 CCW 1 + variable PROPGAIN 1 INTGAIN 2 PHGAIN 3 + variable POSDIR + set POSDIR $CCW + + array set ch1_gains { + 24000 { 0.35 { 10 10 } { 10 10 } { 2.33 2.33 } } + 21000 { 0.40 { 10 15 } { 10 13 } { 2.33 13 } } + 18000 { 0.45 { 10 10 } { 10 10 } { 2.33 2.33 } } + 15000 { 0.55 { 10 10 } { 10 10 } { 2.33 2.33 } } + 12000 { 0.65 { 12 12 } { 10 10 } { 10 10 } } + 9000 { 0.95 { 10 10 } { 10 10 } { 10 10 } } + 6000 { 1.35 { 10 10 } { 10 10 } { 10 10 } } + 3600 { 2.31 { 5 5 } { 5 5 } { 5 5 } } + 3000 { 2.75 { 5 5 } { 5 5 } { 2.33 2.33 } } + } + + array set ch2_gains { + 24000 { 0.35 { 15 15 } { 13 13 } { 13 13 } } + 21000 { 0.40 { 15 15 } { 13 13 } { 13 13 } } + 18000 { 0.45 { 15 15 } { 13 13 } { 13 13 } } + 15000 { 0.55 { 15 15 } { 13 13 } { 13 13 } } + 12000 { 0.65 { 10 10 } { 10 10 } { 2.33 2.33 } } + 9000 { 0.95 { 10 10 } { 10 10 } { 2.33 2.33 } } + 6000 { 1.35 { 10 10 } { 10 10 } { 2.33 2.33 } } + 3000 { 2.75 { 5 5 } { 5 5 } { 2 2 } } + } + + array set ch3_gains { + 24000 { 0.35 { 15 10 } { 13 10 } { 13 2.33 } } + 21000 { 0.40 { 15 10 } { 13 10 } { 13 2.33 } } + 18000 { 0.45 { 15 10 } { 13 10 } { 13 2.33 } } + 15000 { 0.55 { 10 10 } { 10 10 } { 2.33 2.33 } } + 12000 { 0.65 { 10 10 } { 10 10 } { 2.33 2.33 } } + 9000 { 0.95 { 10 10 } { 10 10 } { 2.33 2.33 } } + 6000 { 1.35 { 10 10 } { 10 10 } { 2.33 2.33 } } + 3000 { 2.75 { 5 5 } { 5 5 } { 2 2 } } + } + + 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 + 1.0/2 1 1.5 2 2.5 + 3 3.5 4 4.5 5 + 5.5 6 6.5 7 7.5 + 8 8.5 9 9.5 10 + 10.5 11 11.5 12 12.5 + 13 13.5 14 14.5 15 + 16 17 18 19 20 + 21 22 23 24 25 + 26 27 28 29 30 + } { + lappend speedMult [expr double($mult)] + } + + ## + # Echeck_SpeedMult: Checks that speed is an allowed multiple of the reference signal + # refFreq: Reference signal frequency (Hz) + # speed: Requested chopper speed (RPM) + proc Echeck_SpeedMult {refFreq speed} { + variable speedMult + set refRPM [expr 60 * $refFreq] + foreach mult $speedMult { + if { [expr $mult * $refRPM] == $speed} {return} + } + return -code error "Speed ($speed) is not an allowed multiple of the reference signal" + } + + + proc Echeck_ChSpeed {ch speed} { + set minSpeed 3000 + array set vetoRange {min 9000 max 11000} + + if {$ch != 1 && $ch != 2 && $ch != 3} { + return -code error "Chopper number ($ch) must be 1, 2, or 3" + } + if { ![string is double $speed] } { + return -code error "Speed ($speed) is not a valid number" + } elseif {$speed < $minSpeed} { + return -code error "Speed ($speed) should be >= $minSpeed" + } elseif {$speed >= $vetoRange(min) && $speed <= $vetoRange(max)} { + return -code error "Speed ($speed) is in the vetoed range: $vetoRange(min), $vetoRange(max)" + } + } + + proc Echeck_gainDir {gain dir} { + variable CW + variable CCW + variable PROPGAIN + variable INTGAIN + variable PHGAIN + + if {$gain != $INTGAIN && $gain != $PROPGAIN && $gain != $PHGAIN} { + return -code error "gain identifier ($gain) should be $PROPGAIN (PROPGAIN), $INTGAIN (INTGAIN), or $PHGAIN (PHGAIN)" + } + if {$dir != $CW && $dir != $CCW} { + return -code error "direction identifier ($dir) should be $CW {CW} or $CCW {CCW}" + } + } + + proc getVetoWin {ch speed} { + variable ch1_gains + variable ch2_gains + variable ch3_gains + + # Check argument and return error otherwise return veto window + set catch_status [ catch { + Echeck_ChSpeed $ch $speed + set speed [expr int($speed)] + set ch [expr int($ch)] + switch $ch { + 1 { set gainTable ch1_gains } + 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] + } + } + error "Failed when looking up veto window for speed $speed" + } message ] + handle_exception $catch_status $message + } + + proc getGain {gain ch speed dir} { + variable ch1_gains + variable ch2_gains + variable ch3_gains + variable CW + variable CCW + variable INTGAIN + variable PROPGAIN + variable PHGAIN + + set catch_status [ catch { + Echeck_ChSpeed $ch $speed + Echeck_gainDir $gain $dir + set gain [expr int($gain)] + set dir [expr int($dir)] + set speed [expr int($speed)] + set ch [expr int($ch)] + switch $ch { + 1 { set gainTable ch1_gains } + 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)] $gain $dir] + } + } + error "Failed when looking up integral gain for speed $speed" + } message ] + handle_exception $catch_status $message + } + + + ## + # 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; + # pDriv->GetStatus = TclStat; + # pDriv->GetError = TclError; + # pDriv->TryAndFixIt = TclFix; + # pDriv->SetDriverPar = TclSetPar; + # pDriv->GetDriverPar = TclGetPar; + # pDriv->Halt = TclHalt; + # pDriv->KillPrivate = KillTCL; + variable chPath "/instrument/fermi_chopper" + + proc getSpeed {ch} { + variable chPath + + set rSpeed [hval $chPath/$ch/rotation_speed] + return $rSpeed + } + + ## + # Implement speed and phase motor objects + proc SGetPos {hpath node addr name} { + global SCode + variable CW + variable CCW + variable POSDIR + + 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 + } else { + return -$speed + } + } + + proc 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 + return $SCode(OKOK) + } + + proc SStatus {hpath node addr name} { + global SCode + + return $SCode(HWIdle) + } + + proc SHalt {hpath node addr name} { + global SCode + } + + proc SGetError {hpath node addr name} { + global SCode + return "[info level 0]: TODO error message" + } + + proc SFixit {hpath node addr name icode fVal} { + global SCode + + return $SCode(MOTOK) + } + # Implementation of phase motor interface + proc PhGetPos {hpath node addr name} { + global SCode + return 0 + } + + proc PhRun {hpath node addr name target} { + global SCode + + return $SCode(OKOK) + } + + proc PhStatus {hpath node addr name} { + global SCode + + return $SCode(HWIdle) + } + + proc PhHalt {hpath node addr name} { + global SCode + } + + proc PhGetError {hpath node addr name} { + global SCode + return "[info level 0]: TODO error message" + } + + proc 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 + } + 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" + } + } +} + +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" + ] + $fmot hardlowerlim $lowlim + $fmot hardupperlim $uplim + $fmot softlowerlim $lowlim + $fmot softupperlim $uplim + 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" + ] + $fmot hardlowerlim $lowlim + $fmot hardupperlim $uplim + $fmot softlowerlim $lowlim + $fmot softupperlim $uplim + sicslist setatt $fmot units degrees + sicslist setatt $fmot klass fermi_chopper + sicslist setatt $fmot long_name $fmot +} + +## +# @brief Make the fermichopper driver and speed and phase motors for the master and +# slave choppers. +# +# @param mSpdmot, Name of master chopper speed motor +# @param sSpdmot, Name of slave chopper speed motor +# @param sPhmot, Name of slave chopper phase motor +# @param master, Master chopper hdb node name +# @param maddr, Master chopper modbus address +# @param slave, Slave chopper hdb node name +# @param saddr, Slave chopper modbus address +proc mkFermiMotors {mSpdmot sSpdmot sPhmot master maddr slave saddr} { + set hdbPath [mkChoppers " $master $maddr $slave $saddr"] + # Create Master Chopper Speed motor object + mkFSpeedMot $mSpdmot $hdbPath $master $maddr -24000 24000 + mkFSpeedMot $sSpdmot $hdbPath $slave $saddr -24000 24000 + mkFPhaseMot $sPhmot $hdbPath $slave $saddr 0 180 +} + +mkFermiMotors mchs schs schp mch 1 sch 2 +