From fc1c1b7e093b4c9a85a8e8d3cacf6b51bb6ed82c Mon Sep 17 00:00:00 2001 From: Simen Svale Skogsrud Date: Thu, 10 Feb 2011 23:48:17 +0100 Subject: [PATCH] corrected speed calculation in planner (untested on hardware) --- stepper.c | 2 +- stepper_plan.c | 34 +++++++++++++++------------------- 2 files changed, 16 insertions(+), 20 deletions(-) diff --git a/stepper.c b/stepper.c index 682a6e0..ddfd547 100644 --- a/stepper.c +++ b/stepper.c @@ -101,7 +101,7 @@ inline void trapezoid_generator_tick() { // rounding errors that might leave steps hanging after the last trapezoid tick. if(current_block->rate_delta < trapezoid_adjusted_rate) { trapezoid_adjusted_rate -= current_block->rate_delta; - } + } set_step_events_per_minute(trapezoid_adjusted_rate); } } diff --git a/stepper_plan.c b/stepper_plan.c index 6de02b5..3046ab3 100644 --- a/stepper_plan.c +++ b/stepper_plan.c @@ -195,7 +195,7 @@ void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *n } else { entry_factor = factor_for_safe_speed(current); } - + // Store result current->entry_factor = entry_factor; } @@ -334,10 +334,6 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert target[Y_AXIS] = lround(y*settings.steps_per_mm[Y_AXIS]); target[Z_AXIS] = lround(z*settings.steps_per_mm[Z_AXIS]); - double delta_x = (target[X_AXIS]-position[X_AXIS]); - double delta_y = (target[Y_AXIS]-position[Y_AXIS]); - double delta_z = (target[Z_AXIS]-position[Z_AXIS]); - // Calculate the buffer head after we push this byte int next_buffer_head = (block_buffer_head + 1) % BLOCK_BUFFER_SIZE; // If the buffer is full: good! That means we are well ahead of the robot. @@ -346,18 +342,19 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert // Prepare to set up new block block_t *block = &block_buffer[block_buffer_head]; // Number of steps for each axis - block->steps_x = labs(delta_x); - block->steps_y = labs(delta_y); - block->steps_z = labs(delta_z); - block->millimeters = sqrt( - square(block->steps_x/settings.steps_per_mm[X_AXIS])+ - square(block->steps_y/settings.steps_per_mm[Y_AXIS])+ - square(block->steps_z/settings.steps_per_mm[Z_AXIS])); - + block->steps_x = labs(target[X_AXIS]-position[X_AXIS]); + block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]); + block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]); block->step_event_count = max(block->steps_x, max(block->steps_y, block->steps_z)); // Bail if this is a zero-length block if (block->step_event_count == 0) { return; }; + double delta_x_mm = (target[X_AXIS]-position[X_AXIS])/settings.steps_per_mm[X_AXIS]; + double delta_y_mm = (target[Y_AXIS]-position[Y_AXIS])/settings.steps_per_mm[Y_AXIS]; + double delta_z_mm = (target[Z_AXIS]-position[Z_AXIS])/settings.steps_per_mm[Z_AXIS]; + block->millimeters = sqrt(square(delta_x_mm) + square(delta_y_mm) + square(delta_z_mm)); + + uint32_t microseconds; if (!invert_feed_rate) { microseconds = lround((block->millimeters/feed_rate)*1000000); @@ -367,12 +364,11 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert // Calculate speed in mm/minute for each axis double multiplier = 60.0*1000000.0/microseconds; - block->speed_x = (delta_x)*multiplier; - block->speed_y = (delta_y)*multiplier; - block->speed_z = (delta_z)*multiplier; - // HER ER FEILEN. BEREGNINGEN AV HASTIGHETEN ER PÅ TOK - block->nominal_speed = block->millimeters*multiplier; - block->nominal_rate = ceil(block->step_event_count*multiplier); + block->speed_x = delta_x_mm * multiplier; + block->speed_y = delta_y_mm * multiplier; + block->speed_z = delta_z_mm * multiplier; + block->nominal_speed = block->millimeters * multiplier; + block->nominal_rate = ceil(block->step_event_count * multiplier); block->entry_factor = 0.0; // Compute the acceleration rate for the trapezoid generator. Depending on the slope of the line