Corrected distance calculation. (thanks jv4779)

This commit is contained in:
Erik van der Zalm 2012-02-09 19:53:06 +01:00
parent 598eb1d4f1
commit 6ef8459494

View file

@ -517,8 +517,11 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS]; delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS]; delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS];
delta_mm[E_AXIS] = (target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS]; delta_mm[E_AXIS] = (target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS];
block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + if ( block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0 ) {
square(delta_mm[Z_AXIS]) + square(delta_mm[E_AXIS])); block->millimeters = delta_mm[E_AXIS];
} else {
block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS]));
}
float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides
// Calculate speed in mm/second for each axis. No divide by zero due to previous checks. // Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
@ -527,9 +530,6 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0 block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0
block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0 block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0
if (block->steps_e == 0) { if (block->steps_e == 0) {
if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate; if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate;
} }
@ -537,7 +537,6 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate; if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate;
} }
// slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill // slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill
int moves_queued=(block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1); int moves_queued=(block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1);
#ifdef SLOWDOWN #ifdef SLOWDOWN