diff --git a/site_ansto/instrument/pelican/config/chopper/sct_fermichopper.tcl b/site_ansto/instrument/pelican/config/chopper/sct_fermichopper.tcl index 7abcf1b7..0b7963e3 100644 --- a/site_ansto/instrument/pelican/config/chopper/sct_fermichopper.tcl +++ b/site_ansto/instrument/pelican/config/chopper/sct_fermichopper.tcl @@ -1,249 +1,236 @@ namespace eval ::scobj::chopper { -# MakeSICSObj disk_chopper SCT_OBJECT user int -# sicslist setatt disk_chopper klass NXdisk_chopper -# sicslist setatt disk_chopper long_name disk_chopper + # MakeSICSObj disk_chopper SCT_OBJECT user int + # sicslist setatt disk_chopper klass NXdisk_chopper + # sicslist setatt disk_chopper long_name disk_chopper -proc sndMBquery {rdScript cmd nextReq} { - set chN [sct chopper] - sct nextSndReq $nextReq - sct send "$chN:$cmd" - return $rdScript -} + proc sndMBquery {rdScript cmd nextReq} { + set chN [ lindex [sct P_chAddress] [sct P_chIndex] ] + sct nextSndReq $nextReq + sct send "$chN:$cmd" + return $rdScript + } -proc sndMBset {root cmd} { - set val [sct target] - #TODO convert val to appropriate units, if [convfact] then val = val * [sct convfact] - sct send "$cmd:$val" - sct target 0 - return RDSETREPLY -} -proc rdStatus {root statnode fields} { - set currCh [sct chopper] - set chPath $root/ch$currCh/$statnode - set oldval [hgetpropval $chPath oldval] - set val [sct result] - if {[string match "ASCERR:*" $val]} { - sct geterror $val - return idle + proc sndMBset {root cmd} { + set val [sct target] + #TODO convert val to appropriate units, if [convfact] then val = val * [sct convfact] + sct send "$cmd:$val" + sct target 0 + return RDSETREPLY } - if {$val != $oldval} { - hsetprop $chPath oldval $val - binary scan [binary format c $val] B* binNum - set bitfield [split $binNum ""] - foreach n $fields b $bitfield { - hset $chPath/$n $b - } - } - return [sct nextSndReq] -} -proc rdSysStatus {root nodes} { -# Read status byte and set nodes for each bit - set currCh [sct chopper] - set chPath $root/ch$currCh/system_status - set oldval [hgetpropval $chPath oldval] - set val [sct result] - if {[string match "ASCERR:*" $val]} { - sct geterror $val - return idle - } - if {$val != $oldval} { - hsetprop $chPath oldval $val - binary scan [binary format c $val] B* binNum - set bitfield [split $binNum ""] - binary scan [binary format c $oldval] B* oldbinNum - set oldbitfield [split $oldbinNum ""] - foreach n $nodes b $bitfield ob $oldbitfield { - if {$b != $ob} { - hset $chPath/$n $b - } - } - } - return [sct nextSndReq] -} -proc rdIntLkStatus {root nodes} { -# Read status byte and set nodes for each bit - set currCh [sct chopper] - set chPath $root/ch$currCh/intlck_status - set oldval [hgetpropval $chPath oldval] - set val [sct result] - if {[string match "ASCERR:*" $val]} { - sct geterror $val - return idle - } - if {$val != $oldval} { - hsetprop $chPath oldval $val - binary scan [binary format c $val] B* binNum - set bitfield [split $binNum ""] - binary scan [binary format c $oldval] B* oldbinNum - set oldbitfield [split $oldbinNum ""] - foreach n $nodes b $bitfield ob $oldbitfield { - if {$b != $ob} { - hset $chPath/$n $b - } - } - } - return [sct nextSndReq] -} -## -# Reads a list of values in the result and assigns them to the nodes in the args list -proc rdVal {root nodes} { - set currCh [sct chopper] - set values [sct result] - set chPath $root/ch$currCh - if {[string match "ASCERR:*" $values]} { - sct geterror $values - hset $chPath/device_error $values - return idle - } - if {[hval $chPath/device_error] != ""} { - hset $chPath/device_error "" - } - foreach n $nodes v $values { - set oldval [hval $chPath/$n] -# if {$n == "speed_setpt"} { -# broadcast rdVal,$n $chPath/$n: oldval=$oldval, v=$v -# } - if {$v != $oldval} { -# if {$n == "speed_setpt"} { -# broadcast rdVal,$n: hset $chPath/$n $v -# } - hset $chPath/$n $v - } - } - set nextReq [sct nextSndReq] - if {$nextReq == "NXTCHOPPER"} { - if {$currCh < [sct chMax] } { - incr currCh - sct chopper $currCh - } else { - sct chopper 1 + proc rdStatus {root statnode fields} { + set currCh [ lindex [sct P_name] [sct P_chIndex] ] + set chPath $root/$currCh/$statnode + set oldval [hgetpropval $chPath P_oldval] + set val [sct result] + if {[string match "ASCERR:*" $val]} { + sct geterror $val return idle } + if {$val != $oldval} { + sct update 1 + hsetprop $chPath P_oldval $val + binary scan [binary format c $val] B* binNum + set bitfield [split $binNum ""] + foreach n $fields b $bitfield { + hset $chPath/$n $b + } + } + return [sct nextSndReq] } - return $nextReq -} -proc rdSetCmdReply {root chPath} { - set reply [sct result] - if {[string match "ASCERR:*" $reply]} { - sct geterror $reply - hset $chPath/device_error $reply + proc rdCoil {root node coil} { + set chIndex [sct P_chIndex] + set currCh [ lindex [sct P_name] $chIndex ] + set val [sct result] + set cAddr [expr $coil - 1] + set coilPath $root/$currCh/$node + + set oldval [hgetpropval $coilPath P_oldval] + if {$val != $oldval} { + hsetprop $coilPath P_oldval $val + } + return [sct nextSndReq] + } + ## + # Reads a list of values in the result and assigns them to the nodes in the args list + proc rdVal {root nodes} { + set chIndex [sct P_chIndex] + set currCh [ lindex [sct P_name] $chIndex ] + set values [sct result] + set chPath $root/$currCh + if {[string match "ASCERR:*" $values]} { + sct geterror $values + hset $chPath/device_error $values + return idle + } + if {[hval $chPath/device_error] != ""} { + hset $chPath/device_error "" + } + foreach n $nodes v $values { + set oldval [hval $chPath/$n] + if {$v != $oldval} { + hset $chPath/$n $v + } + } + set nextReq [sct nextSndReq] + if {$nextReq == "NXTCHOPPER"} { + incr chIndex + if {$chIndex < [sct P_numChops] } { + sct P_chIndex $chIndex + } else { + sct P_chIndex 0 + return idle + } + } + return $nextReq + } + + proc rdSetCmdReply {root chPath} { + set reply [sct result] + if {[string match "ASCERR:*" $reply]} { + sct geterror $reply + hset $chPath/device_error $reply + return idle + } + if {[hval $chPath/device_error] != ""} { + hset $chPath/device_error "" + } + sct_fermi queue $root progress read return idle } - if {[hval $chPath/device_error] != ""} { - hset $chPath/device_error "" - } - sct_fermi queue $root progress read - return idle -} -# Create chopper control -set scobjNS ::scobj::chopper -set sim_mode false -set pollrate 5 -MakeSICSObj fermi_chopper SCT_OBJECT user int -sicslist setatt fermi_chopper klass NXfermi_chopper -set fermiPath /sics/fermi_chopper -::scobj::hinitprops fermi_chopper - if {$sim_mode == "false"} { - makesctcontroller sct_fermi tcpmodbus 137.157.202.213:502 - } - hsetprop $fermiPath chopper 1 - set intlck_fields {test_mode cc_shutdown_req dsp_summ_shtdwn cooling_loss spd_sensor_loss ref_sig_loss over_temp vac_fail overspeed_or_brakefail cc_wd_fail ext_fault ups_fail emerg_stop pos_alarm osc_fail dsp_wd_fail} - -set chMax 1 - hsetprop $fermiPath chMax $chMax -for {set chN 1} {$chN <= $chMax} {incr chN} { -set chname ch$chN - sicslist setatt fermi_chopper klass NXfermi_chopper - sicslist setatt fermi_chopper long_name fermi_chopper - hfactory $fermiPath/$chname plain user float - set chPath $fermiPath/$chname - hfactory $chPath/device_error plain user text - - foreach field { - system_status intlck_status rotation_speed - phase_veto_count phase_nonveto_count phase_acc - phase_rep phase_ok vetowin100ns - vetowin50ns mode speed_setpt - prop_gain int_gain phase_gain - ref_delay ref_period sync_srce motdir idle_toggle - } { - hfactory $fermiPath/$chname/$field plain user float - } - hsetprop $chPath/system_status oldval "UNKNOWN" - hsetprop $chPath/intlck_status oldval "UNKNOWN" - - foreach field { - avc_on motdir phase_locked lev_complete alarm run up_to_speed ok - } { - hfactory $fermiPath/$chname/system_status/$field plain user int - } - foreach field $intlck_fields { - hfactory $fermiPath/$chname/intlck_status/$field plain user int - } - - - hfactory $chPath/control plain user none - hfactory $chPath/control/device_error plain user text - - foreach {n cmd} [subst -nocommands { - set_vetowin100 "$chN:16:30:1:U32" - set_vetowin50 "$chN:16:32:1:U32" - set_rotspeed "$chN:16:1000:1:U32" - set_prop_gain "$chN:16:1004:1:F32" - set_int_gain "$chN:16:1006:1:F32" - set_phase_gain "$chN:16:1008:1:F32" - set_ref_delay "$chN:16:1010:1:U32" - set_ref_period "$chN:16:1012:1:U32" - set_sync_source "$chN:16:1014:1:U32" - set_motor_dir "$chN:16:1016:1:U32" - start "$chN:5:1" - stop "$chN:5:2" - idle_toggle "$chN:5:3" - reset "$chN:5:4" - }] { - hfactory $chPath/control/$n plain user float - hsetprop $chPath/control/$n write ${scobjNS}::sndMBset $chPath $cmd - hsetprop $chPath/control/$n RDSETREPLY ${scobjNS}::rdSetCmdReply $fermiPath $chPath/control - if {$sim_mode == "false"} { - sct_fermi write $chPath/control/$n + ## + # @brief Make fermi chopper driver + # + # @param nm_addr_list, list of chopper names and modbus addresses eg mkChoppers {mch 1 sch 2} + proc mkChoppers {nm_addr_list} { + set scobjNS ::scobj::chopper + set pollrate 5 + foreach {n a} $nm_addr_list { + lappend chopperName $n + lappend chAddress $a } - } - } -# Each Req chains to a rdVal proc which chains to the next req -# Add a next state property - #TODO Read idle_toggle status - hsetprop $fermiPath nextSndReq "UNKNOWN" - hsetprop $fermiPath read ${scobjNS}::sndMBquery "RDSYSSTAT" "3:10:1:U16" "INTLKREQ" - hsetprop $fermiPath "NXTCHOPPER" ${scobjNS}::sndMBquery "RDSYSSTAT" "3:10:1:U16" "INTLKREQ" - hsetprop $fermiPath "INTLKREQ" ${scobjNS}::sndMBquery "RDINTLKSTAT" "3:12:1:U16" "ROTSPDREQ" - hsetprop $fermiPath "ROTSPDREQ" ${scobjNS}::sndMBquery "RDROTSPD" "3:14:1:U16" "VETOINF" - hsetprop $fermiPath "VETOINF" ${scobjNS}::sndMBquery "RDVETOINF" "3:18:2:U32" "PHASEINF" - hsetprop $fermiPath "PHASEINF" ${scobjNS}::sndMBquery "RDPHASEINF" "3:24:3:F32" "VETO" - hsetprop $fermiPath "VETO" ${scobjNS}::sndMBquery "RDVETO" "3:30:3:U32" "ROTSPSET" - hsetprop $fermiPath "ROTSPSET" ${scobjNS}::sndMBquery "RDROTSPSET" "3:1000:1:U32" "GAINPHASE" - hsetprop $fermiPath "GAINPHASE" ${scobjNS}::sndMBquery "RDGAINPHASE" "3:1004:3:F32" "SYNMOTDIR" - hsetprop $fermiPath "SYNMOTDIR" ${scobjNS}::sndMBquery "RDSYNMOTDIR" "3:1010:4:U32" "IDLETOGGLE" - hsetprop $fermiPath "IDLETOGGLE" ${scobjNS}::sndMBquery "RDIDLE" "1:3:1" "NXTCHOPPER" + set numChops [llength $chAddress] + + MakeSICSObj fermi_chopper SCT_OBJECT + sicslist setatt fermi_chopper klass NXfermi_chopper + variable fermiPath + set fermiPath /sics/fermi_chopper + ::scobj::hinitprops fermi_chopper + set sim_mode false + if {$sim_mode == "false"} { + makesctcontroller sct_fermi tcpmodbus 137.157.202.213:502 + } + hsetprop $fermiPath P_chIndex 0 + set intlck_fields {test_mode cc_shutdown_req dsp_summ_shtdwn cooling_loss spd_sensor_loss ref_sig_loss over_temp vac_fail overspeed_or_breakfail cc_wd_fail ext_fault ups_fail emerg_stop pos_alarm osc_fail dsp_wd_fail} + + hsetprop $fermiPath P_numChops $numChops + hsetprop $fermiPath P_chAddress $chAddress + hsetprop $fermiPath P_name $chopperName + variable fermiPath + foreach {chname chN} $nm_addr_list { + # set chname [lindex $chopperName [expr $chN - 1]] + sicslist setatt fermi_chopper klass NXfermi_chopper + sicslist setatt fermi_chopper long_name fermi_chopper + hfactory $fermiPath/$chname plain user none + set chPath $fermiPath/$chname + hfactory $chPath/device_error plain user text + + foreach field { + rotation_speed phase_veto_count phase_nonveto_count phase_acc phase_rep + phase_ok vetowin100ns vetowin50ns mode speed_setpt + prop_gain int_gain phase_gain ref_delay ref_period + sync_srce motdir + } { + hfactory $fermiPath/$chname/$field plain user float + } + + hfactory $fermiPath/$chname/idle_toggle plain user int + hsetprop $chPath/idle_toggle P_oldval "UNKNOWN" + + hfactory $fermiPath/$chname/system_status plain user none + hsetprop $chPath/system_status P_oldval "UNKNOWN" + foreach field { + avc_on motdir phase_locked lev_complete alarm run up_to_speed ok + } { + hfactory $fermiPath/$chname/system_status/$field plain user int + } + + hfactory $fermiPath/$chname/intlck_status plain user none + hsetprop $chPath/intlck_status P_oldval "UNKNOWN" + foreach field $intlck_fields { + hfactory $fermiPath/$chname/intlck_status/$field plain user int + } + + + hfactory $chPath/control plain user none + hfactory $chPath/control/device_error plain user text + + foreach {n cmd} [subst -nocommands { + set_vetowin100 "$chN:16:30:1:U32" + set_vetowin50 "$chN:16:32:1:U32" + set_rotspeed "$chN:16:1000:1:U32" + set_prop_gain "$chN:16:1004:1:F32" + set_int_gain "$chN:16:1006:1:F32" + set_phase_gain "$chN:16:1008:1:F32" + set_ref_delay "$chN:16:1010:1:U32" + set_ref_period "$chN:16:1012:1:U32" + set_sync_source "$chN:16:1014:1:U32" + set_motor_dir "$chN:16:1016:1:U32" + start "$chN:5:1" + stop "$chN:5:2" + idle_toggle "$chN:5:3" + reset "$chN:5:4" + }] { + hfactory $chPath/control/$n plain user float + hsetprop $chPath/control/$n write ${scobjNS}::sndMBset $chPath $cmd + hsetprop $chPath/control/$n RDSETREPLY ${scobjNS}::rdSetCmdReply $fermiPath $chPath/control + if {$sim_mode == "false"} { + sct_fermi write $chPath/control/$n + } + } + } + + # Each Req chains to a rdVal proc which chains to the next req + # Add a next state property + #TODO Read idle_toggle status + hsetprop $fermiPath nextSndReq "UNKNOWN" + hsetprop $fermiPath read ${scobjNS}::sndMBquery "RDSYSSTAT" "3:10:1:U16" "INTLKREQ" + hsetprop $fermiPath "NXTCHOPPER" ${scobjNS}::sndMBquery "RDSYSSTAT" "3:10:1:U16" "INTLKREQ" + hsetprop $fermiPath "INTLKREQ" ${scobjNS}::sndMBquery "RDINTLKSTAT" "3:12:1:U16" "IDLETOGGLE" + hsetprop $fermiPath "IDLETOGGLE" ${scobjNS}::sndMBquery "RDIDLE" "1:3:1" "VETOINF" + hsetprop $fermiPath "VETOINF" ${scobjNS}::sndMBquery "RDVETOINF" "3:18:2:U32" "PHASEINF" + hsetprop $fermiPath "PHASEINF" ${scobjNS}::sndMBquery "RDPHASEINF" "3:24:3:F32" "VETO" + hsetprop $fermiPath "VETO" ${scobjNS}::sndMBquery "RDVETO" "3:30:3:U32" "ROTSPSET" + hsetprop $fermiPath "ROTSPSET" ${scobjNS}::sndMBquery "RDROTSPSET" "3:1000:1:U32" "GAINPHASE" + hsetprop $fermiPath "GAINPHASE" ${scobjNS}::sndMBquery "RDGAINPHASE" "3:1004:3:F32" "SYNMOTDIR" + hsetprop $fermiPath "SYNMOTDIR" ${scobjNS}::sndMBquery "RDSYNMOTDIR" "3:1010:4:U32" "ROTSPDREQ" + hsetprop $fermiPath "ROTSPDREQ" ${scobjNS}::sndMBquery "RDROTSPD" "3:14:1:U16" "NXTCHOPPER" - hsetprop $fermiPath "RDSYSSTAT" ${scobjNS}::rdStatus $fermiPath system_status {avc_on motdir phase_locked lev_complete alarm run up_to_speed ok} - hsetprop $fermiPath "RDINTLKSTAT" ${scobjNS}::rdStatus $fermiPath intlck_status $intlck_fields - hsetprop $fermiPath "RDROTSPD" ${scobjNS}::rdVal $fermiPath rotation_speed - hsetprop $fermiPath "RDVETOINF" ${scobjNS}::rdVal $fermiPath {phase_veto_count phase_nonveto_count} - hsetprop $fermiPath "RDPHASEINF" ${scobjNS}::rdVal $fermiPath {phase_acc phase_rep phase_ok} - hsetprop $fermiPath "RDVETO" ${scobjNS}::rdVal $fermiPath {vetowin100ns vetowin50ns mode} - hsetprop $fermiPath "RDROTSPSET" ${scobjNS}::rdVal $fermiPath speed_setpt - hsetprop $fermiPath "RDGAINPHASE" ${scobjNS}::rdVal $fermiPath {prop_gain int_gain phase_gain} - hsetprop $fermiPath "RDSYNMOTDIR" ${scobjNS}::rdVal $fermiPath {ref_delay ref_period sync_srce motdir} - hsetprop $fermiPath "RDIDLE" ${scobjNS}::rdVal $fermiPath idle_toggle - + hsetprop $fermiPath "RDSYSSTAT" ${scobjNS}::rdStatus $fermiPath system_status {avc_on motdir phase_locked lev_complete alarm run up_to_speed ok} + hsetprop $fermiPath "RDINTLKSTAT" ${scobjNS}::rdStatus $fermiPath intlck_status $intlck_fields + hsetprop $fermiPath "RDROTSPD" ${scobjNS}::rdVal $fermiPath rotation_speed + hsetprop $fermiPath "RDIDLE" ${scobjNS}::rdCoil $fermiPath idle_toggle 3 + hsetprop $fermiPath "RDPHASEINF" ${scobjNS}::rdVal $fermiPath {phase_acc phase_rep phase_ok} + hsetprop $fermiPath "RDVETO" ${scobjNS}::rdVal $fermiPath {vetowin100ns vetowin50ns mode} + hsetprop $fermiPath "RDROTSPSET" ${scobjNS}::rdVal $fermiPath speed_setpt + hsetprop $fermiPath "RDGAINPHASE" ${scobjNS}::rdVal $fermiPath {prop_gain int_gain phase_gain} + hsetprop $fermiPath "RDSYNMOTDIR" ${scobjNS}::rdVal $fermiPath {ref_delay ref_period sync_srce motdir} + hsetprop $fermiPath "RDVETOINF" ${scobjNS}::rdVal $fermiPath {phase_veto_count phase_nonveto_count} + - if {$sim_mode == "false"} { - sct_fermi poll $fermiPath $pollrate - sct_fermi queue $fermiPath progress read + if {$sim_mode == "false"} { + sct_fermi poll $fermiPath $pollrate + sct_fermi queue $fermiPath progress read + } + return $fermiPath } + namespace export mkChoppers } +namespace import ::scobj::chopper::* +# mkChoppers { mch 1 sch 2 } + +