marlin/Marlin/src/module/delta.h

/**
 * 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[ABC];

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(const float radius, const float diagonal_rod, const float tower_angle_trim[ABC]);

/**
 * 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(__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__