# 5i20 config file for Smithy 1240 Mill

# load the hostmot2 driver, this doesnt do anything by itself,
# it just waits for low-level drivers to register boards
loadrt hostmot2 debug_idrom=1 debug_module_descriptors=1 debug_pin_descriptors=1 debug_modules=1

# load the low-level driver for the board
# this will load the board's firmware and register it with the hostmot2
# driver loaded above, and it'll show up in HAL
loadrt hm2_pci config="firmware=hm2/5i20/SVST2_4_7I47.BIT num_encoders=1 num_pwmgens=1 num_stepgens=4"
# set up the watchdog
#setp hm2_5i20.0.watchdog.timeout_ns 10000000

# load kinematics
loadrt trivkins

# load 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

# load charge pump
loadrt charge_pump

# These loopbacks are usually disconnected by a postgui halfile
net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed
net tool-prepare-loopback iocontrol.0.tool-prepare iocontrol.0.tool-prepared

#the Enable signal is used to permanently enable things
newsig Enable bit
sets Enable TRUE

#configure the spindle speed
loadrt limit1 count=1
loadrt sum2 count=2
setp sum2.0.gain1 1.0
setp sum2.0.gain0 1.0
setp sum2.1.gain1 1.0

#*** Old parameters for 400-6000 RPM Operation with 10%-90% PWM***
# sum2.1.gain0 = (1.0 - sum2.1.offset)/(limit1.0.max - limit1.0.min)
#setp sum2.1.gain0 0.000142857
# limit1.0.min is the lowest allowable spindle speed
#setp limit1.0.min 400.0
# limit1.0.max is the highest allowable spindle speed
#setp limit1.0.max 6000.0
# sum2.0.offset = -(limit1.0.min)
#setp sum2.0.offset -400.0
# sum2.1.offset is the lowest allowable modulation percentage
# the highest modulation percentage is = (1.0 - sum2.1.offset)
#setp sum2.1.offset 0.1

#*** New parameters for 0-6000 RPM Operation with 0%-100% PWM***
# sum2.1.gain0 = (1.0 - sum2.1.offset)/(limit1.0.max - limit1.0.min)
setp sum2.1.gain0 0.000166667
# limit1.0.min is the lowest allowable spindle speed
setp limit1.0.min 0.0
# limit1.0.max is the highest allowable spindle speed
setp limit1.0.max 6000.0
# sum2.0.offset = -(limit1.0.min)
setp sum2.0.offset 0.0
# sum2.1.offset is the lowest allowable modulation percentage
# the highest modulation percentage is = (1.0 - sum2.1.offset)
setp sum2.1.offset 0.0

net scmd motion.spindle-speed-out limit1.0.in
net slim limit1.0.out sum2.0.in0
net szero sum2.0.in1 sum2.1.in1
sets szero 0.0
net sshift sum2.0.out sum2.1.in0
net spwm sum2.1.out hm2_5i20.0.pwmgen.00.value
net senable hm2_5i20.0.pwmgen.00.enable motion.spindle-on
#setp hm2_5i20.0.pwmgen.00.scale 1.0
setp hm2_5i20.0.pwmgen.00.output-type 1
# Documentation from Sangmutan indicates this should be 500Hz-5Kz
# with 1kHz as the recommended PWM frequency, but according to this:
# http://www.cnczone.com/forums/showpost.php?p=796369&postcount=46
# the correct range is 150-200Hz. Another cnczone post has 235Hz:
# http://www.cnczone.com/forums/attachment.php?attachmentid=111177&d=1279485200
# Sangmutan documatation says the PWM should be 10%-90%, but cnczone posts
# show 0% modulation.
setp hm2_5i20.0.pwmgen.pwm_frequency 1000

# invert the step pulse polarity
setp hm2_5i20.0.gpio.000.invert_output TRUE
setp hm2_5i20.0.gpio.002.invert_output TRUE
setp hm2_5i20.0.gpio.016.invert_output TRUE
setp hm2_5i20.0.gpio.018.invert_output TRUE

