b887636684
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
389 lines
11 KiB
Tcl
389 lines
11 KiB
Tcl
# 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
|
|
|