# This file was created with the Mesa Configuration Tool on Mar 16 2024 07:56:27
# 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)

# hostmot2 driver
loadrt hostmot2
loadrt [HM2](DRIVER) board_ip=[HM2](ADDRESS) config="sserial_port_0=00000000"

setp hm2_[MESA](BOARD).0.watchdog.timeout_ns 5000000

loadrt pid names=pid.x,pid.y,pid.z

# THREADS
addf hm2_[MESA](BOARD).0.read servo-thread
addf motion-command-handler servo-thread
addf motion-controller servo-thread
addf pid.x.do-pid-calcs servo-thread
addf pid.y.do-pid-calcs servo-thread
addf pid.z.do-pid-calcs servo-thread
addf hm2_[MESA](BOARD).0.write servo-thread

# DPLL TIMER
setp hm2_[MESA](BOARD).0.dpll.01.timer-us -50
setp hm2_[MESA](BOARD).0.stepgen.timer-number 1

# amp enable
net motion-enable <= motion.motion-enabled

# Board: 7i95

# Axis: X Joint: 0 Output: 0
# PID Setup
setp pid.x.Pgain [JOINT_0](P)
setp pid.x.Igain [JOINT_0](I)
setp pid.x.Dgain [JOINT_0](D)
setp pid.x.bias [JOINT_0](BIAS)
setp pid.x.FF0 [JOINT_0](FF0)
setp pid.x.FF1 [JOINT_0](FF1)
setp pid.x.FF2 [JOINT_0](FF2)
setp pid.x.deadband [JOINT_0](DEADBAND)
setp pid.x.maxoutput [JOINT_0](MAX_OUTPUT)
setp pid.x.error-previous-target True
# limit stepgen velocity corrections caused by position feedback jitter
setp pid.x.maxerror [JOINT_0](MAX_ERROR)

# joint-0 enable chain
net joint-0-index-enable <=> pid.x.index-enable
net joint-0-index-enable <=> joint.0.index-enable

net joint-0-enable <= joint.0.amp-enable-out
net joint-0-enable => pid.x.enable

net joint-0-enable => hm2_[MESA](BOARD).0.stepgen.00.enable

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

# --- ESTOP-EXT ---
net estop-ext     <=  hm2_7i95.0.inmux.00.input-18-not

# position command and feedback
net joint-0-pos-cmd <= joint.0.motor-pos-cmd
net joint-0-pos-cmd => pid.x.command

net joint-0-pos-fb <= hm2_[MESA](BOARD).0.stepgen.00.position-fb
net joint-0-pos-fb => joint.0.motor-pos-fb
net joint-0-pos-fb => pid.x.feedback

net joint.0.output <= pid.x.output
net joint.0.output => hm2_[MESA](BOARD).0.stepgen.00.velocity-cmd

# Axis: Y Joint: 1 Output: 1
# PID Setup
setp pid.y.Pgain [JOINT_1](P)
setp pid.y.Igain [JOINT_1](I)
setp pid.y.Dgain [JOINT_1](D)
setp pid.y.bias [JOINT_1](BIAS)
setp pid.y.FF0 [JOINT_1](FF0)
setp pid.y.FF1 [JOINT_1](FF1)
setp pid.y.FF2 [JOINT_1](FF2)
setp pid.y.deadband [JOINT_1](DEADBAND)
setp pid.y.maxoutput [JOINT_1](MAX_OUTPUT)
setp pid.y.error-previous-target True
# limit stepgen velocity corrections caused by position feedback jitter
setp pid.y.maxerror [JOINT_1](MAX_ERROR)

# joint-1 enable chain
net joint-1-index-enable <=> pid.y.index-enable
net joint-1-index-enable <=> joint.1.index-enable

net joint-1-enable <= joint.1.amp-enable-out
net joint-1-enable => pid.y.enable

net joint-1-enable => hm2_[MESA](BOARD).0.stepgen.01.enable

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


# position command and feedback
net joint-1-pos-cmd <= joint.1.motor-pos-cmd
net joint-1-pos-cmd => pid.y.command

net joint-1-pos-fb <= hm2_[MESA](BOARD).0.stepgen.01.position-fb
net joint-1-pos-fb => joint.1.motor-pos-fb
net joint-1-pos-fb => pid.y.feedback

net joint.1.output <= pid.y.output
net joint.1.output => hm2_[MESA](BOARD).0.stepgen.01.velocity-cmd

# Axis: Z Joint: 2 Output: 2
# PID Setup
setp pid.z.Pgain [JOINT_2](P)
setp pid.z.Igain [JOINT_2](I)
setp pid.z.Dgain [JOINT_2](D)
setp pid.z.bias [JOINT_2](BIAS)
setp pid.z.FF0 [JOINT_2](FF0)
setp pid.z.FF1 [JOINT_2](FF1)
setp pid.z.FF2 [JOINT_2](FF2)
setp pid.z.deadband [JOINT_2](DEADBAND)
setp pid.z.maxoutput [JOINT_2](MAX_OUTPUT)
setp pid.z.error-previous-target True
# limit stepgen velocity corrections caused by position feedback jitter
setp pid.z.maxerror [JOINT_2](MAX_ERROR)

# joint-2 enable chain
net joint-2-index-enable <=> pid.z.index-enable
net joint-2-index-enable <=> joint.2.index-enable

net joint-2-enable <= joint.2.amp-enable-out
net joint-2-enable => pid.z.enable

net joint-2-enable => hm2_[MESA](BOARD).0.stepgen.02.enable

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


# position command and feedback
net joint-2-pos-cmd <= joint.2.motor-pos-cmd
net joint-2-pos-cmd => pid.z.command

net joint-2-pos-fb <= hm2_[MESA](BOARD).0.stepgen.02.position-fb
net joint-2-pos-fb => joint.2.motor-pos-fb
net joint-2-pos-fb => pid.z.feedback

net joint.2.output <= pid.z.output
net joint.2.output => hm2_[MESA](BOARD).0.stepgen.02.velocity-cmd


#  ---estop signals---
net estop-out     <=  iocontrol.0.user-enable-out
net estop-ext     =>  iocontrol.0.emc-enable-in
# Standard I/O Block - EStop, Etc
# create a signal for the estop loopback
#net estop-loopback iocontrol.0.emc-enable-in <= iocontrol.0.user-enable-out

# qt dragon
# net spindle-speed-feedback spindle.0.speed-out-rps => spindle.0.speed-in
# net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared
# net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed