3DPrinting/MarlinFirmware
1
0
Fork 0
mirror of https://github.com/MarlinFirmware/Marlin.git synced 2024-11-27 22:08:02 +00:00
Code Issues Releases Wiki Activity

Merge pull request #8729 from thinkyhead/bf1_sort_out_leveling

Browse Source 
[1.1.x] UBL - Skew and Dual X Carriage
This commit is contained in:
Scott Lahteine 2017-12-11 01:40:20 -06:00 committed by GitHub
commit 17b05c150c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
17 changed files with 527 additions and 501 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
.travis.yml
View File

@ -103,9 +103,10 @@ script:
- opt_enable_adv CUSTOM_USER_MENUS I2C_POSITION_ENCODERS BABYSTEPPING NANODLP_Z_SYNC
- build_marlin
#
# And with a Sled Z Probe
# Add a Sled Z Probe, use UBL Cartesian moves
#
- opt_enable Z_PROBE_SLED
- opt_enable Z_PROBE_SLED SKEW_CORRECTION SKEW_CORRECTION_FOR_Z SKEW_CORRECTION_GCODE
- opt_disable SEGMENT_LEVELED_MOVES
- opt_enable_adv BABYSTEP_ZPROBE_OFFSET DOUBLECLICK_FOR_Z_BABYSTEPPING
- build_marlin
#
@ -141,7 +142,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

18
Marlin/Conditionals_post.h
View File

