# 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