MacGalempsy

# Basic 5i25/7i77 configuration

# kinematics
loadrt trivkins

# motion controller
loadrt motmod servo_period_nsec=1000000 num_joints=[TRAJ]AXES

# 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 hm2_pci config="num_encoders=6 sserial_port_0=003xxx"

# 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

# X Axis
setp hm2_5i25.0.encoder.00.scale [AXIS_0]INPUT_SCALE
setp pid.0.Pgain [AXIS_0]P
setp pid.0.Igain [AXIS_0]I
setp pid.0.Dgain [AXIS_0]D
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-amp-fault <= hm2_5i25.0.7i77.0.0.input-01
net x-amp-fault => axis.0.amp-fault-in

#net x-jog-select axis.0.jog-enable <= hm2_5i25.0.7i77.0.0.input-00
net x-minlimit <= hm2_5i25.0.7i77.0.0.input-09-not
net x-minlimit => axis.0.home-sw-in
net x-minlimit => axis.0.neg-lim-sw-in
net x-maxlimit <= hm2_5i25.0.7i77.0.0.input-08-not
net x-maxlimit => axis.0.pos-lim-sw-in

# Y Axis
setp hm2_5i25.0.encoder.01.scale [AXIS_1]INPUT_SCALE
setp pid.1.Pgain [AXIS_1]P
setp pid.1.Igain [AXIS_1]I
setp pid.1.Dgain [AXIS_1]D
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-jog-select axis.1.jog-enable <= hm2_5i25.0.7i77.0.0.input-##
net y-minlimit <= hm2_5i25.0.7i77.0.0.input-11-not
net y-minlimit => axis.1.home-sw-in
net y-minlimit => axis.1.neg-lim-sw-in
net y-maxlimit <= hm2_5i25.0.7i77.0.0.input-10-not
net y-maxlimit => axis.1.pos-lim-sw-in

# Z Axis
setp hm2_5i25.0.encoder.02.scale [AXIS_2]INPUT_SCALE
setp pid.2.Pgain [AXIS_2]P
setp pid.2.Igain [AXIS_2]I
setp pid.2.Dgain [AXIS_2]D
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-jog-select axis.1.jog-enable <= hm2_5i25.0.7i77.0.0.input-###
net z-minlimit <= hm2_5i25.0.7i77.0.0.input-13-not
net z-minlimit => axis.2.home-sw-in
net z-minlimit => axis.2.neg-lim-sw-in
net z-maxlimit <= hm2_5i25.0.7i77.0.0.input-12-not
net z-maxlimit => axis.2.pos-lim-sw-in

# 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

# estop
loadrt estop_latch
addf estop-latch.0 servo-thread
#net estop-loopout iocontrol.0.emc-enable-in <= estop-latch.0.ok-out
#net estop-loopin iocontrol.0.user-enable-out => estop-latch.0.ok-in
net estop-reset iocontrol.0.user-request-enable => estop-latch.0.reset
net remote-estop estop-latch.0.fault-in <=  hm2_5i25.0.7i77.0.0.input-00