andrey
/
Marlin_FB4S
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Improve planner kinematics, fix delta ABL

pull/1/head
Josef Pavlik 8 years ago
committed by Scott Lahteine
parent
48761f2021
commit
f8c2473a71
6 changed files with 102 additions and 82 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 40
      Marlin/Marlin_main.cpp
  2. 4
      Marlin/SanityCheck.h
  3. 54
      Marlin/planner.cpp
  4. 62
      Marlin/planner.h
  5. 8
      Marlin/planner_bezier.cpp
  6. 14
      Marlin/ultralcd.cpp

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

Write Preview
Loading…
Cancel
Save