# core HAL config file for XYZC lathe simulation

# Load required RT modules
loadrt [KINS]KINEMATICS
loadrt [EMCMOT]EMCMOT base_period_nsec=[EMCMOT]BASE_PERIOD servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS num_spindles=[TRAJ]SPINDLES num_dio=6 num_aio=3

# Load differentiators for each axis
loadrt ddt names=ddt_x,ddt_xv,ddt_y,ddt_yv,ddt_z,ddt_zv,ddt_c,ddt_cv
# Load hypot blocks for velocity calculations
loadrt hypot names=vel_xy,vel_xyz

# Add functions to servo thread
addf motion-command-handler servo-thread
addf motion-controller servo-thread
addf ddt_x servo-thread
addf ddt_xv servo-thread
addf ddt_y servo-thread
addf ddt_yv servo-thread
addf ddt_z servo-thread
addf ddt_zv servo-thread
addf ddt_c servo-thread
addf ddt_cv servo-thread
addf vel_xy servo-thread
addf vel_xyz servo-thread

# Position command/feedback loopback for each joint
net Xpos joint.0.motor-pos-cmd => joint.0.motor-pos-fb ddt_x.in
net Ypos joint.1.motor-pos-cmd => joint.1.motor-pos-fb ddt_y.in
net Zpos joint.2.motor-pos-cmd => joint.2.motor-pos-fb ddt_z.in
net Cpos joint.3.motor-pos-cmd => joint.3.motor-pos-fb ddt_c.in

# Velocity and acceleration signals for each axis
net Xvel ddt_x.out => ddt_xv.in vel_xy.in0 vel_xyz.in0
net Xacc <= ddt_xv.out
net Yvel ddt_y.out => ddt_yv.in vel_xy.in1 vel_xyz.in1
net Yacc <= ddt_yv.out
net Zvel ddt_z.out => ddt_zv.in vel_xyz.in2
net Zacc <= ddt_zv.out
net Cvel ddt_c.out => ddt_cv.in
net Cacc <= ddt_cv.out

# Cartesian velocities
net XYvel <= vel_xy.out
net XYZvel <= vel_xyz.out

# estop loopback
net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in

# ---manual tool change signals---
net tool-change-request    <= iocontrol.0.tool-change
net tool-change-confirmed  => iocontrol.0.tool-changed
net tool-number            <= iocontrol.0.tool-prep-number

# tool loading loopback
net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared
