bissen21/Marlin_Firmware
1
0
Fork 0
Code Issues Pull Requests Projects Releases 4 Wiki Activity

Merge pull request #8730 from thinkyhead/bf2_fixup_ubl

Browse Source 
[2.0.x] UBL - Skew and Dual X Carriage
This commit is contained in:
Scott Lahteine 2017-12-11 03:37:13 -06:00 committed by GitHub
commit c555a214d2
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
16 changed files with 435 additions and 404 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
.travis.yml
View File

@ -79,13 +79,14 @@ script:
- opt_set TEMP_SENSOR_3 20
- opt_set TEMP_SENSOR_4 999
- opt_set TEMP_SENSOR_BED 1
- opt_enable AUTO_BED_LEVELING_UBL DEBUG_LEVELING_FEATURE G26_MESH_EDITING ENABLE_LEVELING_FADE_HEIGHT EEPROM_SETTINGS EEPROM_CHITCHAT G3D_PANEL
- opt_enable AUTO_BED_LEVELING_UBL DEBUG_LEVELING_FEATURE G26_MESH_EDITING ENABLE_LEVELING_FADE_HEIGHT EEPROM_SETTINGS EEPROM_CHITCHAT G3D_PANEL SKEW_CORRECTION
- opt_enable_adv CUSTOM_USER_MENUS I2C_POSITION_ENCODERS BABYSTEPPING LIN_ADVANCE NANODLP_Z_SYNC
- build_marlin_pio ${TRAVIS_BUILD_DIR} ${TEST_PLATFORM}
#
# And with a Sled Z Probe
# Add a Sled Z Probe, do non-segmented moves
#
- opt_enable Z_PROBE_SLED
- opt_disable SEGMENT_LEVELED_MOVES
- opt_enable_adv BABYSTEP_ZPROBE_OFFSET DOUBLECLICK_FOR_Z_BABYSTEPPING
- build_marlin_pio ${TRAVIS_BUILD_DIR} ${TEST_PLATFORM}
#
@ -121,7 +122,7 @@ script:
- opt_enable ULTIMAKERCONTROLLER SDSUPPORT
- opt_enable PRINTCOUNTER NOZZLE_PARK_FEATURE NOZZLE_CLEAN_FEATURE PCA9632 USE_XMAX_PLUG
- opt_enable_adv BEZIER_CURVE_SUPPORT EXPERIMENTAL_I2CBUS
- opt_enable_adv ADVANCED_PAUSE_FEATURE PARK_HEAD_ON_PAUSE LCD_INFO_MENU
- opt_enable_adv ADVANCED_PAUSE_FEATURE PARK_HEAD_ON_PAUSE LCD_INFO_MENU M114_DETAIL
- opt_set_adv PWM_MOTOR_CURRENT {1300,1300,1250}
- opt_set_adv I2C_SLAVE_ADDRESS 63
- build_marlin_pio ${TRAVIS_BUILD_DIR} ${TEST_PLATFORM}

55
Marlin/src/feature/bedlevel/ubl/ubl.cpp
View File

@ -55,6 +55,59 @@
safe_delay(10);
}
#if ENABLED(UBL_DEVEL_DEBUGGING)
static void debug_echo_axis(const AxisEnum axis) {
if (current_position[axis] == destination[axis])
SERIAL_ECHOPGM("-------------");
else
SERIAL_ECHO_F(destination[X_AXIS], 6);
}
void debug_current_and_destination(const char *title) {
// if the title message starts with a '!' it is so important, we are going to
// ignore the status of the g26_debug_flag
if (*title != '!' && !g26_debug_flag) return;
const float de = destination[E_AXIS] - current_position[E_AXIS];
if (de == 0.0) return; // Printing moves only
const float dx = destination[X_AXIS] - current_position[X_AXIS],
dy = destination[Y_AXIS] - current_position[Y_AXIS],
xy_dist = HYPOT(dx, dy);
if (xy_dist == 0.0) return;
SERIAL_ECHOPGM(" fpmm=");
const float fpmm = de / xy_dist;
SERIAL_ECHO_F(fpmm, 6);
SERIAL_ECHOPGM(" current=( ");
SERIAL_ECHO_F(current_position[X_AXIS], 6);
SERIAL_ECHOPGM(", ");
SERIAL_ECHO_F(current_position[Y_AXIS], 6);
SERIAL_ECHOPGM(", ");
SERIAL_ECHO_F(current_position[Z_AXIS], 6);
SERIAL_ECHOPGM(", ");
SERIAL_ECHO_F(current_position[E_AXIS], 6);
SERIAL_ECHOPGM(" ) destination=( ");
debug_echo_axis(X_AXIS);
SERIAL_ECHOPGM(", ");
debug_echo_axis(Y_AXIS);
SERIAL_ECHOPGM(", ");
debug_echo_axis(Z_AXIS);
SERIAL_ECHOPGM(", ");
debug_echo_axis(E_AXIS);
SERIAL_ECHOPGM(" ) ");
SERIAL_ECHO(title);
SERIAL_EOL();
}
#endif // UBL_DEVEL_DEBUGGING
int8_t unified_bed_leveling::storage_slot;
float unified_bed_leveling::z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
@ -178,7 +231,7 @@
uint8_t error_flag = 0;
if (settings.calc_num_meshes() < 1) {
SERIAL_PROTOCOLLNPGM("?Insufficient EEPROM storage for a mesh of this size.");
SERIAL_PROTOCOLLNPGM("?Mesh too big for EEPROM.");
error_flag++;
}

15
Marlin/src/feature/bedlevel/ubl/ubl.h
View File

