# BP Series 1
loadrt trivkins
loadrt motmod base_period_nsec=100000 servo_period_nsec=1000000 num_joints=[TRAJ]AXES

# hostmot2 drivers
loadrt hostmot2
loadrt hm2_pci config="num_encoders=6 sserial_port_0=002xxx"

# pid
loadrt pid num_chan=3

setp hm2_5i25.0.watchdog.timeout_ns 10000000

# Threads
addf hm2_5i25.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_5i25.0.write servo-thread
addf hm2_5i25.0.pet_watchdog  servo-thread

# MPG
loadrt mux4 names=mpg-scale
addf mpg-scale servo-thread
loadrt or2 count=2
addf or2.0 servo-thread
addf or2.1 servo-thread

net jog-position <= hm2_5i25.0.encoder.05.count
net jog-position => axis.0.jog-counts
net jog-position => axis.1.jog-counts
net jog-position => axis.2.jog-counts

setp mpg-scale.in0 0.0
setp mpg-scale.in1 0.00025
setp mpg-scale.in2 0.0025
setp mpg-scale.in3 0.025

net scale1 or2.0.in0 <= hm2_5i25.0.7i77.0.0.input-03
net scale2 or2.1.in0 <= hm2_5i25.0.7i77.0.0.input-04
net scale3 or2.0.in1 or2.1.in1 <= hm2_5i25.0.7i77.0.0.input-05

net pos2 mpg-scale.sel0 <= or2.0.out
net pos3 mpg-scale.sel1 <= or2.1.out

net axis-mpg-jog-scale axis.0.jog-scale <= mpg-scale.out
net axis-mpg-jog-scale axis.1.jog-scale <= mpg-scale.out
net axis-mpg-jog-scale axis.2.jog-scale <= mpg-scale.out

# Servo Drive
net power-on hm2_5i25.0.7i77.0.0.output-00 <= motion.motion-enabled
net power-on hm2_5i25.0.7i77.0.0.output-01

# X Axis
setp hm2_5i25.0.encoder.00.scale -30000
setp pid.0.Pgain [AXIS_0]P_GAIN
setp pid.0.Igain [AXIS_0]I_GAIN
setp pid.0.Dgain [AXIS_0]D_GAIN
setp pid.0.FF0 [AXIS_0]FF0
setp pid.0.FF1 [AXIS_0]FF1
setp pid.0.FF2 [AXIS_0]FF2

net x-axis-enable pid.0.enable <= axis.0.amp-enable-out
net x-axis-enable hm2_5i25.0.7i77.0.1.analogena
net x-axis-fb pid.0.feedback <= hm2_5i25.0.encoder.00.position
net x-axis-fb axis.0.motor-pos-fb
net x-axis-pos-cmd axis.0.motor-pos-cmd => pid.0.command
net x-axis-command  pid.0.output  =>  hm2_5i25.0.7i77.0.1.analogout0

net x-jog-select axis.0.jog-enable <= hm2_5i25.0.7i77.0.0.input-00

net x-limits <= hm2_5i25.0.7i77.0.0.input-09-not
net x-limits => axis.0.pos-lim-sw-in axis.0.neg-lim-sw-in
net x-limits => axis.0.home-sw-in

# Y Axis
setp hm2_5i25.0.encoder.01.scale -30000
setp pid.1.Pgain [AXIS_1]P_GAIN
setp pid.1.Igain [AXIS_1]I_GAIN
setp pid.1.Dgain [AXIS_1]D_GAIN
setp pid.1.FF0 [AXIS_1]FF0
setp pid.1.FF1 [AXIS_1]FF1
setp pid.1.FF2 [AXIS_1]FF2

net y-axis-enable pid.1.enable <= axis.1.amp-enable-out
net y-axis-fb axis.1.motor-pos-fb <= hm2_5i25.0.encoder.01.position
net y-axis-fb pid.1.feedback <= hm2_5i25.0.encoder.01.position
net y-axis-fb axis.1.motor-pos-fb
net y-axis-pos-cmd axis.1.motor-pos-cmd => pid.1.command
net y-axis-command  pid.1.output  =>  hm2_5i25.0.7i77.0.1.analogout1

net y-limits <= hm2_5i25.0.7i77.0.0.input-10-not
net y-limits => axis.1.pos-lim-sw-in axis.1.neg-lim-sw-in
net y-limits => axis.1.home-sw-in

# Z Axis
setp hm2_5i25.0.encoder.02.scale -32000
setp pid.2.Pgain [AXIS_2]P_GAIN
setp pid.2.Igain [AXIS_2]I_GAIN
setp pid.2.Dgain [AXIS_2]D_GAIN
setp pid.2.FF0 [AXIS_2]FF0
setp pid.2.FF1 [AXIS_2]FF1
setp pid.2.FF2 [AXIS_2]FF2

net z-axis-enable pid.2.enable <= axis.2.amp-enable-out
net z-axis-fb axis.2.motor-pos-fb <= hm2_5i25.0.encoder.02.position
net z-axis-fb pid.2.feedback <= hm2_5i25.0.encoder.02.position
net z-axis-fb axis.2.motor-pos-fb
net z-axis-pos-cmd axis.2.motor-pos-cmd => pid.2.command
net z-axis-command  pid.2.output  =>  hm2_5i25.0.7i77.0.1.analogout2

net z-limits <= hm2_5i25.0.7i77.0.0.input-11-not
net z-limits => axis.2.pos-lim-sw-in axis.2.neg-lim-sw-in
net z-limits => axis.2.home-sw-in

# estop chain
net estop-out <= iocontrol.0.user-enable-out
net estop-out => iocontrol.0.emc-enable-in

# Spindle Drive
loadusr -Wn spindle-vfd gs2_vfd -s 2 -p none -n spindle-vfd -v
# connect the spindle direction pin to the GS2
net gs2-fwd spindle-vfd.spindle-fwd <= motion.spindle-forward
# connect the spindle on pin to the GS2
net gs2-run spindle-vfd.spindle-on <= motion.spindle-on
# connect the GS2 at speed to the motion at speed
net gs2-at-speed motion.spindle-at-speed <= spindle-vfd.at-speed
# connect the spindle RPM to the GS2
net gs2-RPM spindle-vfd.speed-command <= motion.spindle-speed-out
#vfd panel fan
net gs2-run => hm2_5i25.0.7i77.0.0.output-03

# Lube Pump
loadrt comp
addf comp.0 servo-thread
setp comp.0.in0 0.0001
net motion-test comp.0.in1 <= motion.current-vel
net in-motion hm2_5i25.0.7i77.0.0.output-02 <= comp.0.out
net in-motion hm2_5i25.0.7i77.0.0.output-04

# manual tool change
loadusr -W hal_manualtoolchange
net tool-change iocontrol.0.tool-change => hal_manualtoolchange.change
net tool-changed iocontrol.0.tool-changed <= hal_manualtoolchange.changed
net tool-number iocontrol.0.tool-prep-number => hal_manualtoolchange.number
net tool-prepare-loopback iocontrol.0.tool-prepare => iocontrol.0.tool-prepared