# #######################################
#
# HAL file for HostMot2+Modbus with 3 steppers
#
# Based on the other examples in this directory.
#
# ########################################
#
# See also:
#   hostmot2(9)
#   hm2_modbus(9)
#   mesambccc(1)
#
# #####################################################################


# ###################################
# Core EMC/HAL Loads
# ###################################

# kinematics
loadrt [KINS]KINEMATICS

# motion controller, get name and thread periods from ini file
loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS

# hostmot2 driver
loadrt hostmot2
# debug_modules=5

# load low-level driver
loadrt [HOSTMOT2](DRIVER) board_ip=[HOSTMOT2](BOARD_IP) config=[HOSTMOT2](CONFIG)

# load the Modbus driver
loadrt hm2_modbus ports=[MODBUS](PORTS) mbccbs=[MODBUS](MBCCBS)

# load estop latch component
loadrt estop_latch

# ################################################
# THREADS
# ################################################

addf hm2_[HOSTMOT2](BOARD).0.read         servo-thread

addf hm2_modbus.0.process                 servo-thread

addf motion-command-handler               servo-thread
addf motion-controller                    servo-thread
# revel in the free time here from not having to run PID 
addf hm2_[HOSTMOT2](BOARD).0.write        servo-thread
addf estop-latch.0                        servo-thread
       

# ######################################################
# Axis-of-motion Specific Configs (not the GUI)
# ######################################################


# ################
# X [0] Axis
# ################

# axis enable chain
newsig emcmot.00.enable bit
sets emcmot.00.enable FALSE

net emcmot.00.enable <= joint.0.amp-enable-out 
net emcmot.00.enable => hm2_[HOSTMOT2](BOARD).0.stepgen.00.enable


# position command and feedback
net emcmot.00.pos-cmd <= joint.0.motor-pos-cmd
net emcmot.00.pos-cmd => hm2_[HOSTMOT2](BOARD).0.stepgen.00.position-cmd

net motor.00.pos-fb <= hm2_[HOSTMOT2](BOARD).0.stepgen.00.position-fb
net motor.00.pos-fb => joint.0.motor-pos-fb


# timing parameters
setp hm2_[HOSTMOT2](BOARD).0.stepgen.00.dirsetup        [JOINT_0]DIRSETUP
setp hm2_[HOSTMOT2](BOARD).0.stepgen.00.dirhold         [JOINT_0]DIRHOLD

setp hm2_[HOSTMOT2](BOARD).0.stepgen.00.steplen         [JOINT_0]STEPLEN
setp hm2_[HOSTMOT2](BOARD).0.stepgen.00.stepspace       [JOINT_0]STEPSPACE

setp hm2_[HOSTMOT2](BOARD).0.stepgen.00.position-scale  [JOINT_0]SCALE

setp hm2_[HOSTMOT2](BOARD).0.stepgen.00.maxvel          [JOINT_0]STEPGEN_MAX_VEL
setp hm2_[HOSTMOT2](BOARD).0.stepgen.00.maxaccel        [JOINT_0]STEPGEN_MAX_ACC

setp hm2_[HOSTMOT2](BOARD).0.stepgen.00.step_type       0


# ################
# Y [1] Axis
# ################

# axis enable chain
newsig emcmot.01.enable bit
sets emcmot.01.enable FALSE

net emcmot.01.enable <= joint.1.amp-enable-out 
net emcmot.01.enable => hm2_[HOSTMOT2](BOARD).0.stepgen.01.enable


# position command and feedback
net emcmot.01.pos-cmd <= joint.1.motor-pos-cmd
net emcmot.01.pos-cmd => hm2_[HOSTMOT2](BOARD).0.stepgen.01.position-cmd

net motor.01.pos-fb <= hm2_[HOSTMOT2](BOARD).0.stepgen.01.position-fb
net motor.01.pos-fb => joint.1.motor-pos-fb


# timing parameters
setp hm2_[HOSTMOT2](BOARD).0.stepgen.01.dirsetup        [JOINT_1]DIRSETUP
setp hm2_[HOSTMOT2](BOARD).0.stepgen.01.dirhold         [JOINT_1]DIRHOLD

setp hm2_[HOSTMOT2](BOARD).0.stepgen.01.steplen         [JOINT_1]STEPLEN
setp hm2_[HOSTMOT2](BOARD).0.stepgen.01.stepspace       [JOINT_1]STEPSPACE

setp hm2_[HOSTMOT2](BOARD).0.stepgen.01.position-scale  [JOINT_1]SCALE

setp hm2_[HOSTMOT2](BOARD).0.stepgen.01.maxvel          [JOINT_1]STEPGEN_MAX_VEL
setp hm2_[HOSTMOT2](BOARD).0.stepgen.01.maxaccel        [JOINT_1]STEPGEN_MAX_ACC

setp hm2_[HOSTMOT2](BOARD).0.stepgen.01.step_type       0


# ################
# Z [2] Axis
# ################

# axis enable chain
newsig emcmot.02.enable bit
sets emcmot.02.enable FALSE

net emcmot.02.enable <= joint.2.amp-enable-out 
net emcmot.02.enable => hm2_[HOSTMOT2](BOARD).0.stepgen.02.enable


# position command and feedback
net emcmot.02.pos-cmd <= joint.2.motor-pos-cmd
net emcmot.02.pos-cmd => hm2_[HOSTMOT2](BOARD).0.stepgen.02.position-cmd

net motor.02.pos-fb <= hm2_[HOSTMOT2](BOARD).0.stepgen.02.position-fb
net motor.02.pos-fb => joint.2.motor-pos-fb


# timing parameters
setp hm2_[HOSTMOT2](BOARD).0.stepgen.02.dirsetup        [JOINT_2]DIRSETUP
setp hm2_[HOSTMOT2](BOARD).0.stepgen.02.dirhold         [JOINT_2]DIRHOLD

setp hm2_[HOSTMOT2](BOARD).0.stepgen.02.steplen         [JOINT_2]STEPLEN
setp hm2_[HOSTMOT2](BOARD).0.stepgen.02.stepspace       [JOINT_2]STEPSPACE

setp hm2_[HOSTMOT2](BOARD).0.stepgen.02.position-scale  [JOINT_2]SCALE

setp hm2_[HOSTMOT2](BOARD).0.stepgen.02.maxvel          [JOINT_2]STEPGEN_MAX_VEL
setp hm2_[HOSTMOT2](BOARD).0.stepgen.02.maxaccel        [JOINT_2]STEPGEN_MAX_ACC

setp hm2_[HOSTMOT2](BOARD).0.stepgen.02.step_type       0


# ##################################################
# Standard I/O Block - EStop, Etc
# ##################################################

# A basic estop loop that only includes the hostmot watchdog.
net user-enable iocontrol.0.user-request-enable => estop-latch.0.reset
net enable-latch estop-latch.0.ok-out => iocontrol.0.emc-enable-in
net watchdog hm2_[HOSTMOT2](BOARD).0.watchdog.has_bit => estop-latch.0.fault-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