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

Minor bug fixes in planner.

Browse Source
This commit is contained in:
Sonny J 2011-09-03 23:22:27 -06:00
parent 75bd4c5ac3
commit 5e2e935bda
1 changed files with 18 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
planner.c
View File

@ -99,24 +99,20 @@ static double max_allowable_speed(double acceleration, double target_velocity, d
// The kernel called by planner_recalculate() when scanning the plan from last to first entry. // The kernel called by planner_recalculate() when scanning the plan from last to first entry.
static void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) { static void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) {
if (!current) { return; } if (!current) { return; }
// Calculate the entry speed for the current block.
if (previous) { double entry_speed = current->max_entry_speed;
double entry_speed = current->max_entry_speed; double exit_speed;
double exit_speed; if (next) {
if (next) { exit_speed = next->entry_speed;
exit_speed = next->entry_speed;
} else {
exit_speed = 0.0;
}
// If the required deceleration across the block is too rapid, reduce the entry_factor accordingly.
if (entry_speed > exit_speed) {
entry_speed =
min(max_allowable_speed(-settings.acceleration,exit_speed,current->millimeters),entry_speed);
}
current->entry_speed = entry_speed;
} else { } else {
current->entry_speed = 0.0; exit_speed = 0.0;
} }
// If the required deceleration across the block is too rapid, reduce the entry_factor accordingly.
if (entry_speed > exit_speed) {
entry_speed =
min(max_allowable_speed(-settings.acceleration,exit_speed,current->millimeters),entry_speed);
}
current->entry_speed = entry_speed;
} }
@ -144,9 +140,8 @@ static void planner_forward_pass_kernel(block_t *previous, block_t *current, blo
// speed accordingly. // speed accordingly.
if(previous) { if(previous) {
if (previous->entry_speed < current->entry_speed) { if (previous->entry_speed < current->entry_speed) {
current->entry_speed = current->entry_speed = min( min( current->entry_speed, current->max_entry_speed ),
min( max_allowable_speed(-settings.acceleration,current->entry_speed,previous->millimeters), max_allowable_speed(-settings.acceleration,previous->entry_speed,previous->millimeters) );
min( current->max_entry_speed, current->entry_speed ) );
} }
} }
} }
@ -359,18 +354,19 @@ void plan_buffer_line(double x, double y, double z, double feed_rate, int invert
-previous->delta_mm[Z_AXIS] * block->delta_mm[Z_AXIS] )/ -previous->delta_mm[Z_AXIS] * block->delta_mm[Z_AXIS] )/
( previous->millimeters * block->millimeters ); ( previous->millimeters * block->millimeters );
// Avoid divide by zero and set zero max junction velocity for highly acute angles. // Avoid divide by zero for straight junctions around 180 degress
if (cos_theta < 0.9) { if (cos_theta > -0.95) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation // Compute maximum junction velocity based on maximum acceleration and junction deviation
double sin_theta_d2 = sqrt((1-cos_theta)/2); // Trig half angle identity double sin_theta_d2 = sqrt((1-cos_theta)/2); // Trig half angle identity
vmax_junction = vmax_junction =
sqrt(settings.acceleration*60*60 * settings.junction_deviation * sin_theta_d2/(1-sin_theta_d2)); sqrt(settings.acceleration*60*60 * settings.junction_deviation * sin_theta_d2/(1-sin_theta_d2));
vmax_junction = max(0.0,min(vmax_junction, min(previous->nominal_speed,block->nominal_speed))); vmax_junction = max(0.0,min(vmax_junction, min(previous->nominal_speed,block->nominal_speed)));
} else {
vmax_junction = min(previous->nominal_speed,block->nominal_speed);
} }
} }
block->max_entry_speed = vmax_junction; block->max_entry_speed = vmax_junction;
block->entry_speed = 0.0; block->entry_speed = 0.0;
calculate_trapezoid_for_block(block, block->entry_speed, 0.0);
} else { } else {
block->initial_rate = block->nominal_rate; block->initial_rate = block->nominal_rate;
block->final_rate = block->nominal_rate; block->final_rate = block->nominal_rate;