# This file was created with the Mesa Configuration Tool on Nov 19 2023 16:07:41
# 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.z0,pid.z1,pid.x,pid.y

# THREADS
addf hm2_[MESA](BOARD).0.read servo-thread
addf motion-command-handler servo-thread
addf motion-controller servo-thread
addf pid.z0.do-pid-calcs servo-thread
addf pid.z1.do-pid-calcs servo-thread
addf pid.x.do-pid-calcs servo-thread
addf pid.y.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

# Joint 0 Axis Z
# PID Setup
setp pid.z0.Pgain [JOINT_0](P)
setp pid.z0.Igain [JOINT_0](I)
setp pid.z0.Dgain [JOINT_0](D)
setp pid.z0.bias [JOINT_0](BIAS)
setp pid.z0.FF0 [JOINT_0](FF0)
setp pid.z0.FF1 [JOINT_0](FF1)
setp pid.z0.FF2 [JOINT_0](FF2)
setp pid.z0.deadband [JOINT_0](DEADBAND)
setp pid.z0.maxoutput [JOINT_0](MAX_OUTPUT)
setp pid.z0.maxerror [JOINT_0](MAX_ERROR)
setp pid.z0.error-previous-target true

# joint-0 enable chain
net joint-0-index-enable <=> pid.z0.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.z0.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


# position command and feedback
net joint-0-pos-cmd <= joint.0.motor-pos-cmd
net joint-0-pos-cmd => pid.z0.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.z0.feedback

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

# Joint 1 Axis Z
# PID Setup
setp pid.z1.Pgain [JOINT_1](P)
setp pid.z1.Igain [JOINT_1](I)
setp pid.z1.Dgain [JOINT_1](D)
setp pid.z1.bias [JOINT_1](BIAS)
setp pid.z1.FF0 [JOINT_1](FF0)
setp pid.z1.FF1 [JOINT_1](FF1)
setp pid.z1.FF2 [JOINT_1](FF2)
setp pid.z1.deadband [JOINT_1](DEADBAND)
setp pid.z1.maxoutput [JOINT_1](MAX_OUTPUT)
setp pid.z1.maxerror [JOINT_1](MAX_ERROR)
setp pid.z1.error-previous-target true

# joint-1 enable chain
net joint-1-index-enable <=> pid.z1.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.z1.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.z1.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.z1.feedback

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

# Joint 2 Axis X
# PID Setup
setp pid.x.Pgain [JOINT_2](P)
setp pid.x.Igain [JOINT_2](I)
setp pid.x.Dgain [JOINT_2](D)
setp pid.x.bias [JOINT_2](BIAS)
setp pid.x.FF0 [JOINT_2](FF0)
setp pid.x.FF1 [JOINT_2](FF1)
setp pid.x.FF2 [JOINT_2](FF2)
setp pid.x.deadband [JOINT_2](DEADBAND)
setp pid.x.maxoutput [JOINT_2](MAX_OUTPUT)
setp pid.x.maxerror [JOINT_2](MAX_ERROR)
setp pid.x.error-previous-target true

# joint-2 enable chain
net joint-2-index-enable <=> pid.x.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.x.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.x.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.x.feedback

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

# Joint 3 Axis Y
# PID Setup
setp pid.y.Pgain [JOINT_3](P)
setp pid.y.Igain [JOINT_3](I)
setp pid.y.Dgain [JOINT_3](D)
setp pid.y.bias [JOINT_3](BIAS)
setp pid.y.FF0 [JOINT_3](FF0)
setp pid.y.FF1 [JOINT_3](FF1)
setp pid.y.FF2 [JOINT_3](FF2)
setp pid.y.deadband [JOINT_3](DEADBAND)
setp pid.y.maxoutput [JOINT_3](MAX_OUTPUT)
setp pid.y.maxerror [JOINT_3](MAX_ERROR)
setp pid.y.error-previous-target true

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

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

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

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


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

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

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