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

Fix planner leveling and rename arguments 

Use lx, ly, lz for “logical” positions
pull/1/head
Scott Lahteine 8 years ago
parent
d4f21af6b3
commit
c109399bf6
2 changed files with 90 additions and 74 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. 106
      Marlin/planner.cpp
  2. 58
      Marlin/planner.h

106
Marlin/planner.cpp
View File

@ -523,30 +523,44 @@ void Planner::check_axes_activity() {
#if PLANNER_LEVELING #if PLANNER_LEVELING
void Planner::apply_leveling( void Planner::apply_leveling(float &lx, float &ly, float &lz) {
#if ENABLED(MESH_BED_LEVELING) #if ENABLED(MESH_BED_LEVELING)
const float &x, const float &y
#else if (mbl.active())
float &x, float &y lz += mbl.get_z(RAW_X_POSITION(lx), RAW_Y_POSITION(ly));
#elif ENABLED(AUTO_BED_LEVELING_LINEAR)
float dx = RAW_X_POSITION(lx) - (X_TILT_FULCRUM),
dy = RAW_Y_POSITION(ly) - (Y_TILT_FULCRUM),
dz = RAW_Z_POSITION(lz);
apply_rotation_xyz(bed_level_matrix, dx, dy, dz);
lx = LOGICAL_X_POSITION(dx + X_TILT_FULCRUM);
ly = LOGICAL_Y_POSITION(dy + Y_TILT_FULCRUM);
lz = LOGICAL_Z_POSITION(dz);
#endif #endif
, float &z }
) {
void Planner::unapply_leveling(float &lx, float &ly, float &lz) {
#if ENABLED(MESH_BED_LEVELING) #if ENABLED(MESH_BED_LEVELING)
if (mbl.active()) if (mbl.active())
z += mbl.get_z(RAW_X_POSITION(x), RAW_Y_POSITION(y)); lz -= mbl.get_z(RAW_X_POSITION(lx), RAW_Y_POSITION(ly));
#elif ENABLED(AUTO_BED_LEVELING_LINEAR)
#elif ENABLED(AUTO_BED_LEVELING_FEATURE) matrix_3x3 inverse = matrix_3x3::transpose(bed_level_matrix);
float tx = RAW_X_POSITION(x) - (X_TILT_FULCRUM), float dx = lx - (X_TILT_FULCRUM), dy = ly - (Y_TILT_FULCRUM), dz = lz;
ty = RAW_Y_POSITION(y) - (Y_TILT_FULCRUM),
tz = RAW_Z_POSITION(z);
apply_rotation_xyz(bed_level_matrix, tx, ty, tz); apply_rotation_xyz(inverse, dx, dy, dz);
x = LOGICAL_X_POSITION(tx + X_TILT_FULCRUM); lx = LOGICAL_X_POSITION(dx + X_TILT_FULCRUM);
y = LOGICAL_Y_POSITION(ty + Y_TILT_FULCRUM); ly = LOGICAL_Y_POSITION(dy + Y_TILT_FULCRUM);
z = LOGICAL_Z_POSITION(tz); lz = LOGICAL_Z_POSITION(dz);
#endif #endif
} }
@ -562,15 +576,7 @@ void Planner::check_axes_activity() {
* 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(
#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
float x, float y, float z
#else
const float& x, const float& y, const float& z
#endif
, 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);
@ -578,17 +584,17 @@ void Planner::buffer_line(
// 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 ENABLED(MESH_BED_LEVELING) || ENABLED(AUTO_BED_LEVELING_FEATURE) #if PLANNER_LEVELING
apply_leveling(x, y, z); apply_leveling(lx, ly, lz);
#endif #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[NUM_AXIS] = {
lround(x * axis_steps_per_mm[X_AXIS]), lround(lx * axis_steps_per_mm[X_AXIS]),
lround(y * axis_steps_per_mm[Y_AXIS]), lround(ly * axis_steps_per_mm[Y_AXIS]),
lround(z * axis_steps_per_mm[Z_AXIS]), lround(lz * axis_steps_per_mm[Z_AXIS]),
lround(e * axis_steps_per_mm[E_AXIS]) lround(e * axis_steps_per_mm[E_AXIS])
}; };
@ -598,11 +604,22 @@ void Planner::buffer_line(
/* /*
SERIAL_ECHO_START; SERIAL_ECHO_START;
SERIAL_ECHOPAIR("Planner X:", x); SERIAL_ECHOPGM("Planner ", x);
SERIAL_ECHOPAIR(" (", dx); #if IS_KINEMATIC
SERIAL_ECHOPAIR(") Y:", y); SERIAL_ECHOPAIR("A:", x);
SERIAL_ECHOPAIR(" (", dx);
SERIAL_ECHOPAIR(") B:", y);
#else
SERIAL_ECHOPAIR("X:", x);
SERIAL_ECHOPAIR(" (", dx);
SERIAL_ECHOPAIR(") Y:", y);
#endif
SERIAL_ECHOPAIR(" (", dy); SERIAL_ECHOPAIR(" (", dy);
SERIAL_ECHOPAIR(") Z:", z); #elif ENABLED(DELTA)
SERIAL_ECHOPAIR(") C:", z);
#else
SERIAL_ECHOPAIR(") Z:", z);
#endif
SERIAL_ECHOPAIR(" (", dz); SERIAL_ECHOPAIR(" (", dz);
SERIAL_ECHOLNPGM(")"); SERIAL_ECHOLNPGM(")");
//*/ //*/
@ -671,7 +688,7 @@ void Planner::buffer_line(
// For a mixing extruder, get a magnified step_event_count for each // For a mixing extruder, get a magnified step_event_count for each
#if ENABLED(MIXING_EXTRUDER) #if ENABLED(MIXING_EXTRUDER)
for (uint8_t i = 0; i < MIXING_STEPPERS; i++) for (uint8_t i = 0; i < MIXING_STEPPERS; i++)
block->mix_event_count[i] = (mixing_factor[i] < 0.0001) ? 0 : block->step_event_count / mixing_factor[i]; block->mix_event_count[i] = UNEAR_ZERO(mixing_factor[i]) ? 0 : block->step_event_count / mixing_factor[i];
#endif #endif
#if FAN_COUNT > 0 #if FAN_COUNT > 0
@ -1124,7 +1141,7 @@ void Planner::buffer_line(
block->advance_rate = acc_dist ? advance / (float)acc_dist : 0; block->advance_rate = acc_dist ? advance / (float)acc_dist : 0;
} }
/** /**
SERIAL_ECHO_START; SERIAL_ECHO_START;
SERIAL_ECHOPGM("advance :"); SERIAL_ECHOPGM("advance :");
SERIAL_ECHO(block->advance/256.0); SERIAL_ECHO(block->advance/256.0);
SERIAL_ECHOPGM("advance rate :"); SERIAL_ECHOPGM("advance rate :");
@ -1152,22 +1169,15 @@ void Planner::buffer_line(
* *
* 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( void Planner::set_position_mm(ARG_X, ARG_Y, ARG_Z, const float &e) {
#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
float x, float y, float z
#else
const float& x, const float& y, const float& z
#endif
, const float& e
) {
#if ENABLED(MESH_BED_LEVELING) || ENABLED(AUTO_BED_LEVELING_FEATURE) #if PLANNER_LEVELING
apply_leveling(x, y, z); apply_leveling(lx, ly, lz);
#endif #endif
long nx = position[X_AXIS] = lround(x * axis_steps_per_mm[X_AXIS]), long nx = position[X_AXIS] = lround(lx * axis_steps_per_mm[X_AXIS]),
ny = position[Y_AXIS] = lround(y * axis_steps_per_mm[Y_AXIS]), ny = position[Y_AXIS] = lround(ly * axis_steps_per_mm[Y_AXIS]),
nz = position[Z_AXIS] = lround(z * axis_steps_per_mm[Z_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(nx, ny, nz, 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.

58
Marlin/planner.h
View File

@ -202,39 +202,45 @@ class Planner {
static bool is_full() { return (block_buffer_tail == BLOCK_MOD(block_buffer_head + 1)); } static bool is_full() { return (block_buffer_tail == BLOCK_MOD(block_buffer_head + 1)); }
#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING) #if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
#define ARG_X float lx
#define ARG_Y float ly
#define ARG_Z float lz
#else
#define ARG_X const float &lx
#define ARG_Y const float &ly
#define ARG_Z const float &lz
#endif
#if ENABLED(MESH_BED_LEVELING) #if PLANNER_LEVELING
static void apply_leveling(const float &x, const float &y, float &z);
#else
static void apply_leveling(float &x, float &y, float &z);
#endif
/** /**
* Add a new linear movement to the buffer. * Apply leveling to transform a cartesian position
* * as it will be given to the planner and steppers.
* 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(float x, float y, float z, const float& e, float fr_mm_s, const uint8_t extruder); static void apply_leveling(float &lx, float &ly, float &lz);
static void unapply_leveling(float &lx, float &ly, float &lz);
/** #endif
* Set the planner.position and individual stepper positions.
* Used by G92, G28, G29, and other procedures.
*
* Multiplies by axis_steps_per_mm[] and does necessary conversion
* for COREXY / COREXZ / COREYZ to set the corresponding stepper positions.
*
* Clears previous speed values.
*/
static void set_position_mm(float x, float y, float z, const float& e);
#else
static void buffer_line(const float& x, const float& y, const float& z, const float& e, float fr_mm_s, const uint8_t extruder); /**
static void set_position_mm(const float& x, const float& y, const float& z, const float& e); * Add a new linear movement to the buffer.
*
* 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);
#endif // AUTO_BED_LEVELING_FEATURE || MESH_BED_LEVELING /**
* Set the planner.position and individual stepper positions.
* Used by G92, G28, G29, and other procedures.
*
* Multiplies by axis_steps_per_mm[] and does necessary conversion
* for COREXY / COREXZ / COREYZ to set the corresponding stepper positions.
*
* Clears previous speed values.
*/
static void set_position_mm(ARG_X, ARG_Y, ARG_Z, const float& e);
/** /**
* Set the E position (mm) of the planner (and the E stepper) * Set the E position (mm) of the planner (and the E stepper)

Write Preview
Loading…
Cancel
Save