# configure the stepgen parameters
setp hm2_5i20.0.stepgen.00.dirsetup        5000
setp hm2_5i20.0.stepgen.00.dirhold         2000
setp hm2_5i20.0.stepgen.00.steplen         2000
setp hm2_5i20.0.stepgen.00.stepspace       2000
setp hm2_5i20.0.stepgen.00.position-scale  [AXIS_0]SCALE
setp hm2_5i20.0.stepgen.00.maxaccel        [AXIS_0]STEPGEN_MAXACCEL
setp hm2_5i20.0.stepgen.00.maxvel          [AXIS_0]STEPGEN_MAXVEL
setp hm2_5i20.0.stepgen.01.dirsetup        5000
setp hm2_5i20.0.stepgen.01.dirhold         2000
setp hm2_5i20.0.stepgen.01.steplen         2000
setp hm2_5i20.0.stepgen.01.stepspace       2000
setp hm2_5i20.0.stepgen.01.position-scale  [AXIS_1]SCALE
setp hm2_5i20.0.stepgen.01.maxaccel        [AXIS_1]STEPGEN_MAXACCEL
setp hm2_5i20.0.stepgen.01.maxvel          [AXIS_1]STEPGEN_MAXVEL
setp hm2_5i20.0.stepgen.02.dirsetup        5000
setp hm2_5i20.0.stepgen.02.dirhold         2000
setp hm2_5i20.0.stepgen.02.steplen         2000
setp hm2_5i20.0.stepgen.02.stepspace       2000
setp hm2_5i20.0.stepgen.02.position-scale  [AXIS_2]SCALE
setp hm2_5i20.0.stepgen.02.maxaccel        [AXIS_2]STEPGEN_MAXACCEL
setp hm2_5i20.0.stepgen.02.maxvel          [AXIS_2]STEPGEN_MAXVEL
setp hm2_5i20.0.stepgen.03.dirsetup        5000
setp hm2_5i20.0.stepgen.03.dirhold         2000
setp hm2_5i20.0.stepgen.03.steplen         2000
setp hm2_5i20.0.stepgen.03.stepspace       2000
setp hm2_5i20.0.stepgen.03.position-scale  [AXIS_3]SCALE
setp hm2_5i20.0.stepgen.03.maxaccel        [AXIS_3]STEPGEN_MAXACCEL
setp hm2_5i20.0.stepgen.03.maxvel          [AXIS_3]STEPGEN_MAXVEL

# connect position commands from motion module to step generator
net Xpos-cmd axis.0.motor-pos-cmd => hm2_5i20.0.stepgen.00.position-cmd
net Ypos-cmd axis.1.motor-pos-cmd => hm2_5i20.0.stepgen.01.position-cmd
net Zpos-cmd axis.2.motor-pos-cmd => hm2_5i20.0.stepgen.02.position-cmd
#net Apos-cmd axis.3.motor-pos-cmd => hm2_5i20.0.stepgen.03.position-cmd

# connect position feedback from step generators to motion module
net Xpos-fb hm2_5i20.0.stepgen.00.position-fb => axis.0.motor-pos-fb
net Ypos-fb hm2_5i20.0.stepgen.01.position-fb => axis.1.motor-pos-fb
net Zpos-fb hm2_5i20.0.stepgen.02.position-fb => axis.2.motor-pos-fb
#net Apos-fb hm2_5i20.0.stepgen.03.position-fb => axis.3.motor-pos-fb

# connect enable signals for step generators
net Xen axis.0.amp-enable-out => hm2_5i20.0.stepgen.00.enable
net Yen axis.1.amp-enable-out => hm2_5i20.0.stepgen.01.enable
net Zen axis.2.amp-enable-out => hm2_5i20.0.stepgen.02.enable
#net Aen axis.3.amp-enable-out => hm2_5i20.0.stepgen.03.enable

# enable velocity mode jogging
net Enable axis.0.jog-vel-mode
net Enable axis.1.jog-vel-mode
net Enable axis.2.jog-vel-mode
#net Enable axis.3.jog-vel-mode

# enable the charge pump
net Enable => charge-pump.enable

# connect the charge pump signal to an output
setp hm2_5i20.0.gpio.022.is_output TRUE
net chargepump hm2_5i20.0.gpio.022.out => charge-pump.out

