Consolidate "bedlevel" code

Marlin/src/module/delta.h

@@ -0,0 +1,141 @@
+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * delta.h - Delta-specific functions
+ */
+
+#ifndef __DELTA_H__
+#define __DELTA_H__
+
+extern float delta_endstop_adj[ABC],
+             delta_radius,
+             delta_diagonal_rod,
+             delta_segments_per_second,
+             delta_calibration_radius,
+             delta_tower_angle_trim[2];
+
+extern float delta_tower[ABC][2],
+             delta_diagonal_rod_2_tower[ABC],
+             delta_clip_start_height;
+
+/**
+ * Recalculate factors used for delta kinematics whenever
+ * settings have been changed (e.g., by M665).
+ */
+void recalc_delta_settings(float radius, float diagonal_rod);
+
+/**
+ * Delta Inverse Kinematics
+ *
+ * Calculate the tower positions for a given logical
+ * position, storing the result in the delta[] array.
+ *
+ * This is an expensive calculation, requiring 3 square
+ * roots per segmented linear move, and strains the limits
+ * of a Mega2560 with a Graphical Display.
+ *
+ * Suggested optimizations include:
+ *
+ * - Disable the home_offset (M206) and/or position_shift (G92)
+ *   features to remove up to 12 float additions.
+ *
+ * - Use a fast-inverse-sqrt function and add the reciprocal.
+ *   (see above)
+ */
+
+#if ENABLED(DELTA_FAST_SQRT) && defined(ARDUINO_ARCH_AVR)
+  /**
+   * Fast inverse sqrt from Quake III Arena
+   * See: https://en.wikipedia.org/wiki/Fast_inverse_square_root
+   */
+  float Q_rsqrt(float number);
+  #define _SQRT(n) (1.0f / Q_rsqrt(n))
+#else
+  #define _SQRT(n) SQRT(n)
+#endif
+
+// Macro to obtain the Z position of an individual tower
+#define DELTA_Z(T) raw[Z_AXIS] + _SQRT(     \
+  delta_diagonal_rod_2_tower[T] - HYPOT2(   \
+      delta_tower[T][X_AXIS] - raw[X_AXIS], \
+      delta_tower[T][Y_AXIS] - raw[Y_AXIS]  \
+    )                                       \
+  )
+
+#define DELTA_RAW_IK() do {        \
+  delta[A_AXIS] = DELTA_Z(A_AXIS); \
+  delta[B_AXIS] = DELTA_Z(B_AXIS); \
+  delta[C_AXIS] = DELTA_Z(C_AXIS); \
+}while(0)
+
+#define DELTA_LOGICAL_IK() do {      \
+  const float raw[XYZ] = {           \
+    RAW_X_POSITION(logical[X_AXIS]), \
+    RAW_Y_POSITION(logical[Y_AXIS]), \
+    RAW_Z_POSITION(logical[Z_AXIS])  \
+  };                                 \
+  DELTA_RAW_IK();                    \
+}while(0)
+
+void inverse_kinematics(const float logical[XYZ]);
+
+/**
+ * Calculate the highest Z position where the
+ * effector has the full range of XY motion.
+ */
+float delta_safe_distance_from_top();
+
+/**
+ * Delta Forward Kinematics
+ *
+ * See the Wikipedia article "Trilateration"
+ * https://en.wikipedia.org/wiki/Trilateration
+ *
+ * Establish a new coordinate system in the plane of the
+ * three carriage points. This system has its origin at
+ * tower1, with tower2 on the X axis. Tower3 is in the X-Y
+ * plane with a Z component of zero.
+ * We will define unit vectors in this coordinate system
+ * in our original coordinate system. Then when we calculate
+ * the Xnew, Ynew and Znew values, we can translate back into
+ * the original system by moving along those unit vectors
+ * by the corresponding values.
+ *
+ * Variable names matched to Marlin, c-version, and avoid the
+ * use of any vector library.
+ *
+ * by Andreas Hardtung 2016-06-07
+ * based on a Java function from "Delta Robot Kinematics V3"
+ * by Steve Graves
+ *
+ * The result is stored in the cartes[] array.
+ */
+void forward_kinematics_DELTA(float z1, float z2, float z3);
+
+FORCE_INLINE void forward_kinematics_DELTA(float point[ABC]) {
+  forward_kinematics_DELTA(point[A_AXIS], point[B_AXIS], point[C_AXIS]);
+}
+
+bool home_delta();
+
+#endif // __DELTA_H__