# HAL config file for simulated servo machine

# first load all the RT modules that will be needed
# kinematics
loadrt trivkins
# 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 traj_period_nsec=[EMCMOT]TRAJ_PERIOD key=[EMCMOT]SHMEM_KEY num_joints=[TRAJ]AXES
# PID module, for three PID loops
loadrt pid num_chan=3
# 6 differentiators (for velocity and accel sigs),
loadrt ddt count=6
# three scale blocks (to simulate motor and leadscrew scaling),
loadrt scale count=3
# three lowpass filters (to simulate motor inertia), and nine
loadrt lowpass count=3
# window comparators (to simulate limit and home switches)
loadrt wcomp count=9

# simulated encoders
loadrt sim_encoder num_chan=3
# software encoder counters, 3 for feedback, 3 for actual axis pos
loadrt encoder num_chan=6

# add encoder counter and simulator functions to high speed thread
addf encoder.update-counters base-thread
addf sim-encoder.make-pulses base-thread

# add all required functions to servo thread
addf encoder.capture-position servo-thread
addf wcomp.0 servo-thread
addf wcomp.1 servo-thread
addf wcomp.2 servo-thread
addf wcomp.3 servo-thread
addf wcomp.4 servo-thread
addf wcomp.5 servo-thread
addf wcomp.6 servo-thread
addf wcomp.7 servo-thread
addf wcomp.8 servo-thread
addf motion-command-handler servo-thread
addf motion-controller servo-thread
addf pid.0.do-pid-calcs servo-thread
addf pid.1.do-pid-calcs servo-thread
addf pid.2.do-pid-calcs servo-thread
addf scale.0 servo-thread
addf scale.1 servo-thread
addf scale.2 servo-thread
addf lowpass.0 servo-thread
addf lowpass.1 servo-thread
addf lowpass.2 servo-thread
addf sim-encoder.update-speed servo-thread

# link the differentiator functions into the code
addf ddt.0 servo-thread
addf ddt.1 servo-thread
addf ddt.2 servo-thread
addf ddt.3 servo-thread
addf ddt.4 servo-thread
addf ddt.5 servo-thread

# get position feedback from encoder module
# connect position feedback to PID loop and motion module
net Xpos-fb encoder.0.position => pid.0.feedback axis.0.motor-pos-fb
net Ypos-fb encoder.1.position => pid.1.feedback axis.1.motor-pos-fb
net Zpos-fb encoder.2.position => pid.2.feedback axis.2.motor-pos-fb

# set position feedback scaling
setp encoder.0.position-scale [AXIS_0]INPUT_SCALE
setp encoder.1.position-scale [AXIS_1]INPUT_SCALE
setp encoder.2.position-scale [AXIS_2]INPUT_SCALE

# connect encoder index-enables for homing on index
net Xindex-enable encoder.0.index-enable <=> axis.0.index-enable
net Yindex-enable encoder.1.index-enable <=> axis.1.index-enable
net Zindex-enable encoder.2.index-enable <=> axis.2.index-enable

# connect position commands from motion controller to PID input
net Xpos-cmd <= axis.0.motor-pos-cmd => pid.0.command
net Ypos-cmd <= axis.1.motor-pos-cmd => pid.1.command
net Zpos-cmd <= axis.2.motor-pos-cmd => pid.2.command

# connect motion controller enables to PID blocks
net Xenable axis.0.amp-enable-out => pid.0.enable
net Yenable axis.1.amp-enable-out => pid.1.enable
net Zenable axis.2.amp-enable-out => pid.2.enable

# connect PID loops to scale blocks that translate to motor revs per sec
net Xoutput pid.0.output => scale.0.in
net Youtput pid.1.output => scale.1.in
net Zoutput pid.2.output => scale.2.in