@ -23,6 +23,8 @@
#ifndef UNIFIED_BED_LEVELING_H
#define UNIFIED_BED_LEVELING_H
//#define UBL_DEVEL_DEBUGGING
#include "../bedlevel.h"
#include "../../../module/planner.h"
#include "../../../module/motion.h"
@ -37,7 +39,11 @@
// ubl_motion.cpp
void debug_current_and_destination(const char * const title);
#if ENABLED(UBL_DEVEL_DEBUGGING)
void debug_current_and_destination(const char * const title);
#else
FORCE_INLINE void debug_current_and_destination(const char * const title) { UNUSED(title); }
#endif
// ubl_G29.cpp
@ -315,8 +321,11 @@ class unified_bed_leveling {
return i < GRID_MAX_POINTS_Y ? pgm_read_float(&_mesh_index_to_ypos[i]) : MESH_MIN_Y + i * (MESH_Y_DIST);
}
static bool prepare_segmented_line_to(const float rtarget[XYZE], const float &feedrate);
static void line_to_destination_cartesian(const float &fr, uint8_t e);
#if UBL_SEGMENTED
static bool prepare_segmented_line_to(const float (&rtarget)[XYZE], const float &feedrate);
#else
static void line_to_destination_cartesian(const float &fr, const uint8_t e);
#endif
#define _CMPZ(a,b) (z_values[a][b] == z_values[a][b+1])
#define CMPZ(a) (_CMPZ(a, 0) && _CMPZ(a, 1))

166
Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp
View File

