ManuelW/grbl-LPC-CoreXY
1
0
Fork 0
Code Issues Pull Requests Releases 1 Wiki Activity

corrected speed calculation in planner (untested on hardware)

Browse Source
This commit is contained in:
Simen Svale Skogsrud 2011-02-10 23:48:17 +01:00
parent fb5db05b6d
commit fc1c1b7e09
2 changed files with 16 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
stepper.c
View File

@ -101,7 +101,7 @@ inline void trapezoid_generator_tick() {
// rounding errors that might leave steps hanging after the last trapezoid tick. // rounding errors that might leave steps hanging after the last trapezoid tick.
if(current_block->rate_delta < trapezoid_adjusted_rate) { if(current_block->rate_delta < trapezoid_adjusted_rate) {
trapezoid_adjusted_rate -= current_block->rate_delta; trapezoid_adjusted_rate -= current_block->rate_delta;
} }
set_step_events_per_minute(trapezoid_adjusted_rate); set_step_events_per_minute(trapezoid_adjusted_rate);
} }
} }

34
stepper_plan.c
View File

@ -195,7 +195,7 @@ void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *n
} else { } else {
entry_factor = factor_for_safe_speed(current); entry_factor = factor_for_safe_speed(current);
} }
// Store result // Store result
current->entry_factor = entry_factor; 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[Y_AXIS] = lround(y*settings.steps_per_mm[Y_AXIS]);
target[Z_AXIS] = lround(z*settings.steps_per_mm[Z_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 // Calculate the buffer head after we push this byte
int next_buffer_head = (block_buffer_head + 1) % BLOCK_BUFFER_SIZE; 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. // 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 // Prepare to set up new block
block_t *block = &block_buffer[block_buffer_head]; block_t *block = &block_buffer[block_buffer_head];
// Number of steps for each axis // Number of steps for each axis
block->steps_x = labs(delta_x); block->steps_x = labs(target[X_AXIS]-position[X_AXIS]);
block->steps_y = labs(delta_y); block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]);
block->steps_z = labs(delta_z); block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]);
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->step_event_count = max(block->steps_x, max(block->steps_y, block->steps_z)); block->step_event_count = max(block->steps_x, max(block->steps_y, block->steps_z));
// Bail if this is a zero-length block // Bail if this is a zero-length block
if (block->step_event_count == 0) { return; }; 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; uint32_t microseconds;
if (!invert_feed_rate) { if (!invert_feed_rate) {
microseconds = lround((block->millimeters/feed_rate)*1000000); 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 // Calculate speed in mm/minute for each axis
double multiplier = 60.0*1000000.0/microseconds; double multiplier = 60.0*1000000.0/microseconds;
block->speed_x = (delta_x)*multiplier; block->speed_x = delta_x_mm * multiplier;
block->speed_y = (delta_y)*multiplier; block->speed_y = delta_y_mm * multiplier;
block->speed_z = (delta_z)*multiplier; block->speed_z = delta_z_mm * multiplier;
// HER ER FEILEN. BEREGNINGEN AV HASTIGHETEN ER PÅ TOK block->nominal_speed = block->millimeters * multiplier;
block->nominal_speed = block->millimeters*multiplier; block->nominal_rate = ceil(block->step_event_count * multiplier);
block->nominal_rate = ceil(block->step_event_count*multiplier);
block->entry_factor = 0.0; block->entry_factor = 0.0;
// Compute the acceleration rate for the trapezoid generator. Depending on the slope of the line // Compute the acceleration rate for the trapezoid generator. Depending on the slope of the line