bissen21/Marlin_Firmware
1
0
Fork 0
Code Issues Pull Requests Projects Releases 4 Wiki 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
This commit is contained in:
Thomas Moore
2018-09-16 22:24:15 -04:00
committed by Scott Lahteine
parent 8323a08642
commit c437bb08f1
39 changed files with 655 additions and 597 deletions
Download Patch File Download Diff File Expand all files Collapse all files

215
Marlin/src/module/motion.cpp
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();
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);
#else
// Tell the planner the axis is at 0
current_position[axis] = 0;
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);
current_position[axis] = distance;
planner.buffer_line(current_position, fr_mm_s ? fr_mm_s : homing_feedrate(axis), active_extruder);
#else
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)