@ -23,93 +23,42 @@
#if ENABLED(AUTO_BED_LEVELING_UBL)
#include "../bedlevel.h"
#include "../../../module/planner.h"
#include "../../../module/stepper.h"
#include "../../../module/motion.h"
#include "../bedlevel.h"
#include "../../../module/planner.h"
#include "../../../module/stepper.h"
#include "../../../module/motion.h"
#if ENABLED(DELTA)
#if ENABLED(DELTA)
#include "../../../module/delta.h"
#endif
#endif
#include "../../../Marlin.h"
#include <math.h>
#include "../../../Marlin.h"
#include <math.h>
extern float destination[XYZE];
#if AVR_AT90USB1286_FAMILY // Teensyduino & Printrboard IDE extensions have compile errors without this
#if AVR_AT90USB1286_FAMILY // Teensyduino & Printrboard IDE extensions have compile errors without this
inline void set_current_from_destination() { COPY(current_position, destination); }
#else
#else
extern void set_current_from_destination();
#endif
#endif
static void debug_echo_axis(const AxisEnum axis) {
if (current_position[axis] == destination[axis])
SERIAL_ECHOPGM("-------------");
else
SERIAL_ECHO_F(destination[X_AXIS], 6);
}
#if !UBL_SEGMENTED
void debug_current_and_destination(const char *title) {
// if the title message starts with a '!' it is so important, we are going to
// ignore the status of the g26_debug_flag
if (*title != '!' && !g26_debug_flag) return;
const float de = destination[E_AXIS] - current_position[E_AXIS];
if (de == 0.0) return; // Printing moves only
const float dx = destination[X_AXIS] - current_position[X_AXIS],
dy = destination[Y_AXIS] - current_position[Y_AXIS],
xy_dist = HYPOT(dx, dy);
if (xy_dist == 0.0) return;
SERIAL_ECHOPGM(" fpmm=");
const float fpmm = de / xy_dist;
SERIAL_ECHO_F(fpmm, 6);
SERIAL_ECHOPGM(" current=( ");
SERIAL_ECHO_F(current_position[X_AXIS], 6);
SERIAL_ECHOPGM(", ");
SERIAL_ECHO_F(current_position[Y_AXIS], 6);
SERIAL_ECHOPGM(", ");
SERIAL_ECHO_F(current_position[Z_AXIS], 6);
SERIAL_ECHOPGM(", ");
SERIAL_ECHO_F(current_position[E_AXIS], 6);
SERIAL_ECHOPGM(" ) destination=( ");
debug_echo_axis(X_AXIS);
SERIAL_ECHOPGM(", ");
debug_echo_axis(Y_AXIS);
SERIAL_ECHOPGM(", ");
debug_echo_axis(Z_AXIS);
SERIAL_ECHOPGM(", ");
debug_echo_axis(E_AXIS);
SERIAL_ECHOPGM(" ) ");
SERIAL_ECHO(title);
SERIAL_EOL();
}
void unified_bed_leveling::line_to_destination_cartesian(const float &feed_rate, uint8_t extruder) {
void unified_bed_leveling::line_to_destination_cartesian(const float &feed_rate, const uint8_t extruder) {
/**
* Much of the nozzle movement will be within the same cell. So we will do as little computation
* as possible to determine if this is the case. If this move is within the same cell, we will
* just do the required Z-Height correction, call the Planner's buffer_line() routine, and leave
*/
const float start[XYZE] = {
current_position[X_AXIS],
current_position[Y_AXIS],
current_position[Z_AXIS],
current_position[E_AXIS]
},
end[XYZE] = {
destination[X_AXIS],
destination[Y_AXIS],
destination[Z_AXIS],
destination[E_AXIS]
};
#if ENABLED(SKEW_CORRECTION)
// For skew correction just adjust the destination point and we're done
float start[XYZE] = { current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS] },
end[XYZE] = { destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS] };
planner.skew(start[X_AXIS], start[Y_AXIS], start[Z_AXIS]);
planner.skew(end[X_AXIS], end[Y_AXIS], end[Z_AXIS]);
#else
const float (&start)[XYZE] = current_position,
(&end)[XYZE] = destination;
#endif
const int cell_start_xi = get_cell_index_x(start[X_AXIS]),
cell_start_yi = get_cell_index_y(start[Y_AXIS]),
@ -117,13 +66,13 @@
cell_dest_yi = get_cell_index_y(end[Y_AXIS]);
if (g26_debug_flag) {
SERIAL_ECHOPAIR(" ubl.line_to_destination(xe=", end[X_AXIS]);
SERIAL_ECHOPAIR(", ye=", end[Y_AXIS]);
SERIAL_ECHOPAIR(", ze=", end[Z_AXIS]);
SERIAL_ECHOPAIR(", ee=", end[E_AXIS]);
SERIAL_ECHOPAIR(" ubl.line_to_destination_cartesian(xe=", destination[X_AXIS]);
SERIAL_ECHOPAIR(", ye=", destination[Y_AXIS]);
SERIAL_ECHOPAIR(", ze=", destination[Z_AXIS]);
SERIAL_ECHOPAIR(", ee=", destination[E_AXIS]);
SERIAL_CHAR(')');
SERIAL_EOL();
debug_current_and_destination(PSTR("Start of ubl.line_to_destination()"));
debug_current_and_destination(PSTR("Start of ubl.line_to_destination_cartesian()"));
}
if (cell_start_xi == cell_dest_xi && cell_start_yi == cell_dest_yi) { // if the whole move is within the same cell,
@ -139,11 +88,11 @@
// Note: There is no Z Correction in this case. We are off the grid and don't know what
// a reasonable correction would be.
planner._buffer_line(end[X_AXIS], end[Y_AXIS], end[Z_AXIS], end[E_AXIS], feed_rate, extruder);
planner.buffer_segment(end[X_AXIS], end[Y_AXIS], end[Z_AXIS], end[E_AXIS], feed_rate, extruder);
set_current_from_destination();
if (g26_debug_flag)
debug_current_and_destination(PSTR("out of bounds in ubl.line_to_destination()"));
debug_current_and_destination(PSTR("out of bounds in ubl.line_to_destination_cartesian()"));
return;
}
@ -183,10 +132,10 @@
*/
if (isnan(z0)) z0 = 0.0;
planner._buffer_line(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + z0, end[E_AXIS], feed_rate, extruder);
planner.buffer_segment(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + z0, end[E_AXIS], feed_rate, extruder);
if (g26_debug_flag)
debug_current_and_destination(PSTR("FINAL_MOVE in ubl.line_to_destination()"));
debug_current_and_destination(PSTR("FINAL_MOVE in ubl.line_to_destination_cartesian()"));
set_current_from_destination();
return;
@ -274,7 +223,7 @@
* Without this check, it is possible for the algorithm to generate a zero length move in the case
* where the line is heading down and it is starting right on a Mesh Line boundary. For how often that
* happens, it might be best to remove the check and always 'schedule' the move because
* the planner._buffer_line() routine will filter it if that happens.
* the planner.buffer_segment() routine will filter it if that happens.
*/
if (ry != start[Y_AXIS]) {
if (!inf_normalized_flag) {
@ -287,12 +236,12 @@
z_position = end[Z_AXIS];
}
planner._buffer_line(rx, ry, z_position + z0, e_position, feed_rate, extruder);
planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder);
} //else printf("FIRST MOVE PRUNED ");
}
if (g26_debug_flag)
debug_current_and_destination(PSTR("vertical move done in ubl.line_to_destination()"));
debug_current_and_destination(PSTR("vertical move done in ubl.line_to_destination_cartesian()"));
//
// Check if we are at the final destination. Usually, we won't be, but if it is on a Y Mesh Line, we are done.
@ -338,7 +287,7 @@
* Without this check, it is possible for the algorithm to generate a zero length move in the case
* where the line is heading left and it is starting right on a Mesh Line boundary. For how often
* that happens, it might be best to remove the check and always 'schedule' the move because
* the planner._buffer_line() routine will filter it if that happens.
* the planner.buffer_segment() routine will filter it if that happens.
*/
if (rx != start[X_AXIS]) {
if (!inf_normalized_flag) {
@ -351,12 +300,12 @@
z_position = end[Z_AXIS];
}
planner._buffer_line(rx, ry, z_position + z0, e_position, feed_rate, extruder);
planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder);
} //else printf("FIRST MOVE PRUNED ");
}
if (g26_debug_flag)
debug_current_and_destination(PSTR("horizontal move done in ubl.line_to_destination()"));
debug_current_and_destination(PSTR("horizontal move done in ubl.line_to_destination_cartesian()"));
if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
goto FINAL_MOVE;
@ -413,7 +362,7 @@
e_position = end[E_AXIS];
z_position = end[Z_AXIS];
}
planner._buffer_line(rx, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder);
planner.buffer_segment(rx, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder);
current_yi += dyi;
yi_cnt--;
}
@ -441,7 +390,7 @@
z_position = end[Z_AXIS];
}
planner._buffer_line(next_mesh_line_x, ry, z_position + z0, e_position, feed_rate, extruder);
planner.buffer_segment(next_mesh_line_x, ry, z_position + z0, e_position, feed_rate, extruder);
current_xi += dxi;
xi_cnt--;
}
@ -450,7 +399,7 @@
}
if (g26_debug_flag)
debug_current_and_destination(PSTR("generic move done in ubl.line_to_destination()"));
debug_current_and_destination(PSTR("generic move done in ubl.line_to_destination_cartesian()"));
if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
goto FINAL_MOVE;
@ -458,29 +407,28 @@
set_current_from_destination();
}
#if UBL_DELTA
// macro to inline copy exactly 4 floats, don't rely on sizeof operator
#define COPY_XYZE( target, source ) { \
target[X_AXIS] = source[X_AXIS]; \
target[Y_AXIS] = source[Y_AXIS]; \
target[Z_AXIS] = source[Z_AXIS]; \
target[E_AXIS] = source[E_AXIS]; \
}
#else // UBL_SEGMENTED
#if IS_SCARA // scale the feed rate from mm/s to degrees/s
static float scara_feed_factor, scara_oldA, scara_oldB;
#endif
// We don't want additional apply_leveling() performed by regular buffer_line or buffer_line_kinematic,
// so we call _buffer_line directly here. Per-segmented leveling and kinematics performed first.
// so we call buffer_segment directly here. Per-segmented leveling and kinematics performed first.
inline void _O2 ubl_buffer_segment_raw(const float raw[XYZE], const float &fr) {
inline void _O2 ubl_buffer_segment_raw(const float (&in_raw)[XYZE], const float &fr) {
#if ENABLED(SKEW_CORRECTION)
float raw[XYZE] = { in_raw[X_AXIS], in_raw[Y_AXIS], in_raw[Z_AXIS] };
planner.skew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]);
#else
const float (&raw)[XYZE] = in_raw;
#endif
#if ENABLED(DELTA) // apply delta inverse_kinematics
DELTA_RAW_IK();
planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], fr, active_extruder);
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], in_raw[E_AXIS], fr, active_extruder);
#elif IS_SCARA // apply scara inverse_kinematics (should be changed to save raw->logical->raw)
@ -493,11 +441,11 @@
scara_oldB = delta[B_AXIS];
float s_feedrate = max(adiff, bdiff) * scara_feed_factor;
planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], s_feedrate, active_extruder);
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], in_raw[E_AXIS], s_feedrate, active_extruder);
#else // CARTESIAN
planner._buffer_line(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], raw[E_AXIS], fr, active_extruder);
planner.buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], in_raw[E_AXIS], fr, active_extruder);
#endif
}
@ -516,11 +464,11 @@
/**
* Prepare a segmented linear move for DELTA/SCARA/CARTESIAN with UBL and FADE semantics.
* This calls planner._buffer_line multiple times for small incremental moves.
* This calls planner.buffer_segment multiple times for small incremental moves.
* Returns true if did NOT move, false if moved (requires current_position update).
*/
bool _O2 unified_bed_leveling::prepare_segmented_line_to(const float rtarget[XYZE], const float &feedrate) {
bool _O2 unified_bed_leveling::prepare_segmented_line_to(const float (&rtarget)[XYZE], const float &feedrate) {
if (!position_is_reachable(rtarget[X_AXIS], rtarget[Y_AXIS])) // fail if moving outside reachable boundary
return true; // did not move, so current_position still accurate
@ -675,6 +623,6 @@
} // cell loop
}
#endif // UBL_DELTA
#endif // UBL_SEGMENTED
#endif // AUTO_BED_LEVELING_UBL

