fermimot.tcl
Create tclmot motors for the master chopper speed (mchs), slave chopper speed (schs) and slave chopper phase (schp). Currently mchs and schs just read rotation speed. TODO: Read phase and set speed and phase. r3748 | ffr | 2012-09-26 09:55:08 +1000 (Wed, 26 Sep 2012) | 6 lines
This commit is contained in:
Ferdi Franceschini
committed by
Douglas Clowes
Douglas Clowes
parent
0b9580f334
commit
b887636684
388
site_ansto/instrument/pelican/config/chopper/fermimot.tcl
Normal file
388
site_ansto/instrument/pelican/config/chopper/fermimot.tcl
Normal file
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user