# set scaling, number of motor revs needed to
# travel one inch
setp scale.0.gain [AXIS_0]DRIVE_RATIO
setp scale.1.gain [AXIS_1]DRIVE_RATIO
setp scale.2.gain [AXIS_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.0.out => lowpass.0.in
net Ymtr-cmd scale.1.out => lowpass.1.in
net Zmtr-cmd scale.2.out => lowpass.2.in

# set "inertia" here, probalby by trial and error
setp lowpass.0.gain 0.1
setp lowpass.1.gain 0.1
setp lowpass.2.gain 0.1

# "actual" motor speed signals
# output of lowpass is simulated motor speed
# speed goes to simulated encoders
net Xmtr-spd lowpass.0.out => sim-encoder.0.speed
net Ymtr-spd lowpass.1.out => sim-encoder.1.speed
net Zmtr-spd lowpass.2.out => sim-encoder.2.speed

# set simulated encoder scaling
setp sim-encoder.0.ppr [AXIS_0]MOTOR_PPR
setp sim-encoder.1.ppr [AXIS_1]MOTOR_PPR
setp sim-encoder.2.ppr [AXIS_2]MOTOR_PPR

# simulated encoder output signals
# connect them up
net XphA sim-encoder.0.phase-A => encoder.0.phase-A
net XphB sim-encoder.0.phase-B => encoder.0.phase-B
net XphZ sim-encoder.0.phase-Z => encoder.0.phase-Z
net YphA sim-encoder.1.phase-A => encoder.1.phase-A
net YphB sim-encoder.1.phase-B => encoder.1.phase-B
net YphZ sim-encoder.1.phase-Z => encoder.1.phase-Z
net ZphA sim-encoder.2.phase-A => encoder.2.phase-A
net ZphB sim-encoder.2.phase-B => encoder.2.phase-B
net ZphZ sim-encoder.2.phase-Z => encoder.2.phase-Z

# set PID loop output limits to max velocity
setp pid.0.maxoutput [AXIS_0]MAX_VELOCITY
setp pid.1.maxoutput [AXIS_1]MAX_VELOCITY
setp pid.2.maxoutput [AXIS_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.0.Pgain [AXIS_0]PGAIN
setp pid.0.Igain [AXIS_0]IGAIN
setp pid.0.Dgain [AXIS_0]DGAIN
setp pid.0.bias [AXIS_0]BIAS
setp pid.0.FF0 [AXIS_0]FF0
setp pid.0.FF1 [AXIS_0]FF1
setp pid.0.FF2 [AXIS_0]FF2
# deadband should be just over 1 count
setp pid.0.deadband [AXIS_0]DEADBAND

setp pid.1.Pgain [AXIS_1]PGAIN
setp pid.1.Igain [AXIS_1]IGAIN
setp pid.1.Dgain [AXIS_1]DGAIN
setp pid.1.bias [AXIS_1]BIAS
setp pid.1.FF0 [AXIS_1]FF0
setp pid.1.FF1 [AXIS_1]FF1
setp pid.1.FF2 [AXIS_1]FF2
# deadband should be just over 1 count
setp pid.1.deadband [AXIS_1]DEADBAND

setp pid.2.Pgain [AXIS_2]PGAIN
setp pid.2.Igain [AXIS_2]IGAIN
setp pid.2.Dgain [AXIS_2]DGAIN
setp pid.2.bias [AXIS_2]BIAS
setp pid.2.FF0 [AXIS_2]FF0
setp pid.2.FF1 [AXIS_2]FF1
setp pid.2.FF2 [AXIS_2]FF2
# deadband should be just over 1 count
setp pid.2.deadband [AXIS_2]DEADBAND

# send the position commands thru differentiators to
# generate velocity and accel signals
net Xvel ddt.0.out => ddt.1.in
net Xacc <= ddt.1.out 
net Yvel ddt.2.out => ddt.3.in
net Yacc <= ddt.3.out 
net Zvel ddt.4.out => ddt.5.in
net Zacc <= ddt.5.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 => axis.0.amp-fault-in
net yflt => axis.1.amp-fault-in
net zflt => axis.2.amp-fault-in

# a second set of encoder counters keeps track of position
net XphA => encoder.3.phase-A
net XphB => encoder.3.phase-B
net YphA => encoder.4.phase-A
net YphB => encoder.4.phase-B
net ZphA => encoder.5.phase-A
net ZphB => encoder.5.phase-B

setp encoder.3.position-scale [AXIS_0]INPUT_SCALE
setp encoder.4.position-scale [AXIS_1]INPUT_SCALE
setp encoder.5.position-scale [AXIS_2]INPUT_SCALE

# connect "actual" position from encoders
# to window comparators
net Xaxis-pos encoder.3.position => wcomp.0.in wcomp.1.in wcomp.2.in
net Yaxis-pos encoder.4.position => wcomp.3.in wcomp.4.in wcomp.5.in
net Zaxis-pos encoder.5.position => wcomp.6.in wcomp.7.in wcomp.8.in

# connect simulated switch outputs to motion controller
net Xminlim wcomp.0.out => axis.0.neg-lim-sw-in
net Xmaxlim wcomp.1.out => axis.0.pos-lim-sw-in
net Xhome wcomp.2.out => axis.0.home-sw-in

net Yminlim wcomp.3.out => axis.0.neg-lim-sw-in
net Ymaxlim wcomp.4.out => axis.0.pos-lim-sw-in
net Yhome wcomp.5.out => axis.0.home-sw-in

net Zminlim wcomp.6.out => axis.0.neg-lim-sw-in
net Zmaxlim wcomp.7.out => axis.0.pos-lim-sw-in
net Zhome wcomp.9.out => axis.0.home-sw-in

# configure the points at which the simulated switches trip
# X axis first
# min limit switch
setp wcomp.0.max [AXIS_0]MIN_HARD_LIMIT
setp wcomp.0.min [AXIS_0]MIN_HARD_LIMIT_RELEASE
# max limit switch
setp wcomp.1.min [AXIS_0]MAX_HARD_LIMIT
setp wcomp.1.max [AXIS_0]MAX_HARD_LIMIT_RELEASE
# home switch
setp wcomp.2.min [AXIS_0]HOME_SW_MIN
setp wcomp.2.max [AXIS_0]HOME_SW_MAX

# Y axis
# min limit switch
setp wcomp.3.max [AXIS_1]MIN_HARD_LIMIT
setp wcomp.3.min [AXIS_1]MIN_HARD_LIMIT_RELEASE
# max limit switch
setp wcomp.4.min [AXIS_1]MAX_HARD_LIMIT
setp wcomp.4.max [AXIS_1]MAX_HARD_LIMIT_RELEASE
# home switch
setp wcomp.5.min [AXIS_1]HOME_SW_MIN
setp wcomp.5.max [AXIS_1]HOME_SW_MAX

# Z axis
# min limit switch
setp wcomp.6.max [AXIS_2]MIN_HARD_LIMIT
setp wcomp.6.min [AXIS_2]MIN_HARD_LIMIT_RELEASE
# max limit switch
setp wcomp.7.min [AXIS_2]MAX_HARD_LIMIT
setp wcomp.7.max [AXIS_2]MAX_HARD_LIMIT_RELEASE
# home switch
setp wcomp.8.min [AXIS_2]HOME_SW_MIN
setp wcomp.8.max [AXIS_2]HOME_SW_MAX