8
Marlin/src/gcode/bedlevel/G26.cpp
View File

@ -220,7 +220,7 @@ mesh_index_pair find_closest_circle_to_print(const float &X, const float &Y) {
void G26_line_to_destination(const float &feed_rate) {
const float save_feedrate = feedrate_mm_s;
feedrate_mm_s = feed_rate; // use specified feed rate
prepare_move_to_destination(); // will ultimately call ubl.line_to_destination_cartesian or ubl.prepare_linear_move_to for UBL_DELTA
prepare_move_to_destination(); // will ultimately call ubl.line_to_destination_cartesian or ubl.prepare_linear_move_to for UBL_SEGMENTED
feedrate_mm_s = save_feedrate; // restore global feed rate
}
@ -261,16 +261,16 @@ void move_to(const float &rx, const float &ry, const float &z, const float &e_de
set_destination_from_current();
}
FORCE_INLINE void move_to(const float where[XYZE], const float &de) { move_to(where[X_AXIS], where[Y_AXIS], where[Z_AXIS], de); }
FORCE_INLINE void move_to(const float (&where)[XYZE], const float &de) { move_to(where[X_AXIS], where[Y_AXIS], where[Z_AXIS], de); }
void retract_filament(const float where[XYZE]) {
void retract_filament(const float (&where)[XYZE]) {
if (!g26_retracted) { // Only retract if we are not already retracted!
g26_retracted = true;
move_to(where, -1.0 * g26_retraction_multiplier);
}
}
void recover_filament(const float where[XYZE]) {
void recover_filament(const float (&where)[XYZE]) {
if (g26_retracted) { // Only un-retract if we are retracted.
move_to(where, 1.2 * g26_retraction_multiplier);
g26_retracted = false;

13
Marlin/src/gcode/host/M114.cpp
View File

@ -28,7 +28,7 @@
#if ENABLED(M114_DETAIL)
void report_xyze(const float pos[XYZE], const uint8_t n = 4, const uint8_t precision = 3) {
void report_xyze(const float pos[], const uint8_t n = 4, const uint8_t precision = 3) {
char str[12];
for (uint8_t i = 0; i < n; i++) {
SERIAL_CHAR(' ');
@ -39,7 +39,7 @@
SERIAL_EOL();
}
inline void report_xyz(const float pos[XYZ]) { report_xyze(pos, 3); }
inline void report_xyz(const float pos[]) { report_xyze(pos, 3); }
void report_current_position_detail() {
@ -80,8 +80,13 @@
#endif
SERIAL_PROTOCOLPGM("Stepper:");
const float step_count[XYZE] = { stepper.position(X_AXIS), stepper.position(Y_AXIS), stepper.position(Z_AXIS), stepper.position(E_AXIS) };
report_xyze(step_count, 4, 0);
LOOP_XYZE(i) {
SERIAL_CHAR(' ');
SERIAL_CHAR(axis_codes[i]);
SERIAL_CHAR(':');
SERIAL_PROTOCOL(stepper.position((AxisEnum)i));
}
SERIAL_EOL();
#if IS_SCARA
const float deg[XYZ] = {

18
Marlin/src/gcode/motion/G2_G3.cpp
View File

@ -44,9 +44,9 @@
* options for G2/G3 arc generation. In future these options may be GCode tunable.
*/
void plan_arc(
float rtarget[XYZE], // Destination position
float *offset, // Center of rotation relative to current_position
uint8_t clockwise // Clockwise?
const float (&cart)[XYZE], // Destination position
const float (&offset)[2], // Center of rotation relative to current_position
const uint8_t clockwise // Clockwise?
) {
#if ENABLED(CNC_WORKSPACE_PLANES)
AxisEnum p_axis, q_axis, l_axis;
@ -66,10 +66,10 @@ void plan_arc(
const float radius = HYPOT(r_P, r_Q),
center_P = current_position[p_axis] - r_P,
center_Q = current_position[q_axis] - r_Q,
rt_X = rtarget[p_axis] - center_P,
rt_Y = rtarget[q_axis] - center_Q,
linear_travel = rtarget[l_axis] - current_position[l_axis],
extruder_travel = rtarget[E_AXIS] - current_position[E_AXIS];
rt_X = cart[p_axis] - center_P,
rt_Y = cart[q_axis] - center_Q,
linear_travel = cart[l_axis] - current_position[l_axis],
extruder_travel = cart[E_AXIS] - current_position[E_AXIS];
// CCW angle of rotation between position and target from the circle center. Only one atan2() trig computation required.
float angular_travel = ATAN2(r_P * rt_Y - r_Q * rt_X, r_P * rt_X + r_Q * rt_Y);
@ -77,7 +77,7 @@ void plan_arc(
if (clockwise) angular_travel -= RADIANS(360);
// Make a circle if the angular rotation is 0 and the target is current position
if (angular_travel == 0 && current_position[p_axis] == rtarget[p_axis] && current_position[q_axis] == rtarget[q_axis])
if (angular_travel == 0 && current_position[p_axis] == cart[p_axis] && current_position[q_axis] == cart[q_axis])
angular_travel = RADIANS(360);
const float mm_of_travel = HYPOT(angular_travel * radius, FABS(linear_travel));
@ -177,7 +177,7 @@ void plan_arc(
}
// Ensure last segment arrives at target location.
planner.buffer_line_kinematic(rtarget, fr_mm_s, active_extruder);
planner.buffer_line_kinematic(cart, fr_mm_s, active_extruder);
// As far as the parser is concerned, the position is now == target. In reality the
// motion control system might still be processing the action and the real tool position

4
Marlin/src/gcode/motion/G5.cpp
View File

@ -27,7 +27,7 @@
#include "../../module/motion.h"
#include "../../module/planner_bezier.h"
void plan_cubic_move(const float offset[4]) {
void plan_cubic_move(const float (&offset)[4]) {
cubic_b_spline(current_position, destination, offset, MMS_SCALED(feedrate_mm_s), active_extruder);
// As far as the parser is concerned, the position is now == destination. In reality the
@ -62,7 +62,7 @@ void GcodeSuite::G5() {
get_destination_from_command();
const float offset[] = {
const float offset[4] = {
parser.linearval('I'),
parser.linearval('J'),
parser.linearval('P'),

4
Marlin/src/inc/Conditionals_post.h
View File

@ -977,7 +977,7 @@
/**
* Set granular options based on the specific type of leveling
*/
#define UBL_DELTA (ENABLED(AUTO_BED_LEVELING_UBL) && (ENABLED(DELTA) || ENABLED(SEGMENT_LEVELED_MOVES)))
#define UBL_SEGMENTED (ENABLED(AUTO_BED_LEVELING_UBL) && (ENABLED(DELTA) || ENABLED(SEGMENT_LEVELED_MOVES)))
#define ABL_PLANAR (ENABLED(AUTO_BED_LEVELING_LINEAR) || ENABLED(AUTO_BED_LEVELING_3POINT))
#define ABL_GRID (ENABLED(AUTO_BED_LEVELING_LINEAR) || ENABLED(AUTO_BED_LEVELING_BILINEAR))
#define OLDSCHOOL_ABL (ABL_PLANAR || ABL_GRID)
@ -985,7 +985,7 @@
#define HAS_LEVELING (HAS_ABL || ENABLED(MESH_BED_LEVELING))
#define HAS_AUTOLEVEL (HAS_ABL && DISABLED(PROBE_MANUALLY))
#define HAS_MESH (ENABLED(AUTO_BED_LEVELING_BILINEAR) || ENABLED(AUTO_BED_LEVELING_UBL) || ENABLED(MESH_BED_LEVELING))
#define PLANNER_LEVELING (OLDSCHOOL_ABL || ENABLED(MESH_BED_LEVELING) || UBL_DELTA)
#define PLANNER_LEVELING (OLDSCHOOL_ABL || ENABLED(MESH_BED_LEVELING))
#define HAS_PROBING_PROCEDURE (HAS_ABL || ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST))
#if HAS_PROBING_PROCEDURE
#define PROBE_BED_WIDTH abs(RIGHT_PROBE_BED_POSITION - (LEFT_PROBE_BED_POSITION))

5
Marlin/src/inc/SanityCheck.h
View File

@ -603,7 +603,7 @@ static_assert(1 >= 0
#error "Delta probably shouldn't use Z_MIN_PROBE_ENDSTOP. Comment out this line to continue."
#elif DISABLED(USE_XMAX_PLUG) && DISABLED(USE_YMAX_PLUG) && DISABLED(USE_ZMAX_PLUG)
#error "You probably want to use Max Endstops for DELTA!"
#elif ENABLED(ENABLE_LEVELING_FADE_HEIGHT) && DISABLED(AUTO_BED_LEVELING_BILINEAR) && !UBL_DELTA
#elif ENABLED(ENABLE_LEVELING_FADE_HEIGHT) && DISABLED(AUTO_BED_LEVELING_BILINEAR) && !UBL_SEGMENTED
#error "ENABLE_LEVELING_FADE_HEIGHT on DELTA requires AUTO_BED_LEVELING_BILINEAR or AUTO_BED_LEVELING_UBL."
#elif ENABLED(DELTA_AUTO_CALIBRATION) && !(HAS_BED_PROBE || ENABLED(ULTIPANEL))
#error "DELTA_AUTO_CALIBRATION requires a probe or LCD Controller."
@ -1497,9 +1497,6 @@ static_assert(COUNT(sanity_arr_3) <= XYZE_N, "DEFAULT_MAX_ACCELERATION has too m
#endif
#if ENABLED(SKEW_CORRECTION)
#if ENABLED(AUTO_BED_LEVELING_UBL) && !ENABLED(SEGMENT_LEVELED_MOVES)
#error "SKEW_CORRECTION with AUTO_BED_LEVELING_UBL requires SEGMENT_LEVELED_MOVES."
#endif
#if !defined(XY_SKEW_FACTOR) && !(defined(XY_DIAG_AC) && defined(XY_DIAG_BD) && defined(XY_SIDE_AD))
#error "SKEW_CORRECTION requires XY_SKEW_FACTOR or XY_DIAG_AC, XY_DIAG_BD, XY_SIDE_AD."
#endif

26
Marlin/src/module/motion.cpp
View File

@ -264,7 +264,7 @@ void buffer_line_to_destination(const float fr_mm_s) {
gcode.refresh_cmd_timeout();
#if UBL_DELTA
#if UBL_SEGMENTED
// ubl segmented line will do z-only moves in single segment
ubl.prepare_segmented_line_to(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s));
#else
@ -495,7 +495,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
#endif
#if !UBL_DELTA
#if !UBL_SEGMENTED
#if IS_KINEMATIC
#if ENABLED(AUTO_BED_LEVELING_BILINEAR)
@ -517,13 +517,19 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
/**
* Prepare a linear move in a DELTA or SCARA setup.
*
* Called from prepare_move_to_destination as the
* default Delta/SCARA segmenter.
*
* This calls planner.buffer_line several times, adding
* small incremental moves for DELTA or SCARA.
*
* For Unified Bed Leveling (Delta or Segmented Cartesian)
* the ubl.prepare_segmented_line_to method replaces this.
*
* For Auto Bed Leveling (Bilinear) with SEGMENT_LEVELED_MOVES
* this is replaced by segmented_line_to_destination below.
*/
inline bool prepare_kinematic_move_to(float rtarget[XYZE]) {
inline bool prepare_kinematic_move_to(const float (&rtarget)[XYZE]) {
// Get the top feedrate of the move in the XY plane
const float _feedrate_mm_s = MMS_SCALED(feedrate_mm_s);
@ -756,7 +762,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
}
#endif // !IS_KINEMATIC
#endif // !UBL_DELTA
#endif // !UBL_SEGMENTED
#if ENABLED(DUAL_X_CARRIAGE) || ENABLED(DUAL_NOZZLE_DUPLICATION_MODE)
bool extruder_duplication_enabled = false; // Used in Dual X mode 2
@ -790,7 +796,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
*
* Return true if current_position[] was set to destination[]
*/
inline bool prepare_move_to_destination_dualx() {
inline bool dual_x_carriage_unpark() {
if (active_extruder_parked) {
switch (dual_x_carriage_mode) {
case DXC_FULL_CONTROL_MODE:
@ -859,7 +865,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
break;
}
}
return prepare_move_to_destination_cartesian();
return false;
}
#endif // DUAL_X_CARRIAGE
@ -900,13 +906,15 @@ void prepare_move_to_destination() {
#endif // PREVENT_COLD_EXTRUSION || PREVENT_LENGTHY_EXTRUDE
#if ENABLED(DUAL_X_CARRIAGE)
if (dual_x_carriage_unpark()) return;
#endif
if (
#if UBL_DELTA // Also works for CARTESIAN (smaller segments follow mesh more closely)
#if UBL_SEGMENTED
ubl.prepare_segmented_line_to(destination, MMS_SCALED(feedrate_mm_s))
#elif IS_KINEMATIC
prepare_kinematic_move_to(destination)
#elif ENABLED(DUAL_X_CARRIAGE)
prepare_move_to_destination_dualx()
#else
prepare_move_to_destination_cartesian()
#endif

42
Marlin/src/module/planner.cpp
View File

@ -580,14 +580,7 @@ void Planner::calculate_volumetric_multipliers() {
void Planner::apply_leveling(float &rx, float &ry, float &rz) {
#if ENABLED(SKEW_CORRECTION)
if (WITHIN(rx, X_MIN_POS + 1, X_MAX_POS) && WITHIN(ry, Y_MIN_POS + 1, Y_MAX_POS)) {
const float tempry = ry - (rz * planner.yz_skew_factor),
temprx = rx - (ry * planner.xy_skew_factor) - (rz * (planner.xz_skew_factor - (planner.xy_skew_factor * planner.yz_skew_factor)));
if (WITHIN(temprx, X_MIN_POS, X_MAX_POS) && WITHIN(tempry, Y_MIN_POS, Y_MAX_POS)) {
rx = temprx;
ry = tempry;
}
}
skew(rx, ry, rz);
#endif
if (!leveling_active) return;
@ -616,7 +609,7 @@ void Planner::calculate_volumetric_multipliers() {
#endif
rz += (
#if ENABLED(AUTO_BED_LEVELING_UBL) // UBL_DELTA
#if ENABLED(AUTO_BED_LEVELING_UBL)
ubl.get_z_correction(rx, ry) * fade_scaling_factor
#elif ENABLED(MESH_BED_LEVELING)
mbl.get_z(rx, ry
@ -678,14 +671,7 @@ void Planner::calculate_volumetric_multipliers() {
}
#if ENABLED(SKEW_CORRECTION)
if (WITHIN(raw[X_AXIS], X_MIN_POS, X_MAX_POS) && WITHIN(raw[Y_AXIS], Y_MIN_POS, Y_MAX_POS)) {
const float temprx = raw[X_AXIS] + raw[Y_AXIS] * planner.xy_skew_factor + raw[Z_AXIS] * planner.xz_skew_factor,
tempry = raw[Y_AXIS] + raw[Z_AXIS] * planner.yz_skew_factor;
if (WITHIN(temprx, X_MIN_POS, X_MAX_POS) && WITHIN(tempry, Y_MIN_POS, Y_MAX_POS)) {
raw[X_AXIS] = temprx;
raw[Y_AXIS] = tempry;
}
}
unskew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]);
#endif
}
@ -1365,7 +1351,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
} // _buffer_steps()
/**
* Planner::_buffer_line
* Planner::buffer_segment
*
* Add a new linear movement to the buffer in axis units.
*
@ -1375,7 +1361,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
* fr_mm_s - (target) speed of the move
* extruder - target extruder
*/
void Planner::_buffer_line(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder) {
void Planner::buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder) {
// When changing extruders recalculate steps corresponding to the E position
#if ENABLED(DISTINCT_E_FACTORS)
if (last_extruder != extruder && axis_steps_per_mm[E_AXIS_N] != axis_steps_per_mm[E_AXIS + last_extruder]) {
@ -1394,7 +1380,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
};
/* <-- add a slash to enable
SERIAL_ECHOPAIR(" _buffer_line FR:", fr_mm_s);
SERIAL_ECHOPAIR(" buffer_segment FR:", fr_mm_s);
#if IS_KINEMATIC
SERIAL_ECHOPAIR(" A:", a);
SERIAL_ECHOPAIR(" (", position[A_AXIS]);
@ -1441,7 +1427,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
stepper.wake_up();
} // _buffer_line()
} // buffer_segment()
/**
* Directly set the planner XYZ position (and stepper positions)
@ -1466,18 +1452,18 @@ void Planner::_set_position_mm(const float &a, const float &b, const float &c, c
ZERO(previous_speed);
}
void Planner::set_position_mm_kinematic(const float position[NUM_AXIS]) {
void Planner::set_position_mm_kinematic(const float (&cart)[XYZE]) {
#if PLANNER_LEVELING
float lpos[XYZ] = { position[X_AXIS], position[Y_AXIS], position[Z_AXIS] };
apply_leveling(lpos);
float raw[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] };
apply_leveling(raw);
#else
const float * const lpos = position;
const float (&raw)[XYZE] = cart;
#endif
#if IS_KINEMATIC
inverse_kinematics(lpos);
_set_position_mm(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], position[E_AXIS]);
inverse_kinematics(raw);
_set_position_mm(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS]);
#else
_set_position_mm(lpos[X_AXIS], lpos[Y_AXIS], lpos[Z_AXIS], position[E_AXIS]);
_set_position_mm(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_AXIS]);
#endif
}

44
Marlin/src/module/planner.h
View File

@ -146,7 +146,7 @@ class Planner {
* head!=tail : blocks are in the buffer
* head==(tail-1)%size : the buffer is full
*
* Writer of head is Planner::_buffer_line().
* Writer of head is Planner::buffer_segment().
* Reader of tail is Stepper::isr(). Always consider tail busy / read-only
*/
static block_t block_buffer[BLOCK_BUFFER_SIZE];
@ -345,6 +345,30 @@ class Planner {
#endif
#if ENABLED(SKEW_CORRECTION)
FORCE_INLINE static void skew(float &cx, float &cy, const float &cz) {
if (WITHIN(cx, X_MIN_POS + 1, X_MAX_POS) && WITHIN(cy, Y_MIN_POS + 1, Y_MAX_POS)) {
const float sx = cx - (cy * xy_skew_factor) - (cz * (xz_skew_factor - (xy_skew_factor * yz_skew_factor))),
sy = cy - (cz * yz_skew_factor);
if (WITHIN(sx, X_MIN_POS, X_MAX_POS) && WITHIN(sy, Y_MIN_POS, Y_MAX_POS)) {
cx = sx; cy = sy;
}
}
}
FORCE_INLINE static void unskew(float &cx, float &cy, const float &cz) {
if (WITHIN(cx, X_MIN_POS, X_MAX_POS) && WITHIN(cy, Y_MIN_POS, Y_MAX_POS)) {
const float sx = cx + cy * xy_skew_factor + cz * xz_skew_factor,
sy = cy + cz * yz_skew_factor;
if (WITHIN(sx, X_MIN_POS, X_MAX_POS) && WITHIN(sy, Y_MIN_POS, Y_MAX_POS)) {
cx = sx; cy = sy;
}
}
}
#endif // SKEW_CORRECTION
#if PLANNER_LEVELING
#define ARG_X float rx
@ -356,7 +380,7 @@ class Planner {
* as it will be given to the planner and steppers.
*/
static void apply_leveling(float &rx, float &ry, float &rz);
static void apply_leveling(float raw[XYZ]) { apply_leveling(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); }
static void apply_leveling(float (&raw)[XYZ]) { apply_leveling(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); }
static void unapply_leveling(float raw[XYZ]);
#else
@ -379,7 +403,7 @@ class Planner {
static void _buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const uint8_t extruder);
/**
* Planner::_buffer_line
* Planner::buffer_segment
*
* Add a new linear movement to the buffer in axis units.
*
@ -389,7 +413,7 @@ class Planner {
* fr_mm_s - (target) speed of the move
* extruder - target extruder
*/
static void _buffer_line(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder);
static void buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder);
static void _set_position_mm(const float &a, const float &b, const float &c, const float &e);
@ -409,7 +433,7 @@ class Planner {
#if PLANNER_LEVELING && IS_CARTESIAN
apply_leveling(rx, ry, rz);
#endif
_buffer_line(rx, ry, rz, e, fr_mm_s, extruder);
buffer_segment(rx, ry, rz, e, fr_mm_s, extruder);
}
/**
@ -421,18 +445,18 @@ class Planner {
* fr_mm_s - (target) speed of the move (mm/s)
* extruder - target extruder
*/
FORCE_INLINE static void buffer_line_kinematic(const float cart[XYZE], const float &fr_mm_s, const uint8_t extruder) {
FORCE_INLINE static void buffer_line_kinematic(const float (&cart)[XYZE], const float &fr_mm_s, const uint8_t extruder) {
#if PLANNER_LEVELING
float raw[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] };
apply_leveling(raw);
#else
const float * const raw = cart;
const float (&raw)[XYZE] = cart;
#endif
#if IS_KINEMATIC
inverse_kinematics(raw);
_buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS], fr_mm_s, extruder);
buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS], fr_mm_s, extruder);
#else
_buffer_line(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_AXIS], fr_mm_s, extruder);
buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_AXIS], fr_mm_s, extruder);
#endif
}
@ -451,7 +475,7 @@ class Planner {
#endif
_set_position_mm(rx, ry, rz, e);
}
static void set_position_mm_kinematic(const float position[NUM_AXIS]);
static void set_position_mm_kinematic(const float (&cart)[XYZE]);
static void set_position_mm(const AxisEnum axis, const float &v);
FORCE_INLINE static void set_z_position_mm(const float &z) { set_position_mm(Z_AXIS, z); }
FORCE_INLINE static void set_e_position_mm(const float &e) { set_position_mm(AxisEnum(E_AXIS), e); }

2
Marlin/src/module/probe.cpp
View File

@ -107,7 +107,7 @@ inline void do_probe_raise(const float z_raise) {
#elif ENABLED(Z_PROBE_ALLEN_KEY)
FORCE_INLINE void do_blocking_move_to(const float raw[XYZ], const float &fr_mm_s) {
FORCE_INLINE void do_blocking_move_to(const float (&raw)[XYZ], const float &fr_mm_s) {
do_blocking_move_to(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], fr_mm_s);
}

6
Marlin/src/module/stepper.cpp
View File

@ -1193,7 +1193,7 @@ void Stepper::set_e_position(const long &e) {
/**
* Get a stepper's position in steps.
*/
long Stepper::position(AxisEnum axis) {
long Stepper::position(const AxisEnum axis) {
CRITICAL_SECTION_START;
const long count_pos = count_position[axis];
CRITICAL_SECTION_END;
@ -1204,7 +1204,7 @@ long Stepper::position(AxisEnum axis) {
* Get an axis position according to stepper position(s)
* For CORE machines apply translation from ABC to XYZ.
*/
float Stepper::get_axis_position_mm(AxisEnum axis) {
float Stepper::get_axis_position_mm(const AxisEnum axis) {
float axis_steps;
#if IS_CORE
// Requesting one of the "core" axes?
@ -1242,7 +1242,7 @@ void Stepper::quick_stop() {
#endif
}
void Stepper::endstop_triggered(AxisEnum axis) {
void Stepper::endstop_triggered(const AxisEnum axis) {
#if IS_CORE

12
Marlin/src/module/stepper.h
View File

@ -183,7 +183,7 @@ class Stepper {
//
// Get the position of a stepper, in steps
//
static long position(AxisEnum axis);
static long position(const AxisEnum axis);
//
// Report the positions of the steppers, in steps
@ -193,13 +193,13 @@ class Stepper {
//
// Get the position (mm) of an axis based on stepper position(s)
//
static float get_axis_position_mm(AxisEnum axis);
static float get_axis_position_mm(const AxisEnum axis);
//
// SCARA AB axes are in degrees, not mm
//
#if IS_SCARA
FORCE_INLINE static float get_axis_position_degrees(AxisEnum axis) { return get_axis_position_mm(axis); }
FORCE_INLINE static float get_axis_position_degrees(const AxisEnum axis) { return get_axis_position_mm(axis); }
#endif
//
@ -221,7 +221,7 @@ class Stepper {
//
// The direction of a single motor
//
FORCE_INLINE static bool motor_direction(AxisEnum axis) { return TEST(last_direction_bits, axis); }
FORCE_INLINE static bool motor_direction(const AxisEnum axis) { return TEST(last_direction_bits, axis); }
#if HAS_DIGIPOTSS || HAS_MOTOR_CURRENT_PWM
static void digitalPotWrite(const int16_t address, const int16_t value);
@ -263,12 +263,12 @@ class Stepper {
//
// Handle a triggered endstop
//
static void endstop_triggered(AxisEnum axis);
static void endstop_triggered(const AxisEnum axis);
//
// Triggered position of an axis in mm (not core-savvy)
//
FORCE_INLINE static float triggered_position_mm(AxisEnum axis) {
FORCE_INLINE static float triggered_position_mm(const AxisEnum axis) {
return endstops_trigsteps[axis] * planner.steps_to_mm[axis];
}