0x90/Marlin
12
0
Fork 0
Code Issues Pull requests Packages Projects Releases 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
Show stats Download patch file Download diff file Expand all files Collapse all files

4
Marlin/Conditionals_post.h
View file

@ -706,4 +706,8 @@
// Stepper pulse duration, in cycles // Stepper pulse duration, in cycles
#define STEP_PULSE_CYCLES ((MINIMUM_STEPPER_PULSE) * CYCLES_PER_MICROSECOND) #define STEP_PULSE_CYCLES ((MINIMUM_STEPPER_PULSE) * CYCLES_PER_MICROSECOND)
#ifndef DELTA_ENDSTOP_ADJ
#define DELTA_ENDSTOP_ADJ { 0 }
#endif
#endif // CONDITIONALS_POST_H #endif // CONDITIONALS_POST_H

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() #define XY_PROBE_FEEDRATE_MM_S PLANNER_XY_FEEDRATE()
#endif #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) #if ENABLED(Z_DUAL_ENDSTOPS)
float z_endstop_adj = 0; float z_endstop_adj = 0;
#endif #endif
@ -711,8 +723,7 @@ inline void sync_plan_position_e() { planner.set_e_position_mm(current_position[
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position_kinematic", current_position); if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position_kinematic", current_position);
#endif #endif
inverse_kinematics(current_position); planner.set_position_mm_kinematic(current_position);
planner.set_position_mm(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], current_position[E_AXIS]);
} }
#define SYNC_PLAN_POSITION_KINEMATIC() sync_plan_position_kinematic() #define SYNC_PLAN_POSITION_KINEMATIC() sync_plan_position_kinematic()
@ -1541,8 +1552,7 @@ inline void set_destination_to_current() { memcpy(destination, current_position,
) return; ) return;
refresh_cmd_timeout(); refresh_cmd_timeout();
inverse_kinematics(destination); planner.buffer_line_kinematic(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s), active_extruder);
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);
set_current_to_destination(); set_current_to_destination();
} }
#endif // IS_KINEMATIC #endif // IS_KINEMATIC
@ -6778,8 +6788,7 @@ inline void gcode_M503() {
// Define runplan for move axes // Define runplan for move axes
#if IS_KINEMATIC #if IS_KINEMATIC
#define RUNPLAN(RATE_MM_S) inverse_kinematics(destination); \ #define RUNPLAN(RATE_MM_S) planner.buffer_line_kinematic(destination, RATE_MM_S, active_extruder);
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], destination[E_AXIS], RATE_MM_S, active_extruder);
#else #else
#define RUNPLAN(RATE_MM_S) line_to_destination(RATE_MM_S); #define RUNPLAN(RATE_MM_S) line_to_destination(RATE_MM_S);
#endif #endif
@ -6894,17 +6903,14 @@ inline void gcode_M503() {
KEEPALIVE_STATE(IN_HANDLER); KEEPALIVE_STATE(IN_HANDLER);
// Set extruder to saved position // Set extruder to saved position
current_position[E_AXIS] = lastpos[E_AXIS]; destination[E_AXIS] = current_position[E_AXIS] = lastpos[E_AXIS];
destination[E_AXIS] = lastpos[E_AXIS];
planner.set_e_position_mm(current_position[E_AXIS]); planner.set_e_position_mm(current_position[E_AXIS]);
#if IS_KINEMATIC #if IS_KINEMATIC
// Move XYZ to starting position, then E // Move XYZ to starting position
inverse_kinematics(lastpos); planner.buffer_line_kinematic(lastpos, FILAMENT_CHANGE_XY_FEEDRATE, active_extruder);
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);
#else #else
// Move XY to starting position, then Z, then E // Move XY to starting position, then Z
destination[X_AXIS] = lastpos[X_AXIS]; destination[X_AXIS] = lastpos[X_AXIS];
destination[Y_AXIS] = lastpos[Y_AXIS]; destination[Y_AXIS] = lastpos[Y_AXIS];
RUNPLAN(FILAMENT_CHANGE_XY_FEEDRATE); 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], 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); z_raise = 0.3 + (z_diff > 0.0 ? z_diff : 0.0);
set_destination_to_current();
// Always raise by some amount // Always raise by some amount
planner.buffer_line( destination[Z_AXIS] += z_raise;
current_position[X_AXIS], planner.buffer_line_kinematic(destination, planner.max_feedrate_mm_s[Z_AXIS], active_extruder);
current_position[Y_AXIS],
current_position[Z_AXIS] + z_raise,
current_position[E_AXIS],
planner.max_feedrate_mm_s[Z_AXIS],
active_extruder
);
stepper.synchronize(); stepper.synchronize();
move_extruder_servo(active_extruder); 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 // Move back down, if needed
if (z_raise != z_diff) { if (z_raise != z_diff) {
planner.buffer_line( destination[Z_AXIS] = current_position[Z_AXIS] + z_diff;
current_position[X_AXIS], planner.buffer_line_kinematic(destination, planner.max_feedrate_mm_s[Z_AXIS], active_extruder);
current_position[Y_AXIS],
current_position[Z_AXIS] + z_diff,
current_position[E_AXIS],
planner.max_feedrate_mm_s[Z_AXIS],
active_extruder
);
stepper.synchronize(); stepper.synchronize();
} }
#endif #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 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]) { if (ltarget[X_AXIS] == current_position[X_AXIS] && ltarget[Y_AXIS] == current_position[Y_AXIS]) {
inverse_kinematics(ltarget); planner.buffer_line_kinematic(ltarget, _feedrate_mm_s, active_extruder);
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], ltarget[E_AXIS], _feedrate_mm_s, active_extruder);
return true; 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; #define DELTA_NEXT(ADDEND) LOOP_XYZ(i) DELTA_VAR[i] += ADDEND;
// Get the starting delta if interpolation is possible // 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 // Loop using decrement
for (uint16_t s = segments + 1; --s;) { 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 // Get the exact delta for the move after this
DELTA_IK(); DELTA_IK();
ADJUST_DELTA(DELTA_VAR); // Adjust Z if bed leveling is enabled
// Move to the interpolated delta position first // Move to the interpolated delta position first
planner.buffer_line( planner.buffer_line(
@ -8801,6 +8800,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
DELTA_NEXT(segment_distance[i]); DELTA_NEXT(segment_distance[i]);
DELTA_VAR[E_AXIS] += segment_distance[E_AXIS]; DELTA_VAR[E_AXIS] += segment_distance[E_AXIS];
DELTA_IK(); DELTA_IK();
ADJUST_DELTA(DELTA_VAR); // Adjust Z if bed leveling is enabled
} }
// Move to the non-interpolated position // 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;) { for (uint16_t s = segments + 1; --s;) {
DELTA_NEXT(segment_distance[i]); DELTA_NEXT(segment_distance[i]);
DELTA_IK(); 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); 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, // Since segment_distance is only approximate,
// the final move must be to the exact destination. // the final move must be to the exact destination.
inverse_kinematics(ltarget); planner.buffer_line_kinematic(ltarget, _feedrate_mm_s, active_extruder);
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], ltarget[E_AXIS], _feedrate_mm_s, active_extruder);
return true; return true;
} }
@ -9063,21 +9063,11 @@ void prepare_move_to_destination() {
clamp_to_software_endstops(arc_target); clamp_to_software_endstops(arc_target);
#if IS_KINEMATIC planner.buffer_line_kinematic(arc_target, fr_mm_s, active_extruder);
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
} }
// Ensure last segment arrives at target location. // Ensure last segment arrives at target location.
#if IS_KINEMATIC planner.buffer_line_kinematic(logical, fr_mm_s, active_extruder);
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
// As far as the parser is concerned, the position is now == target. In reality the // 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 // 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 #endif // !SWITCHING_EXTRUDER
previous_cmd_ms = ms; // refresh_cmd_timeout() previous_cmd_ms = ms; // refresh_cmd_timeout()
planner.buffer_line(
current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], #if IS_KINEMATIC
current_position[E_AXIS] + EXTRUDER_RUNOUT_EXTRUDE, inverse_kinematics(current_position);
MMM_TO_MMS(EXTRUDER_RUNOUT_SPEED), active_extruder 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(); stepper.synchronize();
planner.set_e_position_mm(current_position[E_AXIS]); planner.set_e_position_mm(current_position[E_AXIS]);
#if ENABLED(SWITCHING_EXTRUDER) #if ENABLED(SWITCHING_EXTRUDER)

