MPCNC/Firmware2
1
0
Fork 0
Code Issues 1 Pull Requests Releases Wiki Activity

Improve planner kinematics, fix delta ABL

Browse Source
This commit is contained in:
Josef Pavlik 2016-10-06 10:56:05 +02:00 committed by Scott Lahteine
parent 48761f2021
commit f8c2473a71
6 changed files with 102 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
Marlin/Marlin_main.cpp
View File

@ -711,8 +711,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 +1540,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
@ -6779,8 +6777,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
@ -6900,12 +6897,10 @@ inline void gcode_M503() {
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);
@ -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 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;
} }
@ -8815,16 +8809,14 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
// For non-interpolated delta calculate every segment // For non-interpolated delta calculate every segment
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(); planner.buffer_line_kinematic(DELTA_VAR, _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);
} }
#endif #endif
// 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;
} }
@ -9064,21 +9056,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

4
Marlin/SanityCheck.h
View File

@ -518,6 +518,10 @@
*/ */
#if HAS_ABL #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 * Delta and SCARA have limited bed leveling options
*/ */

54
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,21 +551,9 @@ 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 };
#if ENABLED(DELTA)
float offset = bilinear_z_offset(tmp);
lx += offset;
ly += offset;
lz += offset;
#else
lz += bilinear_z_offset(tmp); lz += bilinear_z_offset(tmp);
#endif #endif
#endif
} }
void Planner::unapply_leveling(float logical[XYZ]) { void Planner::unapply_leveling(float logical[XYZ]) {
@ -601,15 +591,16 @@ 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.
* Not apply the leveling.
* *
* x,y,z,e - target position in mm * x,y,z,e - target position in mm
* fr_mm_s - (target) speed of the move * fr_mm_s - (target) speed of the move
* extruder - target extruder * 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 // 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,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. // 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
@ -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. * 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]), 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]), ny = position[Y_AXIS] = lround(ly * axis_steps_per_mm[Y_AXIS]),
nz = position[Z_AXIS] = lround(lz * axis_steps_per_mm[Z_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)); 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 +1236,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();
} }

62
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,63 @@ 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
/** /**
* Planner::_buffer_line
*
* Add a new linear movement to the buffer. * 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 * x,y,z,e - target position in mm
* 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 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. * Set the planner.position and individual stepper positions.
@ -240,9 +291,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_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 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
} }
} }

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;
} }
} }