839 lines
20 KiB
Tcl
839 lines
20 KiB
Tcl
# global bank 2 usage
|
|
# persistent:
|
|
# 0 zero
|
|
# 1 id
|
|
# 2 lower limit
|
|
# 3 upper limit
|
|
# 4 precision
|
|
# 6 feedback available
|
|
# 8 pump number
|
|
# 9 hysteresis
|
|
# non-persistent:
|
|
# 255 init (0/1)
|
|
|
|
namespace eval trinamic {} {
|
|
variable cmds
|
|
|
|
set cmds(motor_stop) 3
|
|
set cmds(move_to_pos) 4
|
|
set cmds(set_axis_par) 5
|
|
set cmds(get_axis_par) 6
|
|
set cmds(save_axis_par) 7
|
|
set cmds(load_axis_par) 8
|
|
set cmds(set_glob_par) 9
|
|
set cmds(get_glob_par) 10
|
|
set cmds(save_glob_par) 11
|
|
set cmds(load_glob_par) 12
|
|
set cmds(set_io) 14
|
|
set cmds(get_io) 15
|
|
|
|
# command numbers are translated 1:1
|
|
# foreach key [array names cmds] {
|
|
# set val $cmds($key)
|
|
# set cmds($val) $val
|
|
# }
|
|
}
|
|
|
|
proc stdConfig::trinamic {{usage undefined} {sensirion 0}} {
|
|
|
|
# bin <checksum-algorithm> <timeout> <number-of-retries>
|
|
controller bin chksum-crc 1 2
|
|
prop write trinamic::write
|
|
prop read trinamic::read
|
|
prop complete trinamic::complete
|
|
prop update trinamic::update
|
|
|
|
variable node
|
|
set node $node/tasks
|
|
set tasknode $node
|
|
prop start trinamic::startCmd
|
|
# prop startcmd "str1 n int 10 / str9"
|
|
|
|
pollperiod 5 5
|
|
|
|
set f [expr 256 / 1.8]
|
|
# set f 1
|
|
|
|
obj trinamic -drive out
|
|
prop status idle
|
|
prop check trinamic::setpos
|
|
prop write trinamic::runmsg
|
|
prop halt trinamic::halt
|
|
prop fact $f
|
|
prop usezero 1
|
|
prop cormode 0
|
|
prop @due 0
|
|
prop runlabel pos
|
|
hsetprop $tasknode objectnode $node
|
|
|
|
kids "trinamic stepper motor" {
|
|
|
|
node pos rd
|
|
prop adr axis_par 1
|
|
prop fact $f
|
|
prop usezero 1
|
|
prop update trinamic::updatePos
|
|
|
|
node encoder rd
|
|
prop adr axis_par 209
|
|
prop fact $f
|
|
prop usezero 1
|
|
|
|
node zero wr
|
|
prop fact $f
|
|
prop write trinamic::writezero
|
|
prop read trinamic::readzero
|
|
prop eeprom 1
|
|
prop adr glob_par 0 2
|
|
default -999
|
|
|
|
node lowerlimit wr
|
|
prop adr glob_par 2 2
|
|
prop eeprom 1
|
|
prop fact $f
|
|
|
|
node upperlimit wr
|
|
prop adr glob_par 3 2
|
|
prop eeprom 1
|
|
prop fact $f
|
|
|
|
node disablelimits par 0
|
|
prop enum 1
|
|
|
|
node verbose par 0
|
|
prop enum 1
|
|
|
|
node target rd
|
|
prop adr axis_par 0
|
|
prop fact $f
|
|
prop usezero 1
|
|
|
|
node runstate upd
|
|
prop enum idle,running,finished,error
|
|
|
|
# node targetreached rd 5 slow
|
|
# prop adr axis_par 8
|
|
# prop update trinamic::updateTargetReached
|
|
# prop enum idle,running,finished
|
|
|
|
node precision wr
|
|
default 0.9
|
|
prop fact $f
|
|
prop eeprom 1
|
|
prop adr glob_par 5 2
|
|
|
|
node maxencdif wr
|
|
default 1.8
|
|
prop fact $f
|
|
prop eeprom 1
|
|
prop adr axis_par 212
|
|
|
|
# node blindrun wr 0
|
|
# prop eeprom 1
|
|
# prop adr glob_par 8 2
|
|
# prop enum 1
|
|
|
|
node id wr 0
|
|
prop eeprom 1
|
|
prop adr glob_par 1 2
|
|
|
|
if {$usage eq "hepump"} {
|
|
node pump_number wr 0
|
|
prop eeprom 1
|
|
prop adr glob_par 8 2
|
|
}
|
|
|
|
node init wr 0
|
|
prop adr glob_par 255 2
|
|
|
|
# node currentspeed rd
|
|
# prop adr axis_par 3
|
|
# prop read trinamic::read_when_running
|
|
# prop update trinamic::updateCurrentSpeed
|
|
# prop running_time 0
|
|
# prop fast 0
|
|
|
|
node maxspeed wr
|
|
prop adr axis_par 4
|
|
prop eeprom 1
|
|
|
|
node acceleration wr
|
|
prop adr axis_par 5
|
|
prop eeprom 1
|
|
|
|
node maxcurrent wr
|
|
prop adr axis_par 6
|
|
prop eeprom 1
|
|
|
|
node standbycurrent wr
|
|
prop adr axis_par 7
|
|
prop eeprom 1
|
|
|
|
node freewheeling wr
|
|
prop adr axis_par 204
|
|
prop eeprom 1
|
|
prop enum 1
|
|
|
|
node output0 wr
|
|
prop adr io 0 2
|
|
prop enum 1
|
|
|
|
node output1 wr
|
|
prop adr io 1 2
|
|
prop enum 1
|
|
|
|
node input3 rd
|
|
prop adr io 3 0
|
|
prop enum 1
|
|
prop update trinamic::updateInput3
|
|
|
|
# node input0 rd
|
|
# prop adr io 0 1
|
|
# prop fact 30
|
|
# prop offset 1.1
|
|
# prop update trinamic::updateInput0
|
|
# prop last 0
|
|
# prop old 0
|
|
# prop perform_calib 0
|
|
|
|
# node input0raw upd
|
|
|
|
node pullup wr
|
|
prop adr io 0 0
|
|
|
|
if {$sensirion} {
|
|
node flow rd
|
|
prop read stdSct::read
|
|
prop update trinamic::updateFlow
|
|
prop readcmd "str1 ? / str18"
|
|
|
|
node flowstddev upd
|
|
|
|
# ?<nsample>_ nsample=160: <~= 1 sec, nsample=800: <~= 5 sec
|
|
node flowsamples -int out
|
|
prop write stdSct::write
|
|
prop complete trinamic::updatecalib
|
|
prop writecmd "str8 n%6dr / str18"
|
|
|
|
node flowoffset wr
|
|
prop check trinamic::checkCalib
|
|
prop read stdSct::read
|
|
prop write stdSct::write
|
|
prop update trinamic::updateCalib
|
|
prop complete trinamic::updateCalib
|
|
prop readcmd "str1 r / str18"
|
|
prop writecmd "str8 t%6.3fr / str18"
|
|
|
|
node flowscale out
|
|
prop check trinamic::checkCalib
|
|
prop write stdSct::write
|
|
prop complete trinamic::updateCalib
|
|
prop writecmd "str8 g%6.3fr / str18"
|
|
|
|
node flowsave out
|
|
prop enum 1
|
|
prop write trinamic::saveCalib
|
|
prop help "unchecked: current calib is not saved. set checked: save calib"
|
|
}
|
|
|
|
# node hysteresis wr
|
|
# prop fact $f
|
|
# prop eeprom 1
|
|
# prop adr glob_par 9 2
|
|
|
|
if {$usage eq "hepump"} {
|
|
node nopumpfeedback wr
|
|
prop eeprom 1
|
|
prop adr glob_par 6 2
|
|
prop enum 1
|
|
}
|
|
# node stopstatus rd
|
|
# prop adr axis_par 207
|
|
# prop update trinamic::updateStopstatus
|
|
|
|
node eeprom out
|
|
default 1
|
|
prop enum ok,dirty,save,load
|
|
prop write trinamic::eewrite
|
|
prop label "eeprom status"
|
|
|
|
node customadr out -text
|
|
default "axis_par 209"
|
|
prop check trinamic::setcustom
|
|
prop write stdSct::complete
|
|
|
|
node custompar wr
|
|
prop adr axis_par 209
|
|
|
|
}
|
|
variable ctrl
|
|
variable path
|
|
|
|
# load
|
|
# $ctrl queue $path start trinamic::init
|
|
|
|
# read zero first (global bank2 parameter 0)
|
|
$ctrl queue $path/zero progress trinamic::readzero
|
|
}
|
|
|
|
proc trinamic::updateFlow {} {
|
|
lassign [sct result] flow stddev
|
|
sct update $flow
|
|
updateval [sct parent]/flowstddev $stddev
|
|
return idle
|
|
}
|
|
|
|
proc trinamic::currentCalib {} {
|
|
return [format {%.4f %.4f} [sctval [sct parent]/flowoffset] [sctval [sct parent]/flowscale]]
|
|
}
|
|
|
|
proc trinamic::updateCalib {} {
|
|
lassign [sct result] offset scale
|
|
updateval [sct parent]/flowoffset $offset
|
|
updateval [sct parent]/flowscale $scale
|
|
set node [sct parent]/flowsave
|
|
if {[silent -1 hgetpropval $node saved] == -1} {
|
|
hsetprop $node saved [currentCalib]
|
|
}
|
|
return idle
|
|
}
|
|
|
|
proc trinamic::checkCalib {} {
|
|
if {[silent "" hgetpropval [sct parent]/flowsave saved] ne [currentCalib]} {
|
|
hupdate [sct parent]/flowsave 0
|
|
} else {
|
|
hupdate [sct parent]/flowsave 1
|
|
}
|
|
}
|
|
|
|
proc trinamic::saveCalib {} {
|
|
if {[sct target]} {
|
|
sct send "str2 sr / str18"
|
|
sct saved [currentCalib]
|
|
sct print "saved calib [sct saved]"
|
|
} else {
|
|
sct print "marked calib as dirty"
|
|
}
|
|
sct update [sct target]
|
|
return stdSct::complete
|
|
}
|
|
|
|
proc trinamic::send {cmd {nr 0} {bank 0} {value 0}} {
|
|
variable cmds
|
|
set cmd $cmds($cmd)
|
|
# sct send "int 1 $cmd $nr $bank int4 $value crc 0 / 2 1 100 $cmd int4 crc"
|
|
sct send "int 1 $cmd $nr $bank int4 $value crc 0 / 2 1 skip1 $cmd int4 crc"
|
|
}
|
|
|
|
proc trinamic::response {} {
|
|
if {[llength [sct result]] != 1} {
|
|
error "illegal result on [sct]: [sct result]"
|
|
}
|
|
return [string trim [sct result]]
|
|
}
|
|
|
|
proc trinamic::complete {} {
|
|
response
|
|
return idle
|
|
}
|
|
|
|
proc trinamic::logmsg {text} {
|
|
if {[hval [sct objectPath]/verbose]} {
|
|
clientput $text
|
|
}
|
|
}
|
|
|
|
proc trinamic::halt {} {
|
|
logmsg HALT
|
|
trinamic::endrun
|
|
send motor_stop
|
|
return trinamic::complete
|
|
}
|
|
|
|
proc trinamic::init {} {
|
|
# mark nodes for load
|
|
foreach var [hlist [sct]] {
|
|
if {[silent 1 hgetpropval [sct]/$var eeprom] != 1} {
|
|
hsetprop [sct]/$var eeprom 1
|
|
}
|
|
}
|
|
# load eeprom (with "halt" priority, higher than write)
|
|
[sct controller] queue [sct]/eeprom halt "trinamic::eeprom load"
|
|
hsetprop [sct]/encoder adjust_zero 1
|
|
# set pullup resistors on
|
|
send set_io 0 0 1
|
|
return trinamic::complete
|
|
}
|
|
|
|
proc trinamic::startCmd {} {
|
|
sct try_cnt 3
|
|
send get_glob_par 255 2
|
|
return trinamic::startCmd0
|
|
}
|
|
|
|
proc trinamic::startCmd0 {} {
|
|
if {[response] ne "1" && [response] ne "2"} {
|
|
if {[sct try_cnt] > 0} {
|
|
sct try_cnt [expr [sct try_cnt] - 1]
|
|
send get_glob_par 255 2
|
|
return trinamic::startCmd0
|
|
}
|
|
clientput "[sct] reboot detected"
|
|
send set_glob_par 255 2 1
|
|
return trinamic::startCmd1
|
|
}
|
|
send get_glob_par 1 2
|
|
return complete
|
|
}
|
|
proc trinamic::startCmd1 {} {
|
|
# clientput "start cmd1 [response]"
|
|
send get_glob_par 1 2
|
|
return trinamic::startUpd
|
|
}
|
|
|
|
proc trinamic::startUpd {} {
|
|
# clientput "start upd [response]"
|
|
[sct controller] queue [sct objectnode] start trinamic::init
|
|
return complete
|
|
}
|
|
|
|
proc trinamic::eewrite {} {
|
|
# mark nodes for load or save
|
|
foreach var [hlist [sct parent]] {
|
|
if {[silent 1 hgetpropval [sct parent]/$var eeprom] == 0} {
|
|
hsetprop [sct parent]/$var eeprom 1
|
|
}
|
|
}
|
|
switch [sct target] {
|
|
2 {
|
|
return "trinamic::eeprom save"
|
|
}
|
|
3 {
|
|
return "trinamic::eeprom load"
|
|
}
|
|
}
|
|
clientlog "nothing to do"
|
|
sct update [hvali [sct]]
|
|
return idle
|
|
}
|
|
|
|
proc trinamic::eeprom {saveorload} {
|
|
foreach var [hlist [sct parent]] {
|
|
if {[silent 0 hgetpropval [sct parent]/$var eeprom]} {
|
|
lassign "[hgetpropval [sct parent]/$var adr] 0 0" typ nr bank
|
|
if {$saveorload eq "load"} {
|
|
set cmd load_$typ
|
|
[sct controller] queue [sct parent]/$var progress read
|
|
} else {
|
|
set cmd save_$typ
|
|
}
|
|
# sct send "int 1 $cmd $nr $bank int4 0 crc / skip8 crc"
|
|
send $cmd $nr $bank
|
|
hsetprop [sct parent]/$var eeprom 0
|
|
# clientlog "$saveorload $var"
|
|
return "trinamic::eeprom $saveorload"
|
|
}
|
|
}
|
|
# set o.k.
|
|
sct update 0
|
|
# clientlog "finished eeprom $saveorload"
|
|
return idle
|
|
}
|
|
|
|
proc trinamic::write {} {
|
|
lassign "[sct adr] 0 0" typ nr bank
|
|
hupdate [sct parent]/eeprom 1
|
|
set val [expr round([sct target] * [silent 1 sct fact])]
|
|
# sct send "int 1 $cmd $nr $bank int4 $val crc / skip8 crc"
|
|
send set_$typ $nr $bank $val
|
|
set old [hvali [sct]]
|
|
if {$old != [sct target]} {
|
|
#clientput "SET [sct] $old -> [sct target]"
|
|
}
|
|
set trigger [silent "" sct trigger_path]
|
|
if {$trigger ne ""} {
|
|
[sct controller] queue $trigger read read
|
|
}
|
|
return read
|
|
}
|
|
|
|
proc trinamic::read {} {
|
|
if {[DoubleTime] < [sct @due]} {
|
|
return idle
|
|
}
|
|
lassign "[sct adr] 0 0" typ nr bank
|
|
# sct send "int 1 $cmd $nr $bank int4 0 crc / skip4 int4 crc"
|
|
send get_$typ $nr $bank
|
|
sct crctry 3
|
|
# clientlog [sct]
|
|
return update
|
|
}
|
|
|
|
proc trinamic::update {} {
|
|
set v [response]
|
|
set f [silent 1 sct fact]
|
|
set z 0
|
|
|
|
sct rawvalue $v
|
|
set vv [expr $v / double($f)]
|
|
if {[silent 0 sct usezero]} {
|
|
set z [silent -999 hval [sct objectPath]/zero]
|
|
if {$z == -999} {
|
|
error "undefined zero"
|
|
}
|
|
}
|
|
set vv [format %.2f [expr $vv + $z]]
|
|
|
|
if {[silent 0 sct adjust_zero]} {
|
|
#clientput "encoder $vv raw [expr $vv - $z]"
|
|
set uplim [hval [sct objectPath]/upperlimit]
|
|
set lolim [hval [sct objectPath]/lowerlimit]
|
|
set reserve [expr 360 - ($uplim - $lolim)]
|
|
if {$reserve > 0} {
|
|
set uplim [expr $uplim + $reserve * 0.5]
|
|
set lolim [expr $lolim - $reserve * 0.5]
|
|
}
|
|
#clientput "limits $lolim $uplim reserve $reserve"
|
|
set dz 0
|
|
if {$vv > $uplim && $vv - 360 <= $uplim && $vv - 360 >= $lolim} {
|
|
set dz -360
|
|
} elseif {$vv < $lolim && $vv + 360 >= $lolim && $vv + 360 <= $uplim} {
|
|
set dz 360
|
|
}
|
|
if {$dz != 0} {
|
|
clientput "adjust zero $z to [expr $z + $dz]"
|
|
set z [expr $z + $dz]
|
|
hset [sct objectPath]/zero $z
|
|
set vv [expr $vv + $dz]
|
|
}
|
|
sct adjust_zero 0
|
|
}
|
|
# switch [sct] {
|
|
# /nv/nvmot/pos - /nv/nvmot/target - /nv/nvmot/custompar {
|
|
# clientput "[expr fmod([DoubleTime],60)] [sct] $vv"
|
|
# }
|
|
# }
|
|
sct update $vv
|
|
return idle
|
|
}
|
|
|
|
proc trinamic::writezero {} {
|
|
set val [expr round([sct target] * [silent 1 sct fact])]
|
|
if {$val <= 0} {
|
|
incr val -1
|
|
}
|
|
if {[silent -1 sct eeprom] == 0} {
|
|
sct eeprom 1
|
|
hupdate [sct parent]/eeprom 1
|
|
}
|
|
# sct send "int 1 9 0 2 int4 $val crc / skip8 crc"
|
|
send set_glob_par 0 2 $val
|
|
#clientput "set zero [hvali [sct]] -> [sct target] $val"
|
|
return read
|
|
}
|
|
|
|
proc trinamic::readzero {} {
|
|
if {[DoubleTime] < [sct @due]} {
|
|
return idle
|
|
}
|
|
send get_glob_par 0 2
|
|
return trinamic::updatezero
|
|
}
|
|
|
|
proc trinamic::updatezero {} {
|
|
set v [response]
|
|
if {$v == 0} {
|
|
clientlog "zero undefined"
|
|
}
|
|
if {$v < 0} {
|
|
incr v
|
|
}
|
|
set v [format %.2f [expr $v / double([silent 1 sct fact])]]
|
|
# if {$v != 0} {
|
|
# clientput "zero $v [sct result]"
|
|
# }
|
|
sct update $v
|
|
return idle
|
|
}
|
|
|
|
proc trinamic::updatePos {} {
|
|
set res [trinamic::update]
|
|
updateval [sct parent] [hvali [sct]]
|
|
return $res
|
|
}
|
|
|
|
proc trinamic::setcustom {} {
|
|
hsetprop [sct parent]/custompar adr [sct target]
|
|
sct update [sct target]
|
|
[sct controller] queue [sct parent]/custompar read read
|
|
return idle
|
|
}
|
|
|
|
proc trinamic::setpos {} {
|
|
if {![hvali [sct]/disablelimits]} {
|
|
if {[sct target] > [hval [sct]/upperlimit]} {
|
|
error "ERROR: [sct target] is above upper limit"
|
|
}
|
|
if {[sct target] < [hval [sct]/lowerlimit]} {
|
|
error "ERROR: [sct target] is below lower limit"
|
|
}
|
|
}
|
|
sct goto [sct target]
|
|
set period 0
|
|
if {[sct status] eq "run"} {
|
|
set period 0.1
|
|
}
|
|
if {[silent 0 sct startime] == 0} {
|
|
sct startime [DoubleTime]
|
|
}
|
|
# if {[sct] eq "/nv/nvmot"} {
|
|
# debug nvmot
|
|
# }
|
|
[sct controller] poll [sct] $period progress trinamic::start
|
|
}
|
|
|
|
proc trinamic::start {} {
|
|
set now [DoubleTime]
|
|
if {[sct status] eq "run"} {
|
|
if {$now < [sct startime] + 60} {
|
|
return idle
|
|
}
|
|
clientlog "[sct] timeout when waiting for idle"
|
|
}
|
|
sct startime 0
|
|
sct adjustcnt 0
|
|
[sct controller] poll [sct] 0.1 progress trinamic::adjustPosNew
|
|
sct afteradjust trinamic::start1
|
|
# set idle mode
|
|
hupdate [sct]/runstate 0
|
|
sct status run
|
|
return unpoll
|
|
}
|
|
|
|
proc trinamic::calcdelay {} {
|
|
set s [hgetpropval [sct]/maxspeed actvalue]
|
|
set a [hgetpropval [sct]/acceleration actvalue]
|
|
set p [hgetpropval [sct]/pos rawvalue]
|
|
set t [sct moveto]
|
|
# calculate time: pulse_div 3 assumed, ramp_div 7 assumed"
|
|
set delay [expr abs($t - $p) / 30.5 / $s + $s / $a / 15.25 + 0.5]
|
|
return $delay
|
|
}
|
|
|
|
proc trinamic::adjustPosNew {} {
|
|
set adjustcnt [sct adjustcnt]
|
|
sct adjustcnt [expr $adjustcnt+1]
|
|
if {$adjustcnt % 10 == 0} {
|
|
# read encoder and pos before start
|
|
[sct controller] queue [sct]/pos progress trinamic::read
|
|
[sct controller] queue [sct]/encoder progress trinamic::read
|
|
return idle
|
|
}
|
|
if {$adjustcnt % 10 == 1} {
|
|
set enc [hgetpropval [sct]/encoder rawvalue]
|
|
set pos [hgetpropval [sct]/pos rawvalue]
|
|
if {abs($enc - $pos) < 256} {
|
|
[sct controller] queue [sct] progress [sct afteradjust]
|
|
return unpoll
|
|
}
|
|
send set_axis_par 1 0 $enc
|
|
logmsg "[sct]: correct pos [hvali [sct]/pos] to enc [hvali [sct]/encoder]"
|
|
return "trinamic::adjustPosNew1 $enc"
|
|
}
|
|
return idle
|
|
}
|
|
|
|
proc trinamic::adjustPosNew1 {target} {
|
|
send set_axis_par 0 0 $target
|
|
return "trinamic::complete"
|
|
}
|
|
|
|
proc trinamic::adjustPos {next} {
|
|
set e [hval [sct]/encoder]
|
|
set p [hval [sct]/pos]
|
|
set dif [format %.2f [expr $e - $p]]
|
|
set prec [hvali [sct]/precision]
|
|
if {abs($dif) <= $prec && abs($dif) <= [hvali [sct]/maxencdif] * 0.9} {
|
|
return $next
|
|
}
|
|
temp_value maxcurrent 0
|
|
temp_value maxspeed 2047
|
|
temp_value acceleration 2047
|
|
temp_value maxencdif 0
|
|
sct moveto [hgetpropval [sct]/encoder rawvalue]
|
|
# if {abs($dif) > [hvali [sct]/maxencdif]} {
|
|
# clientput "[sct] correct motor position by $dif"
|
|
# }
|
|
logmsg "[sct] correct motor position by $dif"
|
|
sct delay [trinamic::calcdelay]
|
|
sct afterrun $next
|
|
return trinamic::setrunpars
|
|
}
|
|
|
|
proc trinamic::setrunpars {} {
|
|
foreach var [hlist [sct]] {
|
|
set val [silent none hgetpropval [sct]/$var actvalue]
|
|
if {$val ne "none"} {
|
|
set f [silent 1 hgetpropval [sct]/$var fact]
|
|
set val [expr round($val * $f)]
|
|
lassign "[hgetpropval [sct]/$var adr] 0 0" typ nr bank
|
|
send set_$typ $nr $bank $val
|
|
hsetprop [sct]/$var actvalue none
|
|
return trinamic::setrunpars
|
|
}
|
|
}
|
|
sct @due [expr [DoubleTime] + [sct delay]]
|
|
send move_to_pos 0 0 [sct moveto]
|
|
[sct controller] poll [sct] 0.1 progress "trinamic::wait"
|
|
return trinamic::complete
|
|
}
|
|
|
|
proc trinamic::wait {} {
|
|
set now [DoubleTime]
|
|
if {$now < [sct @due]} {
|
|
return idle
|
|
}
|
|
[sct controller] queue [sct]/pos progress trinamic::read
|
|
[sct controller] queue [sct]/encoder progress trinamic::read
|
|
sct finitime $now
|
|
[sct controller] queue [sct] progress trinamic::fini
|
|
return unpoll
|
|
}
|
|
|
|
proc trinamic::fini {} {
|
|
# get target reached
|
|
send get_axis_par 8
|
|
return trinamic::targetReached
|
|
}
|
|
|
|
proc trinamic::targetReached {} {
|
|
set r [response]
|
|
#clientput "targetreached [expr fmod([DoubleTime],60)] $r"
|
|
if {$r == 1} {
|
|
updateval [sct]/runstate 2 ;# finished
|
|
# get stop status (for clearing only)
|
|
send get_axis_par 207
|
|
return [sct afterrun]
|
|
}
|
|
if {$r != 0} {
|
|
error "[sct] illegal value for target reached: $r"
|
|
}
|
|
# get stop status
|
|
send get_axis_par 207
|
|
return trinamic::stopStatus
|
|
}
|
|
|
|
proc trinamic::stopStatus {} {
|
|
set r [response]
|
|
if {$r > 0 && $r <= 3} {
|
|
logmsg "stopped with error $r"
|
|
updateval [sct]/runstate 3
|
|
return [sct afterrun]
|
|
}
|
|
if {$r == 0} {
|
|
if {[DoubleTime] < [sct finitime] + 2} {
|
|
return trinamic::fini
|
|
}
|
|
updateval [sct]/runstate 3
|
|
clientlog "[sct] run timeout [sct delay] [expr [DoubleTime] - [sct @due]]"
|
|
return [sct afterrun]
|
|
}
|
|
error "[sct] illegal stopstatus: $r"
|
|
}
|
|
|
|
proc trinamic::runmsg {} {
|
|
# sct print "run [sct objectName] to [sct target]"
|
|
return idle
|
|
}
|
|
|
|
proc trinamic::start1 {} {
|
|
normal_value maxcurrent
|
|
normal_value maxspeed
|
|
normal_value acceleration
|
|
normal_value maxencdif
|
|
sct enc0 [hvali [sct]/encoder]
|
|
set val [sct goto]
|
|
set f [silent 1 sct fact]
|
|
set val [expr round(($val - [hval [sct objectPath]/zero]) * $f)]
|
|
sct moveto $val
|
|
sct delay [trinamic::calcdelay]
|
|
# sct afterrun "trinamic::adjustPos trinamic::endrun"
|
|
sct afterrun "trinamic::endrun"
|
|
return trinamic::setrunpars
|
|
}
|
|
|
|
proc trinamic::temp_value {var badval} {
|
|
set val [sctval [sct]/$var]
|
|
if {$val != $badval} {
|
|
hsetprop [sct]/$var restorevalue $val
|
|
}
|
|
hsetprop [sct]/$var actvalue $badval
|
|
}
|
|
|
|
proc trinamic::normal_value {var} {
|
|
set val [sctval [sct]/$var]
|
|
hsetprop [sct]/$var actvalue [silent $val hgetpropval [sct]/$var restorevalue]
|
|
}
|
|
|
|
proc trinamic::endrun {} {
|
|
if {[sct] eq "/hepump" && [result _hepump debug] >= 0} {
|
|
debug off
|
|
}
|
|
set enc [hvali [sct]/encoder]
|
|
set dif [expr abs($enc - [sct goto])]
|
|
set prec [hvali [sct]/precision]
|
|
if {[hvali [sct]/runstate] == 3 && $dif > $prec
|
|
&& $dif <= abs([sct enc0] - [sct goto]) - $prec} {
|
|
logmsg "[sct] try again enc=$enc goto=[sct goto]"
|
|
return trinamic::start1
|
|
}
|
|
sct status idle
|
|
foreach var [hlist [sct]] {
|
|
set val [silent none hgetpropval [sct]/$var restorevalue]
|
|
if {$val ne "none"} {
|
|
hset [sct]/$var $val
|
|
hdelprop [sct]/$var restorevalue
|
|
}
|
|
}
|
|
# make sure input3 (endswitch) is read immediately
|
|
send get_io 3 0
|
|
return "trinamic::updateInput3 ismainpath"
|
|
}
|
|
|
|
proc trinamic::updateInput3 {{ismainpath 0}} {
|
|
if {$ismainpath eq "ismainpath"} {
|
|
set path [sct]
|
|
} else {
|
|
set path [sct parent]
|
|
}
|
|
if {[silent 0 hvali $path/nopumpfeedback] == 1} {
|
|
# assume that pump is on when output0 is 1
|
|
updateval $path/input3 [expr ![hval $path/output0]]
|
|
} else {
|
|
updateval $path/input3 [response]
|
|
}
|
|
return idle
|
|
}
|
|
|
|
proc trinamic::updateInput0 {} {
|
|
# input0: 12 bit (0...4095) for 0-10 V
|
|
# make an average over 0.5 sec (about 5 readings)
|
|
set v [response]
|
|
sct sum [expr [silent 0 sct sum] + $v]
|
|
sct cnt [expr [silent 0 sct cnt] + 1]
|
|
set now [DoubleTime]
|
|
if {[sct cnt] < 5 && $now < [sct last] + 1} {
|
|
return idle
|
|
}
|
|
set v [expr double([sct sum]) / [sct cnt]]
|
|
set volt [expr $v / 409.6]
|
|
set scaled [expr ($volt - [sct offset]) * [sct fact]]
|
|
hupdate [sct parent]/input0raw $volt
|
|
sct update $scaled
|
|
sct cnt 0
|
|
sct sum 0
|
|
sct last $now
|
|
return idle
|
|
}
|
|
|