Marlin_Firmware/Marlin/src/module/delta.h

124 lines
3.8 KiB
C
Raw Normal View History

2017-09-08 15:35:25 -05:00
/**
* 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
*/
#pragma once
2017-09-08 15:35:25 -05:00
extern float delta_height,
delta_endstop_adj[ABC],
2017-09-08 15:35:25 -05:00
delta_radius,
delta_diagonal_rod,
delta_segments_per_second,
delta_calibration_radius,
2017-09-24 02:18:15 -05:00
delta_tower_angle_trim[ABC];
2017-09-08 15:35:25 -05:00
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).
*/
2017-11-08 03:07:17 -06:00
void recalc_delta_settings();
2017-09-08 15:35:25 -05:00
/**
* Delta Inverse Kinematics
*
2017-11-02 23:59:42 -05:00
* Calculate the tower positions for a given machine
2017-09-08 15:35:25 -05:00
* 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)
*/
// Macro to obtain the Z position of an individual tower
#define DELTA_Z(V,T) V[Z_AXIS] + SQRT( \
2017-12-21 22:41:57 -06:00
delta_diagonal_rod_2_tower[T] - HYPOT2( \
delta_tower[T][X_AXIS] - V[X_AXIS], \
delta_tower[T][Y_AXIS] - V[Y_AXIS] \
) \
2017-09-08 15:35:25 -05:00
)
#define DELTA_IK(V) do { \
2017-12-21 22:41:57 -06:00
delta[A_AXIS] = DELTA_Z(V, A_AXIS); \
delta[B_AXIS] = DELTA_Z(V, B_AXIS); \
delta[C_AXIS] = DELTA_Z(V, C_AXIS); \
2017-09-08 15:35:25 -05:00
}while(0)
void inverse_kinematics(const float (&raw)[XYZ]);
FORCE_INLINE void inverse_kinematics(const float (&raw)[XYZE]) {
const float raw_xyz[XYZ] = { raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS] };
inverse_kinematics(raw_xyz);
}
2017-09-08 15:35:25 -05:00
/**
* 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(const float &z1, const float &z2, const float &z3);
2017-09-08 15:35:25 -05:00
FORCE_INLINE void forward_kinematics_DELTA(const float (&point)[ABC]) {
2017-09-08 15:35:25 -05:00
forward_kinematics_DELTA(point[A_AXIS], point[B_AXIS], point[C_AXIS]);
}
void home_delta();