# #######################################
#
# HAL file for HostMot2 with 3 servos
#
# Derived from Ted Hyde's original hm2-servo config
#
# Based up work and discussion with Seb & Peter & Jeff
# GNU license references - insert here. www.linuxcnc.org
#
#
# ########################################
# Firmware files are in /lib/firmware/hm2/7i43/
# Must symlink the hostmot2 firmware directory of sanbox to
# /lib/firmware before running EMC2...
# sudo ln -s $HOME/emc2-sandbox/src/hal/drivers/mesa-hostmot2/firmware /lib/firmware/hm2
#
# See also:
# <http://www.linuxcnc.org/docs/devel/html/man/man9/hostmot2.9.html#config%20modparam>
# and http://wiki.linuxcnc.org/cgi-bin/emcinfo.pl?HostMot2
#
# #####################################################################


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

# kinematics
loadrt trivkins

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

# standard components
loadrt pid num_chan=3

# only the 7i43 needs this, but it doesnt hurt the others
loadrt probe_parport

# hostmot2 driver
# if you have any firmware trouble, enable the debug flags here and see what's going on in the syslog
#loadrt hostmot2 debug_idrom=1 debug_module_descriptors=1 debug_pin_descriptors=1 debug_modules=1
loadrt hostmot2

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

# setp hm2_5i23.0.3pwmgen.frequency 1600


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

addf hm2_[HOSTMOT2](BOARD).0.read          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 hm2_[HOSTMOT2](BOARD).0.write         servo-thread
addf hm2_[HOSTMOT2](BOARD).0.pet_watchdog  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 => pid.0.enable
# net emcmot.00.enable => hm2_5i23.0.3pwmgen.00.A-value.enable
net emcmot.00.enable <= axis.0.amp-enable-out

# encoder feedback
setp hm2_[HOSTMOT2](BOARD).0.encoder.00.counter-mode 0
setp hm2_[HOSTMOT2](BOARD).0.encoder.00.filter 1
setp hm2_[HOSTMOT2](BOARD).0.encoder.00.index-invert 0
setp hm2_[HOSTMOT2](BOARD).0.encoder.00.index-mask 0
setp hm2_[HOSTMOT2](BOARD).0.encoder.00.index-mask-invert 0

setp  hm2_[HOSTMOT2](BOARD).0.encoder.00.scale  [AXIS_0]INPUT_SCALE
net motor.00.pos-fb hm2_[HOSTMOT2](BOARD).0.encoder.00.position => pid.0.feedback
net motor.00.pos-fb => axis.0.motor-pos-fb #push copy back to Axis GUI

# set PID loop gains from inifile
setp pid.0.Pgain [AXIS_0]P
setp pid.0.Igain [AXIS_0]I
setp pid.0.Dgain [AXIS_0]D
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
setp pid.0.deadband [AXIS_0]DEADBAND
setp pid.0.maxoutput [AXIS_0]MAX_OUTPUT

# position command signals
# setp hm2_5i23.0.3pwmgen.00.A-value.output-type 1 #pwm on pin1, dir on pin2
# setp hm2_5i23.0.3pwmgen.00.A-value.scale  [AXIS_0]OUTPUT_SCALE

net emcmot.00.pos-cmd axis.0.motor-pos-cmd => pid.0.command
# net motor.00.command  pid.0.output  =>  hm2_5i23.0.3pwmgen.00.A-value.value


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

# axis enable chain
newsig emcmot.01.enable bit
sets emcmot.01.enable FALSE
net emcmot.01.enable => pid.1.enable
# net emcmot.01.enable => hm2_5i23.0.3pwmgen.00.B-value.enable
net emcmot.01.enable <= axis.1.amp-enable-out

# encoder feedback
setp hm2_[HOSTMOT2](BOARD).0.encoder.01.counter-mode 0
setp hm2_[HOSTMOT2](BOARD).0.encoder.01.filter 1
setp hm2_[HOSTMOT2](BOARD).0.encoder.01.index-invert 0
setp hm2_[HOSTMOT2](BOARD).0.encoder.01.index-mask 0
setp hm2_[HOSTMOT2](BOARD).0.encoder.01.index-mask-invert 0