@ -933,15 +933,15 @@
/**
* 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 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)
#define HAS_ABL (OLDSCHOOL_ABL || ENABLED(AUTO_BED_LEVELING_UBL))
#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 || ENABLED(SKEW_CORRECTION))
#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)
#define HAS_ABL (OLDSCHOOL_ABL || ENABLED(AUTO_BED_LEVELING_UBL))
#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_SEGMENTED || ENABLED(SKEW_CORRECTION))
#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))

3
Marlin/G26_Mesh_Validation_Tool.cpp
View File

@ -137,7 +137,6 @@
#if ENABLED(ULTRA_LCD)
extern char lcd_status_message[];
#endif
extern float destination[XYZE];
void set_destination_from_current();
void prepare_move_to_destination();
inline void sync_plan_position_e() { planner.set_e_position_mm(current_position[E_AXIS]); }
@ -189,7 +188,7 @@
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
}

2
Marlin/Marlin.h
View File

@ -223,7 +223,7 @@ extern volatile bool wait_for_heatup;
extern volatile bool wait_for_user;
#endif
extern float current_position[NUM_AXIS];
extern float current_position[XYZE], destination[XYZE];
// Workspace offsets
#if HAS_WORKSPACE_OFFSET

54
Marlin/Marlin_main.cpp
View File

@ -706,7 +706,7 @@ FORCE_INLINE signed char pgm_read_any(const signed char *p) { return pgm_read_by
#define XYZ_CONSTS_FROM_CONFIG(type, array, CONFIG) \
static const PROGMEM type array##_P[XYZ] = { X_##CONFIG, Y_##CONFIG, Z_##CONFIG }; \
static inline type array(AxisEnum axis) { return pgm_read_any(&array##_P[axis]); } \
static inline type array(const AxisEnum axis) { return pgm_read_any(&array##_P[axis]); } \
typedef void __void_##CONFIG##__
XYZ_CONSTS_FROM_CONFIG(float, base_min_pos, MIN_POS);
@ -733,11 +733,11 @@ void get_cartesian_from_steppers();
void set_current_from_steppers_for_axis(const AxisEnum axis);
#if ENABLED(ARC_SUPPORT)
void plan_arc(float target[XYZE], float* offset, uint8_t clockwise);
void plan_arc(const float (&cart)[XYZE], const float (&offset)[2], const bool clockwise);
#endif
#if ENABLED(BEZIER_CURVE_SUPPORT)
void plan_cubic_move(const float offset[4]);
void plan_cubic_move(const float (&offset)[4]);
#endif
void tool_change(const uint8_t tmp_extruder, const float fr_mm_s=0.0, bool no_move=false);
@ -1550,7 +1550,7 @@ inline void set_destination_from_current() { COPY(destination, current_position)
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
@ -1808,7 +1808,7 @@ static void clean_up_after_endstop_or_probe_move() {
#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);
}
@ -8326,7 +8326,7 @@ void report_current_position() {
#ifdef 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(' ');
@ -8337,7 +8337,7 @@ void report_current_position() {
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() {
@ -12647,7 +12647,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
#endif // AUTO_BED_LEVELING_BILINEAR
#endif // IS_CARTESIAN
#if !UBL_DELTA
#if !UBL_SEGMENTED
#if IS_KINEMATIC
/**
@ -12659,7 +12659,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
* For Unified Bed Leveling (Delta or Segmented Cartesian)
* the ubl.prepare_segmented_line_to method replaces this.
*/
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);
@ -12819,7 +12819,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
}
#endif // !IS_KINEMATIC
#endif // !UBL_DELTA
#endif // !UBL_SEGMENTED
#if ENABLED(DUAL_X_CARRIAGE)
@ -12895,7 +12895,13 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
break;
}
}
return prepare_move_to_destination_cartesian();
return (
#if UBL_SEGMENTED
ubl.prepare_segmented_line_to(destination, MMS_SCALED(feedrate_mm_s))
#else
prepare_move_to_destination_cartesian()
#endif
);
}
#endif // DUAL_X_CARRIAGE
@ -12937,12 +12943,12 @@ void prepare_move_to_destination() {
#endif
if (
#if UBL_DELTA // Also works for CARTESIAN (smaller segments follow mesh more closely)
#if ENABLED(DUAL_X_CARRIAGE)
prepare_move_to_destination_dualx()
#elif 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
@ -12968,9 +12974,9 @@ void prepare_move_to_destination() {
* options for G2/G3 arc generation. In future these options may be GCode tunable.
*/
void plan_arc(
float raw[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 bool clockwise // Clockwise?
) {
#if ENABLED(CNC_WORKSPACE_PLANES)
AxisEnum p_axis, q_axis, l_axis;
@ -12990,10 +12996,10 @@ void prepare_move_to_destination() {
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 = raw[p_axis] - center_P,
rt_Y = raw[q_axis] - center_Q,
linear_travel = raw[l_axis] - current_position[l_axis],
extruder_travel = raw[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);
@ -13001,7 +13007,7 @@ void prepare_move_to_destination() {
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] == raw[p_axis] && current_position[q_axis] == raw[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));
@ -13101,7 +13107,7 @@ void prepare_move_to_destination() {
}
// Ensure last segment arrives at target location.
planner.buffer_line_kinematic(raw, 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
@ -13113,7 +13119,7 @@ void prepare_move_to_destination() {
#if ENABLED(BEZIER_CURVE_SUPPORT)
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

5
Marlin/SanityCheck.h
View File

@ -590,7 +590,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 either a probe or an LCD Controller."
@ -1539,9 +1539,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

42
Marlin/planner.cpp
View File

@ -569,14 +569,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;
@ -605,7 +598,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
@ -667,14 +660,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
}
@ -1354,7 +1340,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.
*
@ -1364,7 +1350,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]) {
@ -1383,7 +1369,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]);
@ -1430,7 +1416,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)
@ -1455,18 +1441,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/planner.h
View File

@ -142,7 +142,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];
@ -341,6 +341,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
@ -352,7 +376,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
@ -375,7 +399,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.
*
@ -385,7 +409,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);
@ -405,7 +429,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);
}
/**
@ -417,18 +441,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
}
@ -447,7 +471,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); }

4
Marlin/stepper.cpp
View File

@ -1198,7 +1198,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;
@ -1209,7 +1209,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?

12
Marlin/stepper.h
View File

@ -209,7 +209,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
@ -219,13 +219,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
//
@ -247,7 +247,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);
@ -287,12 +287,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];
}

2
Marlin/stepper_dac.cpp
View File

@ -94,7 +94,7 @@
static float dac_perc(int8_t n) { return 100.0 * mcp4728_getValue(dac_order[n]) * (1.0 / (DAC_STEPPER_MAX)); }
static float dac_amps(int8_t n) { return mcp4728_getDrvPct(dac_order[n]) * (DAC_STEPPER_MAX) * 0.125 * (1.0 / (DAC_STEPPER_SENSE)); }
uint8_t dac_current_get_percent(AxisEnum axis) { return mcp4728_getDrvPct(dac_order[axis]); }
uint8_t dac_current_get_percent(const AxisEnum axis) { return mcp4728_getDrvPct(dac_order[axis]); }
void dac_current_set_percents(const uint8_t pct[XYZE]) {
LOOP_XYZE(i) dac_channel_pct[i] = pct[dac_order[i]];
mcp4728_setDrvPct(dac_channel_pct);

2
Marlin/stepper_dac.h
View File

@ -51,7 +51,7 @@ void dac_current_percent(uint8_t channel, float val);
void dac_current_raw(uint8_t channel, uint16_t val);
void dac_print_values();
void dac_commit_eeprom();
uint8_t dac_current_get_percent(AxisEnum axis);
uint8_t dac_current_get_percent(const AxisEnum axis);
void dac_current_set_percents(const uint8_t pct[XYZE]);
#endif // STEPPER_DAC_H

55
Marlin/ubl.cpp
View File

@ -51,6 +51,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];
@ -174,7 +227,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++;
}

38
Marlin/ubl.h
View File

@ -26,6 +26,9 @@
#include "MarlinConfig.h"
#if ENABLED(AUTO_BED_LEVELING_UBL)
//#define UBL_DEVEL_DEBUGGING
#include "Marlin.h"
#include "planner.h"
#include "math.h"
@ -41,7 +44,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
@ -319,21 +326,24 @@
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))
#define ZZER(a) (z_values[a][0] == 0)
#define _CMPZ(a,b) (z_values[a][b] == z_values[a][b+1])
#define CMPZ(a) (_CMPZ(a, 0) && _CMPZ(a, 1))
#define ZZER(a) (z_values[a][0] == 0)
FORCE_INLINE bool mesh_is_valid() {
return !(
( CMPZ(0) && CMPZ(1) && CMPZ(2) // adjacent z values all equal?
&& ZZER(0) && ZZER(1) && ZZER(2) // all zero at the edge?
)
|| isnan(z_values[0][0])
);
}
FORCE_INLINE bool mesh_is_valid() {
return !(
( CMPZ(0) && CMPZ(1) && CMPZ(2) // adjacent z values all equal?
&& ZZER(0) && ZZER(1) && ZZER(2) // all zero at the edge?
)
|| isnan(z_values[0][0])
);
}
}; // class unified_bed_leveling
extern unified_bed_leveling ubl;

10
Marlin/ubl_G29.cpp
View File

@ -24,8 +24,6 @@
#if ENABLED(AUTO_BED_LEVELING_UBL)
//#define UBL_DEVEL_DEBUGGING
#include "ubl.h"
#include "Marlin.h"
#include "hex_print_routines.h"
@ -1165,12 +1163,12 @@
static uint8_t ubl_state_at_invocation = 0;
#ifdef UBL_DEVEL_DEBUGGING
#if ENABLED(UBL_DEVEL_DEBUGGING)
static uint8_t ubl_state_recursion_chk = 0;
#endif
void unified_bed_leveling::save_ubl_active_state_and_disable() {
#ifdef UBL_DEVEL_DEBUGGING
#if ENABLED(UBL_DEVEL_DEBUGGING)
ubl_state_recursion_chk++;
if (ubl_state_recursion_chk != 1) {
SERIAL_ECHOLNPGM("save_ubl_active_state_and_disabled() called multiple times in a row.");
@ -1186,7 +1184,7 @@
}
void unified_bed_leveling::restore_ubl_active_state_and_leave() {
#ifdef UBL_DEVEL_DEBUGGING
#if ENABLED(UBL_DEVEL_DEBUGGING)
if (--ubl_state_recursion_chk) {
SERIAL_ECHOLNPGM("restore_ubl_active_state_and_leave() called too many times.");
#if ENABLED(NEWPANEL)
@ -1267,7 +1265,7 @@
SERIAL_EOL();
safe_delay(50);
#ifdef UBL_DEVEL_DEBUGGING
#if ENABLED(UBL_DEVEL_DEBUGGING)
SERIAL_PROTOCOLLNPAIR("ubl_state_at_invocation :", ubl_state_at_invocation);
SERIAL_EOL();
SERIAL_PROTOCOLLNPAIR("ubl_state_recursion_chk :", ubl_state_recursion_chk);

729
Marlin/ubl_motion.cpp
View File

@ -30,229 +30,93 @@
#include <avr/io.h>
#include <math.h>
extern float destination[XYZE];
#if AVR_AT90USB1286_FAMILY // Teensyduino & Printrboard IDE extensions have compile errors without this
inline void set_current_from_destination() { COPY(current_position, destination); }
#else
extern void set_current_from_destination();
#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) {
/**
* 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]
};
const int cell_start_xi = get_cell_index_x(start[X_AXIS]),
cell_start_yi = get_cell_index_y(start[Y_AXIS]),
cell_dest_xi = get_cell_index_x(end[X_AXIS]),
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_CHAR(')');
SERIAL_EOL();
debug_current_and_destination(PSTR("Start of ubl.line_to_destination()"));
}
if (cell_start_xi == cell_dest_xi && cell_start_yi == cell_dest_yi) { // if the whole move is within the same cell,
void unified_bed_leveling::line_to_destination_cartesian(const float &feed_rate, const uint8_t extruder) {
/**
* we don't need to break up the move
*
* If we are moving off the print bed, we are going to allow the move at this level.
* But we detect it and isolate it. For now, we just pass along the request.
* 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
*/
#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
if (!WITHIN(cell_dest_xi, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(cell_dest_yi, 0, GRID_MAX_POINTS_Y - 1)) {
const int cell_start_xi = get_cell_index_x(start[X_AXIS]),
cell_start_yi = get_cell_index_y(start[Y_AXIS]),
cell_dest_xi = get_cell_index_x(end[X_AXIS]),
cell_dest_yi = get_cell_index_y(end[Y_AXIS]);
// 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);
set_current_from_destination();
if (g26_debug_flag)
debug_current_and_destination(PSTR("out of bounds in ubl.line_to_destination()"));
return;
if (g26_debug_flag) {
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_cartesian()"));
}
FINAL_MOVE:
if (cell_start_xi == cell_dest_xi && cell_start_yi == cell_dest_yi) { // if the whole move is within the same cell,
/**
* we don't need to break up the move
*
* If we are moving off the print bed, we are going to allow the move at this level.
* But we detect it and isolate it. For now, we just pass along the request.
*/
/**
* Optimize some floating point operations here. We could call float get_z_correction(float x0, float y0) to
* generate the correction for us. But we can lighten the load on the CPU by doing a modified version of the function.
* We are going to only calculate the amount we are from the first mesh line towards the second mesh line once.
* We will use this fraction in both of the original two Z Height calculations for the bi-linear interpolation. And,
* instead of doing a generic divide of the distance, we know the distance is MESH_X_DIST so we can use the preprocessor
* to create a 1-over number for us. That will allow us to do a floating point multiply instead of a floating point divide.
*/
if (!WITHIN(cell_dest_xi, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(cell_dest_yi, 0, GRID_MAX_POINTS_Y - 1)) {
const float xratio = (end[X_AXIS] - mesh_index_to_xpos(cell_dest_xi)) * (1.0 / (MESH_X_DIST));
// Note: There is no Z Correction in this case. We are off the grid and don't know what
// a reasonable correction would be.
float z1 = z_values[cell_dest_xi ][cell_dest_yi ] + xratio *
(z_values[cell_dest_xi + 1][cell_dest_yi ] - z_values[cell_dest_xi][cell_dest_yi ]),
z2 = z_values[cell_dest_xi ][cell_dest_yi + 1] + xratio *
(z_values[cell_dest_xi + 1][cell_dest_yi + 1] - z_values[cell_dest_xi][cell_dest_yi + 1]);
planner.buffer_segment(end[X_AXIS], end[Y_AXIS], end[Z_AXIS], end[E_AXIS], feed_rate, extruder);
set_current_from_destination();
if (cell_dest_xi >= GRID_MAX_POINTS_X - 1) z1 = z2 = 0.0;
if (g26_debug_flag)
debug_current_and_destination(PSTR("out of bounds in ubl.line_to_destination_cartesian()"));
// we are done with the fractional X distance into the cell. Now with the two Z-Heights we have calculated, we
// are going to apply the Y-Distance into the cell to interpolate the final Z correction.
return;
}
const float yratio = (end[Y_AXIS] - mesh_index_to_ypos(cell_dest_yi)) * (1.0 / (MESH_Y_DIST));
float z0 = cell_dest_yi < GRID_MAX_POINTS_Y - 1 ? (z1 + (z2 - z1) * yratio) * planner.fade_scaling_factor_for_z(end[Z_AXIS]) : 0.0;
/**
* If part of the Mesh is undefined, it will show up as NAN
* in z_values[][] and propagate through the
* calculations. If our correction is NAN, we throw it out
* because part of the Mesh is undefined and we don't have the
* information we need to complete the height correction.
*/
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);
if (g26_debug_flag)
debug_current_and_destination(PSTR("FINAL_MOVE in ubl.line_to_destination()"));
set_current_from_destination();
return;
}
/**
* If we get here, we are processing a move that crosses at least one Mesh Line. We will check
* for the simple case of just crossing X or just crossing Y Mesh Lines after we get all the details
* of the move figured out. We can process the easy case of just crossing an X or Y Mesh Line with less
* computation and in fact most lines are of this nature. We will check for that in the following
* blocks of code:
*/
const float dx = end[X_AXIS] - start[X_AXIS],
dy = end[Y_AXIS] - start[Y_AXIS];
const int left_flag = dx < 0.0 ? 1 : 0,
down_flag = dy < 0.0 ? 1 : 0;
const float adx = left_flag ? -dx : dx,
ady = down_flag ? -dy : dy;
const int dxi = cell_start_xi == cell_dest_xi ? 0 : left_flag ? -1 : 1,
dyi = cell_start_yi == cell_dest_yi ? 0 : down_flag ? -1 : 1;
/**
* Compute the scaling factor for the extruder for each partial move.
* We need to watch out for zero length moves because it will cause us to
* have an infinate scaling factor. We are stuck doing a floating point
* divide to get our scaling factor, but after that, we just multiply by this
* number. We also pick our scaling factor based on whether the X or Y
* component is larger. We use the biggest of the two to preserve precision.
*/
const bool use_x_dist = adx > ady;
float on_axis_distance = use_x_dist ? dx : dy,
e_position = end[E_AXIS] - start[E_AXIS],
z_position = end[Z_AXIS] - start[Z_AXIS];
const float e_normalized_dist = e_position / on_axis_distance,
z_normalized_dist = z_position / on_axis_distance;
int current_xi = cell_start_xi,
current_yi = cell_start_yi;
const float m = dy / dx,
c = start[Y_AXIS] - m * start[X_AXIS];
const bool inf_normalized_flag = (isinf(e_normalized_dist) != 0),
inf_m_flag = (isinf(m) != 0);
/**
* This block handles vertical lines. These are lines that stay within the same
* X Cell column. They do not need to be perfectly vertical. They just can
* not cross into another X Cell column.
*/
if (dxi == 0) { // Check for a vertical line
current_yi += down_flag; // Line is heading down, we just want to go to the bottom
while (current_yi != cell_dest_yi + down_flag) {
current_yi += dyi;
const float next_mesh_line_y = mesh_index_to_ypos(current_yi);
FINAL_MOVE:
/**
* if the slope of the line is infinite, we won't do the calculations
* else, we know the next X is the same so we can recover and continue!
* Calculate X at the next Y mesh line
* Optimize some floating point operations here. We could call float get_z_correction(float x0, float y0) to
* generate the correction for us. But we can lighten the load on the CPU by doing a modified version of the function.
* We are going to only calculate the amount we are from the first mesh line towards the second mesh line once.
* We will use this fraction in both of the original two Z Height calculations for the bi-linear interpolation. And,
* instead of doing a generic divide of the distance, we know the distance is MESH_X_DIST so we can use the preprocessor
* to create a 1-over number for us. That will allow us to do a floating point multiply instead of a floating point divide.
*/
const float rx = inf_m_flag ? start[X_AXIS] : (next_mesh_line_y - c) / m;
float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, current_xi, current_yi)
* planner.fade_scaling_factor_for_z(end[Z_AXIS]);
const float xratio = (end[X_AXIS] - mesh_index_to_xpos(cell_dest_xi)) * (1.0 / (MESH_X_DIST));
float z1 = z_values[cell_dest_xi ][cell_dest_yi ] + xratio *
(z_values[cell_dest_xi + 1][cell_dest_yi ] - z_values[cell_dest_xi][cell_dest_yi ]),
z2 = z_values[cell_dest_xi ][cell_dest_yi + 1] + xratio *
(z_values[cell_dest_xi + 1][cell_dest_yi + 1] - z_values[cell_dest_xi][cell_dest_yi + 1]);
if (cell_dest_xi >= GRID_MAX_POINTS_X - 1) z1 = z2 = 0.0;
// we are done with the fractional X distance into the cell. Now with the two Z-Heights we have calculated, we
// are going to apply the Y-Distance into the cell to interpolate the final Z correction.
const float yratio = (end[Y_AXIS] - mesh_index_to_ypos(cell_dest_yi)) * (1.0 / (MESH_Y_DIST));
float z0 = cell_dest_yi < GRID_MAX_POINTS_Y - 1 ? (z1 + (z2 - z1) * yratio) * planner.fade_scaling_factor_for_z(end[Z_AXIS]) : 0.0;
/**
* If part of the Mesh is undefined, it will show up as NAN
@ -263,17 +127,256 @@
*/
if (isnan(z0)) z0 = 0.0;
const float ry = mesh_index_to_ypos(current_yi);
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_cartesian()"));
set_current_from_destination();
return;
}
/**
* If we get here, we are processing a move that crosses at least one Mesh Line. We will check
* for the simple case of just crossing X or just crossing Y Mesh Lines after we get all the details
* of the move figured out. We can process the easy case of just crossing an X or Y Mesh Line with less
* computation and in fact most lines are of this nature. We will check for that in the following
* blocks of code:
*/
const float dx = end[X_AXIS] - start[X_AXIS],
dy = end[Y_AXIS] - start[Y_AXIS];
const int left_flag = dx < 0.0 ? 1 : 0,
down_flag = dy < 0.0 ? 1 : 0;
const float adx = left_flag ? -dx : dx,
ady = down_flag ? -dy : dy;
const int dxi = cell_start_xi == cell_dest_xi ? 0 : left_flag ? -1 : 1,
dyi = cell_start_yi == cell_dest_yi ? 0 : down_flag ? -1 : 1;
/**
* Compute the scaling factor for the extruder for each partial move.
* We need to watch out for zero length moves because it will cause us to
* have an infinate scaling factor. We are stuck doing a floating point
* divide to get our scaling factor, but after that, we just multiply by this
* number. We also pick our scaling factor based on whether the X or Y
* component is larger. We use the biggest of the two to preserve precision.
*/
const bool use_x_dist = adx > ady;
float on_axis_distance = use_x_dist ? dx : dy,
e_position = end[E_AXIS] - start[E_AXIS],
z_position = end[Z_AXIS] - start[Z_AXIS];
const float e_normalized_dist = e_position / on_axis_distance,
z_normalized_dist = z_position / on_axis_distance;
int current_xi = cell_start_xi,
current_yi = cell_start_yi;
const float m = dy / dx,
c = start[Y_AXIS] - m * start[X_AXIS];
const bool inf_normalized_flag = (isinf(e_normalized_dist) != 0),
inf_m_flag = (isinf(m) != 0);
/**
* This block handles vertical lines. These are lines that stay within the same
* X Cell column. They do not need to be perfectly vertical. They just can
* not cross into another X Cell column.
*/
if (dxi == 0) { // Check for a vertical line
current_yi += down_flag; // Line is heading down, we just want to go to the bottom
while (current_yi != cell_dest_yi + down_flag) {
current_yi += dyi;
const float next_mesh_line_y = mesh_index_to_ypos(current_yi);
/**
* if the slope of the line is infinite, we won't do the calculations
* else, we know the next X is the same so we can recover and continue!
* Calculate X at the next Y mesh line
*/
const float rx = inf_m_flag ? start[X_AXIS] : (next_mesh_line_y - c) / m;
float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, current_xi, current_yi)
* planner.fade_scaling_factor_for_z(end[Z_AXIS]);
/**
* If part of the Mesh is undefined, it will show up as NAN
* in z_values[][] and propagate through the
* calculations. If our correction is NAN, we throw it out
* because part of the Mesh is undefined and we don't have the
* information we need to complete the height correction.
*/
if (isnan(z0)) z0 = 0.0;
const float ry = mesh_index_to_ypos(current_yi);
/**
* 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_segment() routine will filter it if that happens.
*/
if (ry != start[Y_AXIS]) {
if (!inf_normalized_flag) {
on_axis_distance = use_x_dist ? rx - start[X_AXIS] : ry - start[Y_AXIS];
e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
}
else {
e_position = end[E_AXIS];
z_position = end[Z_AXIS];
}
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_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.
//
if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
goto FINAL_MOVE;
set_current_from_destination();
return;
}
/**
*
* This block handles horizontal lines. These are lines that stay within the same
* Y Cell row. They do not need to be perfectly horizontal. They just can
* not cross into another Y Cell row.
*
*/
if (dyi == 0) { // Check for a horizontal line
current_xi += left_flag; // Line is heading left, we just want to go to the left
// edge of this cell for the first move.
while (current_xi != cell_dest_xi + left_flag) {
current_xi += dxi;
const float next_mesh_line_x = mesh_index_to_xpos(current_xi),
ry = m * next_mesh_line_x + c; // Calculate Y at the next X mesh line
float z0 = z_correction_for_y_on_vertical_mesh_line(ry, current_xi, current_yi)
* planner.fade_scaling_factor_for_z(end[Z_AXIS]);
/**
* If part of the Mesh is undefined, it will show up as NAN
* in z_values[][] and propagate through the
* calculations. If our correction is NAN, we throw it out
* because part of the Mesh is undefined and we don't have the
* information we need to complete the height correction.
*/
if (isnan(z0)) z0 = 0.0;
const float rx = mesh_index_to_xpos(current_xi);
/**
* 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_segment() routine will filter it if that happens.
*/
if (rx != start[X_AXIS]) {
if (!inf_normalized_flag) {
on_axis_distance = use_x_dist ? rx - start[X_AXIS] : ry - start[Y_AXIS];
e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist; // is based on X or Y because this is a horizontal move
z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
}
else {
e_position = end[E_AXIS];
z_position = end[Z_AXIS];
}
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_cartesian()"));
if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
goto FINAL_MOVE;
set_current_from_destination();
return;
}
/**
*
* This block handles the generic case of a line crossing both X and Y Mesh lines.
*
*/
int xi_cnt = cell_start_xi - cell_dest_xi,
yi_cnt = cell_start_yi - cell_dest_yi;
if (xi_cnt < 0) xi_cnt = -xi_cnt;
if (yi_cnt < 0) yi_cnt = -yi_cnt;
current_xi += left_flag;
current_yi += down_flag;
while (xi_cnt > 0 || yi_cnt > 0) {
const float next_mesh_line_x = mesh_index_to_xpos(current_xi + dxi),
next_mesh_line_y = mesh_index_to_ypos(current_yi + dyi),
ry = m * next_mesh_line_x + c, // Calculate Y at the next X mesh line
rx = (next_mesh_line_y - c) / m; // Calculate X at the next Y mesh line
// (No need to worry about m being zero.
// If that was the case, it was already detected
// as a vertical line move above.)
if (left_flag == (rx > next_mesh_line_x)) { // Check if we hit the Y line first
// Yes! Crossing a Y Mesh Line next
float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, current_xi - left_flag, current_yi + dyi)
* planner.fade_scaling_factor_for_z(end[Z_AXIS]);
/**
* If part of the Mesh is undefined, it will show up as NAN
* in z_values[][] and propagate through the
* calculations. If our correction is NAN, we throw it out
* because part of the Mesh is undefined and we don't have the
* information we need to complete the height correction.
*/
if (isnan(z0)) z0 = 0.0;
/**
* 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.
*/
if (ry != start[Y_AXIS]) {
if (!inf_normalized_flag) {
on_axis_distance = use_x_dist ? rx - start[X_AXIS] : ry - start[Y_AXIS];
on_axis_distance = use_x_dist ? rx - start[X_AXIS] : next_mesh_line_y - start[Y_AXIS];
e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
}
else {
e_position = end[E_AXIS];
z_position = end[Z_AXIS];
}
planner.buffer_segment(rx, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder);
current_yi += dyi;
yi_cnt--;
}
else {
// Yes! Crossing a X Mesh Line next
float z0 = z_correction_for_y_on_vertical_mesh_line(ry, current_xi + dxi, current_yi - down_flag)
* planner.fade_scaling_factor_for_z(end[Z_AXIS]);
/**
* If part of the Mesh is undefined, it will show up as NAN
* in z_values[][] and propagate through the
* calculations. If our correction is NAN, we throw it out
* because part of the Mesh is undefined and we don't have the
* information we need to complete the height correction.
*/
if (isnan(z0)) z0 = 0.0;
if (!inf_normalized_flag) {
on_axis_distance = use_x_dist ? next_mesh_line_x - start[X_AXIS] : ry - start[Y_AXIS];
e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
}
@ -282,200 +385,38 @@
z_position = end[Z_AXIS];
}
planner._buffer_line(rx, ry, z_position + z0, e_position, feed_rate, extruder);
} //else printf("FIRST MOVE PRUNED ");
planner.buffer_segment(next_mesh_line_x, ry, z_position + z0, e_position, feed_rate, extruder);
current_xi += dxi;
xi_cnt--;
}
if (xi_cnt < 0 || yi_cnt < 0) break; // we've gone too far, so exit the loop and move on to FINAL_MOVE
}
if (g26_debug_flag)
debug_current_and_destination(PSTR("vertical move done in ubl.line_to_destination()"));
//
// 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.
//
if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
goto FINAL_MOVE;
set_current_from_destination();
return;
}
/**
*
* This block handles horizontal lines. These are lines that stay within the same
* Y Cell row. They do not need to be perfectly horizontal. They just can
* not cross into another Y Cell row.
*
*/
if (dyi == 0) { // Check for a horizontal line
current_xi += left_flag; // Line is heading left, we just want to go to the left
// edge of this cell for the first move.
while (current_xi != cell_dest_xi + left_flag) {
current_xi += dxi;
const float next_mesh_line_x = mesh_index_to_xpos(current_xi),
ry = m * next_mesh_line_x + c; // Calculate Y at the next X mesh line
float z0 = z_correction_for_y_on_vertical_mesh_line(ry, current_xi, current_yi)
* planner.fade_scaling_factor_for_z(end[Z_AXIS]);
/**
* If part of the Mesh is undefined, it will show up as NAN
* in z_values[][] and propagate through the
* calculations. If our correction is NAN, we throw it out
* because part of the Mesh is undefined and we don't have the
* information we need to complete the height correction.
*/
if (isnan(z0)) z0 = 0.0;
const float rx = mesh_index_to_xpos(current_xi);
/**
* 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.
*/
if (rx != start[X_AXIS]) {
if (!inf_normalized_flag) {
on_axis_distance = use_x_dist ? rx - start[X_AXIS] : ry - start[Y_AXIS];
e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist; // is based on X or Y because this is a horizontal move
z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
}
else {
e_position = end[E_AXIS];
z_position = end[Z_AXIS];
}
planner._buffer_line(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("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;
set_current_from_destination();
return;
}
/**
*
* This block handles the generic case of a line crossing both X and Y Mesh lines.
*
*/
int xi_cnt = cell_start_xi - cell_dest_xi,
yi_cnt = cell_start_yi - cell_dest_yi;
if (xi_cnt < 0) xi_cnt = -xi_cnt;
if (yi_cnt < 0) yi_cnt = -yi_cnt;
current_xi += left_flag;
current_yi += down_flag;
while (xi_cnt > 0 || yi_cnt > 0) {
const float next_mesh_line_x = mesh_index_to_xpos(current_xi + dxi),
next_mesh_line_y = mesh_index_to_ypos(current_yi + dyi),
ry = m * next_mesh_line_x + c, // Calculate Y at the next X mesh line
rx = (next_mesh_line_y - c) / m; // Calculate X at the next Y mesh line
// (No need to worry about m being zero.
// If that was the case, it was already detected
// as a vertical line move above.)
if (left_flag == (rx > next_mesh_line_x)) { // Check if we hit the Y line first
// Yes! Crossing a Y Mesh Line next
float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, current_xi - left_flag, current_yi + dyi)
* planner.fade_scaling_factor_for_z(end[Z_AXIS]);
/**
* If part of the Mesh is undefined, it will show up as NAN
* in z_values[][] and propagate through the
* calculations. If our correction is NAN, we throw it out
* because part of the Mesh is undefined and we don't have the
* information we need to complete the height correction.
*/
if (isnan(z0)) z0 = 0.0;
if (!inf_normalized_flag) {
on_axis_distance = use_x_dist ? rx - start[X_AXIS] : next_mesh_line_y - start[Y_AXIS];
e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
}
else {
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);
current_yi += dyi;
yi_cnt--;
}
else {
// Yes! Crossing a X Mesh Line next
float z0 = z_correction_for_y_on_vertical_mesh_line(ry, current_xi + dxi, current_yi - down_flag)
* planner.fade_scaling_factor_for_z(end[Z_AXIS]);
/**
* If part of the Mesh is undefined, it will show up as NAN
* in z_values[][] and propagate through the
* calculations. If our correction is NAN, we throw it out
* because part of the Mesh is undefined and we don't have the
* information we need to complete the height correction.
*/
if (isnan(z0)) z0 = 0.0;
if (!inf_normalized_flag) {
on_axis_distance = use_x_dist ? next_mesh_line_x - start[X_AXIS] : ry - start[Y_AXIS];
e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
}
else {
e_position = end[E_AXIS];
z_position = end[Z_AXIS];
}
planner._buffer_line(next_mesh_line_x, ry, z_position + z0, e_position, feed_rate, extruder);
current_xi += dxi;
xi_cnt--;
}
if (xi_cnt < 0 || yi_cnt < 0) break; // we've gone too far, so exit the loop and move on to FINAL_MOVE
}
if (g26_debug_flag)
debug_current_and_destination(PSTR("generic move done in ubl.line_to_destination()"));
if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
goto FINAL_MOVE;
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 (&raw)[XYZE], const float &fr) {
#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], raw[E_AXIS], fr, active_extruder);
#elif IS_SCARA // apply scara inverse_kinematics (should be changed to save raw->logical->raw)
@ -488,11 +429,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], 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], raw[E_AXIS], fr, active_extruder);
#endif
}
@ -511,15 +452,23 @@
/**
* 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 (&in_target)[XYZE], const float &feedrate) {
if (!position_is_reachable(rtarget[X_AXIS], rtarget[Y_AXIS])) // fail if moving outside reachable boundary
if (!position_is_reachable(in_target[X_AXIS], in_target[Y_AXIS])) // fail if moving outside reachable boundary
return true; // did not move, so current_position still accurate
#if ENABLED(SKEW_CORRECTION)
// For skew correction just adjust the destination point and we're done
float rtarget[XYZE] = { in_target[X_AXIS], in_target[Y_AXIS], in_target[Z_AXIS], in_target[E_AXIS] };
planner.skew(rtarget[X_AXIS], rtarget[Y_AXIS], rtarget[Z_AXIS]);
#else
const float (&rtarget)[XYZE] = in_target;
#endif
const float total[XYZE] = {
rtarget[X_AXIS] - current_position[X_AXIS],
rtarget[Y_AXIS] - current_position[Y_AXIS],
@ -564,6 +513,10 @@
current_position[E_AXIS]
};
#if ENABLED(SKEW_CORRECTION)
planner.skew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]);
#endif
// Only compute leveling per segment if ubl active and target below z_fade_height.
if (!planner.leveling_active || !planner.leveling_active_at_z(rtarget[Z_AXIS])) { // no mesh leveling
while (--segments) {
@ -670,6 +623,6 @@
} // cell loop
}
#endif // UBL_DELTA
#endif // UBL_SEGMENTED
#endif // AUTO_BED_LEVELING_UBL

1
Marlin/ultralcd.cpp
View File

@ -2755,7 +2755,6 @@ void kill_screen(const char* lcd_msg) {
#if IS_KINEMATIC
extern float feedrate_mm_s;
extern float destination[XYZE];
void set_destination_from_current();
void prepare_move_to_destination();
#endif