286 lines
10 KiB
Plaintext
Executable File
286 lines
10 KiB
Plaintext
Executable File
# HAL config file for simulated servo machine
|
|
|
|
# first load all the RT modules that will be needed
|
|
# kinematics
|
|
loadrt [KINS]KINEMATICS
|
|
# motion controller, get name and thread periods from INI file
|
|
loadrt [EMCMOT]EMCMOT base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS
|
|
# PID module, for three PID loops
|
|
loadrt pid names=pid_x,pid_y,pid_z
|
|
# 6 differentiators (for velocity and accel sigs)
|
|
loadrt ddt names=ddt_x,ddt_xv,ddt_y,ddt_yv,ddt_z,ddt_zv
|
|
# three scale blocks (to simulate motor and leadscrew scaling)
|
|
loadrt scale names=scale_x,scale_y,scale_z
|
|
# three lowpass filters (to simulate motor inertia), and nine
|
|
loadrt lowpass names=lowpass_x,lowpass_y,lowpass_z
|
|
# window comparators (to simulate limit and home switches)
|
|
loadrt wcomp names=wcomp_xneg,wcomp_xpos,wcomp_xhome,wcomp_yneg,wcomp_ypos,wcomp_yhome,wcomp_zneg,wcomp_zpos,wcomp_zhome
|
|
|
|
# simulated encoders
|
|
loadrt sim_encoder names=sim_encoder_px,sim_encoder_py,sim_encoder_pz
|
|
# software encoder counters, 3 for feedback, 3 for actual axis pos
|
|
loadrt encoder names=encoder_px,encoder_py,encoder_pz,encoder_x,encoder_y,encoder_z
|
|
# add encoder counter and simulator functions to high speed thread
|
|
addf sim-encoder.make-pulses base-thread
|
|
addf encoder.update-counters base-thread
|
|
|
|
# add all required functions to servo thread
|
|
addf encoder.capture-position servo-thread
|
|
addf wcomp_xneg servo-thread
|
|
addf wcomp_xpos servo-thread
|
|
addf wcomp_xhome servo-thread
|
|
addf wcomp_yneg servo-thread
|
|
addf wcomp_ypos servo-thread
|
|
addf wcomp_yhome servo-thread
|
|
addf wcomp_zneg servo-thread
|
|
addf wcomp_zpos servo-thread
|
|
addf wcomp_zhome servo-thread
|
|
addf motion-command-handler servo-thread
|
|
addf motion-controller servo-thread
|
|
addf pid_x.do-pid-calcs servo-thread
|
|
addf pid_y.do-pid-calcs servo-thread
|
|
addf pid_z.do-pid-calcs servo-thread
|
|
addf scale_x servo-thread
|
|
addf scale_y servo-thread
|
|
addf scale_z servo-thread
|
|
addf lowpass_x servo-thread
|
|
addf lowpass_y servo-thread
|
|
addf lowpass_z servo-thread
|
|
addf sim-encoder.update-speed servo-thread
|
|
|
|
# link the differentiator functions into the code
|
|
addf ddt_x servo-thread
|
|
addf ddt_xv servo-thread
|
|
addf ddt_y servo-thread
|
|
addf ddt_yv servo-thread
|
|
addf ddt_z servo-thread
|
|
addf ddt_zv servo-thread
|
|
|
|
# get position feedback from encoder module
|
|
# connect position feedback to PID loop and motion module
|
|
net Xpos-fb encoder_px.position => pid_x.feedback joint.0.motor-pos-fb
|
|
net Ypos-fb encoder_py.position => pid_y.feedback joint.1.motor-pos-fb
|
|
net Zpos-fb encoder_pz.position => pid_z.feedback joint.2.motor-pos-fb
|
|
|
|
# set position feedback scaling
|
|
setp encoder_px.position-scale [JOINT_0]INPUT_SCALE
|
|
setp encoder_py.position-scale [JOINT_1]INPUT_SCALE
|
|
setp encoder_pz.position-scale [JOINT_2]INPUT_SCALE
|
|
|
|
# connect encoder index-enables for homing on index
|
|
net Xindex-enable encoder_px.index-enable <=> joint.0.index-enable pid_x.index-enable
|
|
net Yindex-enable encoder_py.index-enable <=> joint.1.index-enable
|
|
net Zindex-enable encoder_pz.index-enable <=> joint.2.index-enable
|
|
|
|
# connect position commands from motion controller to PID input
|
|
net Xpos-cmd <= joint.0.motor-pos-cmd => pid_x.command
|
|
net Ypos-cmd <= joint.1.motor-pos-cmd => pid_y.command
|
|
net Zpos-cmd <= joint.2.motor-pos-cmd => pid_z.command
|
|
|
|
# connect motion controller enables to PID blocks
|
|
net Xenable joint.0.amp-enable-out => pid_x.enable
|
|
net Yenable joint.1.amp-enable-out => pid_y.enable
|
|
net Zenable joint.2.amp-enable-out => pid_z.enable
|
|
|
|
# connect PID loops to scale blocks that translate to motor revs per sec
|
|
net Xoutput pid_x.output => scale_x.in
|
|
net Youtput pid_y.output => scale_y.in
|
|
net Zoutput pid_z.output => scale_z.in
|
|
|
|
# set scaling, number of motor revs needed to
|
|
# travel one inch
|
|
setp scale_x.gain [JOINT_0]DRIVE_RATIO
|
|
setp scale_y.gain [JOINT_1]DRIVE_RATIO
|
|
setp scale_z.gain [JOINT_2]DRIVE_RATIO
|
|
|
|
# motor speed command sigs come from scale blocks
|
|
# motor speed commands go thru lowpass filters
|
|
# to simulate motor inertia
|
|
net Xmtr-cmd scale_x.out => lowpass_x.in
|
|
net Ymtr-cmd scale_y.out => lowpass_y.in
|
|
net Zmtr-cmd scale_z.out => lowpass_z.in
|
|
|
|
# set "inertia" here, probably by trial and error
|
|
setp lowpass_x.gain 0.1
|
|
setp lowpass_y.gain 0.1
|
|
setp lowpass_z.gain 0.1
|
|
|
|
# "actual" motor speed signals
|
|
# output of lowpass is simulated motor speed
|
|
# speed goes to simulated encoders
|
|
net Xmtr-spd lowpass_x.out => sim_encoder_px.speed
|
|
net Ymtr-spd lowpass_y.out => sim_encoder_py.speed
|
|
net Zmtr-spd lowpass_z.out => sim_encoder_pz.speed
|
|
|
|
# set simulated encoder scaling
|
|
setp sim_encoder_px.ppr [JOINT_0]MOTOR_PPR
|
|
setp sim_encoder_py.ppr [JOINT_1]MOTOR_PPR
|
|
setp sim_encoder_pz.ppr [JOINT_2]MOTOR_PPR
|
|
|
|
# simulated encoder output signals
|
|
# connect them up
|
|
net XphA sim_encoder_px.phase-A => encoder_px.phase-A
|
|
net XphB sim_encoder_px.phase-B => encoder_px.phase-B
|
|
net XphZ sim_encoder_px.phase-Z => encoder_px.phase-Z
|
|
net YphA sim_encoder_py.phase-A => encoder_py.phase-A
|
|
net YphB sim_encoder_py.phase-B => encoder_py.phase-B
|
|
net YphZ sim_encoder_py.phase-Z => encoder_py.phase-Z
|
|
net ZphA sim_encoder_pz.phase-A => encoder_pz.phase-A
|
|
net ZphB sim_encoder_pz.phase-B => encoder_pz.phase-B
|
|
net ZphZ sim_encoder_pz.phase-Z => encoder_pz.phase-Z
|
|
|
|
# set PID loop output limits to max velocity
|
|
setp pid_x.maxoutput [JOINT_0]MAX_VELOCITY
|
|
setp pid_y.maxoutput [JOINT_1]MAX_VELOCITY
|
|
setp pid_z.maxoutput [JOINT_2]MAX_VELOCITY
|
|
|
|
# set PID loop gains
|
|
# NOTE: eventually these will be non-zero values as
|
|
# needed to tune the performance of each axis. The
|
|
# initial values shown here are extremely conservative
|
|
# to prevent unexpected behavior. After this file
|
|
# has been "executed" by halcmd, the gains can be
|
|
# interactively adjusted using commands like
|
|
# "halcmd setp pid.<channel>.Pgain <value>"
|
|
# Once the axis has been tuned to your satisfaction,
|
|
# do "halcmd show param | grep pid" to get a listing
|
|
# of the tuning parameters, and enter those values here.
|
|
|
|
# the values below come from the INI
|
|
setp pid_x.Pgain [JOINT_0]PGAIN
|
|
setp pid_x.Igain [JOINT_0]IGAIN
|
|
setp pid_x.Dgain [JOINT_0]DGAIN
|
|
setp pid_x.bias [JOINT_0]BIAS
|
|
setp pid_x.FF0 [JOINT_0]FF0
|
|
setp pid_x.FF1 [JOINT_0]FF1
|
|
setp pid_x.FF2 [JOINT_0]FF2
|
|
# deadband should be just over 1 count
|
|
setp pid_x.deadband [JOINT_0]DEADBAND
|
|
|
|
setp pid_y.Pgain [JOINT_1]PGAIN
|
|
setp pid_y.Igain [JOINT_1]IGAIN
|
|
setp pid_y.Dgain [JOINT_1]DGAIN
|
|
setp pid_y.bias [JOINT_1]BIAS
|
|
setp pid_y.FF0 [JOINT_1]FF0
|
|
setp pid_y.FF1 [JOINT_1]FF1
|
|
setp pid_y.FF2 [JOINT_1]FF2
|
|
# deadband should be just over 1 count
|
|
setp pid_y.deadband [JOINT_1]DEADBAND
|
|
|
|
setp pid_z.Pgain [JOINT_2]PGAIN
|
|
setp pid_z.Igain [JOINT_2]IGAIN
|
|
setp pid_z.Dgain [JOINT_2]DGAIN
|
|
setp pid_z.bias [JOINT_2]BIAS
|
|
setp pid_z.FF0 [JOINT_2]FF0
|
|
setp pid_z.FF1 [JOINT_2]FF1
|
|
setp pid_z.FF2 [JOINT_2]FF2
|
|
# deadband should be just over 1 count
|
|
setp pid_z.deadband [JOINT_2]DEADBAND
|
|
|
|
# send the position commands thru differentiators to
|
|
# generate velocity and accel signals
|
|
net Xvel ddt_x.out => ddt_xv.in
|
|
net Xacc <= ddt_xv.out
|
|
net Yvel ddt_y.out => ddt_yv.in
|
|
net Yacc <= ddt_yv.out
|
|
net Zvel ddt_z.out => ddt_zv.in
|
|
net Zacc <= ddt_zv.out
|
|
|
|
# estop loopback
|
|
net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in
|
|
|
|
# create signals for tool loading loopback
|
|
net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared
|
|
net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed
|
|
|
|
net xflt => joint.0.amp-fault-in
|
|
net yflt => joint.1.amp-fault-in
|
|
net zflt => joint.2.amp-fault-in
|
|
|
|
# a second set of encoder counters keeps track of position
|
|
net XphA => encoder_x.phase-A
|
|
net XphB => encoder_x.phase-B
|
|
net YphA => encoder_y.phase-A
|
|
net YphB => encoder_y.phase-B
|
|
net ZphA => encoder_z.phase-A
|
|
net ZphB => encoder_z.phase-B
|
|
|
|
setp encoder_x.position-scale [JOINT_0]INPUT_SCALE
|
|
setp encoder_y.position-scale [JOINT_1]INPUT_SCALE
|
|
setp encoder_z.position-scale [JOINT_2]INPUT_SCALE
|
|
|
|
# connect "actual" position from encoders
|
|
# to window comparators
|
|
net Xaxis-pos encoder_x.position => wcomp_xneg.in wcomp_xpos.in wcomp_xhome.in
|
|
net Yaxis-pos encoder_y.position => wcomp_yneg.in wcomp_ypos.in wcomp_yhome.in
|
|
net Zaxis-pos encoder_z.position => wcomp_zneg.in wcomp_zpos.in wcomp_zhome.in
|
|
|
|
# connect simulated switch outputs to motion controller
|
|
net Xminlim wcomp_xneg.out => joint.0.neg-lim-sw-in
|
|
net Xmaxlim wcomp_xpos.out => joint.0.pos-lim-sw-in
|
|
net Xhome wcomp_xhome.out => joint.0.home-sw-in
|
|
|
|
net Yminlim wcomp_yneg.out => joint.1.neg-lim-sw-in
|
|
net Ymaxlim wcomp_ypos.out => joint.1.pos-lim-sw-in
|
|
net Yhome wcomp_yhome.out => joint.1.home-sw-in
|
|
|
|
net Zminlim wcomp_zneg.out => joint.2.neg-lim-sw-in
|
|
net Zmaxlim wcomp_zpos.out => joint.2.pos-lim-sw-in
|
|
net Zhome wcomp_zhome.out => joint.2.home-sw-in
|
|
|
|
# configure the points at which the simulated switches trip
|
|
# X axis first
|
|
# min limit switch
|
|
setp wcomp_xneg.max [JOINT_0]MIN_HARD_LIMIT
|
|
setp wcomp_xneg.min [JOINT_0]MIN_HARD_LIMIT_RELEASE
|
|
# max limit switch
|
|
setp wcomp_xpos.min [JOINT_0]MAX_HARD_LIMIT
|
|
setp wcomp_xpos.max [JOINT_0]MAX_HARD_LIMIT_RELEASE
|
|
# home switch
|
|
setp wcomp_xhome.min [JOINT_0]HOME_SW_MIN
|
|
setp wcomp_xhome.max [JOINT_0]HOME_SW_MAX
|
|
|
|
# Y axis
|
|
# min limit switch
|
|
setp wcomp_yneg.max [JOINT_1]MIN_HARD_LIMIT
|
|
setp wcomp_yneg.min [JOINT_1]MIN_HARD_LIMIT_RELEASE
|
|
# max limit switch
|
|
setp wcomp_ypos.min [JOINT_1]MAX_HARD_LIMIT
|
|
setp wcomp_ypos.max [JOINT_1]MAX_HARD_LIMIT_RELEASE
|
|
# home switch
|
|
setp wcomp_yhome.min [JOINT_1]HOME_SW_MIN
|
|
setp wcomp_yhome.max [JOINT_1]HOME_SW_MAX
|
|
|
|
# Z axis
|
|
# min limit switch
|
|
setp wcomp_zneg.max [JOINT_2]MIN_HARD_LIMIT
|
|
setp wcomp_zneg.min [JOINT_2]MIN_HARD_LIMIT_RELEASE
|
|
# max limit switch
|
|
setp wcomp_zpos.min [JOINT_2]MAX_HARD_LIMIT
|
|
setp wcomp_zpos.max [JOINT_2]MAX_HARD_LIMIT_RELEASE
|
|
# home switch
|
|
setp wcomp_zhome.min [JOINT_2]HOME_SW_MIN
|
|
setp wcomp_zhome.max [JOINT_2]HOME_SW_MAX
|
|
|
|
# Configure fake probing
|
|
loadrt sphereprobe names=sphereprobe
|
|
|
|
addf sphereprobe base-thread 2
|
|
|
|
setp sphereprobe.cx -2811 # this is where it ends up after homing
|
|
setp sphereprobe.cz -6000 # this is where it ends up after homing
|
|
setp sphereprobe.r 5000 # 5/6 inch
|
|
net px encoder_px.rawcounts => sphereprobe.px
|
|
net py encoder_py.rawcounts => sphereprobe.py
|
|
net pz encoder_pz.rawcounts => sphereprobe.pz
|
|
net probe-out sphereprobe.probe-out => motion.probe-input
|
|
net probe-out => encoder_px.latch-input encoder_py.latch-input
|
|
net probe-out => encoder_pz.latch-input
|
|
|
|
setp encoder_px.latch-rising 1
|
|
setp encoder_px.latch-falling 1
|
|
setp encoder_py.latch-rising 1
|
|
setp encoder_py.latch-falling 1
|
|
setp encoder_pz.latch-rising 1
|
|
setp encoder_pz.latch-falling 1
|