# -----------------------------
# Custom EtherCAT and Motion Config
# -----------------------------

# Load kinematics and motion modules
loadrt [KINS]KINEMATICS
loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS

# Load EtherCAT configuration from XML
loadusr -W lcec_conf ethercat-conf.xml

# Load EtherCAT and CIA402 drivers
loadrt lcec
loadrt cia402 count=1

# Add EtherCAT and motion processing functions to the servo thread
addf    lcec.read-all            servo-thread
addf    cia402.0.read-all        servo-thread
addf    motion-command-handler   servo-thread
addf    motion-controller        servo-thread
addf    cia402.0.write-all       servo-thread
addf    lcec.write-all           servo-thread

# Enable EMC control
setp iocontrol.0.emc-enable-in 1


# -----------------------------
# AXIS X Configuration
# -----------------------------

# Set CIA402 into Profile Position Mode
setp cia402.0.csp-mode 1
# pos-scale = (encoder counts per revolution) × (mechanical ratio) ÷ (units per revolution)
setp cia402.0.pos-scale 13107.2

# Motion signal mappings for axis X
net x-enable      <= joint.0.amp-enable-out  => cia402.0.enable
net x-amp-fault   => joint.0.amp-fault-in     <= cia402.0.drv-fault
net x-pos-cmd     <= joint.0.motor-pos-cmd    => cia402.0.pos-cmd
net x-pos-fb      => joint.0.motor-pos-fb     <= cia402.0.pos-fb

# EtherCAT to CIA402 driver mapping
net x-statusword      lcec.0.A6.status-word       => cia402.0.statusword
net x-drv-act-pos     lcec.0.A6.pos-actual        => cia402.0.drv-actual-position

# CIA402 driver to EtherCAT mapping
net x-controlword         cia402.0.controlword            => lcec.0.A6.control-word
net x-drv-target-pos      cia402.0.drv-target-position    => lcec.0.A6.target-position

# -----------------------------
# PROBE Configuration
# -----------------------------

# Map the processed probe status to a HAL net
net di5-input <= lcec.0.A6.DI5

# Link DI5 and probe-input
net di5-input => motion.probe-input