From f8c2473a71c50728d6c12e8fe8bf0f426c4efb94 Mon Sep 17 00:00:00 2001 From: Josef Pavlik Date: Thu, 6 Oct 2016 10:56:05 +0200 Subject: [PATCH] Improve planner kinematics, fix delta ABL --- Marlin/Marlin_main.cpp | 40 +++++++------------------ Marlin/SanityCheck.h | 4 +++ Marlin/planner.cpp | 56 ++++++++++++++++------------------- Marlin/planner.h | 62 +++++++++++++++++++++++++++++++++++++-- Marlin/planner_bezier.cpp | 8 +---- Marlin/ultralcd.cpp | 14 ++------- 6 files changed, 102 insertions(+), 82 deletions(-) diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 5a4338af7f..5a073145a7 100755 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -711,8 +711,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 +1540,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 @@ -6779,8 +6777,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 @@ -6900,12 +6897,10 @@ inline void gcode_M503() { 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); @@ -8671,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; } @@ -8815,16 +8809,14 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) { // For non-interpolated delta calculate every segment for (uint16_t s = segments + 1; --s;) { DELTA_NEXT(segment_distance[i]); - DELTA_IK(); - planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], DELTA_VAR[E_AXIS], _feedrate_mm_s, active_extruder); + planner.buffer_line_kinematic(DELTA_VAR, _feedrate_mm_s, active_extruder); } #endif // 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; } @@ -9064,21 +9056,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 diff --git a/Marlin/SanityCheck.h b/Marlin/SanityCheck.h index 44950ffdf7..0eca5743d3 100644 --- a/Marlin/SanityCheck.h +++ b/Marlin/SanityCheck.h @@ -518,6 +518,10 @@ */ #if HAS_ABL + #if ENABLED(USE_RAW_KINEMATICS) || ENABLED(USE_DELTA_IK_INTERPOLATION) + #error "USE_RAW_KINEMATICS and USE_DELTA_IK_INTERPOLATION are not compatible with AUTO_BED_LEVELING" + #endif + /** * Delta and SCARA have limited bed leveling options */ diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index 959b109e59..183fe0c172 100644 --- a/Marlin/planner.cpp +++ b/Marlin/planner.cpp @@ -522,7 +522,9 @@ void Planner::check_axes_activity() { } #if PLANNER_LEVELING - + /** + * lx, ly, lz - logical (cartesian, not delta) positions in mm + */ void Planner::apply_leveling(float &lx, float &ly, float &lz) { #if HAS_ABL @@ -549,19 +551,7 @@ void Planner::check_axes_activity() { #elif ENABLED(AUTO_BED_LEVELING_BILINEAR) float tmp[XYZ] = { lx, ly, 0 }; - - #if ENABLED(DELTA) - - float offset = bilinear_z_offset(tmp); - lx += offset; - ly += offset; - lz += offset; - - #else - - lz += bilinear_z_offset(tmp); - - #endif + lz += bilinear_z_offset(tmp); #endif } @@ -601,15 +591,16 @@ void Planner::check_axes_activity() { #endif // PLANNER_LEVELING /** - * Planner::buffer_line + * Planner::_buffer_line * * Add a new linear movement to the buffer. + * Not apply the leveling. * * x,y,z,e - target position in mm * 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 &lx, const float &ly, const float &lz, const float &e, float fr_mm_s, const uint8_t extruder) { // Calculate the buffer head after we push this byte int next_buffer_head = next_block_index(block_buffer_head); @@ -617,10 +608,6 @@ 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. 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 // 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 @@ -1196,12 +1183,8 @@ 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. */ -void Planner::set_position_mm(ARG_X, ARG_Y, ARG_Z, const float &e) { - - #if PLANNER_LEVELING - apply_leveling(lx, ly, lz); - #endif +void Planner::_set_position_mm(const float &lx, const float &ly, const float &lz, const float &e) { 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]), @@ -1212,6 +1195,22 @@ void Planner::set_position_mm(ARG_X, ARG_Y, ARG_Z, const float &e) { 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) */ @@ -1237,12 +1236,7 @@ void Planner::reset_acceleration_rates() { // Recalculate position, steps_to_mm if axis_steps_per_mm changes! void Planner::refresh_positioning() { LOOP_XYZE(i) steps_to_mm[i] = 1.0 / axis_steps_per_mm[i]; - #if IS_KINEMATIC - 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 + set_position_mm_kinematic(current_position); reset_acceleration_rates(); } diff --git a/Marlin/planner.h b/Marlin/planner.h index 0e95be4cd9..1d7e65a25d 100644 --- a/Marlin/planner.h +++ b/Marlin/planner.h @@ -43,6 +43,12 @@ class 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 * @@ -218,18 +224,63 @@ class Planner { * 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 logical[XYZ]) { apply_leveling(logical[X_AXIS], logical[Y_AXIS], logical[Z_AXIS]); } static void unapply_leveling(float logical[XYZ]); #endif /** + * Planner::_buffer_line + * * Add a new linear movement to the buffer. + * Doesn't apply the leveling. + * + * x,y,z,e - target position in mm + * fr_mm_s - (target) speed of the move + * extruder - target extruder + */ + static void _buffer_line(const float &lx, const float &ly, const float &lz, const float &e, float fr_mm_s, const uint8_t extruder); + + static void _set_position_mm(const float &lx, const float &ly, const float &lz, const float &e); + + /** + * Add a new linear movement to the buffer. + * The target is NOT translated to delta/scara * * x,y,z,e - target position in mm * fr_mm_s - (target) speed of the move (mm/s) * 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 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_KINEMATIC + 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[NUM_AXIS], 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. @@ -240,9 +291,14 @@ class Planner { * * 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_KINEMATIC + 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 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); } diff --git a/Marlin/planner_bezier.cpp b/Marlin/planner_bezier.cpp index ad46f89f0e..d7dd960900 100644 --- a/Marlin/planner_bezier.cpp +++ b/Marlin/planner_bezier.cpp @@ -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[E_AXIS] = interp(position[E_AXIS], target[E_AXIS], t); clamp_to_software_endstops(bez_target); - - #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 + planner.buffer_line_kinematic(bez_target, fr_mm_s, extruder); } } diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp index 2cf83a6090..fdf879f750 100755 --- a/Marlin/ultralcd.cpp +++ b/Marlin/ultralcd.cpp @@ -561,12 +561,7 @@ void kill_screen(const char* lcd_msg) { #if ENABLED(ULTIPANEL) inline void line_to_current(AxisEnum axis) { - #if ENABLED(DELTA) - 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 + planner.buffer_line_kinematic(current_position, MMM_TO_MMS(manual_feedrate_mm_m[axis]), active_extruder); } #if ENABLED(SDSUPPORT) @@ -1351,12 +1346,7 @@ void kill_screen(const char* lcd_msg) { */ inline void manage_manual_move() { if (manual_move_axis != (int8_t)NO_AXIS && ELAPSED(millis(), manual_move_start_time) && !planner.is_full()) { - #if ENABLED(DELTA) - 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 + planner.buffer_line_kinematic(current_position, MMM_TO_MMS(manual_feedrate_mm_m[manual_move_axis]), manual_move_e_index); manual_move_axis = (int8_t)NO_AXIS; } }