loadrt [KINS]KINEMATICS
loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS
loadrt litexcnc
loadrt litexcnc_eth config_file="[LITEXCNC]CONFIG_FILE"
loadrt pos2vel number=6
loadusr -W hal_manualtoolchange
# Add the functions to the thread
addf [LITEXCNC](NAME).read servo-thread
addf motion-command-handler servo-thread
addf motion-controller servo-thread
addf pos2vel.convert servo-thread
addf [LITEXCNC](NAME).write servo-thread
########################################################################
#STEPGEN
########################################################################
# - timings (prevent re-calculation)
net pos2vel.period-s <= [LITEXCNC](NAME).stepgen.period-s
net pos2vel.period-s-recip <= [LITEXCNC](NAME).stepgen.period-s-recip
#STEPGEN - X-AXIS
########################################################################
# POS2VEL
# - position control
net xpos-fb <= [LITEXCNC](NAME).stepgen.00.position_prediction
net xpos-fb => joint.0.motor-pos-fb
net xpos-fb => pos2vel.0.position-feedback
net xvel-fb pos2vel.0.velocity-feedback <= [LITEXCNC](NAME).stepgen.00.velocity-prediction
net xpos-cmd pos2vel.0.position-cmd <= joint.0.motor-pos-cmd
# - settings
setp pos2vel.0.max-acceleration [JOINT_0]STEPGEN_MAXACCEL
# setp pos2vel.0.debug 1
# STEPGEN
# - Setup of timings
setp [LITEXCNC](NAME).stepgen.00.position-scale [JOINT_0]SCALE
setp [LITEXCNC](NAME).stepgen.00.steplen 5000
setp [LITEXCNC](NAME).stepgen.00.stepspace 5000
setp [LITEXCNC](NAME).stepgen.00.dir-hold-time 10000
setp [LITEXCNC](NAME).stepgen.00.dir-setup-time 10000
setp [LITEXCNC](NAME).stepgen.00.max-velocity [JOINT_0]MAX_VELOCITY
setp [LITEXCNC](NAME).stepgen.00.max-acceleration [JOINT_0]STEPGEN_MAXACCEL
# setp [LITEXCNC](NAME).stepgen.00.debug 1
# - Connect velocity command
net xvel_cmd pos2vel.0.velocity-cmd => [LITEXCNC](NAME).stepgen.00.velocity-cmd
# - Set the acceleration to be used (NOTE: pos2vel has fixed acceleration)
setp [LITEXCNC](NAME).stepgen.00.acceleration-cmd [JOINT_0]STEPGEN_MAXACCEL
# - enable the drive
net xenable joint.0.amp-enable-out => [LITEXCNC](NAME).stepgen.00.enable
#STEPGEN - Y-AXIS
########################################################################
# POS2VEL
# - position control
net ypos-fb <= [LITEXCNC](NAME).stepgen.01.position_prediction
net ypos-fb => joint.1.motor-pos-fb
net ypos-fb => pos2vel.1.position-feedback
net yvel-fb pos2vel.1.velocity-feedback <= [LITEXCNC](NAME).stepgen.01.velocity-prediction
net ypos-cmd pos2vel.1.position-cmd <= joint.1.motor-pos-cmd
# - settings
setp pos2vel.1.max-acceleration [JOINT_1]STEPGEN_MAXACCEL
# setp pos2vel.0.debug 1
# STEPGEN
# - Setup of timings
setp [LITEXCNC](NAME).stepgen.01.position-scale [JOINT_1]SCALE
setp [LITEXCNC](NAME).stepgen.01.steplen 5000
setp [LITEXCNC](NAME).stepgen.01.stepspace 5000
setp [LITEXCNC](NAME).stepgen.01.dir-hold-time 10000
setp [LITEXCNC](NAME).stepgen.01.dir-setup-time 10000
setp [LITEXCNC](NAME).stepgen.01.max-velocity [JOINT_1]MAX_VELOCITY
setp [LITEXCNC](NAME).stepgen.01.max-acceleration [JOINT_1]STEPGEN_MAXACCEL
# setp [LITEXCNC](NAME).stepgen.00.debug 1
# - Connect velocity command
net yvel_cmd pos2vel.1.velocity-cmd => [LITEXCNC](NAME).stepgen.01.velocity-cmd
# - Set the acceleration to be used (NOTE: pos2vel has fixed acceleration)
setp [LITEXCNC](NAME).stepgen.01.acceleration-cmd [JOINT_1]STEPGEN_MAXACCEL
# - enable the drive
net yenable joint.1.amp-enable-out => [LITEXCNC](NAME).stepgen.01.enable
#//////////////////////////////////////////////////////////////////////////////
#STEPGEN - Z-AXIS
########################################################################
# POS2VEL
# - position control
net zpos-fb <= [LITEXCNC](NAME).stepgen.02.position_prediction
net zpos-fb => joint.2.motor-pos-fb
net zpos-fb => pos2vel.2.position-feedback
net zvel-fb pos2vel.2.velocity-feedback <= [LITEXCNC](NAME).stepgen.02.velocity-prediction
net zpos-cmd pos2vel.2.position-cmd <= joint.2.motor-pos-cmd
# - settings
setp pos2vel.2.max-acceleration [JOINT_2]STEPGEN_MAXACCEL
# setp pos2vel.0.debug 1
# STEPGEN
# - Setup of timings
setp [LITEXCNC](NAME).stepgen.02.position-scale [JOINT_2]SCALE
setp [LITEXCNC](NAME).stepgen.02.steplen 5000
setp [LITEXCNC](NAME).stepgen.02.stepspace 5000
setp [LITEXCNC](NAME).stepgen.02.dir-hold-time 10000
setp [LITEXCNC](NAME).stepgen.02.dir-setup-time 10000
setp [LITEXCNC](NAME).stepgen.02.max-velocity [JOINT_2]MAX_VELOCITY
setp [LITEXCNC](NAME).stepgen.02.max-acceleration [JOINT_2]STEPGEN_MAXACCEL
# setp [LITEXCNC](NAME).stepgen.00.debug 1
# - Connect velocity command
net zvel_cmd pos2vel.2.velocity-cmd => [LITEXCNC](NAME).stepgen.02.velocity-cmd
# - Set the acceleration to be used (NOTE: pos2vel has fixed acceleration)
setp [LITEXCNC](NAME).stepgen.02.acceleration-cmd [JOINT_2]STEPGEN_MAXACCEL
# - enable the drive
net zenable joint.2.amp-enable-out => [LITEXCNC](NAME).stepgen.02.enable
#net estop-out => [LITEXCNC](NAME).gpio.j1:8.out
# toolchange setup
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