From 7194e4434ff0ce4177bfd49388abf3d7de702cb8 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Thu, 15 Sep 2016 15:43:59 -0500
Subject: [PATCH] SCARA support in position_is_reachable
---
Marlin/Marlin_main.cpp | 24 +++++++++++++++++-------
1 file changed, 17 insertions(+), 7 deletions(-)
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 0c7e97d01..06804103b 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -2501,16 +2501,26 @@ void unknown_command_error() {
bool position_is_reachable(float target[XYZ]) {
float dx = RAW_X_POSITION(target[X_AXIS]),
- dy = RAW_Y_POSITION(target[Y_AXIS]);
+ dy = RAW_Y_POSITION(target[Y_AXIS]),
+ dz = RAW_Z_POSITION(target[Z_AXIS]);
- #if ENABLED(DELTA)
- return HYPOT2(dx, dy) <= sq(DELTA_PRINTABLE_RADIUS);
+ bool good;
+ #if IS_SCARA
+ #if MIDDLE_DEAD_ZONE_R > 0
+ const float R2 = HYPOT2(dx - SCARA_OFFSET_X, dy - SCARA_OFFSET_Y);
+ good = (R2 >= sq(float(MIDDLE_DEAD_ZONE_R))) && (R2 <= sq(L1 + L2));
+ #else
+ good = HYPOT2(dx - SCARA_OFFSET_X, dy - SCARA_OFFSET_Y) <= sq(L1 + L2);
+ #endif
+ #elif ENABLED(DELTA)
+ good = HYPOT2(dx, dy) <= sq(DELTA_PRINTABLE_RADIUS);
#else
- float dz = RAW_Z_POSITION(target[Z_AXIS]);
- return dx >= X_MIN_POS - 0.0001 && dx <= X_MAX_POS + 0.0001
- && dy >= Y_MIN_POS - 0.0001 && dy <= Y_MAX_POS + 0.0001
- && dz >= Z_MIN_POS - 0.0001 && dz <= Z_MAX_POS + 0.0001;
+ good = true;
#endif
+
+ return good && dx >= X_MIN_POS - 0.0001 && dx <= X_MAX_POS + 0.0001
+ && dy >= Y_MIN_POS - 0.0001 && dy <= Y_MAX_POS + 0.0001
+ && dz >= Z_MIN_POS - 0.0001 && dz <= Z_MAX_POS + 0.0001;
}
/**************************************************