# ======================================================================== # ONLINE HAL (EtherCAT enabled) # - Clean at-speed logic (fixed 50 RPM tolerance) # - No non-stock components (no max2/compare) # - Single link for spindle.0.speed-out via spindle-vel-cmd # - Drum absolute: reuse u-drv-act-pos (no double-link) # ======================================================================== # --- Core subsystems --------------------------------------------------------- loadrt [KINS]KINEMATICS loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS num_dio=6 num_aio=3 addf motion-command-handler servo-thread addf motion-controller servo-thread # --- EtherCAT / CiA-402 ----------------------------------------------------- loadusr -W lcec_conf ethercat-conf.xml loadrt lcec loadrt cia402 count=4 # --- Control / helper components ------------------------------------------- loadrt pid num_chan=2 loadrt orient names=orient loadrt edge count=1 loadrt near names=near-orient,spindle-at-speed-near loadrt mux2 names=orient-mux,zlimit_mux loadrt or2 count=1 loadrt not count=2 loadrt and2 count=2 loadrt conv_s32_float names=udrv_cnt2float # Math/scale blocks loadrt abs names=orient-abs,rev-abs,spindle_rpm_err_abs loadrt sum2 names=spindle_rpm_err loadrt scale names=spindle_freq_to_rps,spindle_rps_to_rpm,cmd_rpm_to_rps,spindle_fb_rps_to_rpm,spindle_cmd_rps_to_rpm,drum_cnt_to_rev # --- Function add order ----------------------------------------------------- addf lcec.read-all servo-thread addf cia402.0.read-all servo-thread addf cia402.1.read-all servo-thread addf cia402.2.read-all servo-thread addf cia402.3.read-all servo-thread addf edge.0 servo-thread addf near-orient servo-thread addf orient-mux servo-thread addf orient servo-thread addf pid.0.do-pid-calcs servo-thread addf pid.1.do-pid-calcs servo-thread addf orient-abs servo-thread addf rev-abs servo-thread addf spindle_freq_to_rps servo-thread addf spindle_rps_to_rpm servo-thread addf cmd_rpm_to_rps servo-thread addf spindle_fb_rps_to_rpm servo-thread addf spindle_cmd_rps_to_rpm servo-thread addf spindle_rpm_err servo-thread addf spindle_rpm_err_abs servo-thread addf spindle-at-speed-near servo-thread addf drum_cnt_to_rev servo-thread addf udrv_cnt2float servo-thread addf zlimit_mux servo-thread addf or2.0 servo-thread addf not.0 servo-thread addf and2.0 servo-thread addf and2.1 servo-thread addf cia402.0.write-all servo-thread addf cia402.1.write-all servo-thread addf cia402.2.write-all servo-thread addf cia402.3.write-all servo-thread addf lcec.write-all servo-thread # --- Edge detector (optional) ----------------------------------------------- setp edge.0.both 0 setp edge.0.in-edge 0 setp edge.0.out-width-ns 10000000 # --- Spindle orient / PID setup --------------------------------------------- setp orient.tolerance 0 setp pid.0.Pgain [SPINDLE]PGAIN_V setp pid.0.Igain [SPINDLE]IGAIN_V setp pid.0.Dgain [SPINDLE]DGAIN_V setp pid.0.FF0 [SPINDLE]FF0_V setp pid.0.FF1 [SPINDLE]FF1_V setp pid.0.error-previous-target 1 setp pid.1.Pgain [SPINDLE]PGAIN_P setp pid.1.Igain [SPINDLE]IGAIN_P setp pid.1.Dgain [SPINDLE]DGAIN_P setp pid.1.FF0 [SPINDLE]FF0_P setp pid.1.FF1 [SPINDLE]FF1_P # --- CiA-402 base setup ------------------------------------------------------ setp cia402.0.csp-mode 1 setp cia402.1.csp-mode 1 setp cia402.2.csp-mode 1 setp cia402.3.csp-mode 1 setp cia402.0.pos-scale 1677721.6 setp cia402.1.pos-scale 1677721.6 setp cia402.2.pos-scale 1677721.6 setp cia402.3.pos-scale 364.0888888889 # --- X axis nets ------------------------------------------------------------- net x-statusword lcec.0.Xdrive.cia-statusword => cia402.0.statusword net x-opmode-display lcec.0.Xdrive.opmode-display => cia402.0.opmode-display net x-drv-act-pos lcec.0.Xdrive.actual-position => cia402.0.drv-actual-position net x-controlword cia402.0.controlword => lcec.0.Xdrive.cia-controlword net x-opmode cia402.0.opmode => lcec.0.Xdrive.opmode net x-target-pos cia402.0.drv-target-position => lcec.0.Xdrive.target-position 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 net x-enable joint.0.amp-enable-out => cia402.0.enable net x-amp-fault joint.0.amp-fault-in <= cia402.0.drv-fault # --- Y axis nets ------------------------------------------------------------- net y-statusword lcec.0.Ydrive.cia-statusword => cia402.1.statusword net y-opmode-display lcec.0.Ydrive.opmode-display => cia402.1.opmode-display net y-drv-act-pos lcec.0.Ydrive.actual-position => cia402.1.drv-actual-position net y-controlword cia402.1.controlword => lcec.0.Ydrive.cia-controlword net y-opmode cia402.1.opmode => lcec.0.Ydrive.opmode net y-target-pos cia402.1.drv-target-position => lcec.0.Ydrive.target-position net y-pos-cmd joint.1.motor-pos-cmd => cia402.1.pos-cmd net y-pos-fb joint.1.motor-pos-fb <= cia402.1.pos-fb net y-enable joint.1.amp-enable-out => cia402.1.enable net y-amp-fault joint.1.amp-fault-in <= cia402.1.drv-fault # --- Z axis nets ------------------------------------------------------------- net z-statusword lcec.0.Zdrive.cia-statusword => cia402.2.statusword net z-opmode-display lcec.0.Zdrive.opmode-display => cia402.2.opmode-display net z-drv-act-pos lcec.0.Zdrive.actual-position => cia402.2.drv-actual-position net z-controlword cia402.2.controlword => lcec.0.Zdrive.cia-controlword net z-opmode cia402.2.opmode => lcec.0.Zdrive.opmode net z-target-pos cia402.2.drv-target-position => lcec.0.Zdrive.target-position net z-pos-cmd joint.2.motor-pos-cmd => cia402.2.pos-cmd net z-pos-fb joint.2.motor-pos-fb <= cia402.2.pos-fb net z-enable joint.2.amp-enable-out => cia402.2.enable net z-amp-fault joint.2.amp-fault-in <= cia402.2.drv-fault # --- U axis nets (drum) ------------------------------------------------------ net u-statusword lcec.0.Udrive.cia-statusword => cia402.3.statusword net u-opmode-display lcec.0.Udrive.opmode-display => cia402.3.opmode-display net u-drv-act-pos lcec.0.Udrive.actual-position => cia402.3.drv-actual-position net u-controlword cia402.3.controlword => lcec.0.Udrive.cia-controlword net u-opmode cia402.3.opmode => lcec.0.Udrive.opmode net u-target-pos cia402.3.drv-target-position => lcec.0.Udrive.target-position net u-pos-cmd joint.3.motor-pos-cmd => cia402.3.pos-cmd net u-pos-fb joint.3.motor-pos-fb <= cia402.3.pos-fb net u-enable joint.3.amp-enable-out => cia402.3.enable net u-amp-fault joint.3.amp-fault-in <= cia402.3.drv-fault # --- Spindle orient / velocity mux ------------------------------------------ net spindle-pos pid.1.feedback orient.position net spindle-vel-cmd spindle.0.speed-out pid.0.command net spindle-vel pid.0.feedback net spindle-vel-pid pid.0.output orient-mux.in0 net spindle-pos-cmd orient.command pid.1.command net spindle-angle spindle.0.orient-angle orient.angle net spindle-pos-pid pid.1.output orient-mux.in1 net orient-complete spindle.0.is-oriented orient.is-oriented net orient-mode spindle.0.orient orient.enable pid.1.enable net velocity-mode spindle.0.on pid.0.enable net orient-dir spindle.0.orient-mode orient.mode net orient-mode orient-mux.sel # --- EL4132 AO2 channel 1: ±10V speed command (±6000 rpm => ±10V) ---------- setp lcec.0.AO2.aout-1-enable 1 setp lcec.0.AO2.aout-1-absmode 0 setp lcec.0.AO2.aout-1-offset 0 setp lcec.0.AO2.aout-1-scale 6000 # Command to DAC in RPM (see at-speed block for spindle-cmd-rpm) net spindle-cmd-rpm => lcec.0.AO2.aout-1-value # --- EL2809 output 0 = spindle enable lamp/relay ----------------------------- net spindle-enable or2.0.out net orient-mode or2.0.in0 net velocity-mode or2.0.in1 net spindle-enable => lcec.0.DO16.dout-0 # --- EL5101 spindle encoder → RPS and RPM ----------------------------------- # enc-frequency is counts/sec; 1024 PPR quadrature = 4096 counts/rev setp spindle_freq_to_rps.gain 0.000244140625 net spindle-enc-freq <= lcec.0.ENC1.enc-frequency net spindle-enc-freq => spindle_freq_to_rps.in net spindle-rps <= spindle_freq_to_rps.out # For convenience/GUI RPM readout (feedback) setp spindle_rps_to_rpm.gain 60 net spindle-rps => spindle_rps_to_rpm.in net spindle-rpm <= spindle_rps_to_rpm.out # LinuxCNC expects spindle.0.speed-in in rev/s net spindle-rps => spindle.0.speed-in # --- Spindle at-speed window (FIXED 50 RPM) --------------------------------- # Convert command/feedback to RPM for comparison setp spindle_fb_rps_to_rpm.gain 60 setp spindle_cmd_rps_to_rpm.gain 60 # Reuse existing spindle-vel-cmd signal (single link for speed-out) net spindle-vel-cmd => spindle_cmd_rps_to_rpm.in net spindle-cmd-rpm spindle_cmd_rps_to_rpm.out net spindle-rps => spindle_fb_rps_to_rpm.in net spindle-fb-rpm spindle_fb_rps_to_rpm.out # Error magnitude: |cmd_rpm - fb_rpm| setp spindle_rpm_err.gain0 1 setp spindle_rpm_err.gain1 -1 net spindle-cmd-rpm => spindle_rpm_err.in0 net spindle-fb-rpm => spindle_rpm_err.in1 net spindle-rpm-err spindle_rpm_err.out => spindle_rpm_err_abs.in net spindle-diff-abs spindle_rpm_err_abs.out # near(): declare at-speed when error <= 50 RPM setp spindle-at-speed-near.in2 50 net spindle-diff-abs => spindle-at-speed-near.in1 net spindle-at-speed spindle-at-speed-near.out net spindle-at-speed => spindle.0.at-speed # --- Toolchange mode (M64/M65 P0): Z soft limit 300/420 and limit masking --- setp zlimit_mux.in0 300 setp zlimit_mux.in1 420 net toolchange-mode motion.digital-out-00 net toolchange-mode => zlimit_mux.sel net z-softmax zlimit_mux.out net toolchange-mode => not.0.in net toolchange-mode-not <= not.0.out net z-neg-raw <= lcec.0.DI16.din-8 net z-pos-raw <= lcec.0.DI16.din-9 net z-neg-raw => and2.0.in0 net toolchange-mode-not => and2.0.in1 net z-pos-raw => and2.1.in0 net toolchange-mode-not => and2.1.in1 net z-neg-limit-sw and2.0.out => joint.2.neg-lim-sw-in net z-pos-limit-sw and2.1.out => joint.2.pos-lim-sw-in # --- Home/limit inputs X/Y/Z from EL1809 ------------------------------------ #net x-home-raw <= lcec.0.DI16.din-0 #net x-neg-raw <= lcec.0.DI16.din-1 #net x-pos-raw <= lcec.0.DI16.din-2 #net y-home-raw <= lcec.0.DI16.din-3 #net y-neg-raw <= lcec.0.DI16.din-4 #net y-pos-raw <= lcec.0.DI16.din-5 #net z-home-raw <= lcec.0.DI16.din-6 net x-home-raw => joint.0.home-sw-in net x-neg-raw => joint.0.neg-lim-sw-in net x-pos-raw => joint.0.pos-lim-sw-in net y-home-raw => joint.1.home-sw-in net y-neg-raw => joint.1.neg-lim-sw-in net y-pos-raw => joint.1.pos-lim-sw-in net z-home-raw => joint.2.home-sw-in # --- Drum absolute revolutions from Udrive actual-position (17-bit abs) ---- # Reuse existing signal from U drive to avoid double-link # u-drv-act-pos is already: lcec.0.Udrive.actual-position => cia402.3.drv-actual-position net u-drv-act-pos => udrv_cnt2float.in # Convert to float then to revolutions net udrv-counts-float <= udrv_cnt2float.out # 1 / 131072 = 0.00000762939453125 rev/count (17-bit) setp drum_cnt_to_rev.gain 0.00000762939453125 net udrv-counts-float => drum_cnt_to_rev.in net drum-abs-revs <= drum_cnt_to_rev.out # --- G-code visible inputs --------------------------------------------------- net atc-home <= lcec.0.DI16.din-10 => motion.digital-in-00 net drum-inpos <= lcec.0.DI16.din-11 => motion.digital-in-01 net orient-complete => motion.digital-in-02 # --- Optional drum IO controls via M62/M63 ---------------------------------- net drum-enable <= motion.digital-out-01 => lcec.0.DO16.dout-4 net drum-cw <= motion.digital-out-02 => lcec.0.DO16.dout-5 net drum-ccw <= motion.digital-out-03 => lcec.0.DO16.dout-6 net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in