setp  hm2_[HOSTMOT2](BOARD).0.encoder.01.scale  [AXIS_1]INPUT_SCALE
net motor.01.pos-fb hm2_[HOSTMOT2](BOARD).0.encoder.01.position => pid.1.feedback
net motor.01.pos-fb => axis.1.motor-pos-fb #push copy back to Axis GUI

# set PID loop gains from inifile
setp pid.1.Pgain [AXIS_1]P
setp pid.1.Igain [AXIS_1]I
setp pid.1.Dgain [AXIS_1]D
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
setp pid.1.deadband [AXIS_1]DEADBAND
setp pid.1.maxoutput [AXIS_1]MAX_OUTPUT

# position command signals
# setp hm2_5i23.0.3pwmgen.00.B-value.output-type 1 #pwm on pin1, dir on pin2
# setp hm2_5i23.0.3pwmgen.00.B-value.scale [AXIS_1]OUTPUT_SCALE

net emcmot.01.pos-cmd axis.1.motor-pos-cmd => pid.1.command
# net motor.01.command  pid.1.output  =>  hm2_[HOSTMOT2]#(BOARD).0.pwmgen.01.value


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

# axis enable chain
newsig emcmot.02.enable bit
sets emcmot.02.enable FALSE
net emcmot.02.enable => pid.2.enable
# net emcmot.02.enable => hm2_5i23.0.3pwmgen.00.C-value.enable
net emcmot.02.enable <= axis.2.amp-enable-out

# encoder feedback
setp hm2_[HOSTMOT2](BOARD).0.encoder.02.counter-mode 0
setp hm2_[HOSTMOT2](BOARD).0.encoder.02.filter 1
setp hm2_[HOSTMOT2](BOARD).0.encoder.02.index-invert 0
setp hm2_[HOSTMOT2](BOARD).0.encoder.02.index-mask 0
setp hm2_[HOSTMOT2](BOARD).0.encoder.02.index-mask-invert 0

setp  hm2_[HOSTMOT2](BOARD).0.encoder.02.scale  [AXIS_2]INPUT_SCALE
net motor.02.pos-fb hm2_[HOSTMOT2](BOARD).0.encoder.02.position => pid.2.feedback
net motor.02.pos-fb => axis.2.motor-pos-fb #push copy back to Axis GUI

# set PID loop gains from inifile
setp pid.2.Pgain [AXIS_2]P
setp pid.2.Igain [AXIS_2]I
setp pid.2.Dgain [AXIS_2]D
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
setp pid.2.deadband [AXIS_2]DEADBAND
setp pid.2.maxoutput [AXIS_2]MAX_OUTPUT

# position command signals
# setp hm2_5i23.0.3pwmgen.00.C-value.output-type 1 #pwm on pin1, dir on #pin2
# setp hm2_5i23.0.3pwmgen.00.C-value.scale [AXIS_2]OUTPUT_SCALE

net emcmot.02.pos-cmd axis.2.motor-pos-cmd => pid.2.command
# net motor.02.command  pid.2.output  =>  hm2_5i23.0.3pwmgen.00.C-value.value


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

# create a signal for the 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


#
# homing
#

#
# In this example, each of the three axes have their own home switch.  All
# home switches are connected to GPIO 25, though hostmot2 boards generally
# have enough GPIO pins to put each axis' home switch on its own pin.
#
# Each switch is normally open, momentarily closed.  When the switch is open,
# GPIO 25 floats high (because that is the hostmot2 way).  When the switch is
# closed, it shorts GPIO 25 to ground.
#
# EMC expects the value on the .home-sw-in HAL pin to be active high, ie
# True when the switch is closed and False when the switch is open.
# We get this behavior by linking the GPIO .in_not pin instead of the .in
# pin.
#

net home-switch <= hm2_[HOSTMOT2](BOARD).0.gpio.025.in_not
net home-switch => axis.0.home-sw-in
net home-switch => axis.1.home-sw-in
net home-switch => axis.2.home-sw-in

# only the Y servo has an index, X and Z home without using the index
net y-index-enable hm2_[HOSTMOT2](BOARD).0.encoder.01.index-enable <=> axis.1.index-enable