jeka/MyOwnEtherCATDevice
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
9a41ccd2bcadfadfe40c0f5a8f07d98ae9223329
MyOwnEtherCATDevice/linuxcnc/sim.gmoccapy.lathe_configs/hallib/sim_lib.tcl
Hakan Bastedt 9a41ccd2bc Added linuxcnc test config before video release
2024-01-06 20:54:15 +01:00

499 lines
16 KiB
Tcl
Executable File
Raw Blame History

# sim_lib.tcl: haltcl procs for sim configurations
#----------------------------------------------------------------------
# Notes (Joints-Axes):
# 1) if ::KINS(KINEMATICS) exists:
# loadrt the kins using any included parameters
# example: for inifile item:
# [KINS]KINEMATICS = trivkins coordinates=XZ kinstype=BOTH
# use:
# loadrt trivkins coordinates=xz kinstype=BOTH
# else:
# loadrt trivkins
#
# 2) NB: If $::KINS(KINEMATICS) specifies coordinates=, the
# coordinates must agree with ::TRAJ(COORDINATES)
#
# 3) if known kins (trivkins) and xyz, make hypotenuse velocity
# pins for xy,xyz
#----------------------------------------------------------------------
proc indices_for_trivkins {axes} {
# ref: src/emc/kinematics/trivkins.c
if {"$axes" == ""} {set axes {x y z a b c u v w}}
set i 0
foreach a [string tolower $axes] {
# assign to consecutive joints:
set ::SIM_LIB(jointidx,$a) $i
incr i
}
} ;# indices_for_trivkins
proc get_traj_coordinates {} {
# initraj.cc: coordinates may be with or without spaces X Z or XZ
# convert either form to list like {x z}
set coordinates [lindex $::TRAJ(COORDINATES) 0]
set coordinates [string map {" " "" "\t" ""} $coordinates]
set coordinates [split $coordinates ""]
return [string tolower $coordinates]
} ;# get_traj_coordinates
proc check_ini_items {} {
foreach {section item} {KINS KINEMATICS
KINS JOINTS
TRAJ COORDINATES
} {
if ![info exists ::${section}($item)] {
return -code error "Missing inifile item: \[$section\]$item"
}
}
if { [info exists ::DISPLAY(LATHE)]
&& [lsearch $::KINS(KINEMATICS) trivkins] >= 0
} {
# reject historical lathe config using default trivkins coordinates (all)
if {[string first "=" $::KINS(KINEMATICS)] < 0} {
set msg "trivkins lathe config must specify coordinates= "
set msg "$msg\n(typically use \[KINS]KINEMATICS trivkins coordinates=XZ)"
return -code error "$msg"
}
}
set n_extrajoints 0
if [info exists ::EMCMOT(EMCMOT)] {
set mot [split [lindex $::EMCMOT(EMCMOT) 0]]
if {[string first motmod $mot] >= 0} {
foreach item $mot {
if {[string first num_extrajoints= $item] < 0} continue
set n_extrajoints [lindex [split $item =] end]
} ;# foreach
}
}
set kins [split [lindex $::KINS(KINEMATICS) 0]]
if {[string first trivkins $kins] >= 0} {
foreach item $kins {
if {[string first coordinates= $item] < 0} continue
set tcoords [lindex [split $item =] end]
set len_tcoords [string len $tcoords]
set expected_joints [expr $len_tcoords + $n_extrajoints]
if {$expected_joints != $::KINS(JOINTS)} {
set m "\ncheck_ini_items: WARNING:\n"
set m "$m trivkins coordinates=$tcoords specifies $len_tcoords joints\n"
set m "$m trivkins extrajoints=$n_extrajoints\n"
set m "$m trivkins totaljoints=$expected_joints\n"
set m "$m !!! but \[KINS\]JOINTS=$::KINS(JOINTS)\n"
puts stderr $m
}
} ;# foreach
}
return
} ;# check_ini_items
proc setup_kins {axes} {
if ![info exists ::KINS(KINEMATICS)] {
puts stderr "setup_kins: NO \[KINS\]KINEMATICS, trying default trivkins"
loadrt trivkins
return
}
set kins_kinematics [lindex $::KINS(KINEMATICS) end]
set cmd "loadrt $kins_kinematics" ;# may include parms
set kins_module [lindex $kins_kinematics 0]
puts stderr "setup_kins: cmd=$cmd"
if [catch {eval $cmd} msg] {
puts stderr "\nmsg=$msg\n"
}
# set up axis indices for known kins
switch $kins_module {
trivkins {indices_for_trivkins $axes}
default {
puts stderr "setup_kins: unknown \[KINS\]KINEMATICS=<$::KINS(KINEMATICS)>"
}
}
} ;# setup_kins
proc core_sim {axes
number_of_joints
servo_period
{base_period 0}
{emcmot motmod}
} {
# adapted as haltcl proc from core_sim.hal
# note: with default emcmot==motmot,
# thread will not be added for (default) base_pariod == 0
setup_kins $axes
if {"$emcmot" == "motmod"} {
if [catch {loadrt $emcmot \
base_period_nsec=$base_period \
servo_period_nsec=$servo_period \
num_joints=$number_of_joints} msg
] {
# typ: too many joints attempted
puts stderr "\n"
puts stderr "core_sim: loadrt $emcmot FAIL"
puts stderr " msg=$msg\n"
exit 1
}
} else {
# known special case with additional parameter:
# unlock_joint_mask=0xNN
# num_extrajoints=n
set module [split [lindex $emcmot 0]]
set modname [lindex $module 0]
set modparm [lreplace $module 0 0]
if [catch {eval loadrt $modname $modparm \
base_period_nsec=$base_period \
servo_period_nsec=$servo_period \
num_joints=$number_of_joints} msg
] {
puts stderr "\n"
puts stderr "core_sim:unhandled emcmot<$emcmot>"
puts stderr " modname=$modname"
puts stderr " modparm=$modparm"
puts stderr " msg=$msg\n"
exit 1
}
}
addf motion-command-handler servo-thread
addf motion-controller servo-thread
set pid_names ""
set mux_names ""
for {set jno 0} {$jno < $number_of_joints} {incr jno} {
set pid_names "${pid_names},J${jno}_pid"
set mux_names "${mux_names},J${jno}_mux"
}
set pid_names [string trimleft $pid_names ,]
set mux_names [string trimleft $mux_names ,]
loadrt pid names=$pid_names
loadrt mux2 names=$mux_names
# pid components
# The pid comp is used as a pass-thru device (FF0=1,all other gains=0)
# to emulate the connectivity of a servo system
# (e.g., no short-circuit connection of motor-pos-cmd to motor-pos-fb)
foreach cname [split $pid_names ,] {
addf ${cname}.do-pid-calcs servo-thread
# FF0 == 1 for pass-thru, all others 0
do_setp ${cname}.FF0 1.0
foreach pin {Pgain Dgain Igain FF1 FF2} { do_setp ${cname}.$pin 0 }
}
# mux components
# The mux comp is used as a sample-hold to simulate a machine
# with encoders that measure output position when power
# is not applied to the motors or controllers
foreach cname [split $mux_names ,] {
addf $cname servo-thread
}
# signal connections:
net estop:loop <= iocontrol.0.user-enable-out
net estop:loop => iocontrol.0.emc-enable-in
net tool:prep-loop <= iocontrol.0.tool-prepare
net tool:prep-loop => iocontrol.0.tool-prepared
net tool:change-loop <= iocontrol.0.tool-change
net tool:change-loop => iocontrol.0.tool-changed
net sample:enable <= motion.motion-enabled
for {set jno 0} {$jno < $number_of_joints} {incr jno} {
net sample:enable => J${jno}_mux.sel
net J${jno}:enable <= joint.$jno.amp-enable-out
net J${jno}:enable => J${jno}_pid.enable
net J${jno}:pos-cmd <= joint.$jno.motor-pos-cmd
net J${jno}:pos-cmd => J${jno}_pid.command
net J${jno}:on-pos <= J${jno}_pid.output
net J${jno}:on-pos => J${jno}_mux.in1 ;# pass thru when motion-enabled
net J${jno}:pos-fb <= J${jno}_mux.out
net J${jno}:pos-fb => J${jno}_mux.in0 ;# hold position when !motion-enabled
net J${jno}:pos-fb => joint.$jno.motor-pos-fb
}
} ;# core_sim
proc make_ddts {number_of_joints} {
# make vel,accel ddts and signals for all joints
# if xyz, make hypotenuse xy,xyz vels
set ddt_names ""
set ddt_ct 0
for {set jno 0} {$jno < $number_of_joints} {incr jno} {
incr ddt_ct 2
set ddt_names "${ddt_names},J${jno}_vel,J${jno}_accel"
}
set ddt_names [string trimleft $ddt_names ,]
loadrt ddt names=$ddt_names
foreach cname [split $ddt_names ,] {
addf $cname servo-thread
}
# joint vel,accel signal connections:
set ddt_ct 0
for {set jno 0} {$jno < $number_of_joints} {incr jno} {
incr ddt_ct 2
net J${jno}:pos-fb => J${jno}_vel.in ;# net presumed to exist
net J${jno}:vel <= J${jno}_vel.out
net J${jno}:vel => J${jno}_accel.in
net J${jno}:acc <= J${jno}_accel.out
}
set has_xyz 1
foreach letter {x y z} {
if ![info exists ::SIM_LIB(jointidx,$letter)] {
set has_xyz 0
break
}
}
if $has_xyz {
loadrt hypot names=hyp_xy,hyp_xyz ;# vector velocities
addf hyp_xy servo-thread
addf hyp_xyz servo-thread
net J$::SIM_LIB(jointidx,x):vel <= J$::SIM_LIB(jointidx,x)_vel.out
net J$::SIM_LIB(jointidx,x):vel => hyp_xy.in0
net J$::SIM_LIB(jointidx,x):vel => hyp_xyz.in0
net J$::SIM_LIB(jointidx,y):vel <= J$::SIM_LIB(jointidx,y)_vel.out
net J$::SIM_LIB(jointidx,y):vel => hyp_xy.in1
net J$::SIM_LIB(jointidx,y):vel => hyp_xyz.in1
net J$::SIM_LIB(jointidx,z):vel <= J$::SIM_LIB(jointidx,z)_vel.out
net J$::SIM_LIB(jointidx,z):vel => hyp_xyz.in2
net xy:vel => hyp_xy.out
net xyz:vel <= hyp_xyz.out
}
} ;# make_ddts
proc use_hal_manualtoolchange {} {
# adapted as haltcl proc from axis_manualtoolchange.hal
loadusr -W hal_manualtoolchange
# disconnect if previously connected:
unlinkp iocontrol.0.tool-change
unlinkp iocontrol.0.tool-changed
# remove signal with no connections:
delsig tool:change-loop
net tool:change <= iocontrol.0.tool-change
net tool:change => hal_manualtoolchange.change
net tool:changed <= hal_manualtoolchange.changed
net tool:changed => iocontrol.0.tool-changed
net tool:prep-number <= hal_manualtoolchange.number
net tool:prep-number => iocontrol.0.tool-prep-number
} ;# use_hal_manualtoolchange
proc simulated_home {number_of_joints} {
# uses sim_home_switch component
set switch_names ""
for {set jno 0} {$jno < $number_of_joints} {incr jno} {
set switch_names "${switch_names},J${jno}_switch"
}
set switch_names [string trimleft $switch_names ,]
loadrt sim_home_switch names=$switch_names
foreach cname [split $switch_names ,] {
addf $cname servo-thread
}
for {set jno 0} {$jno < $number_of_joints} {incr jno} {
# add pin to pre-existing signal:
net J${jno}:pos-fb => J${jno}_switch.cur-pos
net J${jno}:homesw <= J${jno}_switch.home-sw
net J${jno}:homesw => joint.$jno.home-sw-in
# set sim_home_switch .hysteresis,.home-pos pins
# according to traj units and joint type
if ![info exists ::JOINT_[set jno](TYPE)] {
# use component defaults
} else {
if {"[set ::JOINT_[set jno](TYPE)]" == "ANGULAR"} {
# use component defaults
} else {
if ![info exists ::TRAJ(LINEAR_UNITS)] {
# use component defaults
} else {
switch $::TRAJ(LINEAR_UNITS) {
in - inch - imperial {
do_setp J${jno}_switch.hysteresis 0.05
do_setp J${jno}_switch.home-pos 0.10
}
default { # use component default }
}
}
if { [info exists ::JOINT_[set jno](HOME_SEARCH_VEL)]
&& [set ::JOINT_[set jno](HOME_SEARCH_VEL)] < 0} {
do_setp J${jno}_switch.home-pos -[getp J${jno}_switch.home-pos]
}
}
} ;# type
if [info exists ::JOINT_[set jno](HOME_USE_INDEX)] {
if [set ::JOINT_[set jno](HOME_USE_INDEX)] {
# Note: use default for joint.${jno}.index-delay-ms
net J${jno}:index-enable <= joint.${jno}.index-enable
net J${jno}:index-enable => J${jno}_switch.index-enable
}
}
} ;# for
} ;# simulated_home
proc sim_spindle {} {
# adapted as haltcl proc from sim_spindle_encoder.hal
# simulated spindle encoder (for spindle-synced moves)
loadrt sim_spindle names=sim_spindle
do_setp sim_spindle.scale 0.01666667
loadrt limit2 names=limit_speed
loadrt lowpass names=spindle_mass
loadrt near names=near_speed
loadrt scale names=rpm_rps
setp rpm_rps.gain .0167
# this limit doesn't make any sense to me:
do_setp limit_speed.maxv 5000.0 ;# rpm/second
# encoder reset control
# hook up motion controller's sync output
net spindle-index-enable <=> spindle.0.index-enable
net spindle-index-enable <=> sim_spindle.index-enable
# report our revolution count to the motion controller
net spindle-pos <= sim_spindle.position-fb
net spindle-pos => spindle.0.revs
# simulate spindle mass
do_setp spindle_mass.gain .07
# spindle speed control
net spindle-speed-cmd <= spindle.0.speed-out
net spindle-speed-cmd => limit_speed.in
net spindle-speed-cmd => near_speed.in1
net spindle-speed-limited <= limit_speed.out
net spindle-speed-limited => sim_spindle.velocity-cmd
net spindle-speed-limited => spindle_mass.in
# for spindle velocity estimate
net spindle-rpm-filtered <= spindle_mass.out
net spindle-rpm-filtered rpm_rps.in
net spindle-rps-filtered rpm_rps.out spindle.0.speed-in
net spindle-rpm-filtered => near_speed.in2
# at-speed detection
do_setp near_speed.scale 1.1
do_setp near_speed.difference 10
net spindle-at-speed <= near_speed.out
net spindle-at-speed => spindle.0.at-speed
net spindle-orient <= spindle.0.orient
net spindle-orient <= spindle.0.is-oriented
addf limit_speed servo-thread
addf spindle_mass servo-thread
addf rpm_rps servo-thread
addf near_speed servo-thread
addf sim_spindle servo-thread
} ;# sim_spindle
proc save_hal_cmds {savefilename {options ""} } {
set tmpfile /tmp/save_hal_cmds_tmp
set date [clock format [clock seconds]]
set script [info script]
set save_arg all ;# suffices if only basic_sim in use
if {[llength $::HAL(HALFILE)] > 1} {
set save_arg allu ;# do *all* unconnected pins including
;# pins from other HALFILEs
}
set fd [open $savefilename w] ;# overwrite any existing file
puts $fd "# $date
#
# This file: $savefilename
# Created by: $script
# With options: $::argv
# From inifile: $::env(INI_FILE_NAME)
# Halfiles: $::HAL(HALFILE)
#
# This file contains the hal commands produced by [file tail $script]
# (and any hal commands executed prior to its execution).
# ------------------------------------------------------------------
# To use $savefilename in the original inifile (or a copy of it),
# edit to change:
# \[HAL\]
# HALFILE = LIB:basic_sim.tcl parameters
# to:
# \[HAL\]
# HALFILE = $savefilename
#
# Notes:
# 1) Inifile Variables substitutions specified in the inifile
# and interpreted by halcmd are automatically substituted
# in the created halfile ($savefilename).
# 2) Input pins connected to a signal with no writer are
# not included in the setp listings herein so must be added
# manually
#
"
if {[lsearch $options use_hal_manualtoolchange] >= 0} {
puts $fd "# user space components"
puts $fd "loadusr -W hal_manualtoolchange"
puts $fd ""
}
hal save $save_arg $tmpfile
set ftmp [open $tmpfile r]
set gave_msg 0
set setp_fmt "%-3s %-30s %s"
while {![eof $ftmp]} {
gets $ftmp line
if {([string first "unconnected pin values" $line] >0) && !$gave_msg} {
set gave_msg 1
puts $fd "# Note: ALL unconnected pins follow"
puts $fd "# (includes pins using default with no explicit setp command)"
} else {
scan $line "%s %s %s" cmd arg1 remainder
switch $cmd {
setp {puts $fd [format $setp_fmt $cmd $arg1 $remainder]}
loadrt {
if { [string first tpmod [list $line]] >= 0
|| [string first homemod [list $line]] >= 0
} {
puts $fd "#preloaded module: $line"
} else {
puts $fd $line
}
}
default {puts $fd $line}
}
}
} ;# while
close $ftmp
file delete $tmpfile
if {("$save_arg" != "allu") && [info exists ::SIM_LIB(setp_list)]} {
puts $fd "# setp commands for unconnected input pins"
foreach {pname value} $::SIM_LIB(setp_list) {
puts $fd [format $setp_fmt setp $pname $value]
}
}
close $fd
} ;# save_hal_cmds
proc do_setp {pname value} {
setp $pname $value
lappend ::SIM_LIB(setp_list) $pname $value
} ;# do_setp
Reference in New Issue View Git Blame Copy Permalink