# #######################################
#
# HAL file for 9x20 lathe
#
# Servos on X, Z and Spindle
#
# Derived from 2.3 hm2-servo.hal
#
# #####################################################################


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

# 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 num_joints=[TRAJ]AXES

# standard components
loadrt pid num_chan=3 

# hostmot2 driver
loadrt hostmot2 debug_idrom=1 debug_module_descriptors=1 debug_pin_descriptors=1 debug_modules=1

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

setp hm2_[HOSTMOT2](BOARD).0.pwmgen.pwm_frequency 40000

setp hm2_[HOSTMOT2](BOARD).0.watchdog.timeout_ns 10000000


# ################################################
# 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_[HOSTMOT2](BOARD).0.pwmgen.00.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_VELOCITY

# position command signals
setp hm2_[HOSTMOT2](BOARD).0.pwmgen.00.output-type 1 #pwm on pin1, dir on pin2
setp hm2_[HOSTMOT2](BOARD).0.pwmgen.00.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_[HOSTMOT2](BOARD).0.pwmgen.00.value

# ################
# Spindle [1] Axis
# Note, axis requires 0-X,1-Y,2-Z, so we map hm2_ devices 2 to 1
# ################

# spindle enable chain
loadrt near
addf near.0 servo-thread
setp near.0.difference 1
setp near.0.in1 0

loadrt not
addf not.0 servo-thread
net near_not near.0.out => not.0.in

loadrt or2
addf or2.0 servo-thread
net spindle_on motion.spindle-on => or2.0.in0
net near_on not.0.out => or2.0.in1
net and_on or2.0.out => pid.1.enable hm2_[HOSTMOT2](BOARD).0.pwmgen.02.enable

# 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  [SPINDLE]INPUT_SCALE


# set PID loop gains from inifile
setp pid.1.Pgain [SPINDLE]P
setp pid.1.Igain [SPINDLE]I
setp pid.1.Dgain [SPINDLE]D
setp pid.1.bias [SPINDLE]BIAS
setp pid.1.FF0 [SPINDLE]FF0
setp pid.1.FF1 [SPINDLE]FF1
setp pid.1.FF2 [SPINDLE]FF2
setp pid.1.deadband [SPINDLE]DEADBAND
setp pid.1.maxoutput [SPINDLE]MAX_OUTPUT

setp hm2_[HOSTMOT2](BOARD).0.pwmgen.02.output-type 1 #pwm on pin1, dir on pin2
setp hm2_[HOSTMOT2](BOARD).0.pwmgen.02.scale  [SPINDLE]OUTPUT_SCALE


# velocity command signals
loadrt limit3
addf limit3.0 servo-thread
setp limit3.0.min -1500
setp limit3.0.max 1500
setp limit3.0.maxv 500
setp limit3.0.maxa 150

net spindle_rpm_cmd motion.spindle-speed-out => limit3.0.in
net spindle_limit_rpm limit3.0.out => pid.1.command
net spindle_limit_rpm  => near.0.in2

loadrt enc_idx_div
addf enc-idx-div.0 servo-thread
setp enc-idx-div.0.trigger-count 10000
setp enc-idx-div.0.counts-per-rev 12000

net motor_speed_cmd pid.1.output => hm2_[HOSTMOT2](BOARD).0.pwmgen.02.value
net spindle_enc_count hm2_[HOSTMOT2](BOARD).0.encoder.02.count => enc-idx-div.0.encoder-count-in
net spindle_index_en hm2_[HOSTMOT2](BOARD).0.encoder.02.index-enable <==> enc-idx-div.0.encoder-index-enable
net spindle_div_idx_en enc-idx-div.0.index-enable <==> motion.spindle-index-enable





loadrt scale
addf scale.0 servo-thread
setp scale.0.gain 60
loadrt lowpass
addf lowpass.0 servo-thread
setp lowpass.0.gain 0.1
net motor.01.vel-fb-hz hm2_[HOSTMOT2](BOARD).0.encoder.02.velocity => scale.0.in
net motor.01.vel-fb-rpm scale.0.out => lowpass.0.in
net motor.01.vel-fb-rpm-filtered lowpass.0.out => pid.1.feedback
#net motor.01.vel-fb-rpm scale.0.out => pid.1.feedback

net motor.01.vel-fb-rpm-filtered lowpass.0.out => motion.spindle-speed-in
#net motor.01.vel-fb-rpm-scale => motion.spindle-speed-in

net motor.01.pos-fb hm2_[HOSTMOT2](BOARD).0.encoder.02.position motion.spindle-revs


# ################
# Z [2] Axis, is hm2_ device 1
# ################

# 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_[HOSTMOT2](BOARD).0.pwmgen.01.enable
net emcmot.02.enable <= axis.2.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_2]INPUT_SCALE
net motor.02.pos-fb hm2_[HOSTMOT2](BOARD).0.encoder.01.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_VELOCITY

# position command signals
setp hm2_[HOSTMOT2](BOARD).0.pwmgen.01.output-type 1 #pwm on pin1, dir on pin2
setp hm2_[HOSTMOT2](BOARD).0.pwmgen.01.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_[HOSTMOT2](BOARD).0.pwmgen.01.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

# X Home switch
setp hm2_[HOSTMOT2](BOARD).0.gpio.024.is_output 0
net x_home_switch hm2_[HOSTMOT2](BOARD).0.gpio.024.in => axis.0.home-sw-in
net x_home_index_enable axis.0.index-enable => hm2_[HOSTMOT2](BOARD).0.encoder.00.index-enable


# ##################################################
# Spindle
# ##################################################