MPCNC/Firmware2
1
0
Fork 0
Code Issues 1 Pull Requests Releases Wiki Activity

Implement kinematic software endstops

Browse Source
This commit is contained in:
Scott Lahteine 2017-10-30 14:45:11 -05:00
parent 6e5f13a5c2
commit 04c0d9f3c3
1 changed files with 37 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
Marlin/src/module/motion.cpp
View File

@ -454,29 +454,42 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
// Software Endstops are based on the configured limits. // Software Endstops are based on the configured limits.
bool soft_endstops_enabled = true; bool soft_endstops_enabled = true;
#if IS_KINEMATIC
float soft_endstop_radius, soft_endstop_radius_2;
#endif
/** /**
* Constrain the given coordinates to the software endstops. * Constrain the given coordinates to the software endstops.
* *
* NOTE: This will only apply to Z on DELTA and SCARA. XY is * For DELTA/SCARA the XY constraint is based on the smallest
* constrained to a circle on these kinematic systems. * radius within the set software endstops.
*/ */
void clamp_to_software_endstops(float target[XYZ]) { void clamp_to_software_endstops(float target[XYZ]) {
if (!soft_endstops_enabled) return; if (!soft_endstops_enabled) return;
#if IS_KINEMATIC
const float dist_2 = HYPOT2(target[X_AXIS], target[Y_AXIS]);
if (dist_2 > soft_endstop_radius_2) {
const float ratio = soft_endstop_radius / SQRT(dist_2); // 200 / 300 = 0.66
target[X_AXIS] *= ratio;
target[Y_AXIS] *= ratio;
}
#else
#if ENABLED(MIN_SOFTWARE_ENDSTOP_X) #if ENABLED(MIN_SOFTWARE_ENDSTOP_X)
NOLESS(target[X_AXIS], soft_endstop_min[X_AXIS]); NOLESS(target[X_AXIS], soft_endstop_min[X_AXIS]);
#endif #endif
#if ENABLED(MIN_SOFTWARE_ENDSTOP_Y) #if ENABLED(MIN_SOFTWARE_ENDSTOP_Y)
NOLESS(target[Y_AXIS], soft_endstop_min[Y_AXIS]); NOLESS(target[Y_AXIS], soft_endstop_min[Y_AXIS]);
#endif #endif
#if ENABLED(MIN_SOFTWARE_ENDSTOP_Z)
NOLESS(target[Z_AXIS], soft_endstop_min[Z_AXIS]);
#endif
#if ENABLED(MAX_SOFTWARE_ENDSTOP_X) #if ENABLED(MAX_SOFTWARE_ENDSTOP_X)
NOMORE(target[X_AXIS], soft_endstop_max[X_AXIS]); NOMORE(target[X_AXIS], soft_endstop_max[X_AXIS]);
#endif #endif
#if ENABLED(MAX_SOFTWARE_ENDSTOP_Y) #if ENABLED(MAX_SOFTWARE_ENDSTOP_Y)
NOMORE(target[Y_AXIS], soft_endstop_max[Y_AXIS]); NOMORE(target[Y_AXIS], soft_endstop_max[Y_AXIS]);
#endif #endif
#endif
#if ENABLED(MIN_SOFTWARE_ENDSTOP_Z)
NOLESS(target[Z_AXIS], soft_endstop_min[Z_AXIS]);
#endif
#if ENABLED(MAX_SOFTWARE_ENDSTOP_Z) #if ENABLED(MAX_SOFTWARE_ENDSTOP_Z)
NOMORE(target[Z_AXIS], soft_endstop_max[Z_AXIS]); NOMORE(target[Z_AXIS], soft_endstop_max[Z_AXIS]);
#endif #endif
@ -1259,8 +1272,17 @@ void homeaxis(const AxisEnum axis) {
#endif #endif
#if ENABLED(DELTA) #if ENABLED(DELTA)
if (axis == Z_AXIS) switch(axis) {
case X_AXIS:
case Y_AXIS:
// Get a minimum radius for clamping
soft_endstop_radius = MIN3(FABS(max(soft_endstop_min[X_AXIS], soft_endstop_min[Y_AXIS])), soft_endstop_max[X_AXIS], soft_endstop_max[Y_AXIS]);
soft_endstop_radius_2 = sq(soft_endstop_radius);
break;
case Z_AXIS:
delta_clip_start_height = soft_endstop_max[axis] - delta_safe_distance_from_top(); delta_clip_start_height = soft_endstop_max[axis] - delta_safe_distance_from_top();
default: break;
}
#endif #endif
} }