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

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