diff --git a/configs/sim/axis/axis_9axis_scurve.ini b/configs/sim/axis/axis_9axis_scurve.ini index a6abf8106d6..bf1460d099a 100644 --- a/configs/sim/axis/axis_9axis_scurve.ini +++ b/configs/sim/axis/axis_9axis_scurve.ini @@ -7,10 +7,14 @@ # acceleration profiles that can reduce machine vibration and # improve surface finish. # -# IMPORTANT: S-curve planning is OPTIONAL and DISABLED by default. +# IMPORTANT: S-curve planning is OPTIONAL. # Traditional trapezoidal acceleration is used when: # - PLANNER_TYPE = 0, OR -# - MAX_LINEAR_JERK = 0 (or not specified) +# - MAX_LINEAR_JERK = 0 +# +# NOTE: If MAX_LINEAR_JERK is not specified, it defaults to 1e9 (1 billion), +# which effectively disables jerk limiting while still using S-curve calculations. +# This produces motion similar to trapezoidal but not identical. # # ======================================================== [EMC] @@ -64,6 +68,8 @@ PLANNER_TYPE = 1 # Units: machine-units/second^3 (e.g., mm/s^3 or inch/s^3) # # Setting this to 0 disables S-curve planning regardless of PLANNER_TYPE +# If not specified, defaults to 1e9 (effectively no jerk limiting) +# Maximum allowed value is 1e9 (values above are automatically clamped) # # Recommended starting values (adjust based on your machine): # - Light/rigid machines: 1000-10000 @@ -75,16 +81,7 @@ PLANNER_TYPE = 1 # # NOTE: This parameter can be changed at RUNTIME via HAL pin: # ini.traj_max_jerk (allows tuning on-the-fly) -MAX_LINEAR_JERK = 1000.0 - -# DEFAULT_LINEAR_JERK: Default jerk for programmed feed moves -# Units: machine-units/second^3 -# Should be <= MAX_LINEAR_JERK -# Default: 0.0 -# -# NOTE: This parameter can be changed at RUNTIME via HAL pin: -# ini.traj_default_jerk -DEFAULT_LINEAR_JERK = 500.0 +MAX_LINEAR_JERK = 10000.0 # -------------------------------------------------------- # Standard TRAJ parameters (still required with S-curve) @@ -141,91 +138,147 @@ HOME_SEQUENCE = 0 # # This should match or be higher than the trajectory jerk # to avoid limiting coordinated motion. -MAX_JERK = 1000.0 +MAX_JERK = 10000.0 #MIN_LIMIT = -200.0 #disabled to give more space for testing g-codes #MAX_LIMIT = 200.0 #disabled to give more space for testing g-codes +# ======================================================== +# HOMING CONFIGURATION FOR S-CURVE +# ======================================================== +# WARNING: When using S-curve motion (PLANNER_TYPE=1), you MUST set +# HOME_SEARCH_VEL and HOME_LATCH_VEL significantly SLOWER than you +# would with trapezoidal motion planning. +# +# S-curve motion requires MORE DISTANCE to decelerate due to the +# jerk-limited acceleration profile. If homing velocities are too +# high, the axis may overshoot and hit hard limits or sensor limits. +# +# RECOMMENDED: Start with 50% or less of the velocities you would +# use with trapezoidal planning, then increase cautiously. +# +# Example homing configuration (adjust for your machine): +#HOME_SEARCH_VEL = 5.0 # Velocity for initial limit switch search +#HOME_LATCH_VEL = 1.0 # Velocity for precise homing (slower) +#HOME_OFFSET = 0.0 # Distance from switch to home position +#HOME_USE_INDEX = NO # Use encoder index pulse for homing +#HOME_IGNORE_LIMITS = NO # Ignore limit switches during homing move +# ======================================================== + [JOINT_1] TYPE = LINEAR HOME = 0.0 MAX_VELOCITY = 300.0 MAX_ACCELERATION = 1000.0 -MAX_JERK = 1000.0 +MAX_JERK = 10000.0 #MIN_LIMIT = -200.0 #disabled to give more space for testing g-codes #MAX_LIMIT = 200.0 #disabled to give more space for testing g-codes HOME_SEQUENCE = 0 +# WARNING: S-curve homing requires SLOWER velocities than trapezoidal! +# See JOINT_0 for detailed homing configuration examples. +#HOME_SEARCH_VEL = 5.0 +#HOME_LATCH_VEL = 1.0 + [JOINT_2] TYPE = LINEAR HOME = 0.0 MAX_VELOCITY = 200.0 MAX_ACCELERATION = 800.0 -MAX_JERK = 800.0 +MAX_JERK = 10000.0 #MIN_LIMIT = -100.0 #disabled to give more space for testing g-codes #MAX_LIMIT = 100.0 #disabled to give more space for testing g-codes HOME_SEQUENCE = 0 +# WARNING: S-curve homing requires SLOWER velocities than trapezoidal! +# See JOINT_0 for detailed homing configuration examples. +#HOME_SEARCH_VEL = 4.0 +#HOME_LATCH_VEL = 0.8 + [JOINT_3] TYPE = ANGULAR HOME = 0.0 MAX_VELOCITY = 180.0 MAX_ACCELERATION = 600.0 -MAX_JERK = 600.0 +MAX_JERK = 10000.0 #MIN_LIMIT = -360.0 #MAX_LIMIT = 360.0 HOME_SEQUENCE = 0 +# WARNING: S-curve homing requires SLOWER velocities than trapezoidal! +#HOME_SEARCH_VEL = 3.0 +#HOME_LATCH_VEL = 0.6 + [JOINT_4] TYPE = ANGULAR HOME = 0.0 MAX_VELOCITY = 180.0 MAX_ACCELERATION = 600.0 -MAX_JERK = 600.0 +MAX_JERK = 10000.0 #MIN_LIMIT = -360.0 #MAX_LIMIT = 360.0 HOME_SEQUENCE = 0 +# WARNING: S-curve homing requires SLOWER velocities than trapezoidal! +#HOME_SEARCH_VEL = 3.0 +#HOME_LATCH_VEL = 0.6 + [JOINT_5] TYPE = ANGULAR HOME = 0.0 MAX_VELOCITY = 180.0 MAX_ACCELERATION = 600.0 -MAX_JERK = 600.0 +MAX_JERK = 10000.0 #MIN_LIMIT = -360.0 #MAX_LIMIT = 360.0 HOME_SEQUENCE = 0 +# WARNING: S-curve homing requires SLOWER velocities than trapezoidal! +#HOME_SEARCH_VEL = 3.0 +#HOME_LATCH_VEL = 0.6 + [JOINT_6] TYPE = LINEAR HOME = 0.0 MAX_VELOCITY = 300.0 MAX_ACCELERATION = 1000.0 -MAX_JERK = 1000.0 +MAX_JERK = 10000.0 #MIN_LIMIT = -200.0 #MAX_LIMIT = 200.0 HOME_SEQUENCE = 0 +# WARNING: S-curve homing requires SLOWER velocities than trapezoidal! +#HOME_SEARCH_VEL = 5.0 +#HOME_LATCH_VEL = 1.0 + [JOINT_7] TYPE = LINEAR HOME = 0.0 MAX_VELOCITY = 300.0 MAX_ACCELERATION = 1000.0 -MAX_JERK = 1000.0 +MAX_JERK = 10000.0 #MIN_LIMIT = -200.0 #MAX_LIMIT = 200.0 HOME_SEQUENCE = 0 +# WARNING: S-curve homing requires SLOWER velocities than trapezoidal! +#HOME_SEARCH_VEL = 5.0 +#HOME_LATCH_VEL = 1.0 + [JOINT_8] TYPE = LINEAR HOME = 0.0 MAX_VELOCITY = 200.0 MAX_ACCELERATION = 800.0 -MAX_JERK = 800.0 +MAX_JERK = 10000.0 #MIN_LIMIT = -100.0 #MAX_LIMIT = 100.0 HOME_SEQUENCE = 0 +# WARNING: S-curve homing requires SLOWER velocities than trapezoidal! +#HOME_SEARCH_VEL = 4.0 +#HOME_LATCH_VEL = 0.8 + # ======================================================== # AXIS CONFIGURATION # ======================================================== @@ -239,63 +292,63 @@ MAX_ACCELERATION = 1000.0 # MAX_JERK: Maximum jerk for coordinated motion on this axis # Units: machine-units/second^3 # Default: 0.0 (disabled) -MAX_JERK = 1000.0 +MAX_JERK = 10000.0 [AXIS_Y] #MIN_LIMIT = -200.0 #disabled to give more space for testing g-codes #MAX_LIMIT = 200.0 #disabled to give more space for testing g-codes MAX_VELOCITY = 300.0 MAX_ACCELERATION = 1000.0 -MAX_JERK = 1000.0 +MAX_JERK = 10000.0 [AXIS_Z] #MIN_LIMIT = -100.0 #disabled to give more space for testing g-codes #MAX_LIMIT = 100.0 #disabled to give more space for testing g-codes MAX_VELOCITY = 200.0 MAX_ACCELERATION = 800.0 -MAX_JERK = 800.0 +MAX_JERK = 10000.0 [AXIS_A] #MIN_LIMIT = -360.0 #MAX_LIMIT = 360.0 MAX_VELOCITY = 180.0 MAX_ACCELERATION = 600.0 -MAX_JERK = 600.0 +MAX_JERK = 10000.0 [AXIS_B] #MIN_LIMIT = -360.0 #MAX_LIMIT = 360.0 MAX_VELOCITY = 180.0 MAX_ACCELERATION = 600.0 -MAX_JERK = 600.0 +MAX_JERK = 10000.0 [AXIS_C] #MIN_LIMIT = -360.0 #MAX_LIMIT = 360.0 MAX_VELOCITY = 180.0 MAX_ACCELERATION = 600.0 -MAX_JERK = 600.0 +MAX_JERK = 10000.0 [AXIS_U] #MIN_LIMIT = -200.0 #MAX_LIMIT = 200.0 MAX_VELOCITY = 300.0 MAX_ACCELERATION = 1000.0 -MAX_JERK = 1000.0 +MAX_JERK = 10000.0 [AXIS_V] #MIN_LIMIT = -200.0 #MAX_LIMIT = 200.0 MAX_VELOCITY = 300.0 MAX_ACCELERATION = 1000.0 -MAX_JERK = 1000.0 +MAX_JERK = 10000.0 [AXIS_W] #MIN_LIMIT = -100.0 #MAX_LIMIT = 100.0 MAX_VELOCITY = 200.0 MAX_ACCELERATION = 800.0 -MAX_JERK = 800.0 +MAX_JERK = 10000.0 # ======================================================== # TUNING GUIDELINES diff --git a/configs/sim/axis/axis_mm_scurve.ini b/configs/sim/axis/axis_mm_scurve.ini index 035f650de52..76c31947c36 100644 --- a/configs/sim/axis/axis_mm_scurve.ini +++ b/configs/sim/axis/axis_mm_scurve.ini @@ -7,10 +7,14 @@ # acceleration profiles that can reduce machine vibration and # improve surface finish. # -# IMPORTANT: S-curve planning is OPTIONAL and DISABLED by default. +# IMPORTANT: S-curve planning is OPTIONAL. # Traditional trapezoidal acceleration is used when: # - PLANNER_TYPE = 0, OR -# - MAX_LINEAR_JERK = 0 (or not specified) +# - MAX_LINEAR_JERK = 0 +# +# NOTE: If MAX_LINEAR_JERK is not specified, it defaults to 1e9 (1 billion), +# which effectively disables jerk limiting while still using S-curve calculations. +# This produces motion similar to trapezoidal but not identical. # # ======================================================== [EMC] @@ -64,6 +68,8 @@ PLANNER_TYPE = 1 # Units: machine-units/second^3 (e.g., mm/s^3 or inch/s^3) # # Setting this to 0 disables S-curve planning regardless of PLANNER_TYPE +# If not specified, defaults to 1e9 (effectively no jerk limiting) +# Maximum allowed value is 1e9 (values above are automatically clamped) # # Recommended starting values (adjust based on your machine): # - Light/rigid machines: 1000-10000 @@ -75,16 +81,7 @@ PLANNER_TYPE = 1 # # NOTE: This parameter can be changed at RUNTIME via HAL pin: # ini.traj_max_jerk (allows tuning on-the-fly) -MAX_LINEAR_JERK = 1000.0 - -# DEFAULT_LINEAR_JERK: Default jerk for programmed feed moves -# Units: machine-units/second^3 -# Should be <= MAX_LINEAR_JERK -# Default: 0.0 -# -# NOTE: This parameter can be changed at RUNTIME via HAL pin: -# ini.traj_default_jerk -DEFAULT_LINEAR_JERK = 500.0 +MAX_LINEAR_JERK = 10000.0 # -------------------------------------------------------- # Standard TRAJ parameters (still required with S-curve) @@ -141,31 +138,63 @@ HOME_SEQUENCE = 0 # # This should match or be higher than the trajectory jerk # to avoid limiting coordinated motion. -MAX_JERK = 1000.0 +MAX_JERK = 10000.0 #MIN_LIMIT = -200.0 #disabled to give more space for testing g-codes #MAX_LIMIT = 200.0 #disabled to give more space for testing g-codes +# ======================================================== +# HOMING CONFIGURATION FOR S-CURVE +# ======================================================== +# WARNING: When using S-curve motion (PLANNER_TYPE=1), you MUST set +# HOME_SEARCH_VEL and HOME_LATCH_VEL significantly SLOWER than you +# would with trapezoidal motion planning. +# +# S-curve motion requires MORE DISTANCE to decelerate due to the +# jerk-limited acceleration profile. If homing velocities are too +# high, the axis may overshoot and hit hard limits or sensor limits. +# +# RECOMMENDED: Start with 50% or less of the velocities you would +# use with trapezoidal planning, then increase cautiously. +# +# Example homing configuration (adjust for your machine): +#HOME_SEARCH_VEL = 5.0 # Velocity for initial limit switch search +#HOME_LATCH_VEL = 1.0 # Velocity for precise homing (slower) +#HOME_OFFSET = 0.0 # Distance from switch to home position +#HOME_USE_INDEX = NO # Use encoder index pulse for homing +#HOME_IGNORE_LIMITS = NO # Ignore limit switches during homing move +# ======================================================== + [JOINT_1] TYPE = LINEAR HOME = 0.0 MAX_VELOCITY = 300.0 MAX_ACCELERATION = 1000.0 -MAX_JERK = 1000.0 +MAX_JERK = 10000.0 #MIN_LIMIT = -200.0 #disabled to give more space for testing g-codes #MAX_LIMIT = 200.0 #disabled to give more space for testing g-codes HOME_SEQUENCE = 0 +# WARNING: S-curve homing requires SLOWER velocities than trapezoidal! +# See JOINT_0 for detailed homing configuration examples. +#HOME_SEARCH_VEL = 5.0 +#HOME_LATCH_VEL = 1.0 + [JOINT_2] TYPE = LINEAR HOME = 0.0 MAX_VELOCITY = 200.0 MAX_ACCELERATION = 800.0 -MAX_JERK = 800.0 +MAX_JERK = 10000.0 #MIN_LIMIT = -100.0 #disabled to give more space for testing g-codes #MAX_LIMIT = 100.0 #disabled to give more space for testing g-codes HOME_SEQUENCE = 0 +# WARNING: S-curve homing requires SLOWER velocities than trapezoidal! +# See JOINT_0 for detailed homing configuration examples. +#HOME_SEARCH_VEL = 4.0 +#HOME_LATCH_VEL = 0.8 + # ======================================================== # AXIS CONFIGURATION # ======================================================== @@ -179,21 +208,21 @@ MAX_ACCELERATION = 1000.0 # MAX_JERK: Maximum jerk for coordinated motion on this axis # Units: machine-units/second^3 # Default: 0.0 (disabled) -MAX_JERK = 1000.0 +MAX_JERK = 10000.0 [AXIS_Y] #MIN_LIMIT = -200.0 #disabled to give more space for testing g-codes #MAX_LIMIT = 200.0 #disabled to give more space for testing g-codes MAX_VELOCITY = 300.0 MAX_ACCELERATION = 1000.0 -MAX_JERK = 1000.0 +MAX_JERK = 10000.0 [AXIS_Z] #MIN_LIMIT = -100.0 #disabled to give more space for testing g-codes #MAX_LIMIT = 100.0 #disabled to give more space for testing g-codes MAX_VELOCITY = 200.0 MAX_ACCELERATION = 800.0 -MAX_JERK = 800.0 +MAX_JERK = 10000.0 # ======================================================== # TUNING GUIDELINES diff --git a/docs/src/config/core-components.adoc b/docs/src/config/core-components.adoc index 814369da9c7..2c54eebdf18 100644 --- a/docs/src/config/core-components.adoc +++ b/docs/src/config/core-components.adoc @@ -373,14 +373,13 @@ S-curve trajectory planning pins (sampled continuously, can be changed at runtim * 'ini.traj_planner_type' - (s32, in) [TRAJ]PLANNER_TYPE * 'ini.traj_max_jerk' - (float, in) [TRAJ]MAX_LINEAR_JERK -* 'ini.traj_default_jerk' - (float, in) [TRAJ]DEFAULT_LINEAR_JERK Per-axis jerk limit pins (where _L_ is x, y, z, a, b, c, u, v, or w): -* 'ini._L_.jerk' - (float, in) [AXIS__L_]MAX_JERK +* 'ini._L_.max_jerk' - (float, in) [AXIS__L_]MAX_JERK Per-joint jerk limit pins (where _N_ is the joint number 0-8): -* 'ini._N_.jerk' - (float, in) [JOINT__N_]MAX_JERK +* 'ini._N_.max_jerk' - (float, in) [JOINT__N_]MAX_JERK // vim: set syntax=asciidoc: diff --git a/docs/src/config/ini-config.adoc b/docs/src/config/ini-config.adoc index a186de18c05..5c5139059e4 100644 --- a/docs/src/config/ini-config.adoc +++ b/docs/src/config/ini-config.adoc @@ -862,11 +862,11 @@ Finally, no amount of tweaking will speed up a tool path with lots of small, tig * `MAX_LINEAR_ACCELERATION = 20.0` - (((MAX ACCELERATION))) The maximum acceleration for any axis or coordinated axis move, in 'machine units' per second per second. * `PLANNER_TYPE = 0` - (((PLANNER TYPE))) Selects the trajectory planner type: 0 = trapezoidal (default), 1 = S-curve with jerk limiting. S-curve planning is only active when `PLANNER_TYPE = 1` AND `MAX_LINEAR_JERK > 0`. -* `MAX_LINEAR_JERK = 0.0` - (((MAX JERK))) The maximum jerk (rate of change of acceleration) for coordinated moves, in 'machine units' per second cubed. - When set to 0 (default), jerk limiting is disabled. - When greater than 0 and `PLANNER_TYPE = 1`, enables S-curve trajectory planning. -* `DEFAULT_LINEAR_JERK = 0.0` - The default jerk value for coordinated moves, in 'machine units' per second cubed. - When set to 0, `MAX_LINEAR_JERK` is used. +* `MAX_LINEAR_JERK = 10000.0` - (((MAX JERK))) The maximum jerk (rate of change of acceleration) for coordinated moves, in 'machine units' per second cubed. + Default is 1e9 (1 billion) if not specified, which effectively disables jerk limiting while avoiding numerical instability. + Values are clamped to a maximum of 1e9 to prevent numerical issues in S-curve calculations. + When `PLANNER_TYPE = 1`, this enables S-curve trajectory planning. + Note: Not specifying MAX_LINEAR_JERK (defaulting to 1e9) produces motion similar to trapezoidal planning (PLANNER_TYPE = 0) but not identical, as extremely high jerk still uses S-curve calculations. * `POSITION_FILE =` _position.txt_ - If set to a non-empty value, the joint positions are stored between runs in this file. This allows the machine to start with the same coordinates it had on shutdown. This assumes there was no movement of the machine while powered off. diff --git a/docs/src/config/integrator-concepts.adoc b/docs/src/config/integrator-concepts.adoc index 44d2af74c0b..bfff28797eb 100644 --- a/docs/src/config/integrator-concepts.adoc +++ b/docs/src/config/integrator-concepts.adoc @@ -279,6 +279,8 @@ MAX_JERK = 1000.0 S-curve planning is only active when `PLANNER_TYPE = 1` and `MAX_LINEAR_JERK > 0`. +NOTE: If `MAX_LINEAR_JERK` is not specified, it defaults to 1e9 (1 billion), which effectively disables jerk limiting while maintaining S-curve calculations. This produces motion similar to trapezoidal planning but not identical. The maximum allowed value is 1e9 to prevent numerical instability. + === Tuning Start with a conservative jerk value and increase gradually: @@ -293,6 +295,8 @@ Typical values: 100-100,000 units/s^3^ depending on machine rigidity and units Increase `MAX_LINEAR_JERK` until motion becomes sluggish or following errors increase, then reduce slightly. Test with coordinated moves and arcs. +Values above 1e9 are automatically clamped to 1e9 to avoid numerical issues in the S-curve trajectory calculations. + == RTAI The Real Time Application Interface (RTAI) is used to provide the best diff --git a/docs/src/man/man9/motion.9.adoc b/docs/src/man/man9/motion.9.adoc index f617b5b7714..69128e2608a 100644 --- a/docs/src/man/man9/motion.9.adoc +++ b/docs/src/man/man9/motion.9.adoc @@ -411,7 +411,7 @@ Note: Pins marked *(DEBUG)* serve as debugging aids and are subject to change or **joint.**_N_**.jerk-cmd** OUT FLOAT *(DEBUG)*:: The joint's commanded jerk (rate of change of acceleration). Only active when S-curve trajectory planning is enabled (INI file [TRAJ]PLANNER_TYPE=1 and [TRAJ]MAX_LINEAR_JERK>0). - Jerk limits are set via INI file [JOINT_N]MAX_JERK or via the ini.N.jerk HAL pin. + Jerk limits are set via INI file [JOINT_N]MAX_JERK or via the ini.N.max_jerk HAL pin. **joint.**_N_**.wheel-jog-active** OUT BIT *(DEBUG)*:: + diff --git a/src/emc/ini/inihal.cc b/src/emc/ini/inihal.cc index 4825bb08bac..0d25ad396fb 100644 --- a/src/emc/ini/inihal.cc +++ b/src/emc/ini/inihal.cc @@ -159,7 +159,7 @@ int ini_hal_init(int numjoints) MAKE_FLOAT_PIN_IDX(joint_max_limit,max_limit,HAL_IN,idx); MAKE_FLOAT_PIN_IDX(joint_max_velocity,max_velocity,HAL_IN,idx); MAKE_FLOAT_PIN_IDX(joint_max_acceleration,max_acceleration,HAL_IN,idx); - MAKE_FLOAT_PIN_IDX(joint_jerk,jerk,HAL_IN,idx); + MAKE_FLOAT_PIN_IDX(joint_jerk,max_jerk,HAL_IN,idx); MAKE_FLOAT_PIN_IDX(joint_home,home,HAL_IN,idx); MAKE_FLOAT_PIN_IDX(joint_home_offset,home_offset,HAL_IN,idx); MAKE_S32_PIN_IDX( joint_home_sequence,home_sequence,HAL_IN,idx); @@ -170,14 +170,13 @@ int ini_hal_init(int numjoints) MAKE_FLOAT_PIN_LETTER(axis_max_limit,max_limit,HAL_IN,idx,letter); MAKE_FLOAT_PIN_LETTER(axis_max_velocity,max_velocity,HAL_IN,idx,letter); MAKE_FLOAT_PIN_LETTER(axis_max_acceleration,max_acceleration,HAL_IN,idx,letter); - MAKE_FLOAT_PIN_LETTER(axis_jerk,jerk,HAL_IN,idx,letter); + MAKE_FLOAT_PIN_LETTER(axis_jerk,max_jerk,HAL_IN,idx,letter); } MAKE_FLOAT_PIN(traj_default_velocity,HAL_IN); MAKE_FLOAT_PIN(traj_max_velocity,HAL_IN); MAKE_FLOAT_PIN(traj_default_acceleration,HAL_IN); MAKE_FLOAT_PIN(traj_max_acceleration,HAL_IN); - MAKE_FLOAT_PIN(traj_default_jerk,HAL_IN); MAKE_FLOAT_PIN(traj_max_jerk,HAL_IN); MAKE_S32_PIN(traj_planner_type,HAL_IN); @@ -198,7 +197,6 @@ int ini_hal_init_pins(int numjoints) INIT_PIN(traj_max_velocity); INIT_PIN(traj_default_acceleration); INIT_PIN(traj_max_acceleration); - INIT_PIN(traj_default_jerk); INIT_PIN(traj_max_jerk); INIT_PIN(traj_planner_type); @@ -292,16 +290,34 @@ int check_ini_hal_items(int numjoints) rcs_print("check_ini_hal_items:bad return value from emcTrajSetMaxJerk\n"); } } + // Also update the current jerk to the new max value + if (0 != emcTrajSetJerk(NEW(traj_max_jerk))) { + if (emc_debug & EMC_DEBUG_CONFIG) { + rcs_print("check_ini_hal_items:bad return value from emcTrajSetJerk\n"); + } + } + // Force planner type 0 if max_jerk < 1 (S-curve needs valid jerk) + if (NEW(traj_max_jerk) < 1.0) { + if (0 != emcTrajPlannerType(0)) { + if (emc_debug & EMC_DEBUG_CONFIG) { + rcs_print("check_ini_hal_items:bad return value from emcTrajPlannerType\n"); + } + } + } } - + if (CHANGED(traj_planner_type)) { if (debug) SHOW_CHANGE_INT(traj_planner_type) UPDATE(traj_planner_type); // Only 0 and 1 are supported, set to 0 if invalid + // Also force planner type 0 if max_jerk < 1 (S-curve needs valid jerk) int planner_type = NEW(traj_planner_type); if (planner_type != 0 && planner_type != 1) { planner_type = 0; } + if (planner_type == 1 && NEW(traj_max_jerk) < 1.0) { + planner_type = 0; + } if (0 != emcTrajPlannerType(planner_type)) { if (emc_debug & EMC_DEBUG_CONFIG) { rcs_print("check_ini_hal_items:bad return value from emcTrajPlannerType\n"); @@ -309,16 +325,6 @@ int check_ini_hal_items(int numjoints) } } - if (CHANGED(traj_default_jerk)) { - if (debug) SHOW_CHANGE(traj_default_jerk) - UPDATE(traj_default_jerk); - if (0 != emcTrajSetJerk(NEW(traj_default_jerk))) { - if (emc_debug & EMC_DEBUG_CONFIG) { - rcs_print("check_ini_hal_items:bad return value from emcTrajSetJerk\n"); - } - } - } - if ( CHANGED(traj_arc_blend_enable) || CHANGED(traj_arc_blend_fallback_enable) || CHANGED(traj_arc_blend_optimization_depth) diff --git a/src/emc/ini/inihal.hh b/src/emc/ini/inihal.hh index 747bebe4b32..f3b255635a6 100644 --- a/src/emc/ini/inihal.hh +++ b/src/emc/ini/inihal.hh @@ -52,7 +52,6 @@ int ini_hal_init_pins(int numjoints); FIELD(hal_float_t,traj_max_velocity) \ FIELD(hal_float_t,traj_default_acceleration) \ FIELD(hal_float_t,traj_max_acceleration) \ - FIELD(hal_float_t,traj_default_jerk) \ FIELD(hal_float_t,traj_max_jerk) \ FIELD(hal_s32_t,traj_planner_type) \ \ diff --git a/src/emc/ini/initraj.cc b/src/emc/ini/initraj.cc index 3cd2de52c2a..f6389933e95 100644 --- a/src/emc/ini/initraj.cc +++ b/src/emc/ini/initraj.cc @@ -203,8 +203,8 @@ static int loadTraj(EmcIniFile *trajInifile) } old_inihal_data.traj_max_acceleration = acc; - // has to set MAX_* before DEFAULT_* - jerk = 0; + // Set max jerk (default to 1e9 if not specified in INI) + jerk = 1e9; trajInifile->Find(&jerk, "MAX_LINEAR_JERK", "TRAJ"); if (0 != emcTrajSetMaxJerk(jerk)) { if (emc_debug & EMC_DEBUG_CONFIG) { @@ -213,22 +213,23 @@ static int loadTraj(EmcIniFile *trajInifile) return -1; } old_inihal_data.traj_max_jerk = jerk; - - jerk = 0; - trajInifile->Find(&jerk, "DEFAULT_LINEAR_JERK", "TRAJ"); + // Also set current jerk to max_jerk if (0 != emcTrajSetJerk(jerk)) { if (emc_debug & EMC_DEBUG_CONFIG) { rcs_print("bad return value from emcTrajSetJerk\n"); } return -1; } - old_inihal_data.traj_default_jerk = jerk; planner_type = 0; // Default: 0 = trapezoidal, 1 = S-curve trajInifile->Find(&planner_type, "PLANNER_TYPE", "TRAJ"); // Only 0 and 1 are supported, set to 0 if invalid + // Also force planner type 0 if max_jerk < 1 (S-curve needs valid jerk) if (planner_type != 0 && planner_type != 1) { planner_type = 0; } + if (planner_type == 1 && jerk < 1.0) { + planner_type = 0; + } if (0 != emcTrajPlannerType(planner_type)) { if (emc_debug & EMC_DEBUG_CONFIG) { rcs_print("bad return value from emcTrajPlannerType\n"); diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index a17f2981ac5..696293646e6 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -1396,6 +1396,24 @@ static void get_pos_cmds(long period) /* interpolate to get new position and velocity */ joint->pos_cmd = cubicInterpolate(&(joint->cubic), 0, &(joint->vel_cmd), &(joint->acc_cmd), &(joint->jerk_cmd)); } + + /* Use accurate jerk values from TP output (for Cartesian machines only) + * For standard XYZ machines, joint[0-2] correspond to X, Y, Z axes + * TP outputs: current_jerk (path jerk) and current_dir (direction unit vector) + * Per-axis jerk = path_jerk * direction_component + */ + if (emcmotStatus->planner_type == 1) { + // S-curve mode: use accurate jerk values + double path_jerk = emcmotStatus->current_jerk; + PmCartesian dir = emcmotStatus->current_dir; + + // For the first 3 joints (assuming X, Y, Z), use accurate jerk + if (NO_OF_KINS_JOINTS >= 1) joints[0].jerk_cmd = path_jerk * dir.x; + if (NO_OF_KINS_JOINTS >= 2) joints[1].jerk_cmd = path_jerk * dir.y; + if (NO_OF_KINS_JOINTS >= 3) joints[2].jerk_cmd = path_jerk * dir.z; + // Rotary axes (A, B, C) keep the cubic interpolator values for now + } + /* report motion status */ SET_MOTION_INPOS_FLAG(0); if (tpIsDone(&emcmotInternal->coord_tp)) { diff --git a/src/emc/motion/motion.h b/src/emc/motion/motion.h index 4477e9286b1..9aaee5e8ac0 100644 --- a/src/emc/motion/motion.h +++ b/src/emc/motion/motion.h @@ -648,6 +648,14 @@ Suggestion: Split this in to an Error and a Status flag register.. double current_vel; double requested_vel; + /* S-curve motion state - for accurate jerk output */ + double current_acc; /* current path acceleration */ + double current_jerk; /* current path jerk (accurate value from TP) */ + double decel_dist; /* S-curve deceleration distance (dlen1) for debugging */ + double tc_finalvel; /* S-curve segment final velocity for debugging */ + double tc_maxaccel; /* S-curve segment max tangential accel for debugging */ + PmCartesian current_dir; /* current motion direction unit vector */ + unsigned int tcqlen; EmcPose tool_offset; int atspeed_next_feed; /* at next feed move, wait for spindle to be at speed */ diff --git a/src/emc/task/taskintf.cc b/src/emc/task/taskintf.cc index 486747b66d3..2f180645b59 100644 --- a/src/emc/task/taskintf.cc +++ b/src/emc/task/taskintf.cc @@ -1228,6 +1228,9 @@ int emcTrajSetMaxJerk(double jerk) { if (jerk < 0.0) { jerk = 0.0; + } else if (jerk > 1e9) { + // Clamp to 1e9 to prevent numerical instability in S-curve calculations + jerk = 1e9; } TrajConfig.MaxJerk = jerk; diff --git a/src/emc/tp/tc.c b/src/emc/tp/tc.c index 022c4b9b605..a44c4f1aca7 100644 --- a/src/emc/tp/tc.c +++ b/src/emc/tp/tc.c @@ -326,7 +326,39 @@ int tcGetEndTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const ou return 0; } +/** + * Calculate the unit tangent vector at the current progress of a move. + * For linear moves, this is constant. For circular moves, it varies with progress. + */ +int tcGetCurrentTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const out) { + switch (tc->motion_type) { + case TC_LINEAR: + *out = tc->coords.line.xyz.uVec; + break; + case TC_RIGIDTAP: + *out = tc->coords.rigidtap.xyz.uVec; + break; + case TC_CIRCULAR: + { + // Calculate current angle based on progress + double current_angle = 0.0; + if (tc->target > 0.0) { + current_angle = (tc->progress / tc->target) * tc->coords.circle.xyz.angle; + } + pmCircleTangentVector(&tc->coords.circle.xyz, current_angle, out); + } + break; + case TC_SPHERICAL: + // Spherical arcs used for blending - tangent calculation at arbitrary + // progress not yet implemented, direction will be zeroed in caller + return -1; + default: + rtapi_print_msg(RTAPI_MSG_ERR, "Invalid motion type %d!\n", tc->motion_type); + return -1; + } + return 0; +} /** * Calculate the distance left in the trajectory segment in the indicated diff --git a/src/emc/tp/tc.h b/src/emc/tp/tc.h index 3c1afa6dde2..f57fb9063c9 100644 --- a/src/emc/tp/tc.h +++ b/src/emc/tp/tc.h @@ -39,6 +39,7 @@ int tcGetEndAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out) int tcGetStartAccelUnitVector(TC_STRUCT const * const tc, PmCartesian * const out); int tcGetEndTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const out); int tcGetStartTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const out); +int tcGetCurrentTangentUnitVector(TC_STRUCT const * const tc, PmCartesian * const out); double tcGetDistanceToGo(TC_STRUCT const * const tc, int direction); double tcGetTarget(TC_STRUCT const * const tc, int direction); diff --git a/src/emc/tp/tp.c b/src/emc/tp/tp.c index b9a15ba357f..613ea896b39 100644 --- a/src/emc/tp/tp.c +++ b/src/emc/tp/tp.c @@ -2980,6 +2980,13 @@ STATIC int tpUpdateMovementStatus(TP_STRUCT * const tp, TC_STRUCT const * const emcmotStatus->current_vel = 0; emcmotStatus->spindleSync = 0; + // Clear S-curve motion state + emcmotStatus->current_acc = 0; + emcmotStatus->current_jerk = 0; + emcmotStatus->current_dir.x = 0; + emcmotStatus->current_dir.y = 0; + emcmotStatus->current_dir.z = 0; + emcPoseZero(&emcmotStatus->dtg); tp->motionType = 0; @@ -3001,6 +3008,21 @@ STATIC int tpUpdateMovementStatus(TP_STRUCT * const tp, TC_STRUCT const * const emcmotStatus->requested_vel = tc->reqvel; emcmotStatus->current_vel = tc->currentvel; + // Output accurate S-curve motion state (for accurate jerk calculation) + emcmotStatus->current_acc = tc->currentacc; + emcmotStatus->current_jerk = tc->currentjerk; + + // Get current motion direction unit vector (precise tangent at current progress) + PmCartesian dir; + if (tcGetCurrentTangentUnitVector(tc, &dir) == 0) { + emcmotStatus->current_dir = dir; + } else { + // If direction unavailable, use zero vector + emcmotStatus->current_dir.x = 0; + emcmotStatus->current_dir.y = 0; + emcmotStatus->current_dir.z = 0; + } + emcPoseSub(&tc_pos, &tp->currentPos, &emcmotStatus->dtg); return TP_ERR_OK; } diff --git a/tests/interp/m98m99/12-M99-endless-main-program/expected.motion-logger b/tests/interp/m98m99/12-M99-endless-main-program/expected.motion-logger index 2806f32180c..4efb337c04a 100644 --- a/tests/interp/m98m99/12-M99-endless-main-program/expected.motion-logger +++ b/tests/interp/m98m99/12-M99-endless-main-program/expected.motion-logger @@ -3,7 +3,7 @@ SET_NUM_SPINDLES 1 SET_VEL vel=0, ini_maxvel=1.2 SET_VEL_LIMIT vel=4 SET_ACC acc=1e+99 -SET_JERK jerk=0 +SET_JERK jerk=1e+09 SET_PLANNER_TYPE planner_type=0 SETUP_ARC_BLENDS SET_MAX_FEED_OVERRIDE 1 diff --git a/tests/motion-logger/basic/expected.builtin-startup.in b/tests/motion-logger/basic/expected.builtin-startup.in index 0a7e046c0ed..d8bd8075430 100644 --- a/tests/motion-logger/basic/expected.builtin-startup.in +++ b/tests/motion-logger/basic/expected.builtin-startup.in @@ -3,7 +3,7 @@ SET_NUM_SPINDLES 1 SET_VEL vel=0, ini_maxvel=1.2 SET_VEL_LIMIT vel=4 SET_ACC acc=1e+99 -SET_JERK jerk=0 +SET_JERK jerk=1e+09 SET_PLANNER_TYPE planner_type=0 SETUP_ARC_BLENDS SET_MAX_FEED_OVERRIDE 1 diff --git a/tests/motion-logger/mountaindew/expected.motion-logger b/tests/motion-logger/mountaindew/expected.motion-logger index 198c856db25..ba0b740b387 100644 --- a/tests/motion-logger/mountaindew/expected.motion-logger +++ b/tests/motion-logger/mountaindew/expected.motion-logger @@ -3,7 +3,7 @@ SET_NUM_SPINDLES 1 SET_VEL vel=0, ini_maxvel=120 SET_VEL_LIMIT vel=400 SET_ACC acc=1e+99 -SET_JERK jerk=0 +SET_JERK jerk=1e+09 SET_PLANNER_TYPE planner_type=0 SETUP_ARC_BLENDS SET_MAX_FEED_OVERRIDE 1 diff --git a/tests/motion-logger/startup-gcode-abort/expected.motion-logger.in b/tests/motion-logger/startup-gcode-abort/expected.motion-logger.in index b58f0779247..cc3abee97d6 100644 --- a/tests/motion-logger/startup-gcode-abort/expected.motion-logger.in +++ b/tests/motion-logger/startup-gcode-abort/expected.motion-logger.in @@ -8,7 +8,7 @@ SET_NUM_SPINDLES 1 SET_VEL vel=0, ini_maxvel=1.2 SET_VEL_LIMIT vel=4 SET_ACC acc=1e+99 -SET_JERK jerk=0 +SET_JERK jerk=1e+09 SET_PLANNER_TYPE planner_type=0 SETUP_ARC_BLENDS SET_MAX_FEED_OVERRIDE 1