# This file was created with the 7i96 Wizard on Jun 10 2019 11:12:47
# If you make changes to this file DO NOT run the configuration tool again!
# This file will be replaced with a new file if you do!

# kinematics
loadrt [KINS]KINEMATICS

# motion controller
loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS

# standard components
loadrt pid num_chan=4 

# hostmot2 driver
#loadrt [HOSTMOT2](DRIVER) board_ip=[HOSTMOT2](IPADDRESS) config="num_encoders=[HOSTMOT2](ENCODERS)num_stepgens=[HOSTMOT2
(STEPGENS)"sserial_port_0=[HOSTMOT2](SSERIAL_PORT)
#setp hm2_[HOSTMOT2](BOARD).0.watchdog.timeout_ns 25000000

# THREADS
#addf hm2_[HOSTMOT2](BOARD).0.read servo-thread
addf motion-command-handler servo-thread
addf motion-controller servo-thread
#setp hm2_[HOSTMOT2](BOARD).0.dpll.01.timer-us -100
#setp hm2_[HOSTMOT2](BOARD).0.stepgen.timer-number 1 
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 pid.3.do-pid-calcs servo-thread
#addf hm2_[HOSTMOT2](BOARD).0.write servo-thread
#addf hm2_[HOSTMOT2](BOARD).0.read servo-thread

# input switch debouncing
loadrt debounce cfg=4
addf debounce.0 servo-thread
setp debounce.0.delay 4

# near component for spindle at speed
loadrt near
addf near.0 servo-thread
setp near.0.scale 1.01

# Joint 0
# axis enable chain
newsig emcmot.0.enable bit
sets emcmot.0.enable FALSE
net emcmot.0.enable <= joint.0.amp-enable-out
net emcmot.0.enable => hm2_[HOSTMOT2](BOARD).0.stepgen.00.enable pid.0.enable

# position command and feedback
net emcmot.0.pos-cmd joint.0.motor-pos-cmd => pid.0.command
net motor.0.pos-fb <= hm2_[HOSTMOT2](BOARD).0.stepgen.00.position-fb joint.0.motor-pos-fb pid.0.feedback
net motor.0.command pid.0.output hm2_[HOSTMOT2](BOARD).0.stepgen.00.velocity-cmd
setp pid.0.error-previous-target true

setp hm2_[HOSTMOT2](BOARD).0.stepgen.00.dirsetup [JOINT_0]DIRSETUP
setp hm2_[HOSTMOT2](BOARD).0.stepgen.00.dirhold [JOINT_0]DIRHOLD
setp hm2_[HOSTMOT2](BOARD).0.stepgen.00.steplen [JOINT_0]STEPLEN
setp hm2_[HOSTMOT2](BOARD).0.stepgen.00.stepspace [JOINT_0]STEPSPACE
setp hm2_[HOSTMOT2](BOARD).0.stepgen.00.position-scale [JOINT_0]SCALE
setp hm2_[HOSTMOT2](BOARD).0.stepgen.00.maxvel [JOINT_0]STEPGEN_MAX_VEL
setp hm2_[HOSTMOT2](BOARD).0.stepgen.00.maxaccel [JOINT_0]STEPGEN_MAX_ACC
setp hm2_[HOSTMOT2](BOARD).0.stepgen.00.step_type 0
setp hm2_[HOSTMOT2](BOARD).0.stepgen.00.control-type 1

setp pid.0.Pgain [JOINT_0]P
setp pid.0.Igain [JOINT_0]I
setp pid.0.Dgain [JOINT_0]D
setp pid.0.bias [JOINT_0]BIAS
setp pid.0.FF0 [JOINT_0]FF0
setp pid.0.FF1 [JOINT_0]FF1
setp pid.0.FF2 [JOINT_0]FF2
setp pid.0.deadband [JOINT_0]DEADBAND
setp pid.0.maxoutput [JOINT_0]MAX_OUTPUT
setp pid.0.maxerror [JOINT_0]MAX_ERROR

# Joint 1
# axis enable chain
newsig emcmot.1.enable bit
sets emcmot.1.enable FALSE
net emcmot.1.enable <= joint.1.amp-enable-out
net emcmot.1.enable => hm2_[HOSTMOT2](BOARD).0.stepgen.01.enable pid.1.enable

# position command and feedback
net emcmot.1.pos-cmd joint.1.motor-pos-cmd => pid.1.command
net motor.1.pos-fb <= hm2_[HOSTMOT2](BOARD).0.stepgen.01.position-fb joint.1.motor-pos-fb pid.1.feedback
net motor.1.command pid.1.output hm2_[HOSTMOT2](BOARD).0.stepgen.01.velocity-cmd
setp pid.1.error-previous-target true

setp hm2_[HOSTMOT2](BOARD).0.stepgen.01.dirsetup [JOINT_1]DIRSETUP
setp hm2_[HOSTMOT2](BOARD).0.stepgen.01.dirhold [JOINT_1]DIRHOLD
setp hm2_[HOSTMOT2](BOARD).0.stepgen.01.steplen [JOINT_1]STEPLEN
setp hm2_[HOSTMOT2](BOARD).0.stepgen.01.stepspace [JOINT_1]STEPSPACE
setp hm2_[HOSTMOT2](BOARD).0.stepgen.01.position-scale [JOINT_1]SCALE
setp hm2_[HOSTMOT2](BOARD).0.stepgen.01.maxvel [JOINT_1]STEPGEN_MAX_VEL
setp hm2_[HOSTMOT2](BOARD).0.stepgen.01.maxaccel [JOINT_1]STEPGEN_MAX_ACC
setp hm2_[HOSTMOT2](BOARD).0.stepgen.01.step_type 0
setp hm2_[HOSTMOT2](BOARD).0.stepgen.01.control-type 1

