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

Merge pull request #4982 from thinkyhead/rc_abl_bugfix

Browse Source 
Fix planner with kinematics, delta ABL
This commit is contained in:
Scott Lahteine
2016-10-10 13:24:22 -05:00
committed by GitHub
parent f4ec265363 e4e290d957
commit f8199b2cc1
15 changed files with 263 additions and 201 deletions
Download Patch File Download Diff File Expand all files Collapse all files

103
Marlin/Marlin_main.cpp
View File

@@ -456,6 +456,18 @@ static uint8_t target_extruder;
#define XY_PROBE_FEEDRATE_MM_S PLANNER_XY_FEEDRATE()
#endif
#if ENABLED(AUTO_BED_LEVELING_BILINEAR)
#define ADJUST_DELTA(V) \
if (planner.abl_enabled) { \
const float zadj = bilinear_z_offset(V); \
delta[A_AXIS] += zadj; \
delta[B_AXIS] += zadj; \
delta[C_AXIS] += zadj; \
}
#elif IS_KINEMATIC
#define ADJUST_DELTA(V) NOOP
#endif
#if ENABLED(Z_DUAL_ENDSTOPS)
float z_endstop_adj = 0;
#endif
@@ -711,8 +723,7 @@ inline void sync_plan_position_e() { planner.set_e_position_mm(current_position[
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position_kinematic", current_position);
#endif
inverse_kinematics(current_position);
planner.set_position_mm(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], current_position[E_AXIS]);
planner.set_position_mm_kinematic(current_position);
}
#define SYNC_PLAN_POSITION_KINEMATIC() sync_plan_position_kinematic()
@@ -1541,8 +1552,7 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
) return;
refresh_cmd_timeout();
inverse_kinematics(destination);
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], destination[E_AXIS], MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s), active_extruder);
planner.buffer_line_kinematic(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s), active_extruder);
set_current_to_destination();
}
#endif // IS_KINEMATIC
@@ -6778,8 +6788,7 @@ inline void gcode_M503() {
// Define runplan for move axes
#if IS_KINEMATIC
#define RUNPLAN(RATE_MM_S) inverse_kinematics(destination); \
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], destination[E_AXIS], RATE_MM_S, active_extruder);
#define RUNPLAN(RATE_MM_S) planner.buffer_line_kinematic(destination, RATE_MM_S, active_extruder);
#else
#define RUNPLAN(RATE_MM_S) line_to_destination(RATE_MM_S);
#endif
@@ -6894,17 +6903,14 @@ inline void gcode_M503() {
KEEPALIVE_STATE(IN_HANDLER);
// Set extruder to saved position
current_position[E_AXIS] = lastpos[E_AXIS];
destination[E_AXIS] = lastpos[E_AXIS];
destination[E_AXIS] = current_position[E_AXIS] = lastpos[E_AXIS];
planner.set_e_position_mm(current_position[E_AXIS]);
#if IS_KINEMATIC
// Move XYZ to starting position, then E
inverse_kinematics(lastpos);
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], destination[E_AXIS], FILAMENT_CHANGE_XY_FEEDRATE, active_extruder);
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], lastpos[E_AXIS], FILAMENT_CHANGE_XY_FEEDRATE, active_extruder);
// Move XYZ to starting position
planner.buffer_line_kinematic(lastpos, FILAMENT_CHANGE_XY_FEEDRATE, active_extruder);
#else
// Move XY to starting position, then Z, then E
// Move XY to starting position, then Z
destination[X_AXIS] = lastpos[X_AXIS];
destination[Y_AXIS] = lastpos[Y_AXIS];
RUNPLAN(FILAMENT_CHANGE_XY_FEEDRATE);
@@ -7295,15 +7301,11 @@ void tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool n
float z_diff = hotend_offset[Z_AXIS][active_extruder] - hotend_offset[Z_AXIS][tmp_extruder],
z_raise = 0.3 + (z_diff > 0.0 ? z_diff : 0.0);
set_destination_to_current();
// Always raise by some amount
planner.buffer_line(
current_position[X_AXIS],
current_position[Y_AXIS],
current_position[Z_AXIS] + z_raise,
current_position[E_AXIS],
planner.max_feedrate_mm_s[Z_AXIS],
active_extruder
);
destination[Z_AXIS] += z_raise;
planner.buffer_line_kinematic(destination, planner.max_feedrate_mm_s[Z_AXIS], active_extruder);
stepper.synchronize();
move_extruder_servo(active_extruder);
@@ -7311,14 +7313,8 @@ void tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool n
// Move back down, if needed
if (z_raise != z_diff) {
planner.buffer_line(
current_position[X_AXIS],
current_position[Y_AXIS],
current_position[Z_AXIS] + z_diff,
current_position[E_AXIS],
planner.max_feedrate_mm_s[Z_AXIS],
active_extruder
);
destination[Z_AXIS] = current_position[Z_AXIS] + z_diff;
planner.buffer_line_kinematic(destination, planner.max_feedrate_mm_s[Z_AXIS], active_extruder);
stepper.synchronize();
}
#endif
@@ -8670,8 +8666,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
// If the move is only in Z/E don't split up the move
if (ltarget[X_AXIS] == current_position[X_AXIS] && ltarget[Y_AXIS] == current_position[Y_AXIS]) {
inverse_kinematics(ltarget);
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], ltarget[E_AXIS], _feedrate_mm_s, active_extruder);
planner.buffer_line_kinematic(ltarget, _feedrate_mm_s, active_extruder);
return true;
}
@@ -8764,7 +8759,10 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
#define DELTA_NEXT(ADDEND) LOOP_XYZ(i) DELTA_VAR[i] += ADDEND;
// Get the starting delta if interpolation is possible
if (segments >= 2) DELTA_IK();
if (segments >= 2) {
DELTA_IK();
ADJUST_DELTA(DELTA_VAR); // Adjust Z if bed leveling is enabled
}
// Loop using decrement
for (uint16_t s = segments + 1; --s;) {
@@ -8781,6 +8779,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
// Get the exact delta for the move after this
DELTA_IK();
ADJUST_DELTA(DELTA_VAR); // Adjust Z if bed leveling is enabled
// Move to the interpolated delta position first
planner.buffer_line(
@@ -8801,6 +8800,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
DELTA_NEXT(segment_distance[i]);
DELTA_VAR[E_AXIS] += segment_distance[E_AXIS];
DELTA_IK();
ADJUST_DELTA(DELTA_VAR); // Adjust Z if bed leveling is enabled
}
// Move to the non-interpolated position
@@ -8815,6 +8815,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
for (uint16_t s = segments + 1; --s;) {
DELTA_NEXT(segment_distance[i]);
DELTA_IK();
ADJUST_DELTA(DELTA_VAR);
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], DELTA_VAR[E_AXIS], _feedrate_mm_s, active_extruder);
}
@@ -8822,8 +8823,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
// Since segment_distance is only approximate,
// the final move must be to the exact destination.
inverse_kinematics(ltarget);
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], ltarget[E_AXIS], _feedrate_mm_s, active_extruder);
planner.buffer_line_kinematic(ltarget, _feedrate_mm_s, active_extruder);
return true;
}
@@ -9063,21 +9063,11 @@ void prepare_move_to_destination() {
clamp_to_software_endstops(arc_target);
#if IS_KINEMATIC
inverse_kinematics(arc_target);
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], arc_target[E_AXIS], fr_mm_s, active_extruder);
#else
planner.buffer_line(arc_target[X_AXIS], arc_target[Y_AXIS], arc_target[Z_AXIS], arc_target[E_AXIS], fr_mm_s, active_extruder);
#endif
planner.buffer_line_kinematic(arc_target, fr_mm_s, active_extruder);
}
// Ensure last segment arrives at target location.
#if IS_KINEMATIC
inverse_kinematics(logical);
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], fr_mm_s, active_extruder);
#else
planner.buffer_line(logical[X_AXIS], logical[Y_AXIS], logical[Z_AXIS], logical[E_AXIS], fr_mm_s, active_extruder);
#endif
planner.buffer_line_kinematic(logical, fr_mm_s, active_extruder);
// As far as the parser is concerned, the position is now == target. In reality the
// motion control system might still be processing the action and the real tool position
@@ -9472,11 +9462,22 @@ void manage_inactivity(bool ignore_stepper_queue/*=false*/) {
#endif // !SWITCHING_EXTRUDER
previous_cmd_ms = ms; // refresh_cmd_timeout()
planner.buffer_line(
current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS],
current_position[E_AXIS] + EXTRUDER_RUNOUT_EXTRUDE,
MMM_TO_MMS(EXTRUDER_RUNOUT_SPEED), active_extruder
);
#if IS_KINEMATIC
inverse_kinematics(current_position);
ADJUST_DELTA(current_position);
planner.buffer_line(
delta[A_AXIS], delta[B_AXIS], delta[C_AXIS],
current_position[E_AXIS] + EXTRUDER_RUNOUT_EXTRUDE,
MMM_TO_MMS(EXTRUDER_RUNOUT_SPEED), active_extruder
);
#else
planner.buffer_line(
current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS],
current_position[E_AXIS] + EXTRUDER_RUNOUT_EXTRUDE,
MMM_TO_MMS(EXTRUDER_RUNOUT_SPEED), active_extruder
);
#endif
stepper.synchronize();
planner.set_e_position_mm(current_position[E_AXIS]);
#if ENABLED(SWITCHING_EXTRUDER)