4
Marlin/SanityCheck.h
View file

@ -518,6 +518,10 @@
*/ */
#if HAS_ABL #if HAS_ABL
#if ENABLED(USE_RAW_KINEMATICS)
#error "USE_RAW_KINEMATICS is not compatible with AUTO_BED_LEVELING"
#endif
/** /**
* Delta and SCARA have limited bed leveling options * Delta and SCARA have limited bed leveling options
*/ */

5
Marlin/configuration_store.cpp
View file

@ -589,7 +589,10 @@ void Config_ResetDefault() {
#endif #endif
#if ENABLED(DELTA) #if ENABLED(DELTA)
endstop_adj[X_AXIS] = endstop_adj[Y_AXIS] = endstop_adj[Z_AXIS] = 0; const float adj[ABC] = DELTA_ENDSTOP_ADJ;
endstop_adj[A_AXIS] = adj[A_AXIS];
endstop_adj[B_AXIS] = adj[B_AXIS];
endstop_adj[C_AXIS] = adj[C_AXIS];
delta_radius = DELTA_RADIUS; delta_radius = DELTA_RADIUS;
delta_diagonal_rod = DELTA_DIAGONAL_ROD; delta_diagonal_rod = DELTA_DIAGONAL_ROD;
delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND; delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND;

2
Marlin/example_configurations/delta/biv2.5/Configuration.h
View file

@ -441,6 +441,8 @@
// in ultralcd.cpp@lcd_delta_calibrate_menu() // in ultralcd.cpp@lcd_delta_calibrate_menu()
//#define DELTA_CALIBRATION_MENU //#define DELTA_CALIBRATION_MENU
//#define DELTA_ENDSTOP_ADJ { 0, 0, 0 }
#endif #endif
// Enable this option for Toshiba steppers // Enable this option for Toshiba steppers

2
Marlin/example_configurations/delta/generic/Configuration.h
View file

@ -441,6 +441,8 @@
// in ultralcd.cpp@lcd_delta_calibrate_menu() // in ultralcd.cpp@lcd_delta_calibrate_menu()
//#define DELTA_CALIBRATION_MENU //#define DELTA_CALIBRATION_MENU
//#define DELTA_ENDSTOP_ADJ { 0, 0, 0 }
#endif #endif
// Enable this option for Toshiba steppers // Enable this option for Toshiba steppers

2
Marlin/example_configurations/delta/kossel_mini/Configuration.h
View file

@ -441,6 +441,8 @@
// in ultralcd.cpp@lcd_delta_calibrate_menu() // in ultralcd.cpp@lcd_delta_calibrate_menu()
//#define DELTA_CALIBRATION_MENU //#define DELTA_CALIBRATION_MENU
//#define DELTA_ENDSTOP_ADJ { 0, 0, 0 }
#endif #endif
// Enable this option for Toshiba steppers // Enable this option for Toshiba steppers

2
Marlin/example_configurations/delta/kossel_pro/Configuration.h
View file

@ -430,6 +430,8 @@
// in ultralcd.cpp@lcd_delta_calibrate_menu() // in ultralcd.cpp@lcd_delta_calibrate_menu()
//#define DELTA_CALIBRATION_MENU //#define DELTA_CALIBRATION_MENU
//#define DELTA_ENDSTOP_ADJ { 0, 0, 0 }
#endif #endif
// Enable this option for Toshiba steppers // Enable this option for Toshiba steppers

2
Marlin/example_configurations/delta/kossel_xl/Configuration.h
View file

@ -439,6 +439,8 @@
// in ultralcd.cpp@lcd_delta_calibrate_menu() // in ultralcd.cpp@lcd_delta_calibrate_menu()
//#define DELTA_CALIBRATION_MENU //#define DELTA_CALIBRATION_MENU
//#define DELTA_ENDSTOP_ADJ { 0, 0, 0 }
#endif #endif
// Enable this option for Toshiba steppers // Enable this option for Toshiba steppers

209
Marlin/planner.cpp
View file

@ -522,7 +522,9 @@ void Planner::check_axes_activity() {
} }
#if PLANNER_LEVELING #if PLANNER_LEVELING
/**
* lx, ly, lz - logical (cartesian, not delta) positions in mm
*/
void Planner::apply_leveling(float &lx, float &ly, float &lz) { void Planner::apply_leveling(float &lx, float &ly, float &lz) {
#if HAS_ABL #if HAS_ABL
@ -549,19 +551,7 @@ void Planner::check_axes_activity() {
#elif ENABLED(AUTO_BED_LEVELING_BILINEAR) #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
float tmp[XYZ] = { lx, ly, 0 }; float tmp[XYZ] = { lx, ly, 0 };
lz += bilinear_z_offset(tmp);
#if ENABLED(DELTA)
float offset = bilinear_z_offset(tmp);
lx += offset;
ly += offset;
lz += offset;
#else
lz += bilinear_z_offset(tmp);
#endif
#endif #endif
} }
@ -601,15 +591,17 @@ void Planner::check_axes_activity() {
#endif // PLANNER_LEVELING #endif // PLANNER_LEVELING
/** /**
* Planner::buffer_line * Planner::_buffer_line
* *
* Add a new linear movement to the buffer. * Add a new linear movement to the buffer.
* *
* x,y,z,e - target position in mm * Leveling and kinematics should be applied ahead of calling this.
* fr_mm_s - (target) speed of the move *
* extruder - target extruder * a,b,c,e - target positions in mm or degrees
* fr_mm_s - (target) speed of the move
* extruder - target extruder
*/ */
void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, const uint8_t extruder) { void Planner::_buffer_line(const float &a, const float &b, const float &c, const float &e, float fr_mm_s, const uint8_t extruder) {
// Calculate the buffer head after we push this byte // Calculate the buffer head after we push this byte
int next_buffer_head = next_block_index(block_buffer_head); int next_buffer_head = next_block_index(block_buffer_head);
@ -617,43 +609,39 @@ void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, co
// Rest here until there is room in the buffer. // Rest here until there is room in the buffer.
while (block_buffer_tail == next_buffer_head) idle(); while (block_buffer_tail == next_buffer_head) idle();
#if PLANNER_LEVELING
apply_leveling(lx, ly, lz);
#endif
// The target position of the tool in absolute steps // The target position of the tool in absolute steps
// Calculate target position in absolute steps // Calculate target position in absolute steps
//this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow //this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
long target[NUM_AXIS] = { long target[XYZE] = {
lround(lx * axis_steps_per_mm[X_AXIS]), lround(a * axis_steps_per_mm[X_AXIS]),
lround(ly * axis_steps_per_mm[Y_AXIS]), lround(b * axis_steps_per_mm[Y_AXIS]),
lround(lz * axis_steps_per_mm[Z_AXIS]), lround(c * axis_steps_per_mm[Z_AXIS]),
lround(e * axis_steps_per_mm[E_AXIS]) lround(e * axis_steps_per_mm[E_AXIS])
}; };
long dx = target[X_AXIS] - position[X_AXIS], long da = target[X_AXIS] - position[X_AXIS],
dy = target[Y_AXIS] - position[Y_AXIS], db = target[Y_AXIS] - position[Y_AXIS],
dz = target[Z_AXIS] - position[Z_AXIS]; dc = target[Z_AXIS] - position[Z_AXIS];
/* /*
SERIAL_ECHOPAIR(" Planner FR:", fr_mm_s); SERIAL_ECHOPAIR(" Planner FR:", fr_mm_s);
SERIAL_CHAR(' '); SERIAL_CHAR(' ');
#if IS_KINEMATIC #if IS_KINEMATIC
SERIAL_ECHOPAIR("A:", lx); SERIAL_ECHOPAIR("A:", a);
SERIAL_ECHOPAIR(" (", dx); SERIAL_ECHOPAIR(" (", da);
SERIAL_ECHOPAIR(") B:", ly); SERIAL_ECHOPAIR(") B:", b);
#else #else
SERIAL_ECHOPAIR("X:", lx); SERIAL_ECHOPAIR("X:", a);
SERIAL_ECHOPAIR(" (", dx); SERIAL_ECHOPAIR(" (", da);
SERIAL_ECHOPAIR(") Y:", ly); SERIAL_ECHOPAIR(") Y:", b);
#endif #endif
SERIAL_ECHOPAIR(" (", dy); SERIAL_ECHOPAIR(" (", db);
#if ENABLED(DELTA) #if ENABLED(DELTA)
SERIAL_ECHOPAIR(") C:", lz); SERIAL_ECHOPAIR(") C:", c);
#else #else
SERIAL_ECHOPAIR(") Z:", lz); SERIAL_ECHOPAIR(") Z:", c);
#endif #endif
SERIAL_ECHOPAIR(" (", dz); SERIAL_ECHOPAIR(" (", dc);
SERIAL_CHAR(')'); SERIAL_CHAR(')');
SERIAL_EOL; SERIAL_EOL;
//*/ //*/
@ -692,24 +680,24 @@ void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, co
#if ENABLED(COREXY) #if ENABLED(COREXY)
// corexy planning // corexy planning
// these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html // these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
block->steps[A_AXIS] = labs(dx + dy); block->steps[A_AXIS] = labs(da + db);
block->steps[B_AXIS] = labs(dx - dy); block->steps[B_AXIS] = labs(da - db);
block->steps[Z_AXIS] = labs(dz); block->steps[Z_AXIS] = labs(dc);
#elif ENABLED(COREXZ) #elif ENABLED(COREXZ)
// corexz planning // corexz planning
block->steps[A_AXIS] = labs(dx + dz); block->steps[A_AXIS] = labs(da + dc);
block->steps[Y_AXIS] = labs(dy); block->steps[Y_AXIS] = labs(db);
block->steps[C_AXIS] = labs(dx - dz); block->steps[C_AXIS] = labs(da - dc);
#elif ENABLED(COREYZ) #elif ENABLED(COREYZ)
// coreyz planning // coreyz planning
block->steps[X_AXIS] = labs(dx); block->steps[X_AXIS] = labs(da);
block->steps[B_AXIS] = labs(dy + dz); block->steps[B_AXIS] = labs(db + dc);
block->steps[C_AXIS] = labs(dy - dz); block->steps[C_AXIS] = labs(db - dc);
#else #else
// default non-h-bot planning // default non-h-bot planning
block->steps[X_AXIS] = labs(dx); block->steps[X_AXIS] = labs(da);
block->steps[Y_AXIS] = labs(dy); block->steps[Y_AXIS] = labs(db);
block->steps[Z_AXIS] = labs(dz); block->steps[Z_AXIS] = labs(dc);
#endif #endif
block->steps[E_AXIS] = labs(de) * volumetric_multiplier[extruder] * flow_percentage[extruder] * 0.01 + 0.5; block->steps[E_AXIS] = labs(de) * volumetric_multiplier[extruder] * flow_percentage[extruder] * 0.01 + 0.5;
@ -733,33 +721,33 @@ void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, co
block->e_to_p_pressure = baricuda_e_to_p_pressure; block->e_to_p_pressure = baricuda_e_to_p_pressure;
#endif #endif
// Compute direction bits for this block // Compute direction bit-mask for this block
uint8_t db = 0; uint8_t dm = 0;
#if ENABLED(COREXY) #if ENABLED(COREXY)
if (dx < 0) SBI(db, X_HEAD); // Save the real Extruder (head) direction in X Axis if (da < 0) SBI(dm, X_HEAD); // Save the real Extruder (head) direction in X Axis
if (dy < 0) SBI(db, Y_HEAD); // ...and Y if (db < 0) SBI(dm, Y_HEAD); // ...and Y
if (dz < 0) SBI(db, Z_AXIS); if (dc < 0) SBI(dm, Z_AXIS);
if (dx + dy < 0) SBI(db, A_AXIS); // Motor A direction if (da + db < 0) SBI(dm, A_AXIS); // Motor A direction
if (dx - dy < 0) SBI(db, B_AXIS); // Motor B direction if (da - db < 0) SBI(dm, B_AXIS); // Motor B direction
#elif ENABLED(COREXZ) #elif ENABLED(COREXZ)
if (dx < 0) SBI(db, X_HEAD); // Save the real Extruder (head) direction in X Axis if (da < 0) SBI(dm, X_HEAD); // Save the real Extruder (head) direction in X Axis
if (dy < 0) SBI(db, Y_AXIS); if (db < 0) SBI(dm, Y_AXIS);
if (dz < 0) SBI(db, Z_HEAD); // ...and Z if (dc < 0) SBI(dm, Z_HEAD); // ...and Z
if (dx + dz < 0) SBI(db, A_AXIS); // Motor A direction if (da + dc < 0) SBI(dm, A_AXIS); // Motor A direction
if (dx - dz < 0) SBI(db, C_AXIS); // Motor C direction if (da - dc < 0) SBI(dm, C_AXIS); // Motor C direction
#elif ENABLED(COREYZ) #elif ENABLED(COREYZ)
if (dx < 0) SBI(db, X_AXIS); if (da < 0) SBI(dm, X_AXIS);
if (dy < 0) SBI(db, Y_HEAD); // Save the real Extruder (head) direction in Y Axis if (db < 0) SBI(dm, Y_HEAD); // Save the real Extruder (head) direction in Y Axis
if (dz < 0) SBI(db, Z_HEAD); // ...and Z if (dc < 0) SBI(dm, Z_HEAD); // ...and Z
if (dy + dz < 0) SBI(db, B_AXIS); // Motor B direction if (db + dc < 0) SBI(dm, B_AXIS); // Motor B direction
if (dy - dz < 0) SBI(db, C_AXIS); // Motor C direction if (db - dc < 0) SBI(dm, C_AXIS); // Motor C direction
#else #else
if (dx < 0) SBI(db, X_AXIS); if (da < 0) SBI(dm, X_AXIS);
if (dy < 0) SBI(db, Y_AXIS); if (db < 0) SBI(dm, Y_AXIS);
if (dz < 0) SBI(db, Z_AXIS); if (dc < 0) SBI(dm, Z_AXIS);
#endif #endif
if (de < 0) SBI(db, E_AXIS); if (de < 0) SBI(dm, E_AXIS);
block->direction_bits = db; block->direction_bits = dm;
block->active_extruder = extruder; block->active_extruder = extruder;
@ -872,29 +860,29 @@ void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, co
#if ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ) #if ENABLED(COREXY) || ENABLED(COREXZ) || ENABLED(COREYZ)
float delta_mm[7]; float delta_mm[7];
#if ENABLED(COREXY) #if ENABLED(COREXY)
delta_mm[X_HEAD] = dx * steps_to_mm[A_AXIS]; delta_mm[X_HEAD] = da * steps_to_mm[A_AXIS];
delta_mm[Y_HEAD] = dy * steps_to_mm[B_AXIS]; delta_mm[Y_HEAD] = db * steps_to_mm[B_AXIS];
delta_mm[Z_AXIS] = dz * steps_to_mm[Z_AXIS]; delta_mm[Z_AXIS] = dc * steps_to_mm[Z_AXIS];
delta_mm[A_AXIS] = (dx + dy) * steps_to_mm[A_AXIS]; delta_mm[A_AXIS] = (da + db) * steps_to_mm[A_AXIS];
delta_mm[B_AXIS] = (dx - dy) * steps_to_mm[B_AXIS]; delta_mm[B_AXIS] = (da - db) * steps_to_mm[B_AXIS];
#elif ENABLED(COREXZ) #elif ENABLED(COREXZ)
delta_mm[X_HEAD] = dx * steps_to_mm[A_AXIS]; delta_mm[X_HEAD] = da * steps_to_mm[A_AXIS];
delta_mm[Y_AXIS] = dy * steps_to_mm[Y_AXIS]; delta_mm[Y_AXIS] = db * steps_to_mm[Y_AXIS];
delta_mm[Z_HEAD] = dz * steps_to_mm[C_AXIS]; delta_mm[Z_HEAD] = dc * steps_to_mm[C_AXIS];
delta_mm[A_AXIS] = (dx + dz) * steps_to_mm[A_AXIS]; delta_mm[A_AXIS] = (da + dc) * steps_to_mm[A_AXIS];
delta_mm[C_AXIS] = (dx - dz) * steps_to_mm[C_AXIS]; delta_mm[C_AXIS] = (da - dc) * steps_to_mm[C_AXIS];
#elif ENABLED(COREYZ) #elif ENABLED(COREYZ)
delta_mm[X_AXIS] = dx * steps_to_mm[X_AXIS]; delta_mm[X_AXIS] = da * steps_to_mm[X_AXIS];
delta_mm[Y_HEAD] = dy * steps_to_mm[B_AXIS]; delta_mm[Y_HEAD] = db * steps_to_mm[B_AXIS];
delta_mm[Z_HEAD] = dz * steps_to_mm[C_AXIS]; delta_mm[Z_HEAD] = dc * steps_to_mm[C_AXIS];
delta_mm[B_AXIS] = (dy + dz) * steps_to_mm[B_AXIS]; delta_mm[B_AXIS] = (db + dc) * steps_to_mm[B_AXIS];
delta_mm[C_AXIS] = (dy - dz) * steps_to_mm[C_AXIS]; delta_mm[C_AXIS] = (db - dc) * steps_to_mm[C_AXIS];
#endif #endif
#else #else
float delta_mm[4]; float delta_mm[4];
delta_mm[X_AXIS] = dx * steps_to_mm[X_AXIS]; delta_mm[X_AXIS] = da * steps_to_mm[X_AXIS];
delta_mm[Y_AXIS] = dy * steps_to_mm[Y_AXIS]; delta_mm[Y_AXIS] = db * steps_to_mm[Y_AXIS];
delta_mm[Z_AXIS] = dz * steps_to_mm[Z_AXIS]; delta_mm[Z_AXIS] = dc * steps_to_mm[Z_AXIS];
#endif #endif
delta_mm[E_AXIS] = 0.01 * (de * steps_to_mm[E_AXIS]) * volumetric_multiplier[extruder] * flow_percentage[extruder]; delta_mm[E_AXIS] = 0.01 * (de * steps_to_mm[E_AXIS]) * volumetric_multiplier[extruder] * flow_percentage[extruder];
@ -1196,22 +1184,34 @@ void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, co
* *
* On CORE machines stepper ABC will be translated from the given XYZ. * On CORE machines stepper ABC will be translated from the given XYZ.
*/ */
void Planner::set_position_mm(ARG_X, ARG_Y, ARG_Z, const float &e) {
#if PLANNER_LEVELING void Planner::_set_position_mm(const float &a, const float &b, const float &c, const float &e) {
apply_leveling(lx, ly, lz); long na = position[X_AXIS] = lround(a * axis_steps_per_mm[X_AXIS]),
#endif nb = position[Y_AXIS] = lround(b * axis_steps_per_mm[Y_AXIS]),
nc = position[Z_AXIS] = lround(c * axis_steps_per_mm[Z_AXIS]),
long nx = position[X_AXIS] = lround(lx * axis_steps_per_mm[X_AXIS]),
ny = position[Y_AXIS] = lround(ly * axis_steps_per_mm[Y_AXIS]),
nz = position[Z_AXIS] = lround(lz * axis_steps_per_mm[Z_AXIS]),
ne = position[E_AXIS] = lround(e * axis_steps_per_mm[E_AXIS]); ne = position[E_AXIS] = lround(e * axis_steps_per_mm[E_AXIS]);
stepper.set_position(nx, ny, nz, ne); stepper.set_position(na, nb, nc, ne);
previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest. previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
memset(previous_speed, 0, sizeof(previous_speed)); memset(previous_speed, 0, sizeof(previous_speed));
} }
void Planner::set_position_mm_kinematic(const float position[NUM_AXIS]) {
#if PLANNER_LEVELING
float pos[XYZ] = { position[X_AXIS], position[Y_AXIS], position[Z_AXIS] };
apply_leveling(pos);
#else
const float * const pos = position;
#endif
#if IS_KINEMATIC
inverse_kinematics(pos);
_set_position_mm(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], position[E_AXIS]);
#else
_set_position_mm(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], position[E_AXIS]);
#endif
}
/** /**
* Sync from the stepper positions. (e.g., after an interrupted move) * Sync from the stepper positions. (e.g., after an interrupted move)
*/ */
@ -1237,12 +1237,7 @@ void Planner::reset_acceleration_rates() {
// Recalculate position, steps_to_mm if axis_steps_per_mm changes! // Recalculate position, steps_to_mm if axis_steps_per_mm changes!
void Planner::refresh_positioning() { void Planner::refresh_positioning() {
LOOP_XYZE(i) steps_to_mm[i] = 1.0 / axis_steps_per_mm[i]; LOOP_XYZE(i) steps_to_mm[i] = 1.0 / axis_steps_per_mm[i];
#if IS_KINEMATIC set_position_mm_kinematic(current_position);
inverse_kinematics(current_position);
set_position_mm(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], current_position[E_AXIS]);
#else
set_position_mm(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
#endif
reset_acceleration_rates(); reset_acceleration_rates();
} }

71
Marlin/planner.h
View file

@ -43,6 +43,12 @@
class Planner; class Planner;
extern Planner planner; extern Planner planner;
#if IS_KINEMATIC
// for inline buffer_line_kinematic
extern float delta[ABC];
void inverse_kinematics(const float logical[XYZ]);
#endif
/** /**
* struct block_t * struct block_t
* *
@ -218,18 +224,68 @@ class Planner {
* as it will be given to the planner and steppers. * as it will be given to the planner and steppers.
*/ */
static void apply_leveling(float &lx, float &ly, float &lz); static void apply_leveling(float &lx, float &ly, float &lz);
static void apply_leveling(float logical[XYZ]) { apply_leveling(logical[X_AXIS], logical[Y_AXIS], logical[Z_AXIS]); }
static void unapply_leveling(float logical[XYZ]); static void unapply_leveling(float logical[XYZ]);
#endif #endif
/** /**
* Add a new linear movement to the buffer. * Planner::_buffer_line
* *
* x,y,z,e - target position in mm * Add a new direct linear movement to the buffer.
*
* Leveling and kinematics should be applied ahead of this.
*
* a,b,c,e - target position in mm or degrees
* fr_mm_s - (target) speed of the move (mm/s) * fr_mm_s - (target) speed of the move (mm/s)
* extruder - target extruder * extruder - target extruder
*/ */
static void buffer_line(ARG_X, ARG_Y, ARG_Z, const float& e, float fr_mm_s, const uint8_t extruder); static void _buffer_line(const float &a, const float &b, const float &c, const float &e, float fr_mm_s, const uint8_t extruder);
static void _set_position_mm(const float &a, const float &b, const float &c, const float &e);
/**
* 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.)
*
* lx,ly,lz,e - target position in mm or degrees
* fr_mm_s - (target) speed of the move (mm/s)
* extruder - target extruder
*/
static FORCE_INLINE void buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, const uint8_t extruder) {
#if PLANNER_LEVELING && IS_CARTESIAN
apply_leveling(lx, ly, lz);
#endif
_buffer_line(lx, ly, lz, e, fr_mm_s, extruder);
}
/**
* Add a new linear movement to the buffer.
* The target is cartesian, it's translated to delta/scara if
* needed.
*
* target - x,y,z,e CARTESIAN target in mm
* fr_mm_s - (target) speed of the move (mm/s)
* extruder - target extruder
*/
static FORCE_INLINE void buffer_line_kinematic(const float target[XYZE], float fr_mm_s, const uint8_t extruder) {
#if PLANNER_LEVELING
float pos[XYZ] = { target[X_AXIS], target[Y_AXIS], target[Z_AXIS] };
apply_leveling(pos);
#else
const float * const pos = target;
#endif
#if IS_KINEMATIC
inverse_kinematics(pos);
_buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], target[E_AXIS], fr_mm_s, extruder);
#else
_buffer_line(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], target[E_AXIS], fr_mm_s, extruder);
#endif
}
/** /**
* Set the planner.position and individual stepper positions. * Set the planner.position and individual stepper positions.
@ -240,9 +296,14 @@ class Planner {
* *
* Clears previous speed values. * Clears previous speed values.
*/ */
static void set_position_mm(ARG_X, ARG_Y, ARG_Z, const float& e); static FORCE_INLINE void set_position_mm(ARG_X, ARG_Y, ARG_Z, const float &e) {
#if PLANNER_LEVELING && IS_CARTESIAN
apply_leveling(lx, ly, lz);
#endif
_set_position_mm(lx, ly, lz, e);
}
static void set_position_mm_kinematic(const float position[NUM_AXIS]);
static void set_position_mm(const AxisEnum axis, const float& v); static void set_position_mm(const AxisEnum axis, const float& v);
static FORCE_INLINE void set_z_position_mm(const float& z) { set_position_mm(Z_AXIS, z); } static FORCE_INLINE void set_z_position_mm(const float& z) { set_position_mm(Z_AXIS, z); }
static FORCE_INLINE void set_e_position_mm(const float& e) { set_position_mm(E_AXIS, e); } static FORCE_INLINE void set_e_position_mm(const float& e) { set_position_mm(E_AXIS, e); }