setp pid.1.Pgain [JOINT_1]P
setp pid.1.Igain [JOINT_1]I
setp pid.1.Dgain [JOINT_1]D
setp pid.1.bias [JOINT_1]BIAS
setp pid.1.FF0 [JOINT_1]FF0
setp pid.1.FF1 [JOINT_1]FF1
setp pid.1.FF2 [JOINT_1]FF2
setp pid.1.deadband [JOINT_1]DEADBAND
setp pid.1.maxoutput [JOINT_1]MAX_OUTPUT
setp pid.1.maxerror [JOINT_1]MAX_ERROR

# Joint 2
# axis enable chain
newsig emcmot.2.enable bit
sets emcmot.2.enable FALSE
net emcmot.2.enable <= joint.2.amp-enable-out
net emcmot.2.enable => hm2_[HOSTMOT2](BOARD).0.stepgen.02.enable pid.2.enable

# position command and feedback
net emcmot.2.pos-cmd joint.2.motor-pos-cmd => pid.2.command
net motor.2.pos-fb <= hm2_[HOSTMOT2](BOARD).0.stepgen.02.position-fb joint.2.motor-pos-fb pid.2.feedback
net motor.2.command pid.2.output hm2_[HOSTMOT2](BOARD).0.stepgen.02.velocity-cmd
setp pid.2.error-previous-target true

setp hm2_[HOSTMOT2](BOARD).0.stepgen.02.dirsetup [JOINT_2]DIRSETUP
setp hm2_[HOSTMOT2](BOARD).0.stepgen.02.dirhold [JOINT_2]DIRHOLD
setp hm2_[HOSTMOT2](BOARD).0.stepgen.02.steplen [JOINT_2]STEPLEN
setp hm2_[HOSTMOT2](BOARD).0.stepgen.02.stepspace [JOINT_2]STEPSPACE
setp hm2_[HOSTMOT2](BOARD).0.stepgen.02.position-scale [JOINT_2]SCALE
setp hm2_[HOSTMOT2](BOARD).0.stepgen.02.maxvel [JOINT_2]STEPGEN_MAX_VEL
setp hm2_[HOSTMOT2](BOARD).0.stepgen.02.maxaccel [JOINT_2]STEPGEN_MAX_ACC
setp hm2_[HOSTMOT2](BOARD).0.stepgen.02.step_type 0
setp hm2_[HOSTMOT2](BOARD).0.stepgen.02.control-type 1

setp pid.2.Pgain [JOINT_2]P
setp pid.2.Igain [JOINT_2]I
setp pid.2.Dgain [JOINT_2]D
setp pid.2.bias [JOINT_2]BIAS
setp pid.2.FF0 [JOINT_2]FF0
setp pid.2.FF1 [JOINT_2]FF1
setp pid.2.FF2 [JOINT_2]FF2
setp pid.2.deadband [JOINT_2]DEADBAND
setp pid.2.maxoutput [JOINT_2]MAX_OUTPUT
setp pid.2.maxerror [JOINT_2]MAX_ERROR

# Joint 3
# axis enable chain
newsig emcmot.3.enable bit
sets emcmot.3.enable FALSE
net emcmot.3.enable <= joint.3.amp-enable-out
net emcmot.3.enable => hm2_[HOSTMOT2](BOARD).0.stepgen.03.enable pid.3.enable

# position command and feedback
net emcmot.3.pos-cmd joint.3.motor-pos-cmd => pid.3.command
net motor.3.pos-fb <= hm2_[HOSTMOT2](BOARD).0.stepgen.03.position-fb joint.3.motor-pos-fb pid.3.feedback
net motor.3.command pid.3.output hm2_[HOSTMOT2](BOARD).0.stepgen.03.velocity-cmd
setp pid.3.error-previous-target true

setp hm2_[HOSTMOT2](BOARD).0.stepgen.03.dirsetup [JOINT_3]DIRSETUP
setp hm2_[HOSTMOT2](BOARD).0.stepgen.03.dirhold [JOINT_3]DIRHOLD
setp hm2_[HOSTMOT2](BOARD).0.stepgen.03.steplen [JOINT_3]STEPLEN
setp hm2_[HOSTMOT2](BOARD).0.stepgen.03.stepspace [JOINT_3]STEPSPACE
setp hm2_[HOSTMOT2](BOARD).0.stepgen.03.position-scale [JOINT_3]SCALE
setp hm2_[HOSTMOT2](BOARD).0.stepgen.03.maxvel [JOINT_3]STEPGEN_MAX_VEL
setp hm2_[HOSTMOT2](BOARD).0.stepgen.03.maxaccel [JOINT_3]STEPGEN_MAX_ACC
setp hm2_[HOSTMOT2](BOARD).0.stepgen.03.step_type 0
setp hm2_[HOSTMOT2](BOARD).0.stepgen.03.control-type 1

setp pid.3.Pgain [JOINT_3]P
setp pid.3.Igain [JOINT_3]I
setp pid.3.Dgain [JOINT_3]D
setp pid.3.bias [JOINT_3]BIAS
setp pid.3.FF0 [JOINT_3]FF0
setp pid.3.FF1 [JOINT_3]FF1
setp pid.3.FF2 [JOINT_3]FF2
setp pid.3.deadband [JOINT_3]DEADBAND
setp pid.3.maxoutput [JOINT_3]MAX_OUTPUT
setp pid.3.maxerror [JOINT_3]MAX_ERROR