# Safety Relay Monitoring Contacts
net estop-enable hm2_5i20.0.gpio.037.in => iocontrol.0.emc-enable-in
# Run Switch
net machine-on hm2_5i20.0.gpio.039.in_not => halui.machine.on

#connect limit switches to limit inputs
net Xmax hm2_5i20.0.gpio.024.in_not => axis.0.pos-lim-sw-in
net Xmin hm2_5i20.0.gpio.026.in_not => axis.0.neg-lim-sw-in
net Ymax hm2_5i20.0.gpio.028.in_not => axis.1.pos-lim-sw-in
net Ymin hm2_5i20.0.gpio.030.in_not => axis.1.neg-lim-sw-in
net Zmax hm2_5i20.0.gpio.032.in_not => axis.2.pos-lim-sw-in
net Zmin hm2_5i20.0.gpio.034.in_not => axis.2.neg-lim-sw-in

#connect positive limit signals to home inputs
net Xmax => axis.0.home-sw-in
net Ymax => axis.1.home-sw-in
net Zmax => axis.2.home-sw-in

# Spindle Contactor Enable
setp hm2_5i20.0.gpio.041.is_output TRUE
setp hm2_5i20.0.gpio.041.invert_output TRUE
# Door Closed - Energizes spindle contactor
net CLOSED hm2_5i20.0.gpio.029.in_not hm2_5i20.0.gpio.041.out

# Main Contactors - True when both contactors are energized
net ACOK hm2_5i20.0.gpio.035.in_not
# VFD Fault Relay - True when VFD is not faulted
#net VFDOK hm2_5i20.0.gpio.031.in_not
# A Axis Home - On Sieg machines there is no VFD fault signal,
# it is used for the A axis home sensor on 4 axis machines
net VFDOK hm2_5i20.0.gpio.031.in_not
#net AHOME hm2_5i20.0.gpio.031.in axis.3.home-sw-in
# Spindle Contactor - False when spindle contactor is energized
net SOFF hm2_5i20.0.gpio.033.in halui.program.pause motion.feed-hold

# Axis Drive Status Signals
# True when YAKO stepper drive is on a full step position
# True when a Leadshine stepper drive is in the READY state
net DRV0 hm2_5i20.0.gpio.025.in_not
net DRV1 hm2_5i20.0.gpio.027.in_not
net DRV2 hm2_5i20.0.gpio.036.in_not
net DRV3 hm2_5i20.0.gpio.038.in_not

# Output Bits
# F/R
setp hm2_5i20.0.gpio.023.is_output TRUE
#setp hm2_5i20.0.gpio.023.invert_output TRUE
net FR motion.spindle-forward => hm2_5i20.0.gpio.023.out
# ST/SP
setp hm2_5i20.0.gpio.021.is_output TRUE
#setp hm2_5i20.0.gpio.021.invert_output TRUE
net STSP motion.spindle-on => hm2_5i20.0.gpio.021.out
# Flood Coolant
setp hm2_5i20.0.gpio.043.is_output TRUE
setp hm2_5i20.0.gpio.043.invert_output TRUE
net M8 hm2_5i20.0.gpio.043.out => iocontrol.0.coolant-flood
# Mist Coolant
setp hm2_5i20.0.gpio.045.is_output TRUE
setp hm2_5i20.0.gpio.045.invert_output TRUE
net M7 hm2_5i20.0.gpio.045.out => iocontrol.0.coolant-mist

# hook the functions to threads
# read inputs
addf hm2_5i20.0.read servo-thread
addf hm2_5i20.0.read_gpio servo-thread
# handle motion commands
addf motion-command-handler servo-thread
# run the motion controller
addf motion-controller servo-thread
# condition the spindle pwm value
addf limit1.0 servo-thread
addf sum2.0 servo-thread
addf sum2.1 servo-thread
# write outputs
addf hm2_5i20.0.write servo-thread
addf hm2_5i20.0.write_gpio servo-thread
# pet the watchdog so he won't bite us!
addf hm2_5i20.0.pet_watchdog servo-thread
# toggle the charge pump
addf charge-pump servo-thread