136 lines
3.6 KiB
Tcl
136 lines
3.6 KiB
Tcl
# $Revision: 1.8 $
|
|
# $Date: 2008-05-30 00:26:54 $
|
|
# Author: Ferdi Franceschini (ffr@ansto.gov.au)
|
|
# Last revision by: $Author: ffr $
|
|
|
|
# Requires a configuration array for each axis that you want to simulate.
|
|
# eg
|
|
#array set B [list AC 25000 TP 7827107 TD 25000 PA 25000 stepsperx 25000 cntsperx 8192 DC 25000 SP 25000]
|
|
# The mkSimAxes.tcl script creates files with arrays like the example above.
|
|
|
|
# Substitutes position values for _TDx and _TPx
|
|
# then evaluates the expression.
|
|
proc evaluate {args} {
|
|
regsub -all {_T[DP][ABCDEFGH]} $args {[eval [parse &]]} expression;
|
|
regsub -all {\d+(?:\.\d*)?} $expression { double(&) } expression;
|
|
return [uplevel #0 expr $expression];
|
|
}
|
|
|
|
# Evaluates a comma separated list of commands and
|
|
# returns a space separated list of values.
|
|
proc evalArgList {args} {
|
|
foreach cmd [split $args ,] {lappend values [eval [parse $cmd]]}
|
|
return $values;
|
|
}
|
|
|
|
proc dmset {cmd axis args} {
|
|
set num [scan $args {%d} val];
|
|
if {$num == 1} {
|
|
set val $args;
|
|
} else {
|
|
set val [evaluate $args]
|
|
}
|
|
if {[string first $cmd "PA SP AC DC"] != -1} {
|
|
uplevel #0 set ${axis}($cmd) $val;
|
|
} else {
|
|
uplevel #0 $cmd $axis $val;
|
|
}
|
|
}
|
|
|
|
proc dmget {cmd axis} {
|
|
uplevel #0 set ${axis}($cmd)
|
|
}
|
|
|
|
proc dmcall {cmd paxis} {
|
|
uplevel #0 eval $cmd $paxis
|
|
}
|
|
|
|
proc DP {axis val} {
|
|
uplevel #0 eval set ${axis}(TD) $val
|
|
}
|
|
proc TS {axis} {
|
|
uplevel #0 eval set ${axis}(TS)
|
|
}
|
|
proc ST {axis} {
|
|
uplevel #0 eval set ${axis}(ST) 1
|
|
}
|
|
|
|
proc SH {args} {}
|
|
proc MO {args} {}
|
|
proc LV {args} {return "FRED=1\nBARNEY=2"}
|
|
proc TI {args} {return 240}
|
|
proc XQ {args} {return 1}
|
|
|
|
proc BG {_axis} {
|
|
upvar #0 $_axis axis;
|
|
set axis(TS) 140; # moving, limit switches open
|
|
set axis(BG) 1; # motor is moving
|
|
set axis(SC) 0; # motor is running
|
|
set timeStep 0.1; # seconds
|
|
set target $axis(PA);
|
|
set diff [expr $target - $axis(TD)];
|
|
set sign [expr ($axis(PA) - $axis(TD)) < 0 ? -1 : 1];
|
|
set step [expr $sign * $timeStep * $axis(SP) ];
|
|
if {[expr abs($diff) < abs($step)]} {
|
|
set step $diff;
|
|
set timeStep [expr abs($step / $axis(SP))];
|
|
}
|
|
every [expr round($timeStep * 1000)] "nextstep $_axis $step $target"
|
|
|
|
# set diff [expr $target - $axis(TD)];
|
|
# set mult [expr $axis(cntsperx).0/$axis(stepsperx)];
|
|
# set axis(TP) [expr round($diff*$mult + $axis(TP))];
|
|
# set axis(TD) $target;
|
|
}
|
|
|
|
# Don't handle _XQ _HX
|
|
proc MG {args} {
|
|
# Skip formatting
|
|
if {[string index [lindex $args 0] 0] == "F"} {
|
|
set msg [lrange $args 1 end]
|
|
} else {
|
|
set msg $args
|
|
}
|
|
# If msg starts with _ then return val for axis
|
|
if {[string index $msg 0] == "_"} {
|
|
return [evalArgList $msg];
|
|
} else {
|
|
return $msg;
|
|
}
|
|
}
|
|
|
|
|
|
proc every {ms body} {
|
|
if [eval $body] {
|
|
after $ms [list every $ms $body];
|
|
}
|
|
return;
|
|
}
|
|
|
|
proc nextstep {paxis step target} {
|
|
upvar #0 $paxis axis;
|
|
set mult [expr double($axis(cntsperx))/$axis(stepsperx)];
|
|
set axis(TP) [expr int($step * $mult + $axis(TP))];
|
|
set TD_POS [expr int($axis(TD) + $step)];
|
|
set axis(TD) [expr int($TD_POS)];
|
|
if {$axis(ST) == 1} {
|
|
set axis(TS) 44; # Stopped, limit switches open
|
|
set axis(BG) 0; # motor has stopped
|
|
set axis(ST) 0; # make sure stop flag is unset
|
|
set axis(SC) 4; # motor stopped by stop command (ST)
|
|
return 0;
|
|
} elseif {[expr abs($target - $axis(TD)) < abs($step)]} {
|
|
set diff [expr $target - $axis(TD)];
|
|
set axis(TP) [expr int(round($diff*$mult + $axis(TP)))];
|
|
set axis(TD) [expr int($target)];
|
|
set axis(TS) 44; # Stopped, limit switches open
|
|
set axis(BG) 0; # motor has stopped
|
|
set axis(ST) 0; # make sure stop flag is unset
|
|
set axis(SC) 1; # motor stopped at commanded position
|
|
return 0;
|
|
} else {
|
|
return 1;
|
|
}
|
|
}
|
|
|