microlab
/
scube
1
1
Fork 0
Code Issues Pull Requests Releases Activity

Overhaul of the planner (#11578)

Browse Source 
- Move FWRETRACT to the planner
- Combine leveling, skew, etc. in a single modifier method
- Have kinematic and non-kinematic moves call one planner method
2.0.x
Thomas Moore 6 years ago committed by Scott Lahteine
parent 8323a08642
commit c437bb08f1
39 changed files with 652 additions and 594 deletions
Show Stats Download Patch File Download Diff File

6
Marlin/src/Marlin.cpp
Unescape Escape View File

@ -275,7 +275,7 @@ void quickstop_stepper() {
planner.quick_stop();
planner.synchronize();
set_current_from_steppers_for_axis(ALL_AXES);
SYNC_PLAN_POSITION_KINEMATIC();
sync_plan_position();
}
void enable_all_steppers() {
@ -465,7 +465,7 @@ void manage_inactivity(const bool ignore_stepper_queue/*=false*/) {
const float olde = current_position[E_AXIS];
current_position[E_AXIS] += EXTRUDER_RUNOUT_EXTRUDE;
planner.buffer_line_kinematic(current_position, MMM_TO_MMS(EXTRUDER_RUNOUT_SPEED), active_extruder);
planner.buffer_line(current_position, MMM_TO_MMS(EXTRUDER_RUNOUT_SPEED), active_extruder);
current_position[E_AXIS] = olde;
planner.set_e_position_mm(olde);
planner.synchronize();
@ -766,7 +766,7 @@ void setup() {
#endif
// Vital to init stepper/planner equivalent for current_position
SYNC_PLAN_POSITION_KINEMATIC();
sync_plan_position();
thermalManager.init(); // Initialize temperature loop

3
Marlin/src/config/examples/delta/FLSUN/auto_calibrate/Configuration.h
Unescape Escape View File

@ -539,9 +539,6 @@
// and processor overload (too many expensive sqrt calls).
#define DELTA_SEGMENTS_PER_SECOND 160
// Convert feedrates to apply to the Effector instead of the Carriages
#define DELTA_FEEDRATE_SCALING
// After homing move down to a height where XY movement is unconstrained
//#define DELTA_HOME_TO_SAFE_ZONE

3
Marlin/src/config/examples/delta/FLSUN/kossel/Configuration.h
Unescape Escape View File

@ -539,9 +539,6 @@
// and processor overload (too many expensive sqrt calls).
#define DELTA_SEGMENTS_PER_SECOND 160
// Convert feedrates to apply to the Effector instead of the Carriages
#define DELTA_FEEDRATE_SCALING
// After homing move down to a height where XY movement is unconstrained
//#define DELTA_HOME_TO_SAFE_ZONE

3
Marlin/src/config/examples/delta/FLSUN/kossel_mini/Configuration.h
Unescape Escape View File

@ -539,9 +539,6 @@
// and processor overload (too many expensive sqrt calls).
#define DELTA_SEGMENTS_PER_SECOND 160
// Convert feedrates to apply to the Effector instead of the Carriages
#define DELTA_FEEDRATE_SCALING
// After homing move down to a height where XY movement is unconstrained
//#define DELTA_HOME_TO_SAFE_ZONE

3
Marlin/src/config/examples/delta/Hatchbox_Alpha/Configuration.h
Unescape Escape View File

@ -544,9 +544,6 @@
// and processor overload (too many expensive sqrt calls).
#define DELTA_SEGMENTS_PER_SECOND 200
// Convert feedrates to apply to the Effector instead of the Carriages
#define DELTA_FEEDRATE_SCALING
// After homing move down to a height where XY movement is unconstrained
//#define DELTA_HOME_TO_SAFE_ZONE

3
Marlin/src/config/examples/delta/generic/Configuration.h
Unescape Escape View File

@ -529,9 +529,6 @@
// and processor overload (too many expensive sqrt calls).
#define DELTA_SEGMENTS_PER_SECOND 200
// Convert feedrates to apply to the Effector instead of the Carriages
#define DELTA_FEEDRATE_SCALING
// After homing move down to a height where XY movement is unconstrained
//#define DELTA_HOME_TO_SAFE_ZONE

3
Marlin/src/config/examples/delta/kossel_mini/Configuration.h
Unescape Escape View File

@ -529,9 +529,6 @@
// and processor overload (too many expensive sqrt calls).
#define DELTA_SEGMENTS_PER_SECOND 200
// Convert feedrates to apply to the Effector instead of the Carriages
#define DELTA_FEEDRATE_SCALING
// After homing move down to a height where XY movement is unconstrained
//#define DELTA_HOME_TO_SAFE_ZONE

3
Marlin/src/config/examples/delta/kossel_pro/Configuration.h
Unescape Escape View File

@ -515,9 +515,6 @@
// and processor overload (too many expensive sqrt calls).
#define DELTA_SEGMENTS_PER_SECOND 160
// Convert feedrates to apply to the Effector instead of the Carriages
#define DELTA_FEEDRATE_SCALING
// After homing move down to a height where XY movement is unconstrained
//#define DELTA_HOME_TO_SAFE_ZONE

3
Marlin/src/config/examples/delta/kossel_xl/Configuration.h
Unescape Escape View File

@ -533,9 +533,6 @@
// and processor overload (too many expensive sqrt calls).
#define DELTA_SEGMENTS_PER_SECOND 160
// Convert feedrates to apply to the Effector instead of the Carriages
#define DELTA_FEEDRATE_SCALING
// After homing move down to a height where XY movement is unconstrained
//#define DELTA_HOME_TO_SAFE_ZONE

87
Marlin/src/feature/bedlevel/bedlevel.cpp
Unescape Escape View File

@ -25,15 +25,12 @@
#if HAS_LEVELING
#include "bedlevel.h"
#include "../../module/planner.h"
#if ENABLED(MESH_BED_LEVELING) || ENABLED(PROBE_MANUALLY)
#include "../../module/motion.h"
#endif
#if PLANNER_LEVELING
#include "../../module/planner.h"
#endif
#if ENABLED(PROBE_MANUALLY)
bool g29_in_progress = false;
#endif
@ -79,74 +76,24 @@ void set_bed_leveling_enabled(const bool enable/*=true*/) {
planner.synchronize();
#if ENABLED(MESH_BED_LEVELING)
if (!enable)
planner.apply_leveling(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
const bool enabling = enable && leveling_is_valid();
planner.leveling_active = enabling;
if (enabling) planner.unapply_leveling(current_position);
#elif ENABLED(AUTO_BED_LEVELING_UBL)
#if PLANNER_LEVELING
if (planner.leveling_active) { // leveling from on to off
// change unleveled current_position to physical current_position without moving steppers.
planner.apply_leveling(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
planner.leveling_active = false; // disable only AFTER calling apply_leveling
}
else { // leveling from off to on
planner.leveling_active = true; // enable BEFORE calling unapply_leveling, otherwise ignored
// change physical current_position to unleveled current_position without moving steppers.
planner.unapply_leveling(current_position);
}
#else
// UBL equivalents for apply/unapply_leveling
#if ENABLED(SKEW_CORRECTION)
float pos[XYZ] = { current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] };
planner.skew(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS]);
#else
const float (&pos)[XYZE] = current_position;
#endif
if (planner.leveling_active) {
current_position[Z_AXIS] += ubl.get_z_correction(pos[X_AXIS], pos[Y_AXIS]);
planner.leveling_active = false;
}
else {
planner.leveling_active = true;
current_position[Z_AXIS] -= ubl.get_z_correction(pos[X_AXIS], pos[Y_AXIS]);
}
#endif
#else // OLDSCHOOL_ABL
#if ENABLED(AUTO_BED_LEVELING_BILINEAR)
// Force bilinear_z_offset to re-calculate next time
const float reset[XYZ] = { -9999.999, -9999.999, 0 };
(void)bilinear_z_offset(reset);
#endif
// Enable or disable leveling compensation in the planner
planner.leveling_active = enable;
if (!enable)
// When disabling just get the current position from the steppers.
// This will yield the smallest error when first converted back to steps.
set_current_from_steppers_for_axis(
#if ABL_PLANAR
ALL_AXES
#else
Z_AXIS
#endif
);
else
// When enabling, remove compensation from the current position,
// so compensation will give the right stepper counts.
planner.unapply_leveling(current_position);
#if ENABLED(AUTO_BED_LEVELING_BILINEAR)
// Force bilinear_z_offset to re-calculate next time
const float reset[XYZ] = { -9999.999, -9999.999, 0 };
(void)bilinear_z_offset(reset);
#endif
SYNC_PLAN_POSITION_KINEMATIC();
if (planner.leveling_active) { // leveling from on to off
// change unleveled current_position to physical current_position without moving steppers.
planner.apply_leveling(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
planner.leveling_active = false; // disable only AFTER calling apply_leveling
}
else { // leveling from off to on
planner.leveling_active = true; // enable BEFORE calling unapply_leveling, otherwise ignored
// change physical current_position to unleveled current_position without moving steppers.
planner.unapply_leveling(current_position);
}
#endif // OLDSCHOOL_ABL
sync_plan_position();
}
}

73
Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp
Unescape Escape View File

@ -49,12 +49,11 @@
* 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
#if HAS_POSITION_MODIFIERS
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]);
planner.apply_modifiers(start);
planner.apply_modifiers(end);
#else
const float (&start)[XYZE] = current_position,
(&end)[XYZE] = destination;
@ -364,47 +363,6 @@
#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_segment directly here. Per-segmented leveling and kinematics performed first.
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_IK(raw);
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)
inverse_kinematics(raw); // this writes delta[ABC] from raw[XYZE]
// should move the feedrate scaling to scara inverse_kinematics
const float adiff = ABS(delta[A_AXIS] - scara_oldA),
bdiff = ABS(delta[B_AXIS] - scara_oldB);
scara_oldA = delta[A_AXIS];
scara_oldB = delta[B_AXIS];
float s_feedrate = MAX(adiff, bdiff) * scara_feed_factor;
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], in_raw[E_AXIS], s_feedrate, active_extruder);
#else // CARTESIAN
planner.buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], in_raw[E_AXIS], fr, active_extruder);
#endif
}
#if IS_SCARA
#define DELTA_SEGMENT_MIN_LENGTH 0.25 // SCARA minimum segment size is 0.25mm
#elif ENABLED(DELTA)
@ -449,10 +407,9 @@
NOLESS(segments, 1U); // must have at least one segment
const float inv_segments = 1.0f / segments; // divide once, multiply thereafter
#if IS_SCARA // scale the feed rate from mm/s to degrees/s
scara_feed_factor = cartesian_xy_mm * inv_segments * feedrate;
scara_oldA = planner.get_axis_position_degrees(A_AXIS);
scara_oldB = planner.get_axis_position_degrees(B_AXIS);
const float segment_xyz_mm = HYPOT(cartesian_xy_mm, total[Z_AXIS]) * inv_segments; // length of each segment
#if ENABLED(SCARA_FEEDRATE_SCALING)
const float inv_duration = feedrate / segment_xyz_mm;
#endif
const float diff[XYZE] = {
@ -476,9 +433,17 @@
if (!planner.leveling_active || !planner.leveling_active_at_z(rtarget[Z_AXIS])) { // no mesh leveling
while (--segments) {
LOOP_XYZE(i) raw[i] += diff[i];
ubl_buffer_segment_raw(raw, feedrate);
planner.buffer_line(raw, feedrate, active_extruder, segment_xyz_mm
#if ENABLED(SCARA_FEEDRATE_SCALING)
, inv_duration
#endif
);
}
ubl_buffer_segment_raw(rtarget, feedrate);
planner.buffer_line(rtarget, feedrate, active_extruder, segment_xyz_mm
#if ENABLED(SCARA_FEEDRATE_SCALING)
, inv_duration
#endif
);
return false; // moved but did not set_current_from_destination();
}
@ -554,7 +519,11 @@
const float z = raw[Z_AXIS];
raw[Z_AXIS] += z_cxcy;
ubl_buffer_segment_raw(raw, feedrate);
planner.buffer_line(raw, feedrate, active_extruder, segment_xyz_mm
#if ENABLED(SCARA_FEEDRATE_SCALING)
, inv_duration
#endif
);
raw[Z_AXIS] = z;
if (segments == 0) // done with last segment

47
Marlin/src/feature/fwretract.cpp
Unescape Escape View File

@ -54,6 +54,7 @@ float FWRetract::retract_length, // M207 S - G10 Retract len
FWRetract::swap_retract_length, // M207 W - G10 Swap Retract length
FWRetract::swap_retract_recover_length, // M208 W - G11 Swap Recover length
FWRetract::swap_retract_recover_feedrate_mm_s, // M208 R - G11 Swap Recover feedrate
FWRetract::current_retract[EXTRUDERS], // Retract value used by planner
FWRetract::current_hop;
void FWRetract::reset() {
@ -73,6 +74,7 @@ void FWRetract::reset() {
#if EXTRUDERS > 1
retracted_swap[i] = false;
#endif
current_retract[i] = 0.0;
}
}
@ -84,9 +86,6 @@ void FWRetract::reset() {
*
* To simplify the logic, doubled retract/recover moves are ignored.
*
* Note: Z lift is done transparently to the planner. Aborting
* a print between G10 and G11 may corrupt the Z position.
*
* Note: Auto-retract will apply the set Z hop in addition to any Z hop
* included in the G-code. Use M207 Z0 to to prevent double hop.
*/
@ -95,9 +94,6 @@ void FWRetract::retract(const bool retracting
, bool swapping /* =false */
#endif
) {
static float current_hop = 0.0; // Total amount lifted, for use in recover
// Prevent two retracts or recovers in a row
if (retracted[active_extruder] == retracting) return;
@ -129,48 +125,50 @@ void FWRetract::retract(const bool retracting
//*/
const float old_feedrate_mm_s = feedrate_mm_s,
renormalize = RECIPROCAL(planner.e_factor[active_extruder]),
base_retract = swapping ? swap_retract_length : retract_length,
old_z = current_position[Z_AXIS],
old_e = current_position[E_AXIS];
unscale_e = RECIPROCAL(planner.e_factor[active_extruder]),
unscale_fr = 100.0 / feedrate_percentage, // Disable feedrate scaling for retract moves
base_retract = swapping ? swap_retract_length : retract_length;
// The current position will be the destination for E and Z moves
set_destination_from_current();
if (retracting) {
// Retract by moving from a faux E position back to the current E position
feedrate_mm_s = retract_feedrate_mm_s;
destination[E_AXIS] -= base_retract * renormalize;
feedrate_mm_s = retract_feedrate_mm_s * unscale_fr;
current_retract[active_extruder] = base_retract * unscale_e;
prepare_move_to_destination(); // set_current_to_destination
planner.synchronize(); // Wait for move to complete
// Is a Z hop set, and has the hop not yet been done?
if (retract_zlift > 0.01 && !current_hop) { // Apply hop only once
current_hop += retract_zlift; // Add to the hop total (again, only once)
destination[Z_AXIS] += retract_zlift; // Raise Z by the zlift (M207 Z) amount
feedrate_mm_s = planner.max_feedrate_mm_s[Z_AXIS]; // Maximum Z feedrate
feedrate_mm_s = planner.max_feedrate_mm_s[Z_AXIS] * unscale_fr; // Maximum Z feedrate
prepare_move_to_destination(); // Raise up, set_current_to_destination
planner.synchronize(); // Wait for move to complete
}
}
else {
// If a hop was done and Z hasn't changed, undo the Z hop
if (current_hop) {
current_position[Z_AXIS] += current_hop; // Restore the actual Z position
SYNC_PLAN_POSITION_KINEMATIC(); // Unspoof the position planner
feedrate_mm_s = planner.max_feedrate_mm_s[Z_AXIS]; // Z feedrate to max
current_hop = 0.0;
feedrate_mm_s = planner.max_feedrate_mm_s[Z_AXIS] * unscale_fr; // Z feedrate to max
prepare_move_to_destination(); // Lower Z, set_current_to_destination
current_hop = 0.0; // Clear the hop amount
planner.synchronize(); // Wait for move to complete
}
destination[E_AXIS] += (base_retract + (swapping ? swap_retract_recover_length : retract_recover_length)) * renormalize;
feedrate_mm_s = swapping ? swap_retract_recover_feedrate_mm_s : retract_recover_feedrate_mm_s;
const float extra_recover = swapping ? swap_retract_recover_length : retract_recover_length;
if (extra_recover != 0.0) {
current_position[E_AXIS] -= extra_recover; // Adjust the current E position by the extra amount to recover
sync_plan_position_e(); // Sync the planner position so the extra amount is recovered
}
current_retract[active_extruder] = 0.0;
feedrate_mm_s = (swapping ? swap_retract_recover_feedrate_mm_s : retract_recover_feedrate_mm_s) * unscale_fr;
prepare_move_to_destination(); // Recover E, set_current_to_destination
planner.synchronize(); // Wait for move to complete
}
feedrate_mm_s = old_feedrate_mm_s; // Restore original feedrate
current_position[Z_AXIS] = old_z; // Restore Z and E positions
current_position[E_AXIS] = old_e;
SYNC_PLAN_POSITION_KINEMATIC(); // As if the move never took place
retracted[active_extruder] = retracting; // Active extruder now retracted / recovered
// If swap retract/recover update the retracted_swap flag too
@ -194,7 +192,6 @@ void FWRetract::retract(const bool retracting
SERIAL_ECHOLNPAIR("current_position[e] ", current_position[E_AXIS]);
SERIAL_ECHOLNPAIR("current_hop ", current_hop);
//*/
}
#endif // FWRETRACT

3
Marlin/src/feature/fwretract.h
Unescape Escape View File

@ -46,7 +46,8 @@ public:
swap_retract_length, // M207 W - G10 Swap Retract length
swap_retract_recover_length, // M208 W - G11 Swap Recover length
swap_retract_recover_feedrate_mm_s, // M208 R - G11 Swap Recover feedrate
current_hop;
current_retract[EXTRUDERS], // Retract value used by planner
current_hop; // Hop value used by planner
FWRetract() { reset(); }

2
Marlin/src/feature/pause.cpp
Unescape Escape View File

@ -120,7 +120,7 @@ static bool ensure_safe_temperature(const AdvancedPauseMode mode=ADVANCED_PAUSE_
static void do_pause_e_move(const float &length, const float &fr) {
set_destination_from_current();
destination[E_AXIS] += length / planner.e_factor[active_extruder];
planner.buffer_line_kinematic(destination, fr, active_extruder);
planner.buffer_line(destination, fr, active_extruder);
set_current_from_destination();
planner.synchronize();
}

27
Marlin/src/feature/power_loss_recovery.cpp
Unescape Escape View File

@ -39,6 +39,10 @@
#include "../sd/cardreader.h"
#include "../core/serial.h"
#if ENABLED(FWRETRACT)
#include "fwretract.h"
#endif
// Recovery data
job_recovery_info_t job_recovery_info;
JobRecoveryPhase job_recovery_phase = JOB_RECOVERY_IDLE;
@ -90,6 +94,15 @@ extern uint8_t commands_in_queue, cmd_queue_index_r;
SERIAL_PROTOCOLPAIR("leveling: ", int(job_recovery_info.leveling));
SERIAL_PROTOCOLLNPAIR(" fade: ", int(job_recovery_info.fade));
#endif
#if ENABLED(FWRETRACT)
SERIAL_PROTOCOLPGM("retract: ");
for (int8_t e = 0; e < EXTRUDERS; e++) {
SERIAL_PROTOCOL(job_recovery_info.retract[e]);
if (e < EXTRUDERS - 1) SERIAL_CHAR(',');
}
SERIAL_EOL();
SERIAL_PROTOCOLLNPAIR("retract_hop: ", job_recovery_info.retract_hop);
#endif
SERIAL_PROTOCOLLNPAIR("cmd_queue_index_r: ", int(job_recovery_info.cmd_queue_index_r));
SERIAL_PROTOCOLLNPAIR("commands_in_queue: ", int(job_recovery_info.commands_in_queue));
if (recovery)
@ -160,6 +173,15 @@ void check_print_job_recovery() {
}
#endif
#if ENABLED(FWRETRACT)
for (uint8_t e = 0; e < EXTRUDERS; e++) {
if (job_recovery_info.retract[e] != 0.0)
fwretract.current_retract[e] = job_recovery_info.retract[e];
fwretract.retracted[e] = true;
}
fwretract.current_hop = job_recovery_info.retract_hop;
#endif
dtostrf(job_recovery_info.current_position[Z_AXIS] + 2, 1, 3, str_1);
dtostrf(job_recovery_info.current_position[E_AXIS]
#if ENABLED(SAVE_EACH_CMD_MODE)
@ -256,6 +278,11 @@ void save_job_recovery_info() {
);
#endif
#if ENABLED(FWRETRACT)
COPY(job_recovery_info.retract, fwretract.current_retract);
job_recovery_info.retract_hop = fwretract.current_hop;
#endif
// Commands in the queue
job_recovery_info.cmd_queue_index_r = cmd_queue_index_r;
job_recovery_info.commands_in_queue = commands_in_queue;

4
Marlin/src/feature/power_loss_recovery.h
Unescape Escape View File

@ -60,6 +60,10 @@ typedef struct {
float fade;
#endif
#if ENABLED(FWRETRACT)
float retract[EXTRUDERS], retract_hop;
#endif
// Command queue
uint8_t cmd_queue_index_r, commands_in_queue;
char command_queue[BUFSIZE][MAX_CMD_SIZE];

2
Marlin/src/gcode/bedlevel/abl/G29.cpp
Unescape Escape View File

@ -989,7 +989,7 @@ G29_TYPE GcodeSuite::G29() {
KEEPALIVE_STATE(IN_HANDLER);
if (planner.leveling_active)
SYNC_PLAN_POSITION_KINEMATIC();
sync_plan_position();
#if HAS_BED_PROBE && defined(Z_AFTER_PROBING)
move_z_after_probing();

6
Marlin/src/gcode/calibrate/G28.cpp
Unescape Escape View File

@ -101,7 +101,7 @@
if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("Z_SAFE_HOMING >>>");
#endif
SYNC_PLAN_POSITION_KINEMATIC();
sync_plan_position();
/**
* Move the Z probe (or just the nozzle) to the safe homing point
@ -182,7 +182,7 @@ void GcodeSuite::G28(const bool always_home_all) {
#if ENABLED(MARLIN_DEV_MODE)
if (parser.seen('S')) {
LOOP_XYZ(a) set_axis_is_at_home((AxisEnum)a);
SYNC_PLAN_POSITION_KINEMATIC();
sync_plan_position();
SERIAL_ECHOLNPGM("Simulated Homing");
report_current_position();
#if ENABLED(DEBUG_LEVELING_FEATURE)
@ -357,7 +357,7 @@ void GcodeSuite::G28(const bool always_home_all) {
} // home_all || homeZ
#endif // Z_HOME_DIR < 0
SYNC_PLAN_POSITION_KINEMATIC();
sync_plan_position();
#endif // !DELTA (G28)

2
Marlin/src/gcode/calibrate/M852.cpp
Unescape Escape View File

@ -87,7 +87,7 @@ void GcodeSuite::M852() {
// When skew is changed the current position changes
if (setval) {
set_current_from_steppers_for_axis(ALL_AXES);
SYNC_PLAN_POSITION_KINEMATIC();
sync_plan_position();
report_current_position();
}

11
Marlin/src/gcode/config/M200-M205.cpp
Unescape Escape View File

@ -136,14 +136,17 @@ void GcodeSuite::M205() {
const float junc_dev = parser.value_linear_units();
if (WITHIN(junc_dev, 0.01f, 0.3f)) {
planner.junction_deviation_mm = junc_dev;
planner.recalculate_max_e_jerk();
#if ENABLED(LIN_ADVANCE)
planner.recalculate_max_e_jerk();
#endif
}
else {
SERIAL_ERROR_START();
SERIAL_ERRORLNPGM("?J out of range (0.01 to 0.3)");
}
}
#else
#endif
#if HAS_CLASSIC_JERK
if (parser.seen('X')) planner.max_jerk[X_AXIS] = parser.value_linear_units();
if (parser.seen('Y')) planner.max_jerk[Y_AXIS] = parser.value_linear_units();
if (parser.seen('Z')) {
@ -153,6 +156,8 @@ void GcodeSuite::M205() {
SERIAL_ECHOLNPGM("WARNING! Low Z Jerk may lead to unwanted pauses.");
#endif
}
if (parser.seen('E')) planner.max_jerk[E_AXIS] = parser.value_linear_units();
#if DISABLED(JUNCTION_DEVIATION) || DISABLED(LIN_ADVANCE)
if (parser.seen('E')) planner.max_jerk[E_AXIS] = parser.value_linear_units();
#endif
#endif
}

2
Marlin/src/gcode/config/M92.cpp
Unescape Escape View File

@ -39,7 +39,7 @@ void GcodeSuite::M92() {
const float value = parser.value_per_axis_unit((AxisEnum)(E_AXIS + TARGET_EXTRUDER));
if (value < 20) {
float factor = planner.axis_steps_per_mm[E_AXIS + TARGET_EXTRUDER] / value; // increase e constants if M92 E14 is given for netfab.
#if DISABLED(JUNCTION_DEVIATION)
#if HAS_CLASSIC_JERK && (DISABLED(JUNCTION_DEVIATION) || DISABLED(LIN_ADVANCE))
planner.max_jerk[E_AXIS] *= factor;
#endif
planner.max_feedrate_mm_s[E_AXIS + TARGET_EXTRUDER] *= factor;

2
Marlin/src/gcode/geometry/G92.cpp
Unescape Escape View File

@ -92,7 +92,7 @@ void GcodeSuite::G92() {
COPY(coordinate_system[active_coordinate_system], position_shift);
#endif
if (didXYZ) SYNC_PLAN_POSITION_KINEMATIC();
if (didXYZ) sync_plan_position();
else if (didE) sync_plan_position_e();
report_current_position();

2
Marlin/src/gcode/host/M114.cpp
Unescape Escape View File

@ -56,7 +56,7 @@
float leveled[XYZ] = { current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] };
#if PLANNER_LEVELING
#if HAS_LEVELING
SERIAL_PROTOCOLPGM("Leveled:");
planner.apply_leveling(leveled);
report_xyz(leveled);

81
Marlin/src/gcode/motion/G2_G3.cpp
Unescape Escape View File

@ -35,10 +35,6 @@
#include "../../module/scara.h"
#endif
#if HAS_FEEDRATE_SCALING && ENABLED(AUTO_BED_LEVELING_BILINEAR)
#include "../../feature/bedlevel/abl/abl.h"
#endif
#if N_ARC_CORRECTION < 1
#undef N_ARC_CORRECTION
#define N_ARC_CORRECTION 1
@ -139,20 +135,12 @@ void plan_arc(
const float fr_mm_s = MMS_SCALED(feedrate_mm_s);
millis_t next_idle_ms = millis() + 200UL;
#if HAS_FEEDRATE_SCALING
// SCARA needs to scale the feed rate from mm/s to degrees/s
const float inv_segment_length = 1.0f / float(MM_PER_ARC_SEGMENT),
inverse_secs = inv_segment_length * fr_mm_s;
float oldA = planner.position_float[A_AXIS],
oldB = planner.position_float[B_AXIS]
#if ENABLED(DELTA_FEEDRATE_SCALING)
, oldC = planner.position_float[C_AXIS]
#endif
;
#if ENABLED(SCARA_FEEDRATE_SCALING)
const float inv_duration = fr_mm_s / MM_PER_ARC_SEGMENT;
#endif
millis_t next_idle_ms = millis() + 200UL;
#if N_ARC_CORRECTION > 1
int8_t arc_recalc_count = N_ARC_CORRECTION;
#endif
@ -196,57 +184,32 @@ void plan_arc(
clamp_to_software_endstops(raw);
#if HAS_FEEDRATE_SCALING
inverse_kinematics(raw);
ADJUST_DELTA(raw);
#if HAS_LEVELING && !PLANNER_LEVELING
planner.apply_leveling(raw);
#endif
#if ENABLED(SCARA_FEEDRATE_SCALING)
// For SCARA scale the feed rate from mm/s to degrees/s
// i.e., Complete the angular vector in the given time.
if (!planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder, MM_PER_ARC_SEGMENT))
break;
oldA = delta[A_AXIS]; oldB = delta[B_AXIS];
#elif ENABLED(DELTA_FEEDRATE_SCALING)
// For DELTA scale the feed rate from Effector mm/s to Carriage mm/s
// i.e., Complete the linear vector in the given time.
if (!planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], SQRT(sq(delta[A_AXIS] - oldA) + sq(delta[B_AXIS] - oldB) + sq(delta[C_AXIS] - oldC)) * inverse_secs, active_extruder, MM_PER_ARC_SEGMENT))
break;
oldA = delta[A_AXIS]; oldB = delta[B_AXIS]; oldC = delta[C_AXIS];
#elif HAS_UBL_AND_CURVES
float pos[XYZ] = { raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS] };
planner.apply_leveling(pos);
if (!planner.buffer_segment(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], raw[E_AXIS], fr_mm_s, active_extruder, MM_PER_ARC_SEGMENT))
break;
#else
if (!planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder))
break;
#endif
if (!planner.buffer_line(raw, fr_mm_s, active_extruder, MM_PER_ARC_SEGMENT
#if ENABLED(SCARA_FEEDRATE_SCALING)
, inv_duration
#endif
))
break;
}
// Ensure last segment arrives at target location.
#if HAS_FEEDRATE_SCALING
inverse_kinematics(cart);
ADJUST_DELTA(cart);
#endif
COPY(raw, cart);
#if ENABLED(SCARA_FEEDRATE_SCALING)
const float diff2 = HYPOT2(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB);
if (diff2)
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], cart[Z_AXIS], cart[E_AXIS], SQRT(diff2) * inverse_secs, active_extruder, MM_PER_ARC_SEGMENT);
#elif ENABLED(DELTA_FEEDRATE_SCALING)
const float diff2 = sq(delta[A_AXIS] - oldA) + sq(delta[B_AXIS] - oldB) + sq(delta[C_AXIS] - oldC);
if (diff2)
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS], SQRT(diff2) * inverse_secs, active_extruder, MM_PER_ARC_SEGMENT);
#elif HAS_UBL_AND_CURVES
float pos[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] };
planner.apply_leveling(pos);
planner.buffer_segment(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], cart[E_AXIS], fr_mm_s, active_extruder, MM_PER_ARC_SEGMENT);
#else
planner.buffer_line_kinematic(cart, fr_mm_s, active_extruder);
#if HAS_LEVELING && !PLANNER_LEVELING
planner.apply_leveling(raw);
#endif
COPY(current_position, cart);
planner.buffer_line(raw, fr_mm_s, active_extruder, MM_PER_ARC_SEGMENT
#if ENABLED(SCARA_FEEDRATE_SCALING)
, inv_duration
#endif
);
COPY(current_position, raw);
} // plan_arc
/**

4
Marlin/src/gcode/probe/G38.cpp
Unescape Escape View File

@ -56,7 +56,7 @@ static bool G38_run_probe() {
endstops.hit_on_purpose();
set_current_from_steppers_for_axis(ALL_AXES);
SYNC_PLAN_POSITION_KINEMATIC();
sync_plan_position();
if (G38_endstop_hit) {
@ -82,7 +82,7 @@ static bool G38_run_probe() {
G38_move = false;
set_current_from_steppers_for_axis(ALL_AXES);
SYNC_PLAN_POSITION_KINEMATIC();
sync_plan_position();
#endif
}

7
Marlin/src/inc/Conditionals_post.h
Unescape Escape View File

@ -49,6 +49,8 @@
#define IS_KINEMATIC (ENABLED(DELTA) || IS_SCARA)
#define IS_CARTESIAN !IS_KINEMATIC
#define HAS_CLASSIC_JERK (IS_KINEMATIC || DISABLED(JUNCTION_DEVIATION))
/**
* Axis lengths and center
*/
@ -1173,10 +1175,9 @@
#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 PLANNER_LEVELING (HAS_LEVELING && DISABLED(AUTO_BED_LEVELING_UBL))
#define HAS_PROBING_PROCEDURE (HAS_ABL || ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST))
#define HAS_UBL_AND_CURVES (ENABLED(AUTO_BED_LEVELING_UBL) && !PLANNER_LEVELING && (ENABLED(ARC_SUPPORT) || ENABLED(BEZIER_CURVE_SUPPORT)))
#define HAS_FEEDRATE_SCALING (ENABLED(SCARA_FEEDRATE_SCALING) || ENABLED(DELTA_FEEDRATE_SCALING))
#define HAS_POSITION_MODIFIERS (ENABLED(FWRETRACT) || HAS_LEVELING || ENABLED(SKEW_CORRECTION))
#if ENABLED(AUTO_BED_LEVELING_UBL)
#undef LCD_BED_LEVELING

21
Marlin/src/lcd/ultralcd.cpp
Unescape Escape View File

@ -841,7 +841,7 @@ void lcd_quick_feedback(const bool clear_buttons) {
}
inline void line_to_current_z() {
planner.buffer_line_kinematic(current_position, MMM_TO_MMS(manual_feedrate_mm_m[Z_AXIS]), active_extruder);
planner.buffer_line(current_position, MMM_TO_MMS(manual_feedrate_mm_m[Z_AXIS]), active_extruder);
}
inline void line_to_z(const float &z) {
@ -1892,7 +1892,7 @@ void lcd_quick_feedback(const bool clear_buttons) {
break;
#endif
}
planner.buffer_line_kinematic(current_position, MMM_TO_MMS(manual_feedrate_mm_m[X_AXIS]), active_extruder);
planner.buffer_line(current_position, MMM_TO_MMS(manual_feedrate_mm_m[X_AXIS]), active_extruder);
line_to_z(0.0);
if (++bed_corner > 3
#if ENABLED(LEVEL_CENTER_TOO)
@ -2432,7 +2432,7 @@ void lcd_quick_feedback(const bool clear_buttons) {
void ubl_map_move_to_xy() {
current_position[X_AXIS] = pgm_read_float(&ubl._mesh_index_to_xpos[x_plot]);
current_position[Y_AXIS] = pgm_read_float(&ubl._mesh_index_to_ypos[y_plot]);
planner.buffer_line_kinematic(current_position, MMM_TO_MMS(XY_PROBE_SPEED), active_extruder);
planner.buffer_line(current_position, MMM_TO_MMS(XY_PROBE_SPEED), active_extruder);
}
/**
@ -2911,7 +2911,7 @@ void lcd_quick_feedback(const bool clear_buttons) {
#else
planner.buffer_line_kinematic(current_position, MMM_TO_MMS(manual_feedrate_mm_m[manual_move_axis]), manual_move_axis == E_AXIS ? manual_move_e_index : active_extruder);
planner.buffer_line(current_position, MMM_TO_MMS(manual_feedrate_mm_m[manual_move_axis]), manual_move_axis == E_AXIS ? manual_move_e_index : active_extruder);
manual_move_axis = (int8_t)NO_AXIS;
#endif
@ -3746,8 +3746,13 @@ void lcd_quick_feedback(const bool clear_buttons) {
MENU_BACK(MSG_ADVANCED_SETTINGS);
#if ENABLED(JUNCTION_DEVIATION)
MENU_ITEM_EDIT_CALLBACK(float43, MSG_JUNCTION_DEVIATION, &planner.junction_deviation_mm, 0.01f, 0.3f, planner.recalculate_max_e_jerk);
#else
#if ENABLED(LIN_ADVANCE)
MENU_ITEM_EDIT_CALLBACK(float43, MSG_JUNCTION_DEVIATION, &planner.junction_deviation_mm, 0.01f, 0.3f, planner.recalculate_max_e_jerk);
#else
MENU_ITEM_EDIT(float43, MSG_JUNCTION_DEVIATION, &planner.junction_deviation_mm, 0.01f, 0.3f);
#endif
#endif
#if HAS_CLASSIC_JERK
MENU_MULTIPLIER_ITEM_EDIT(float3, MSG_VA_JERK, &planner.max_jerk[A_AXIS], 1, 990);
MENU_MULTIPLIER_ITEM_EDIT(float3, MSG_VB_JERK, &planner.max_jerk[B_AXIS], 1, 990);
#if ENABLED(DELTA)
@ -3755,7 +3760,9 @@ void lcd_quick_feedback(const bool clear_buttons) {
#else
MENU_MULTIPLIER_ITEM_EDIT(float52sign, MSG_VC_JERK, &planner.max_jerk[C_AXIS], 0.1f, 990);
#endif
MENU_MULTIPLIER_ITEM_EDIT(float3, MSG_VE_JERK, &planner.max_jerk[E_AXIS], 1, 990);
#if DISABLED(JUNCTION_DEVIATION) || DISABLED(LIN_ADVANCE)
MENU_MULTIPLIER_ITEM_EDIT(float3, MSG_VE_JERK, &planner.max_jerk[E_AXIS], 1, 990);
#endif
#endif
END_MENU();

47
Marlin/src/module/configuration_store.cpp
Unescape Escape View File

@ -428,12 +428,20 @@ void MarlinSettings::postprocess() {
EEPROM_WRITE(planner.min_feedrate_mm_s);
EEPROM_WRITE(planner.min_travel_feedrate_mm_s);
#if ENABLED(JUNCTION_DEVIATION)
#if HAS_CLASSIC_JERK
EEPROM_WRITE(planner.max_jerk);
#if ENABLED(JUNCTION_DEVIATION) && ENABLED(LIN_ADVANCE)
dummy = float(DEFAULT_EJERK);
EEPROM_WRITE(dummy);
#endif
#else
const float planner_max_jerk[] = { float(DEFAULT_XJERK), float(DEFAULT_YJERK), float(DEFAULT_ZJERK), float(DEFAULT_EJERK) };
EEPROM_WRITE(planner_max_jerk);
#endif
#if ENABLED(JUNCTION_DEVIATION)
EEPROM_WRITE(planner.junction_deviation_mm);
#else
EEPROM_WRITE(planner.max_jerk);
dummy = 0.02f;
EEPROM_WRITE(dummy);
#endif
@ -1062,11 +1070,18 @@ void MarlinSettings::postprocess() {
EEPROM_READ(planner.min_feedrate_mm_s);
EEPROM_READ(planner.min_travel_feedrate_mm_s);
#if ENABLED(JUNCTION_DEVIATION)
#if HAS_CLASSIC_JERK
EEPROM_READ(planner.max_jerk);
#if ENABLED(JUNCTION_DEVIATION) && ENABLED(LIN_ADVANCE)
EEPROM_READ(dummy);
#endif
#else
for (uint8_t q = 4; q--;) EEPROM_READ(dummy);
#endif
#if ENABLED(JUNCTION_DEVIATION)
EEPROM_READ(planner.junction_deviation_mm);
#else
EEPROM_READ(planner.max_jerk);
EEPROM_READ(dummy);
#endif
@ -1808,11 +1823,15 @@ void MarlinSettings::reset(PORTARG_SOLO) {
#if ENABLED(JUNCTION_DEVIATION)
planner.junction_deviation_mm = float(JUNCTION_DEVIATION_MM);
#else
#endif
#if HAS_CLASSIC_JERK
planner.max_jerk[X_AXIS] = DEFAULT_XJERK;
planner.max_jerk[Y_AXIS] = DEFAULT_YJERK;
planner.max_jerk[Z_AXIS] = DEFAULT_ZJERK;
planner.max_jerk[E_AXIS] = DEFAULT_EJERK;
#if DISABLED(JUNCTION_DEVIATION) || DISABLED(LIN_ADVANCE)
planner.max_jerk[E_AXIS] = DEFAULT_EJERK;
#endif
#endif
#if HAS_HOME_OFFSET
@ -2243,11 +2262,12 @@ void MarlinSettings::reset(PORTARG_SOLO) {
SERIAL_ECHOPGM_P(port, "Advanced: B<min_segment_time_us> S<min_feedrate> T<min_travel_feedrate>");
#if ENABLED(JUNCTION_DEVIATION)
SERIAL_ECHOPGM_P(port, " J<junc_dev>");
#else
SERIAL_ECHOPGM_P(port, " X<max_x_jerk> Y<max_y_jerk> Z<max_z_jerk>");
#endif
#if DISABLED(JUNCTION_DEVIATION) || ENABLED(LIN_ADVANCE)
SERIAL_ECHOPGM_P(port, " E<max_e_jerk>");
#if HAS_CLASSIC_JERK
SERIAL_ECHOPGM_P(port, " X<max_x_jerk> Y<max_y_jerk> Z<max_z_jerk>");
#if DISABLED(JUNCTION_DEVIATION) || DISABLED(LIN_ADVANCE)
SERIAL_ECHOPGM_P(port, " E<max_e_jerk>");
#endif
#endif
SERIAL_EOL_P(port);
}
@ -2258,11 +2278,14 @@ void MarlinSettings::reset(PORTARG_SOLO) {
#if ENABLED(JUNCTION_DEVIATION)
SERIAL_ECHOPAIR_P(port, " J", LINEAR_UNIT(planner.junction_deviation_mm));
#else
#endif
#if HAS_CLASSIC_JERK
SERIAL_ECHOPAIR_P(port, " X", LINEAR_UNIT(planner.max_jerk[X_AXIS]));
SERIAL_ECHOPAIR_P(port, " Y", LINEAR_UNIT(planner.max_jerk[Y_AXIS]));
SERIAL_ECHOPAIR_P(port, " Z", LINEAR_UNIT(planner.max_jerk[Z_AXIS]));
SERIAL_ECHOPAIR_P(port, " E", LINEAR_UNIT(planner.max_jerk[E_AXIS]));
#if DISABLED(JUNCTION_DEVIATION) || DISABLED(LIN_ADVANCE)
SERIAL_ECHOPAIR_P(port, " E", LINEAR_UNIT(planner.max_jerk[E_AXIS]));
#endif
#endif
SERIAL_EOL_P(port);

10
Marlin/src/module/delta.cpp
Unescape Escape View File

@ -101,7 +101,7 @@ void recalc_delta_settings() {
SERIAL_ECHOLNPAIR(" C:", delta[C_AXIS]); \
}while(0)
void inverse_kinematics(const float raw[XYZ]) {
void inverse_kinematics(const float (&raw)[XYZ]) {
#if HAS_HOTEND_OFFSET
// Delta hotend offsets must be applied in Cartesian space with no "spoofing"
const float pos[XYZ] = {
@ -224,6 +224,7 @@ void home_delta() {
#endif
// Init the current position of all carriages to 0,0,0
ZERO(current_position);
ZERO(destination);
sync_plan_position();
// Disable stealthChop if used. Enable diag1 pin on driver.
@ -232,9 +233,8 @@ void home_delta() {
#endif
// Move all carriages together linearly until an endstop is hit.
current_position[X_AXIS] = current_position[Y_AXIS] = current_position[Z_AXIS] = (delta_height + 10);
feedrate_mm_s = homing_feedrate(X_AXIS);
line_to_current_position();
destination[Z_AXIS] = (delta_height + 10);
buffer_line_to_destination(homing_feedrate(X_AXIS));
planner.synchronize();
// Re-enable stealthChop if used. Disable diag1 pin on driver.
@ -256,7 +256,7 @@ void home_delta() {
// give the impression that they are the same.
LOOP_XYZ(i) set_axis_is_at_home((AxisEnum)i);
SYNC_PLAN_POSITION_KINEMATIC();
sync_plan_position();
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("<<< home_delta", current_position);

11
Marlin/src/module/delta.h
Unescape Escape View File

@ -24,8 +24,7 @@
* delta.h - Delta-specific functions
*/
#ifndef __DELTA_H__
#define __DELTA_H__
#pragma once
extern float delta_height,
delta_endstop_adj[ABC],
@ -78,7 +77,11 @@ void recalc_delta_settings();
delta[C_AXIS] = DELTA_Z(V, C_AXIS); \
}while(0)
void inverse_kinematics(const float raw[XYZ]);
void inverse_kinematics(const float (&raw)[XYZ]);
FORCE_INLINE void inverse_kinematics(const float (&raw)[XYZE]) {
const float raw_xyz[XYZ] = { raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS] };
inverse_kinematics(raw_xyz);
}
/**
* Calculate the highest Z position where the
@ -118,5 +121,3 @@ FORCE_INLINE void forward_kinematics_DELTA(const float (&point)[ABC]) {
}
void home_delta();
#endif // __DELTA_H__

213
Marlin/src/module/motion.cpp
Unescape Escape View File

@ -75,7 +75,7 @@ bool relative_mode; // = false;
* Cartesian Current Position
* Used to track the native machine position as moves are queued.
* Used by 'buffer_line_to_current_position' to do a move after changing it.
* Used by 'SYNC_PLAN_POSITION_KINEMATIC' to update 'planner.position'.
* Used by 'sync_plan_position' to update 'planner.position'.
*/
float current_position[XYZE] = { 0 };
@ -218,15 +218,22 @@ void get_cartesian_from_steppers() {
* may have been applied.
*
* To prevent small shifts in axis position always call
* SYNC_PLAN_POSITION_KINEMATIC after updating axes with this.
* sync_plan_position after updating axes with this.
*
* To keep hosts in sync, always call report_current_position
* after updating the current_position.
*/
void set_current_from_steppers_for_axis(const AxisEnum axis) {
get_cartesian_from_steppers();
#if PLANNER_LEVELING
planner.unapply_leveling(cartes);
#if HAS_POSITION_MODIFIERS
float pos[XYZE] = { cartes[X_AXIS], cartes[Y_AXIS], cartes[Z_AXIS], current_position[E_AXIS] };
planner.unapply_modifiers(pos
#if HAS_LEVELING
, true
#endif
);
const float (&cartes)[XYZE] = pos;
#endif
if (axis == ALL_AXES)
COPY(current_position, cartes);
@ -252,13 +259,6 @@ void buffer_line_to_destination(const float fr_mm_s) {
#if IS_KINEMATIC
void sync_plan_position_kinematic() {
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position_kinematic", current_position);
#endif
planner.set_position_mm_kinematic(current_position);
}
/**
* Calculate delta, start a line, and set current_position to destination
*/
@ -277,7 +277,7 @@ void buffer_line_to_destination(const float fr_mm_s) {
&& current_position[E_AXIS] == destination[E_AXIS]
) return;
planner.buffer_line_kinematic(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s), active_extruder);
planner.buffer_line(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s), active_extruder);
#endif
set_current_from_destination();
@ -538,7 +538,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
// If the move is only in Z/E don't split up the move
if (!xdiff && !ydiff) {
planner.buffer_line_kinematic(rtarget, _feedrate_mm_s, active_extruder);
planner.buffer_line(rtarget, _feedrate_mm_s, active_extruder);
return false; // caller will update current_position
}
@ -580,53 +580,22 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
ydiff * inv_segments,
zdiff * inv_segments,
ediff * inv_segments
};
#if !HAS_FEEDRATE_SCALING
const float cartesian_segment_mm = cartesian_mm * inv_segments;
},
cartesian_segment_mm = cartesian_mm * inv_segments;
#if ENABLED(SCARA_FEEDRATE_SCALING)
const float inv_duration = _feedrate_mm_s / cartesian_segment_mm;
#endif
/*
SERIAL_ECHOPAIR("mm=", cartesian_mm);
SERIAL_ECHOPAIR(" seconds=", seconds);
SERIAL_ECHOPAIR(" segments=", segments);
#if !HAS_FEEDRATE_SCALING
SERIAL_ECHOPAIR(" segment_mm=", cartesian_segment_mm);
#endif
SERIAL_ECHOPAIR(" segment_mm=", cartesian_segment_mm);
SERIAL_EOL();
//*/
#if HAS_FEEDRATE_SCALING
// SCARA needs to scale the feed rate from mm/s to degrees/s
// i.e., Complete the angular vector in the given time.
const float segment_length = cartesian_mm * inv_segments,
inv_segment_length = 1.0f / segment_length, // 1/mm/segs
inverse_secs = inv_segment_length * _feedrate_mm_s;
float oldA = planner.position_float[A_AXIS],
oldB = planner.position_float[B_AXIS]
#if ENABLED(DELTA_FEEDRATE_SCALING)
, oldC = planner.position_float[C_AXIS]
#endif
;
/*
SERIAL_ECHOPGM("Scaled kinematic move: ");
SERIAL_ECHOPAIR(" segment_length (inv)=", segment_length);
SERIAL_ECHOPAIR(" (", inv_segment_length);
SERIAL_ECHOPAIR(") _feedrate_mm_s=", _feedrate_mm_s);
SERIAL_ECHOPAIR(" inverse_secs=", inverse_secs);
SERIAL_ECHOPAIR(" oldA=", oldA);
SERIAL_ECHOPAIR(" oldB=", oldB);
#if ENABLED(DELTA_FEEDRATE_SCALING)
SERIAL_ECHOPAIR(" oldC=", oldC);
#endif
SERIAL_EOL();
safe_delay(5);
//*/
#endif
// Get the current position as starting point
// Get the current position as starting point
float raw[XYZE];
COPY(raw, current_position);
@ -642,78 +611,20 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
LOOP_XYZE(i) raw[i] += segment_distance[i];
#if ENABLED(DELTA) && HOTENDS < 2
DELTA_IK(raw); // Delta can inline its kinematics
#else
inverse_kinematics(raw);
#endif
ADJUST_DELTA(raw); // Adjust Z if bed leveling is enabled
#if ENABLED(SCARA_FEEDRATE_SCALING)
// For SCARA scale the feed rate from mm/s to degrees/s
// i.e., Complete the angular vector in the given time.
if (!planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder, segment_length))
break;
/*
SERIAL_ECHO(segments);
SERIAL_ECHOPAIR(": X=", raw[X_AXIS]); SERIAL_ECHOPAIR(" Y=", raw[Y_AXIS]);
SERIAL_ECHOPAIR(" A=", delta[A_AXIS]); SERIAL_ECHOPAIR(" B=", delta[B_AXIS]);
SERIAL_ECHOLNPAIR(" F", HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs * 60);
safe_delay(5);
//*/
oldA = delta[A_AXIS]; oldB = delta[B_AXIS];
#elif ENABLED(DELTA_FEEDRATE_SCALING)
// For DELTA scale the feed rate from Effector mm/s to Carriage mm/s
// i.e., Complete the linear vector in the given time.
if (!planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], SQRT(sq(delta[A_AXIS] - oldA) + sq(delta[B_AXIS] - oldB) + sq(delta[C_AXIS] - oldC)) * inverse_secs, active_extruder, segment_length))
break;
/*
SERIAL_ECHO(segments);
SERIAL_ECHOPAIR(": X=", raw[X_AXIS]); SERIAL_ECHOPAIR(" Y=", raw[Y_AXIS]);
SERIAL_ECHOPAIR(" A=", delta[A_AXIS]); SERIAL_ECHOPAIR(" B=", delta[B_AXIS]); SERIAL_ECHOPAIR(" C=", delta[C_AXIS]);
SERIAL_ECHOLNPAIR(" F", SQRT(sq(delta[A_AXIS] - oldA) + sq(delta[B_AXIS] - oldB) + sq(delta[C_AXIS] - oldC)) * inverse_secs * 60);
safe_delay(5);
//*/
oldA = delta[A_AXIS]; oldB = delta[B_AXIS]; oldC = delta[C_AXIS];
#else
if (!planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], _feedrate_mm_s, active_extruder, cartesian_segment_mm))
break;
#endif
if (!planner.buffer_line(raw, _feedrate_mm_s, active_extruder, cartesian_segment_mm
#if ENABLED(SCARA_FEEDRATE_SCALING)
, inv_duration
#endif
))
break;
}
// Ensure last segment arrives at target location.
#if HAS_FEEDRATE_SCALING
inverse_kinematics(rtarget);
ADJUST_DELTA(rtarget);
#endif
#if ENABLED(SCARA_FEEDRATE_SCALING)
const float diff2 = HYPOT2(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB);
if (diff2) {
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], rtarget[Z_AXIS], rtarget[E_AXIS], SQRT(diff2) * inverse_secs, active_extruder, segment_length);
/*
SERIAL_ECHOPAIR("final: A=", delta[A_AXIS]); SERIAL_ECHOPAIR(" B=", delta[B_AXIS]);
SERIAL_ECHOPAIR(" adiff=", delta[A_AXIS] - oldA); SERIAL_ECHOPAIR(" bdiff=", delta[B_AXIS] - oldB);
SERIAL_ECHOLNPAIR(" F", SQRT(diff2) * inverse_secs * 60);
SERIAL_EOL();
safe_delay(5);
//*/
}
#elif ENABLED(DELTA_FEEDRATE_SCALING)
const float diff2 = sq(delta[A_AXIS] - oldA) + sq(delta[B_AXIS] - oldB) + sq(delta[C_AXIS] - oldC);
if (diff2) {
planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], rtarget[E_AXIS], SQRT(diff2) * inverse_secs, active_extruder, segment_length);
/*
SERIAL_ECHOPAIR("final: A=", delta[A_AXIS]); SERIAL_ECHOPAIR(" B=", delta[B_AXIS]); SERIAL_ECHOPAIR(" C=", delta[C_AXIS]);
SERIAL_ECHOPAIR(" adiff=", delta[A_AXIS] - oldA); SERIAL_ECHOPAIR(" bdiff=", delta[B_AXIS] - oldB); SERIAL_ECHOPAIR(" cdiff=", delta[C_AXIS] - oldC);
SERIAL_ECHOLNPAIR(" F", SQRT(diff2) * inverse_secs * 60);
SERIAL_EOL();
safe_delay(5);
//*/
}
#else
planner.buffer_line_kinematic(rtarget, _feedrate_mm_s, active_extruder, cartesian_segment_mm);
#endif
planner.buffer_line(rtarget, _feedrate_mm_s, active_extruder, cartesian_segment_mm
#if ENABLED(SCARA_FEEDRATE_SCALING)
, inv_duration
#endif
);
return false; // caller will update current_position
}
@ -736,7 +647,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
// If the move is only in Z/E don't split up the move
if (!xdiff && !ydiff) {
planner.buffer_line_kinematic(destination, fr_mm_s, active_extruder);
planner.buffer_line(destination, fr_mm_s, active_extruder);
return;
}
@ -766,6 +677,10 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
ediff * inv_segments
};
#if ENABLED(SCARA_FEEDRATE_SCALING)
const float inv_duration = _feedrate_mm_s / cartesian_segment_mm;
#endif
// SERIAL_ECHOPAIR("mm=", cartesian_mm);
// SERIAL_ECHOLNPAIR(" segments=", segments);
// SERIAL_ECHOLNPAIR(" segment_mm=", cartesian_segment_mm);
@ -783,13 +698,21 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
idle();
}
LOOP_XYZE(i) raw[i] += segment_distance[i];
if (!planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder, cartesian_segment_mm))
if (!planner.buffer_line(raw, fr_mm_s, active_extruder, cartesian_segment_mm
#if ENABLED(SCARA_FEEDRATE_SCALING)
, inv_duration
#endif
))
break;
}
// Since segment_distance is only approximate,
// the final move must be to the exact destination.
planner.buffer_line_kinematic(destination, fr_mm_s, active_extruder, cartesian_segment_mm);
planner.buffer_line(destination, fr_mm_s, active_extruder, cartesian_segment_mm
#if ENABLED(SCARA_FEEDRATE_SCALING)
, inv_duration
#endif
);
}
#endif // SEGMENT_LEVELED_MOVES
@ -922,7 +845,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS },
planner.max_feedrate_mm_s[X_AXIS], 1)
) break;
planner.synchronize();
SYNC_PLAN_POSITION_KINEMATIC();
sync_plan_position();
extruder_duplication_enabled = true;
active_extruder_parked = false;
#if ENABLED(DEBUG_LEVELING_FEATURE)
@ -1092,7 +1015,7 @@ inline float get_homing_bump_feedrate(const AxisEnum axis) {
/**
* Home an individual linear axis
*/
static void do_homing_move(const AxisEnum axis, const float distance, const float fr_mm_s=0.0) {
void do_homing_move(const AxisEnum axis, const float distance, const float fr_mm_s=0.0) {
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) {
@ -1139,18 +1062,29 @@ static void do_homing_move(const AxisEnum axis, const float distance, const floa
#endif
}
// Tell the planner the axis is at 0
current_position[axis] = 0;
#if IS_SCARA
SYNC_PLAN_POSITION_KINEMATIC();
// Tell the planner the axis is at 0
current_position[axis] = 0;
sync_plan_position();
current_position[axis] = distance;
inverse_kinematics(current_position);
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], current_position[E_AXIS], fr_mm_s ? fr_mm_s : homing_feedrate(axis), active_extruder);
planner.buffer_line(current_position, fr_mm_s ? fr_mm_s : homing_feedrate(axis), active_extruder);
#else
sync_plan_position();
current_position[axis] = distance; // Set delta/cartesian axes directly
planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], fr_mm_s ? fr_mm_s : homing_feedrate(axis), active_extruder);
float target[ABCE] = { planner.get_axis_position_mm(A_AXIS), planner.get_axis_position_mm(B_AXIS), planner.get_axis_position_mm(C_AXIS), planner.get_axis_position_mm(E_AXIS) };
target[axis] = 0;
planner.set_machine_position_mm(target);
target[axis] = distance;
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
const float delta_mm_cart[XYZE] = {0, 0, 0, 0};
#endif
// Set delta/cartesian axes directly
planner.buffer_segment(target
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, delta_mm_cart
#endif
, fr_mm_s ? fr_mm_s : homing_feedrate(axis), active_extruder
);
#endif
planner.synchronize();
@ -1349,7 +1283,14 @@ void homeaxis(const AxisEnum axis) {
if (axis == Z_AXIS && set_bltouch_deployed(true)) return;
#endif
do_homing_move(axis, 1.5f * max_length(axis) * axis_home_dir);
do_homing_move(axis, 1.5f * max_length(
#if ENABLED(DELTA)
Z_AXIS
#else
axis
#endif
) * axis_home_dir
);
#if HOMING_Z_WITH_PROBE && ENABLED(BLTOUCH)
// BLTOUCH needs to be stowed after trigger to rearm itself
@ -1481,7 +1422,7 @@ void homeaxis(const AxisEnum axis) {
#if IS_SCARA
set_axis_is_at_home(axis);
SYNC_PLAN_POSITION_KINEMATIC();
sync_plan_position();
#elif ENABLED(DELTA)

23
Marlin/src/module/motion.h
Unescape Escape View File

@ -129,13 +129,6 @@ void set_current_from_steppers_for_axis(const AxisEnum axis);
void sync_plan_position();
void sync_plan_position_e();
#if IS_KINEMATIC
void sync_plan_position_kinematic();
#define SYNC_PLAN_POSITION_KINEMATIC() sync_plan_position_kinematic()
#else
#define SYNC_PLAN_POSITION_KINEMATIC() sync_plan_position()
#endif
/**
* Move the planner to the current position from wherever it last moved
* (or from wherever it has been told it is located).
@ -354,20 +347,4 @@ void homeaxis(const AxisEnum axis);
void set_home_offset(const AxisEnum axis, const float v);
#endif
#if ENABLED(AUTO_BED_LEVELING_BILINEAR)
#if ENABLED(DELTA)
#define ADJUST_DELTA(V) \
if (planner.leveling_active) { \
const float zadj = bilinear_z_offset(V); \
delta[A_AXIS] += zadj; \
delta[B_AXIS] += zadj; \
delta[C_AXIS] += zadj; \
}
#else
#define ADJUST_DELTA(V) if (planner.leveling_active) { delta[Z_AXIS] += bilinear_z_offset(V); }
#endif
#else
#define ADJUST_DELTA(V) NOOP
#endif
#endif // MOTION_H

245
Marlin/src/module/planner.cpp
Unescape Escape View File

@ -133,8 +133,13 @@ float Planner::max_feedrate_mm_s[XYZE_N], // (mm/s) M203 XYZE - Max speeds
float Planner::max_e_jerk;
#endif
#endif
#else
float Planner::max_jerk[XYZE]; // (mm/s^2) M205 XYZE - The largest speed change requiring no acceleration.
#endif
#if HAS_CLASSIC_JERK
#if ENABLED(JUNCTION_DEVIATION) && ENABLED(LIN_ADVANCE)
float Planner::max_jerk[XYZ]; // (mm/s^2) M205 XYZ - The largest speed change requiring no acceleration.
#else
float Planner::max_jerk[XYZE]; // (mm/s^2) M205 XYZE - The largest speed change requiring no acceleration.
#endif
#endif
#if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
@ -220,6 +225,10 @@ float Planner::previous_speed[NUM_AXIS],
float Planner::position_float[XYZE]; // Needed for accurate maths. Steps cannot be used!
#endif
#if IS_KINEMATIC
float Planner::position_cart[XYZE];
#endif
#if ENABLED(ULTRA_LCD)
volatile uint32_t Planner::block_buffer_runtime_us = 0;
#endif
@ -235,6 +244,9 @@ void Planner::init() {
#if HAS_POSITION_FLOAT
ZERO(position_float);
#endif
#if IS_KINEMATIC
ZERO(position_cart);
#endif
ZERO(previous_speed);
previous_nominal_speed_sqr = 0;
#if ABL_PLANAR
@ -1354,17 +1366,12 @@ void Planner::check_axes_activity() {
}
#endif
#if PLANNER_LEVELING || HAS_UBL_AND_CURVES
#if HAS_LEVELING
/**
* rx, ry, rz - Cartesian positions in mm
* Leveled XYZ on completion
*/
void Planner::apply_leveling(float &rx, float &ry, float &rz) {
#if ENABLED(SKEW_CORRECTION)
skew(rx, ry, rz);
#endif
if (!leveling_active) return;
#if ABL_PLANAR
@ -1406,10 +1413,6 @@ void Planner::check_axes_activity() {
#endif
}
#endif
#if PLANNER_LEVELING
void Planner::unapply_leveling(float raw[XYZ]) {
if (leveling_active) {
@ -1456,7 +1459,23 @@ void Planner::check_axes_activity() {
#endif
}
#endif // PLANNER_LEVELING
#endif // HAS_LEVELING
#if ENABLED(FWRETRACT)
/**
* rz, e - Cartesian positions in mm
*/
void Planner::apply_retract(float &rz, float &e) {
rz += fwretract.current_hop;
e -= fwretract.current_retract[active_extruder];
}
void Planner::unapply_retract(float &rz, float &e) {
rz -= fwretract.current_hop;
e += fwretract.current_retract[active_extruder];
}
#endif
void Planner::quick_stop() {
@ -1554,6 +1573,7 @@ void Planner::synchronize() {
* Add a new linear movement to the planner queue (in terms of steps).
*
* target - target position in steps units
* target_float - target position in direct (mm, degrees) units. optional
* fr_mm_s - (target) speed of the move
* extruder - target extruder
* millimeters - the length of the movement, if known
@ -1562,7 +1582,10 @@ void Planner::synchronize() {
*/
bool Planner::_buffer_steps(const int32_t (&target)[XYZE]
#if HAS_POSITION_FLOAT
, const float (&target_float)[XYZE]
, const float (&target_float)[ABCE]
#endif
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, const float (&delta_mm_cart)[XYZE]
#endif
, float fr_mm_s, const uint8_t extruder, const float &millimeters
) {
@ -1579,6 +1602,9 @@ bool Planner::_buffer_steps(const int32_t (&target)[XYZE]
#if HAS_POSITION_FLOAT
, target_float
#endif
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, delta_mm_cart
#endif
, fr_mm_s, extruder, millimeters
)) {
// Movement was not queued, probably because it was too short.
@ -1618,9 +1644,12 @@ bool Planner::_buffer_steps(const int32_t (&target)[XYZE]
* Returns true is movement is acceptable, false otherwise
*/
bool Planner::_populate_block(block_t * const block, bool split_move,
const int32_t (&target)[XYZE]
const int32_t (&target)[ABCE]
#if HAS_POSITION_FLOAT
, const float (&target_float)[XYZE]
, const float (&target_float)[ABCE]
#endif
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, const float (&delta_mm_cart)[XYZE]
#endif
, float fr_mm_s, const uint8_t extruder, const float &millimeters/*=0.0*/
) {
@ -2243,12 +2272,21 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
// Unit vector of previous path line segment
static float previous_unit_vec[XYZE];
float unit_vec[] = {
delta_mm[A_AXIS] * inverse_millimeters,
delta_mm[B_AXIS] * inverse_millimeters,
delta_mm[C_AXIS] * inverse_millimeters,
delta_mm[E_AXIS] * inverse_millimeters
};
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
float unit_vec[] = {
delta_mm_cart[X_AXIS] * inverse_millimeters,
delta_mm_cart[Y_AXIS] * inverse_millimeters,
delta_mm_cart[Z_AXIS] * inverse_millimeters,
delta_mm_cart[E_AXIS] * inverse_millimeters
};
#else
float unit_vec[] = {
delta_mm[X_AXIS] * inverse_millimeters,
delta_mm[Y_AXIS] * inverse_millimeters,
delta_mm[Z_AXIS] * inverse_millimeters,
delta_mm[E_AXIS] * inverse_millimeters
};
#endif
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
if (moves_queued && !UNEAR_ZERO(previous_nominal_speed_sqr)) {
@ -2302,7 +2340,9 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
COPY(previous_unit_vec, unit_vec);
#else // Classic Jerk Limiting
#endif
#if HAS_CLASSIC_JERK
/**
* Adapted from Průša MKS firmware
@ -2317,7 +2357,12 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
float safe_speed = nominal_speed;
uint8_t limited = 0;
LOOP_XYZE(i) {
#if ENABLED(JUNCTION_DEVIATION) && ENABLED(LIN_ADVANCE)
LOOP_XYZ(i)
#else
LOOP_XYZE(i)
#endif
{
const float jerk = ABS(current_speed[i]), // cs : Starting from zero, change in speed for this axis
maxj = max_jerk[i]; // mj : The max jerk setting for this axis
if (jerk > maxj) { // cs > mj : New current speed too fast?
@ -2349,7 +2394,12 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
// Now limit the jerk in all axes.
const float smaller_speed_factor = vmax_junction / previous_nominal_speed;
LOOP_XYZE(axis) {
#if ENABLED(JUNCTION_DEVIATION) && ENABLED(LIN_ADVANCE)
LOOP_XYZ(axis)
#else
LOOP_XYZE(axis)
#endif
{
// Limit an axis. We have to differentiate: coasting, reversal of an axis, full stop.
float v_exit = previous_speed[axis] * smaller_speed_factor,
v_entry = current_speed[axis];
@ -2381,7 +2431,12 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
vmax_junction = safe_speed;
previous_safe_speed = safe_speed;
vmax_junction_sqr = sq(vmax_junction);
#if ENABLED(JUNCTION_DEVIATION)
vmax_junction_sqr = MIN(vmax_junction_sqr, sq(vmax_junction));
#else
vmax_junction_sqr = sq(vmax_junction);
#endif
#endif // Classic Jerk Limiting
@ -2466,7 +2521,12 @@ void Planner::buffer_sync_block() {
* extruder - target extruder
* millimeters - the length of the movement, if known
*/
bool Planner::buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder, const float &millimeters/*=0.0*/) {
bool Planner::buffer_segment(const float &a, const float &b, const float &c, const float &e
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, const float (&delta_mm_cart)[XYZE]
#endif
, const float &fr_mm_s, const uint8_t extruder, const float &millimeters/*=0.0*/
) {
// If we are cleaning, do not accept queuing of movements
if (cleaning_buffer_counter) return false;
@ -2534,6 +2594,9 @@ bool Planner::buffer_segment(const float &a, const float &b, const float &c, con
#if HAS_POSITION_FLOAT
, target_float
#endif
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, delta_mm_cart
#endif
, fr_mm_s, extruder, millimeters
)
) return false;
@ -2543,24 +2606,84 @@ bool Planner::buffer_segment(const float &a, const float &b, const float &c, con
} // buffer_segment()
/**
* Directly set the planner XYZ position (and stepper positions)
* Add a new linear movement to the buffer.
* The target is cartesian, it's translated to delta/scara if
* needed.
*
*
* rx,ry,rz,e - target position in mm or degrees
* fr_mm_s - (target) speed of the move (mm/s)
* extruder - target extruder
* millimeters - the length of the movement, if known
* inv_duration - the reciprocal if the duration of the movement, if known (kinematic only if feeedrate scaling is enabled)
*/
bool Planner::buffer_line(const float &rx, const float &ry, const float &rz, const float &e, const float &fr_mm_s, const uint8_t extruder, const float millimeters
#if ENABLED(SCARA_FEEDRATE_SCALING)
, const float &inv_duration
#endif
) {
float raw[XYZE] = { rx, ry, rz, e };
#if HAS_POSITION_MODIFIERS
apply_modifiers(raw);
#endif
#if IS_KINEMATIC
const float delta_mm_cart[] = {
rx - position_cart[X_AXIS],
ry - position_cart[Y_AXIS],
rz - position_cart[Z_AXIS]
#if ENABLED(JUNCTION_DEVIATION)
, e - position_cart[E_AXIS]
#endif
};
float mm = millimeters;
if (mm == 0.0)
mm = (delta_mm_cart[X_AXIS] != 0.0 || delta_mm_cart[Y_AXIS] != 0.0) ? SQRT(sq(delta_mm_cart[X_AXIS]) + sq(delta_mm_cart[Y_AXIS]) + sq(delta_mm_cart[Z_AXIS])) : fabs(delta_mm_cart[Z_AXIS]);
inverse_kinematics(raw);
#if ENABLED(SCARA_FEEDRATE_SCALING)
// For SCARA scale the feed rate from mm/s to degrees/s
// i.e., Complete the angular vector in the given time.
const float duration_recip = inv_duration ? inv_duration : fr_mm_s / mm,
feedrate = HYPOT(delta[A_AXIS] - position_float[A_AXIS], delta[B_AXIS] - position_float[B_AXIS]) * duration_recip;
#else
const float feedrate = fr_mm_s;
#endif
if (buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS]
#if ENABLED(JUNCTION_DEVIATION)
, delta_mm_cart
#endif
, feedrate, extruder, mm
)) {
position_cart[X_AXIS] = rx;
position_cart[Y_AXIS] = ry;
position_cart[Z_AXIS] = rz;
position_cart[E_AXIS] = e;
return true;
}
else
return false;
#else
return buffer_segment(raw, fr_mm_s, extruder, millimeters);
#endif
} // buffer_line()
/**
* Directly set the planner ABC position (and stepper positions)
* converting mm (or angles for SCARA) into steps.
*
* On CORE machines stepper ABC will be translated from the given XYZ.
* The provided ABC position is in machine units.
*/
void Planner::_set_position_mm(const float &a, const float &b, const float &c, const float &e) {
void Planner::set_machine_position_mm(const float &a, const float &b, const float &c, const float &e) {
#if ENABLED(DISTINCT_E_FACTORS)
last_extruder = active_extruder;
#endif
position[A_AXIS] = LROUND(a * axis_steps_per_mm[A_AXIS]);
position[B_AXIS] = LROUND(b * axis_steps_per_mm[B_AXIS]);
position[C_AXIS] = LROUND(axis_steps_per_mm[C_AXIS] * (c +(
#if !IS_KINEMATIC && ENABLED(AUTO_BED_LEVELING_UBL)
leveling_active ? ubl.get_z_correction(a, b) :
#endif
0)
));
position[C_AXIS] = LROUND(c * axis_steps_per_mm[C_AXIS]);
position[E_AXIS] = LROUND(e * axis_steps_per_mm[_EINDEX]);
#if HAS_POSITION_FLOAT
position_float[A_AXIS] = a;
@ -2577,44 +2700,54 @@ void Planner::_set_position_mm(const float &a, const float &b, const float &c, c
stepper.set_position(position[A_AXIS], position[B_AXIS], position[C_AXIS], position[E_AXIS]);
}
void Planner::set_position_mm_kinematic(const float (&cart)[XYZE]) {
#if PLANNER_LEVELING
float raw[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] };
apply_leveling(raw);
#else
const float (&raw)[XYZE] = cart;
void Planner::set_position_mm(const float &rx, const float &ry, const float &rz, const float &e) {
float raw[XYZE] = { rx, ry, rz, e };
#if HAS_POSITION_MODIFIERS
apply_modifiers(raw
#if HAS_LEVELING
, true
#endif
);
#endif
#if IS_KINEMATIC
position_cart[X_AXIS] = rx;
position_cart[Y_AXIS] = ry;
position_cart[Z_AXIS] = rz;
position_cart[E_AXIS] = e;
inverse_kinematics(raw);
_set_position_mm(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS]);
set_machine_position_mm(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS]);
#else
_set_position_mm(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_AXIS]);
set_machine_position_mm(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], raw[E_AXIS]);
#endif
}
/**
* Setters for planner position (also setting stepper position).
*/
void Planner::set_position_mm(const AxisEnum axis, const float &v) {
void Planner::set_e_position_mm(const float &e) {
#if ENABLED(DISTINCT_E_FACTORS)
const uint8_t axis_index = axis + (axis == E_AXIS ? active_extruder : 0);
const uint8_t axis_index = E_AXIS + active_extruder;
last_extruder = active_extruder;
#else
const uint8_t axis_index = axis;
const uint8_t axis_index = E_AXIS;
#endif
position[axis] = LROUND(axis_steps_per_mm[axis_index] * (v + (
#if ENABLED(AUTO_BED_LEVELING_UBL)
axis == Z_AXIS && leveling_active ? ubl.get_z_correction(current_position[X_AXIS], current_position[Y_AXIS]) :
#endif
0)
));
#if ENABLED(FWRETRACT)
float e_new = e - fwretract.current_retract[active_extruder];
#else
const float e_new = e;
#endif
position[E_AXIS] = LROUND(axis_steps_per_mm[axis_index] * e_new);
#if HAS_POSITION_FLOAT
position_float[axis] = v;
position_float[E_AXIS] = e_new;
#endif
#if IS_KINEMATIC
position_cart[E_AXIS] = e;
#endif
if (has_blocks_queued())
buffer_sync_block();
else
stepper.set_position(axis, position[axis]);
stepper.set_position(E_AXIS, position[E_AXIS]);
}
// Recalculate the steps/s^2 acceleration rates, based on the mm/s^2
@ -2638,7 +2771,7 @@ void Planner::reset_acceleration_rates() {
// Recalculate position, steps_to_mm if axis_steps_per_mm changes!
void Planner::refresh_positioning() {
LOOP_XYZE_N(i) steps_to_mm[i] = 1.0f / axis_steps_per_mm[i];
set_position_mm_kinematic(current_position);
set_position_mm(current_position);
reset_acceleration_rates();
}

221
Marlin/src/module/planner.h
Unescape Escape View File

@ -45,6 +45,10 @@
#include "../libs/vector_3.h"
#endif
#if ENABLED(FWRETRACT)
#include "../feature/fwretract.h"
#endif
enum BlockFlagBit : char {
// Recalculate trapezoids on entry junction. For optimization.
BLOCK_BIT_RECALCULATE,
@ -151,7 +155,7 @@ typedef struct {
} block_t;
#define HAS_POSITION_FLOAT (ENABLED(LIN_ADVANCE) || HAS_FEEDRATE_SCALING)
#define HAS_POSITION_FLOAT (ENABLED(LIN_ADVANCE) || ENABLED(SCARA_FEEDRATE_SCALING))
#define BLOCK_MOD(n) ((n)&(BLOCK_BUFFER_SIZE-1))
@ -210,14 +214,22 @@ class Planner {
#if ENABLED(JUNCTION_DEVIATION)
static float junction_deviation_mm; // (mm) M205 J
#if ENABLED(LIN_ADVANCE)
#if ENABLED(DISTINCT_E_FACTORS)
static float max_e_jerk[EXTRUDERS]; // Calculated from junction_deviation_mm
static float max_e_jerk // Calculated from junction_deviation_mm
#if ENABLED(DISTINCT_E_FACTORS)
[EXTRUDERS]
#endif
;
#endif
#endif
#if HAS_CLASSIC_JERK
static float max_jerk[
#if ENABLED(JUNCTION_DEVIATION) && ENABLED(LIN_ADVANCE)
XYZ // (mm/s^2) M205 XYZ - The largest speed change requiring no acceleration.
#else
static float max_e_jerk;
XYZE // (mm/s^2) M205 XYZE - The largest speed change requiring no acceleration.
#endif
#endif
#else
static float max_jerk[XYZE]; // (mm/s^2) M205 XYZE - The largest speed change requiring no acceleration.
];
#endif
#if HAS_LEVELING
@ -240,6 +252,10 @@ class Planner {
static float position_float[XYZE];
#endif
#if IS_KINEMATIC
static float position_cart[XYZE];
#endif
#if ENABLED(SKEW_CORRECTION)
#if ENABLED(SKEW_CORRECTION_GCODE)
static float xy_skew_factor;
@ -410,6 +426,8 @@ class Planner {
}
}
}
FORCE_INLINE static void skew(float (&raw)[XYZ]) { skew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); }
FORCE_INLINE static void skew(float (&raw)[XYZE]) { skew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); }
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)) {
@ -420,29 +438,76 @@ class Planner {
}
}
}
FORCE_INLINE static void unskew(float (&raw)[XYZ]) { unskew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); }
FORCE_INLINE static void unskew(float (&raw)[XYZE]) { unskew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); }
#endif // SKEW_CORRECTION
#if PLANNER_LEVELING || HAS_UBL_AND_CURVES
#if HAS_LEVELING
/**
* Apply leveling to transform a cartesian position
* as it will be given to the planner and steppers.
*/
static void apply_leveling(float &rx, float &ry, float &rz);
FORCE_INLINE static void apply_leveling(float (&raw)[XYZ]) { apply_leveling(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); }
#endif
FORCE_INLINE static void apply_leveling(float (&raw)[XYZE]) { apply_leveling(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); }
#if PLANNER_LEVELING
#define ARG_X float rx
#define ARG_Y float ry
#define ARG_Z float rz
static void unapply_leveling(float raw[XYZ]);
#else
#define ARG_X const float &rx
#define ARG_Y const float &ry
#define ARG_Z const float &rz
#endif
#if ENABLED(FWRETRACT)
static void apply_retract(float &rz, float &e);
FORCE_INLINE static void apply_retract(float (&raw)[XYZE]) { apply_retract(raw[Z_AXIS], raw[E_AXIS]); }
static void unapply_retract(float &rz, float &e);
FORCE_INLINE static void unapply_retract(float (&raw)[XYZE]) { unapply_retract(raw[Z_AXIS], raw[E_AXIS]); }
#endif
#if HAS_POSITION_MODIFIERS
FORCE_INLINE static void apply_modifiers(float (&pos)[XYZE]
#if HAS_LEVELING
, bool leveling =
#if PLANNER_LEVELING
true
#else
false
#endif
#endif
) {
#if ENABLED(SKEW_CORRECTION)
skew(pos);
#endif
#if HAS_LEVELING
if (leveling)
apply_leveling(pos);
#endif
#if ENABLED(FWRETRACT)
apply_retract(pos);
#endif
}
FORCE_INLINE static void unapply_modifiers(float (&pos)[XYZE]
#if HAS_LEVELING
, bool leveling =
#if PLANNER_LEVELING
true
#else
false
#endif
#endif
) {
#if ENABLED(FWRETRACT)
unapply_retract(pos);
#endif
#if HAS_LEVELING
if (leveling)
unapply_leveling(pos);
#endif
#if ENABLED(SKEW_CORRECTION)
unskew(pos);
#endif
}
#endif // HAS_POSITION_MODIFIERS
// Number of moves currently in the planner including the busy block, if any
FORCE_INLINE static uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail); }
@ -489,7 +554,10 @@ class Planner {
*/
static bool _buffer_steps(const int32_t (&target)[XYZE]
#if HAS_POSITION_FLOAT
, const float (&target_float)[XYZE]
, const float (&target_float)[ABCE]
#endif
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, const float (&delta_mm_cart)[XYZE]
#endif
, float fr_mm_s, const uint8_t extruder, const float &millimeters=0.0
);
@ -511,6 +579,9 @@ class Planner {
#if HAS_POSITION_FLOAT
, const float (&target_float)[XYZE]
#endif
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, const float (&delta_mm_cart)[XYZE]
#endif
, float fr_mm_s, const uint8_t extruder, const float &millimeters=0.0
);
@ -520,6 +591,13 @@ class Planner {
*/
static void buffer_sync_block();
#if IS_KINEMATIC
private:
// Allow do_homing_move to access internal functions, such as buffer_segment.
friend void do_homing_move(const AxisEnum, const float, const float);
#endif
/**
* Planner::buffer_segment
*
@ -532,74 +610,83 @@ class Planner {
* extruder - target extruder
* millimeters - the length of the movement, if known
*/
static bool buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder, const float &millimeters=0.0);
static void _set_position_mm(const float &a, const float &b, const float &c, const float &e);
static bool buffer_segment(const float &a, const float &b, const float &c, const float &e
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, const float (&delta_mm_cart)[XYZE]
#endif
, const float &fr_mm_s, const uint8_t extruder, const float &millimeters=0.0
);
/**
* Add a new linear movement to the buffer.
* The target is NOT translated to delta/scara
*
* Leveling will be applied to input on cartesians.
* Kinematic machines should call buffer_line_kinematic (for leveled moves).
* (Cartesians may also call buffer_line_kinematic.)
*
* rx,ry,rz,e - target position in mm or degrees
* fr_mm_s - (target) speed of the move (mm/s)
* extruder - target extruder
* millimeters - the length of the movement, if known
*/
FORCE_INLINE static bool buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, const float &fr_mm_s, const uint8_t extruder, const float millimeters = 0.0) {
#if PLANNER_LEVELING && IS_CARTESIAN
apply_leveling(rx, ry, rz);
FORCE_INLINE static bool buffer_segment(const float (&abce)[ABCE]
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, const float (&delta_mm_cart)[XYZE]
#endif
return buffer_segment(rx, ry, rz, e, fr_mm_s, extruder, millimeters);
, const float &fr_mm_s, const uint8_t extruder, const float &millimeters=0.0
) {
return buffer_segment(abce[A_AXIS], abce[B_AXIS], abce[C_AXIS], abce[E_AXIS]
#if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION)
, delta_mm_cart
#endif
, fr_mm_s, extruder, millimeters);
}
public:
/**
* Add a new linear movement to the buffer.
* The target is cartesian, it's translated to delta/scara if
* needed.
*
* cart - x,y,z,e CARTESIAN target in mm
*
* rx,ry,rz,e - target position in mm or degrees
* fr_mm_s - (target) speed of the move (mm/s)
* extruder - target extruder
* millimeters - the length of the movement, if known
* inv_duration - the reciprocal if the duration of the movement, if known (kinematic only if feeedrate scaling is enabled)
*/
FORCE_INLINE static bool buffer_line_kinematic(const float (&cart)[XYZE], const float &fr_mm_s, const uint8_t extruder, const float millimeters = 0.0) {
#if PLANNER_LEVELING
float raw[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] };
apply_leveling(raw);
#else
const float (&raw)[XYZE] = cart;
static bool buffer_line(const float &rx, const float &ry, const float &rz, const float &e, const float &fr_mm_s, const uint8_t extruder, const float millimeters=0.0
#if ENABLED(SCARA_FEEDRATE_SCALING)
, const float &inv_duration=0.0
#endif
#if IS_KINEMATIC
inverse_kinematics(raw);
return buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS], fr_mm_s, extruder, millimeters);
#else
return buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_AXIS], fr_mm_s, extruder, millimeters);
);
FORCE_INLINE static bool buffer_line(const float (&cart)[XYZE], const float &fr_mm_s, const uint8_t extruder, const float millimeters=0.0
#if ENABLED(SCARA_FEEDRATE_SCALING)
, const float &inv_duration=0.0
#endif
) {
return buffer_line(cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS], cart[E_AXIS], fr_mm_s, extruder, millimeters
#if ENABLED(SCARA_FEEDRATE_SCALING)
, inv_duration
#endif
);
}
/**
* Set the planner.position and individual stepper positions.
* Used by G92, G28, G29, and other procedures.
*
* The supplied position is in the cartesian coordinate space and is
* translated in to machine space as needed. Modifiers such as leveling
* and skew are also applied.
*
* Multiplies by axis_steps_per_mm[] and does necessary conversion
* for COREXY / COREXZ / COREYZ to set the corresponding stepper positions.
*
* Clears previous speed values.
*/
FORCE_INLINE static void set_position_mm(ARG_X, ARG_Y, ARG_Z, const float &e) {
#if PLANNER_LEVELING && IS_CARTESIAN
apply_leveling(rx, ry, rz);
#endif
_set_position_mm(rx, ry, rz, e);
}
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(E_AXIS, e); }
static void set_position_mm(const float &rx, const float &ry, const float &rz, const float &e);
FORCE_INLINE static void set_position_mm(const float (&cart)[XYZE]) { set_position_mm(cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS], cart[E_AXIS]); }
static void set_e_position_mm(const float &e);
/**
* Set the planner.position and individual stepper positions.
*
* The supplied position is in machine space, and no additional
* conversions are applied.
*/
static void set_machine_position_mm(const float &a, const float &b, const float &c, const float &e);
FORCE_INLINE static void set_machine_position_mm(const float (&abce)[ABCE]) { set_machine_position_mm(abce[A_AXIS], abce[B_AXIS], abce[C_AXIS], abce[E_AXIS]); }
/**
* Get an axis position according to stepper position(s)
@ -756,16 +843,14 @@ class Planner {
static void autotemp_M104_M109();
#endif
#if ENABLED(JUNCTION_DEVIATION)
#if ENABLED(JUNCTION_DEVIATION) && ENABLED(LIN_ADVANCE)
FORCE_INLINE static void recalculate_max_e_jerk() {
#define GET_MAX_E_JERK(N) SQRT(SQRT(0.5) * junction_deviation_mm * (N) * RECIPROCAL(1.0 - SQRT(0.5)))
#if ENABLED(LIN_ADVANCE)
#if ENABLED(DISTINCT_E_FACTORS)
for (uint8_t i = 0; i < EXTRUDERS; i++)
max_e_jerk[i] = GET_MAX_E_JERK(max_acceleration_mm_per_s2[E_AXIS + i]);
#else
max_e_jerk = GET_MAX_E_JERK(max_acceleration_mm_per_s2[E_AXIS]);
#endif
#if ENABLED(DISTINCT_E_FACTORS)
for (uint8_t i = 0; i < EXTRUDERS; i++)
max_e_jerk[i] = GET_MAX_E_JERK(max_acceleration_mm_per_s2[E_AXIS + i]);
#else
max_e_jerk = GET_MAX_E_JERK(max_acceleration_mm_per_s2[E_AXIS]);
#endif
}
#endif

12
Marlin/src/module/planner_bezier.cpp
Unescape Escape View File

@ -190,15 +190,15 @@ void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS]
bez_target[E_AXIS] = interp(position[E_AXIS], target[E_AXIS], t);
clamp_to_software_endstops(bez_target);
#if HAS_UBL_AND_CURVES
float pos[XYZ] = { bez_target[X_AXIS], bez_target[Y_AXIS], bez_target[Z_AXIS] };
#if HAS_LEVELING && !PLANNER_LEVELING
float pos[XYZE] = { bez_target[X_AXIS], bez_target[Y_AXIS], bez_target[Z_AXIS], bez_target[E_AXIS] };
planner.apply_leveling(pos);
if (!planner.buffer_segment(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], bez_target[E_AXIS], fr_mm_s, active_extruder))
break;
#else
if (!planner.buffer_line_kinematic(bez_target, fr_mm_s, extruder))
break;
const float (&pos)[XYZE] = bez_target;
#endif
if (!planner.buffer_line(pos, fr_mm_s, active_extruder, step))
break;
}
}

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

@ -537,7 +537,7 @@ static bool do_probe_move(const float z, const float fr_mm_s) {
set_current_from_steppers_for_axis(Z_AXIS);
// Tell the planner where we actually are
SYNC_PLAN_POSITION_KINEMATIC();
sync_plan_position();
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("<<< do_probe_move", current_position);

2
Marlin/src/module/scara.cpp
Unescape Escape View File

@ -104,7 +104,7 @@ void forward_kinematics_SCARA(const float &a, const float &b) {
* Maths and first version by QHARLEY.
* Integrated into Marlin and slightly restructured by Joachim Cerny.
*/
void inverse_kinematics(const float raw[XYZ]) {
void inverse_kinematics(const float (&raw)[XYZ]) {
static float C2, S2, SK1, SK2, THETA, PSI;

11
Marlin/src/module/scara.h
Unescape Escape View File

@ -24,8 +24,7 @@
* scara.h - SCARA-specific functions
*/
#ifndef __SCARA_H__
#define __SCARA_H__
#pragma once
#include "../core/macros.h"
@ -38,9 +37,11 @@ float constexpr L1 = SCARA_LINKAGE_1, L2 = SCARA_LINKAGE_2,
void scara_set_axis_is_at_home(const AxisEnum axis);
void inverse_kinematics(const float raw[XYZ]);
void inverse_kinematics(const float (&raw)[XYZ]);
FORCE_INLINE void inverse_kinematics(const float (&raw)[XYZE]) {
const float raw_xyz[XYZ] = { raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS] };
inverse_kinematics(raw_xyz);
}
void forward_kinematics_SCARA(const float &a, const float &b);
void scara_report_positions();
#endif // __SCARA_H__

36
Marlin/src/module/tool_change.cpp
Unescape Escape View File

@ -134,7 +134,7 @@
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("(1) Raise Z-Axis", current_position);
#endif
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[Z_AXIS], active_extruder);
planner.buffer_line(current_position, planner.max_feedrate_mm_s[Z_AXIS], active_extruder);
planner.synchronize();
// STEP 2
@ -145,7 +145,7 @@
DEBUG_POS("Moving ParkPos", current_position);
}
#endif
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder);
planner.buffer_line(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder);
planner.synchronize();
// STEP 3
@ -163,7 +163,7 @@
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("Move away from parked extruder", current_position);
#endif
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder);
planner.buffer_line(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder);
planner.synchronize();
// STEP 5
@ -178,12 +178,12 @@
// STEP 6
current_position[X_AXIS] = grabpos + (tmp_extruder ? -10 : 10);
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder);
planner.buffer_line(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder);
current_position[X_AXIS] = grabpos;
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("(6) Unpark extruder", current_position);
#endif
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[X_AXIS]/2, active_extruder);
planner.buffer_line(current_position, planner.max_feedrate_mm_s[X_AXIS]/2, active_extruder);
planner.synchronize();
// Step 7
@ -191,7 +191,7 @@
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("(7) Move midway between hotends", current_position);
#endif
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder);
planner.buffer_line(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder);
planner.synchronize();
#if ENABLED(DEBUG_LEVELING_FEATURE)
SERIAL_ECHOLNPGM("Autopark done.");
@ -241,7 +241,7 @@
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("(1) Raise Z-Axis", current_position);
#endif
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[Z_AXIS], active_extruder);
planner.buffer_line(current_position, planner.max_feedrate_mm_s[Z_AXIS], active_extruder);
planner.synchronize();
// STEP 2
@ -252,14 +252,14 @@
DEBUG_POS("Move X SwitchPos", current_position);
}
#endif
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder);
planner.buffer_line(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder);
planner.synchronize();
current_position[Y_AXIS] = SWITCHING_TOOLHEAD_Y_POS - SWITCHING_TOOLHEAD_Y_SECURITY;
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("Move Y SwitchPos + Security", current_position);
#endif
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[Y_AXIS], active_extruder);
planner.buffer_line(current_position, planner.max_feedrate_mm_s[Y_AXIS], active_extruder);
planner.synchronize();
// STEP 3
@ -273,14 +273,14 @@
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("Move Y SwitchPos", current_position);
#endif
planner.buffer_line_kinematic(current_position,(planner.max_feedrate_mm_s[Y_AXIS] * 0.5), active_extruder);
planner.buffer_line(current_position,(planner.max_feedrate_mm_s[Y_AXIS] * 0.5), active_extruder);
planner.synchronize();
safe_delay(200);
current_position[Y_AXIS] -= SWITCHING_TOOLHEAD_Y_CLEAR;
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("Move back Y clear", current_position);
#endif
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[Y_AXIS], active_extruder); // move away from docked toolhead
planner.buffer_line(current_position, planner.max_feedrate_mm_s[Y_AXIS], active_extruder); // move away from docked toolhead
planner.synchronize();
// STEP 4
@ -291,13 +291,13 @@
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("Move to new toolhead X", current_position);
#endif
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder);
planner.buffer_line(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder);
planner.synchronize();
current_position[Y_AXIS] = SWITCHING_TOOLHEAD_Y_POS - SWITCHING_TOOLHEAD_Y_SECURITY;
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("Move Y SwitchPos + Security", current_position);
#endif
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[Y_AXIS], active_extruder);
planner.buffer_line(current_position, planner.max_feedrate_mm_s[Y_AXIS], active_extruder);
planner.synchronize();
// STEP 5
@ -308,7 +308,7 @@
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("Move Y SwitchPos", current_position);
#endif
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[Y_AXIS] * 0.5, active_extruder);
planner.buffer_line(current_position, planner.max_feedrate_mm_s[Y_AXIS] * 0.5, active_extruder);
planner.synchronize();
safe_delay(200);
@ -319,7 +319,7 @@
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("Move back Y clear", current_position);
#endif
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[Y_AXIS], active_extruder); // move away from docked toolhead
planner.buffer_line(current_position, planner.max_feedrate_mm_s[Y_AXIS], active_extruder); // move away from docked toolhead
planner.synchronize();
// STEP 6
@ -524,7 +524,7 @@ void tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool n
#if ENABLED(SWITCHING_NOZZLE)
// Always raise by at least 1 to avoid workpiece
current_position[Z_AXIS] += MAX(-zdiff, 0.0) + 1;
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[Z_AXIS], active_extruder);
planner.buffer_line(current_position, planner.max_feedrate_mm_s[Z_AXIS], active_extruder);
move_nozzle_servo(tmp_extruder);
#endif
#endif
@ -549,7 +549,7 @@ void tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool n
#endif // !DUAL_X_CARRIAGE
// Tell the planner the new "current position"
SYNC_PLAN_POSITION_KINEMATIC();
sync_plan_position();
#if ENABLED(DELTA)
//LOOP_XYZ(i) update_software_endstops(i); // or modify the constrain function
@ -563,7 +563,7 @@ void tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool n
#if DISABLED(SWITCHING_NOZZLE)
// Do a small lift to avoid the workpiece in the move back (below)
current_position[Z_AXIS] += 1.0;
planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[Z_AXIS], active_extruder);
planner.buffer_line(current_position, planner.max_feedrate_mm_s[Z_AXIS], active_extruder);
#endif
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("Move back", destination);

Write Preview
Loading…
Cancel
Save