Firmware2/Marlin/src/module/delta.cpp

288 lines
10 KiB
C++
Raw Normal View History

2017-09-08 22:35:25 +02:00
/**
* Marlin 3D Printer Firmware
2020-02-03 15:00:57 +01:00
* Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
2017-09-08 22:35:25 +02:00
*
* Based on Sprinter and grbl.
2019-06-28 06:57:50 +02:00
* Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
2017-09-08 22:35:25 +02:00
*
* 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
2020-07-23 05:20:14 +02:00
* along with this program. If not, see <https://www.gnu.org/licenses/>.
2017-09-08 22:35:25 +02:00
*
*/
/**
* delta.cpp
*/
#include "../inc/MarlinConfig.h"
#if ENABLED(DELTA)
#include "delta.h"
#include "motion.h"
// For homing:
#include "planner.h"
2017-09-08 22:35:25 +02:00
#include "endstops.h"
2020-10-17 02:36:25 +02:00
#include "../lcd/marlinui.h"
#include "../MarlinCore.h"
2017-09-08 22:35:25 +02:00
2019-02-04 12:25:50 +01:00
#if HAS_BED_PROBE
#include "probe.h"
#endif
#if ENABLED(SENSORLESS_HOMING)
#include "../feature/tmc_util.h"
2019-09-01 02:44:45 +02:00
#include "stepper/indirection.h"
#endif
#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
#include "../core/debug_out.h"
2017-09-08 22:35:25 +02:00
// Initialized by settings.load()
2019-09-29 11:25:39 +02:00
float delta_height;
abc_float_t delta_endstop_adj{0};
float delta_radius,
2017-09-08 22:35:25 +02:00
delta_diagonal_rod,
segments_per_second;
2019-09-29 11:25:39 +02:00
abc_float_t delta_tower_angle_trim;
xy_float_t delta_tower[ABC];
abc_float_t delta_diagonal_rod_2_tower;
float delta_clip_start_height = Z_MAX_POS;
abc_float_t delta_diagonal_rod_trim;
2017-09-08 22:35:25 +02:00
float delta_safe_distance_from_top();
/**
* Recalculate factors used for delta kinematics whenever
* settings have been changed (e.g., by M665).
*/
2017-11-08 10:07:17 +01:00
void recalc_delta_settings() {
constexpr abc_float_t trt = DELTA_RADIUS_TRIM_TOWER;
2019-09-29 11:25:39 +02:00
delta_tower[A_AXIS].set(cos(RADIANS(210 + delta_tower_angle_trim.a)) * (delta_radius + trt.a), // front left tower
sin(RADIANS(210 + delta_tower_angle_trim.a)) * (delta_radius + trt.a));
delta_tower[B_AXIS].set(cos(RADIANS(330 + delta_tower_angle_trim.b)) * (delta_radius + trt.b), // front right tower
sin(RADIANS(330 + delta_tower_angle_trim.b)) * (delta_radius + trt.b));
delta_tower[C_AXIS].set(cos(RADIANS( 90 + delta_tower_angle_trim.c)) * (delta_radius + trt.c), // back middle tower
sin(RADIANS( 90 + delta_tower_angle_trim.c)) * (delta_radius + trt.c));
delta_diagonal_rod_2_tower.set(sq(delta_diagonal_rod + delta_diagonal_rod_trim.a),
sq(delta_diagonal_rod + delta_diagonal_rod_trim.b),
sq(delta_diagonal_rod + delta_diagonal_rod_trim.c));
2017-11-08 10:07:17 +01:00
update_software_endstops(Z_AXIS);
set_all_unhomed();
2017-09-08 22:35:25 +02:00
}
/**
* Get a safe radius for calibration
*/
2020-01-12 00:14:33 +01:00
#if EITHER(DELTA_AUTO_CALIBRATION, DELTA_CALIBRATION_MENU)
2020-01-12 00:14:33 +01:00
#if ENABLED(DELTA_AUTO_CALIBRATION)
float calibration_radius_factor = 1;
#endif
float delta_calibration_radius() {
return calibration_radius_factor * (
#if HAS_BED_PROBE
FLOOR((DELTA_PRINTABLE_RADIUS) - _MAX(HYPOT(probe.offset_xy.x, probe.offset_xy.y), PROBING_MARGIN))
2020-01-12 00:14:33 +01:00
#else
DELTA_PRINTABLE_RADIUS
#endif
);
}
#endif
2017-09-08 22:35:25 +02:00
/**
* Delta Inverse Kinematics
*
2017-11-03 05:59:42 +01:00
* Calculate the tower positions for a given machine
2017-09-08 22:35:25 +02: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.
*/
#define DELTA_DEBUG(VAR) do { \
SERIAL_ECHOLNPAIR_P(PSTR("Cartesian X"), VAR.x, SP_Y_STR, VAR.y, SP_Z_STR, VAR.z); \
2020-06-28 06:27:28 +02:00
SERIAL_ECHOLNPAIR_P(PSTR("Delta A"), delta.a, SP_B_STR, delta.b, SP_C_STR, delta.c); \
2017-09-08 22:35:25 +02:00
}while(0)
2019-09-29 11:25:39 +02:00
void inverse_kinematics(const xyz_pos_t &raw) {
2018-08-25 04:26:29 +02:00
#if HAS_HOTEND_OFFSET
// Delta hotend offsets must be applied in Cartesian space with no "spoofing"
2019-09-29 11:25:39 +02:00
xyz_pos_t pos = { raw.x - hotend_offset[active_extruder].x,
raw.y - hotend_offset[active_extruder].y,
raw.z };
DELTA_IK(pos);
//DELTA_DEBUG(pos);
#else
DELTA_IK(raw);
//DELTA_DEBUG(raw);
#endif
2017-09-08 22:35:25 +02:00
}
/**
* Calculate the highest Z position where the
* effector has the full range of XY motion.
*/
float delta_safe_distance_from_top() {
2019-09-29 11:25:39 +02:00
xyz_pos_t cartesian{0};
2017-09-08 22:35:25 +02:00
inverse_kinematics(cartesian);
2019-09-29 11:25:39 +02:00
const float centered_extent = delta.a;
cartesian.y = DELTA_PRINTABLE_RADIUS;
2017-09-08 22:35:25 +02:00
inverse_kinematics(cartesian);
2019-09-29 11:25:39 +02:00
return ABS(centered_extent - delta.a);
2017-09-08 22:35:25 +02:00
}
/**
* 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.
*/
2021-03-04 00:57:24 +01:00
void forward_kinematics(const float &z1, const float &z2, const float &z3) {
2017-09-08 22:35:25 +02:00
// Create a vector in old coordinates along x axis of new coordinate
2019-09-29 11:25:39 +02:00
const float p12[3] = { delta_tower[B_AXIS].x - delta_tower[A_AXIS].x, delta_tower[B_AXIS].y - delta_tower[A_AXIS].y, z2 - z1 },
2017-09-08 22:35:25 +02:00
// Get the reciprocal of Magnitude of vector.
2018-07-07 03:41:08 +02:00
d2 = sq(p12[0]) + sq(p12[1]) + sq(p12[2]), inv_d = RSQRT(d2),
2017-09-08 22:35:25 +02:00
// Create unit vector by multiplying by the inverse of the magnitude.
2018-07-07 03:41:08 +02:00
ex[3] = { p12[0] * inv_d, p12[1] * inv_d, p12[2] * inv_d },
2017-09-08 22:35:25 +02:00
// Get the vector from the origin of the new system to the third point.
2019-09-29 11:25:39 +02:00
p13[3] = { delta_tower[C_AXIS].x - delta_tower[A_AXIS].x, delta_tower[C_AXIS].y - delta_tower[A_AXIS].y, z3 - z1 },
2017-09-08 22:35:25 +02:00
// Use the dot product to find the component of this vector on the X axis.
2018-07-07 03:41:08 +02:00
i = ex[0] * p13[0] + ex[1] * p13[1] + ex[2] * p13[2],
2017-09-08 22:35:25 +02:00
// Create a vector along the x axis that represents the x component of p13.
2018-07-07 03:41:08 +02:00
iex[3] = { ex[0] * i, ex[1] * i, ex[2] * i };
2017-09-08 22:35:25 +02:00
// Subtract the X component from the original vector leaving only Y. We use the
// variable that will be the unit vector after we scale it.
float ey[3] = { p13[0] - iex[0], p13[1] - iex[1], p13[2] - iex[2] };
// The magnitude and the inverse of the magnitude of Y component
const float j2 = sq(ey[0]) + sq(ey[1]) + sq(ey[2]), inv_j = RSQRT(j2);
2017-09-08 22:35:25 +02:00
// Convert to a unit vector
ey[0] *= inv_j; ey[1] *= inv_j; ey[2] *= inv_j;
2017-09-08 22:35:25 +02:00
// The cross product of the unit x and y is the unit z
// float[] ez = vectorCrossProd(ex, ey);
const float ez[3] = {
2017-09-08 22:35:25 +02:00
ex[1] * ey[2] - ex[2] * ey[1],
ex[2] * ey[0] - ex[0] * ey[2],
ex[0] * ey[1] - ex[1] * ey[0]
2018-07-07 03:41:08 +02:00
},
2017-09-08 22:35:25 +02:00
// We now have the d, i and j values defined in Wikipedia.
// Plug them into the equations defined in Wikipedia for Xnew, Ynew and Znew
2019-09-29 11:25:39 +02:00
Xnew = (delta_diagonal_rod_2_tower.a - delta_diagonal_rod_2_tower.b + d2) * inv_d * 0.5,
Ynew = ((delta_diagonal_rod_2_tower.a - delta_diagonal_rod_2_tower.c + sq(i) + j2) * 0.5 - i * Xnew) * inv_j,
Znew = SQRT(delta_diagonal_rod_2_tower.a - HYPOT2(Xnew, Ynew));
2017-09-08 22:35:25 +02:00
// Start from the origin of the old coordinates and add vectors in the
// old coords that represent the Xnew, Ynew and Znew to find the point
// in the old system.
2019-09-29 11:25:39 +02:00
cartes.set(delta_tower[A_AXIS].x + ex[0] * Xnew + ey[0] * Ynew - ez[0] * Znew,
delta_tower[A_AXIS].y + ex[1] * Xnew + ey[1] * Ynew - ez[1] * Znew,
z1 + ex[2] * Xnew + ey[2] * Ynew - ez[2] * Znew);
2017-09-08 22:35:25 +02:00
}
/**
* A delta can only safely home all axes at the same time
* This is like quick_home_xy() but for 3 towers.
*/
void home_delta() {
DEBUG_SECTION(log_home_delta, "home_delta", DEBUGGING(LEVELING));
2017-09-08 22:35:25 +02:00
// Init the current position of all carriages to 0,0,0
2019-09-29 11:25:39 +02:00
current_position.reset();
destination.reset();
2017-09-08 22:35:25 +02:00
sync_plan_position();
// Disable stealthChop if used. Enable diag1 pin on driver.
#if ENABLED(SENSORLESS_HOMING)
TERN_(X_SENSORLESS, sensorless_t stealth_states_x = start_sensorless_homing_per_axis(X_AXIS));
TERN_(Y_SENSORLESS, sensorless_t stealth_states_y = start_sensorless_homing_per_axis(Y_AXIS));
TERN_(Z_SENSORLESS, sensorless_t stealth_states_z = start_sensorless_homing_per_axis(Z_AXIS));
#endif
2017-09-08 22:35:25 +02:00
// Move all carriages together linearly until an endstop is hit.
2020-04-22 23:35:03 +02:00
current_position.z = (delta_height + 10 - TERN0(HAS_BED_PROBE, probe.offset.z));
line_to_current_position(homing_feedrate(Z_AXIS));
planner.synchronize();
2017-09-08 22:35:25 +02:00
// Re-enable stealthChop if used. Disable diag1 pin on driver.
#if ENABLED(SENSORLESS_HOMING)
TERN_(X_SENSORLESS, end_sensorless_homing_per_axis(X_AXIS, stealth_states_x));
TERN_(Y_SENSORLESS, end_sensorless_homing_per_axis(Y_AXIS, stealth_states_y));
TERN_(Z_SENSORLESS, end_sensorless_homing_per_axis(Z_AXIS, stealth_states_z));
#endif
endstops.validate_homing_move();
2017-09-08 22:35:25 +02:00
// At least one carriage has reached the top.
// Now re-home each carriage separately.
2018-07-01 01:13:13 +02:00
homeaxis(A_AXIS);
homeaxis(B_AXIS);
homeaxis(C_AXIS);
2017-09-08 22:35:25 +02:00
// Set all carriages to their home positions
// Do this here all at once for Delta, because
// XYZ isn't ABC. Applying this per-tower would
// give the impression that they are the same.
LOOP_XYZ(i) set_axis_is_at_home((AxisEnum)i);
sync_plan_position();
2017-09-08 22:35:25 +02:00
#if DISABLED(DELTA_HOME_TO_SAFE_ZONE) && defined(HOMING_BACKOFF_POST_MM)
constexpr xyz_float_t endstop_backoff = HOMING_BACKOFF_POST_MM;
if (endstop_backoff.z) {
current_position.z -= ABS(endstop_backoff.z) * Z_HOME_DIR;
line_to_current_position(homing_feedrate(Z_AXIS));
}
#endif
2017-09-08 22:35:25 +02:00
}
#endif // DELTA