8
Marlin/planner_bezier.cpp
View file

@ -187,13 +187,7 @@ void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS]
bez_target[Z_AXIS] = interp(position[Z_AXIS], target[Z_AXIS], t); bez_target[Z_AXIS] = interp(position[Z_AXIS], target[Z_AXIS], t);
bez_target[E_AXIS] = interp(position[E_AXIS], target[E_AXIS], t); bez_target[E_AXIS] = interp(position[E_AXIS], target[E_AXIS], t);
clamp_to_software_endstops(bez_target); clamp_to_software_endstops(bez_target);
planner.buffer_line_kinematic(bez_target, fr_mm_s, extruder);
#if IS_KINEMATIC
inverse_kinematics(bez_target);
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], bez_target[E_AXIS], fr_mm_s, extruder);
#else
planner.buffer_line(bez_target[X_AXIS], bez_target[Y_AXIS], bez_target[Z_AXIS], bez_target[E_AXIS], fr_mm_s, extruder);
#endif
} }
} }

30
Marlin/stepper.cpp
View file

@ -966,7 +966,7 @@ void Stepper::synchronize() { while (planner.blocks_queued()) idle(); }
* This allows get_axis_position_mm to correctly * This allows get_axis_position_mm to correctly
* derive the current XYZ position later on. * derive the current XYZ position later on.
*/ */
void Stepper::set_position(const long& x, const long& y, const long& z, const long& e) { void Stepper::set_position(const long &a, const long &b, const long &c, const long &e) {
synchronize(); // Bad to set stepper counts in the middle of a move synchronize(); // Bad to set stepper counts in the middle of a move
@ -975,37 +975,37 @@ void Stepper::set_position(const long& x, const long& y, const long& z, const lo
#if ENABLED(COREXY) #if ENABLED(COREXY)
// corexy positioning // corexy positioning
// these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html // these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
count_position[A_AXIS] = x + y; count_position[A_AXIS] = a + b;
count_position[B_AXIS] = x - y; count_position[B_AXIS] = a - b;
count_position[Z_AXIS] = z; count_position[Z_AXIS] = c;
#elif ENABLED(COREXZ) #elif ENABLED(COREXZ)
// corexz planning // corexz planning
count_position[A_AXIS] = x + z; count_position[A_AXIS] = a + c;
count_position[Y_AXIS] = y; count_position[Y_AXIS] = b;
count_position[C_AXIS] = x - z; count_position[C_AXIS] = a - c;
#elif ENABLED(COREYZ) #elif ENABLED(COREYZ)
// coreyz planning // coreyz planning
count_position[X_AXIS] = x; count_position[X_AXIS] = a;
count_position[B_AXIS] = y + z; count_position[B_AXIS] = y + c;
count_position[C_AXIS] = y - z; count_position[C_AXIS] = y - c;
#else #else
// default non-h-bot planning // default non-h-bot planning
count_position[X_AXIS] = x; count_position[X_AXIS] = a;
count_position[Y_AXIS] = y; count_position[Y_AXIS] = b;
count_position[Z_AXIS] = z; count_position[Z_AXIS] = c;
#endif #endif
count_position[E_AXIS] = e; count_position[E_AXIS] = e;
CRITICAL_SECTION_END; CRITICAL_SECTION_END;
} }
void Stepper::set_position(const AxisEnum &axis, const long& v) { void Stepper::set_position(const AxisEnum &axis, const long &v) {
CRITICAL_SECTION_START; CRITICAL_SECTION_START;
count_position[axis] = v; count_position[axis] = v;
CRITICAL_SECTION_END; CRITICAL_SECTION_END;
} }
void Stepper::set_e_position(const long& e) { void Stepper::set_e_position(const long &e) {
CRITICAL_SECTION_START; CRITICAL_SECTION_START;
count_position[E_AXIS] = e; count_position[E_AXIS] = e;
CRITICAL_SECTION_END; CRITICAL_SECTION_END;

6
Marlin/stepper.h
View file

@ -188,9 +188,9 @@ class Stepper {
// //
// Set the current position in steps // Set the current position in steps
// //
static void set_position(const long& x, const long& y, const long& z, const long& e); static void set_position(const long &a, const long &b, const long &c, const long &e);
static void set_position(const AxisEnum& a, const long& v); static void set_position(const AxisEnum &a, const long &v);
static void set_e_position(const long& e); static void set_e_position(const long &e);
// //
// Set direction bits for all steppers // Set direction bits for all steppers

14
Marlin/ultralcd.cpp
View file

@ -561,12 +561,7 @@ void kill_screen(const char* lcd_msg) {
#if ENABLED(ULTIPANEL) #if ENABLED(ULTIPANEL)
inline void line_to_current(AxisEnum axis) { inline void line_to_current(AxisEnum axis) {
#if ENABLED(DELTA) planner.buffer_line_kinematic(current_position, MMM_TO_MMS(manual_feedrate_mm_m[axis]), active_extruder);
inverse_kinematics(current_position);
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], current_position[E_AXIS], MMM_TO_MMS(manual_feedrate_mm_m[axis]), active_extruder);
#else // !DELTA
planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], MMM_TO_MMS(manual_feedrate_mm_m[axis]), active_extruder);
#endif // !DELTA
} }
#if ENABLED(SDSUPPORT) #if ENABLED(SDSUPPORT)
@ -1351,12 +1346,7 @@ void kill_screen(const char* lcd_msg) {
*/ */
inline void manage_manual_move() { inline void manage_manual_move() {
if (manual_move_axis != (int8_t)NO_AXIS && ELAPSED(millis(), manual_move_start_time) && !planner.is_full()) { if (manual_move_axis != (int8_t)NO_AXIS && ELAPSED(millis(), manual_move_start_time) && !planner.is_full()) {
#if ENABLED(DELTA) planner.buffer_line_kinematic(current_position, MMM_TO_MMS(manual_feedrate_mm_m[manual_move_axis]), manual_move_e_index);
inverse_kinematics(current_position);
planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], current_position[E_AXIS], MMM_TO_MMS(manual_feedrate_mm_m[manual_move_axis]), manual_move_e_index);
#else
planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], MMM_TO_MMS(manual_feedrate_mm_m[manual_move_axis]), manual_move_e_index);
#endif
manual_move_axis = (int8_t)NO_AXIS; manual_move_axis = (int8_t)NO_AXIS;
} }
} }