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

removed a grave little bug in the planner and added a baseline safe speed so that motion sequences do not attempt to go to speed 0, but to a safe, higher speed based on the max_jerk setting

Browse Source
This commit is contained in:
Simen Svale Skogsrud 2011-02-04 22:09:09 +01:00
parent 0bd0ba6e6e
commit 59a9b64087
3 changed files with 40 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
stepper.c
View File

@ -39,10 +39,11 @@
#define STEPPING_MASK (STEP_MASK | DIRECTION_MASK) // All stepping-related bits (step/direction)
#define LIMIT_MASK ((1<<X_LIMIT_BIT)|(1<<Y_LIMIT_BIT)|(1<<Z_LIMIT_BIT)) // All limit bits
#define MINIMUM_STEPS_PER_MINUTE 1200 // The stepper subsystem will never run slower than this, exept when sleeping
#define CYCLES_PER_ACCELERATION_TICK ((TICKS_PER_MICROSECOND*1000000)/ACCELERATION_TICKS_PER_SECOND)
#define ENABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 |= (1<<OCIE1A)
#define DISABLE_STEPPER_DRIVER_INTERRUPT() TIMSK1 &= ~(1<<OCIE1A)
#define MINIMUM_STEPS_PER_MINUTE 1200
#define CYCLES_PER_ACCELERATION_TICK ((TICKS_PER_MICROSECOND*1000000)/ACCELERATION_TICKS_PER_SECOND)
static block_t *current_block; // A convenience pointer to the block currently being traced
@ -171,6 +172,12 @@ SIGNAL(TIMER1_COMPA_vect)
// If current block is finished, reset pointer
step_events_completed += 1;
if (step_events_completed >= current_block->step_event_count) {
// printInteger(current_block->exit_rate);
// printString(" == ");
// printInteger(trapezoid_adjusted_rate);
// printString(" <-- exit, actual\n\r");
// printInteger(current_block->rate_delta);
// printString(" <-- delta\n\r");
current_block = NULL;
// move the block buffer tail to the next instruction
block_buffer_tail = (block_buffer_tail + 1) % BLOCK_BUFFER_SIZE;

34
stepper_plan.c
View File

@ -139,6 +139,7 @@ void calculate_trapezoid_for_block(block_t *block, double entry_factor, double e
block->accelerate_until = accelerate_steps;
block->decelerate_after = accelerate_steps+plateau_steps;
block->exit_rate = lround(block->nominal_rate*exit_factor); // Debug line please delete me soon
// printInteger(block->accelerate_until);printString(",");
// printInteger(block->decelerate_after);printString(" of ");
// printInteger(block->step_event_count); printString(" <- profile\n\r");
@ -173,6 +174,12 @@ inline double junction_jerk(block_t *before, block_t *after) {
);
}
// Calculate a braking factor to reach baseline speed which is max_jerk/2, e.g. the
// speed under which you cannot exceed max_jerk no matter what you do.
double factor_for_safe_speed(block_t *block) {
return(settings.max_jerk/block->nominal_speed);
}
// The kernel called by planner_recalculate() when scanning the plan from last to first entry.
void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *next) {
if(!current) { return; }
@ -183,7 +190,7 @@ void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *n
if (next) {
exit_factor = next->entry_factor;
} else {
exit_factor = 0.0;
exit_factor = factor_for_safe_speed(current);
}
// Calculate the entry_factor for the current block.
@ -215,8 +222,12 @@ void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *n
// printString("e2\n");
}
} else {
entry_factor = 0.0;
entry_factor = factor_for_safe_speed(current);
}
// printInteger(current->nominal_speed*1000);printString("<- ns\n\r");
// printInteger(entry_factor*1000); printString("<- entry-f\n\r");
// printInteger(exit_factor*1000); printString("<- exit-f\n\r");
// printInteger((uint16_t)current); printString("<-addr\n\r");
// Store result
current->entry_factor = entry_factor;
@ -227,13 +238,16 @@ void planner_reverse_pass_kernel(block_t *previous, block_t *current, block_t *n
void planner_reverse_pass() {
auto int8_t block_index = block_buffer_head;
block_t *block[3] = {NULL, NULL, NULL};
while(block_index != block_buffer_tail) {
while(block_index != block_buffer_tail) {
block_index--;
if(block_index < 0) {
block_index = BLOCK_BUFFER_SIZE-1;
}
// printInteger(block_index); printString(" <-- index");
block[2]= block[1];
block[1]= block[0];
block[0] = &block_buffer[block_index];
planner_reverse_pass_kernel(block[0], block[1], block[2]);
block_index--;
if(block_index < 0) {block_index = BLOCK_BUFFER_SIZE-1;}
}
planner_reverse_pass_kernel(NULL, block[0], block[1]);
}
@ -287,7 +301,7 @@ void planner_recalculate_trapezoids() {
}
block_index = (block_index+1) % BLOCK_BUFFER_SIZE;
}
calculate_trapezoid_for_block(next, next->entry_factor, 0.0);
calculate_trapezoid_for_block(next, next->entry_factor, factor_for_safe_speed(next));
}
// Recalculates the motion plan according to the following algorithm:
@ -355,11 +369,16 @@ void plan_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_
if (block->step_event_count == 0) { return; };
// Calculate speed in mm/minute for each axis
double multiplier = 60.0*1000000.0/microseconds;
// printInteger(multiplier*1000); printString("<-multi\n\r");
block->speed_x = steps_x*multiplier/settings.steps_per_mm[0];
block->speed_y = steps_y*multiplier/settings.steps_per_mm[1];
block->speed_z = steps_z*multiplier/settings.steps_per_mm[2];
block->nominal_speed = millimeters*multiplier;
// printInteger(millimeters*1000); printString("<-mm\n\r");
// printInteger(block->nominal_speed*1000); printString("<-ns\n\r");
block->nominal_rate = ceil(block->step_event_count*multiplier);
// printInteger(block->nominal_rate*1000); printString("<-nr\n\r");
// printInteger((uint16_t)block); printString("<-addr\n\r");
block->millimeters = millimeters;
block->entry_factor = 0.0;
@ -374,7 +393,8 @@ void plan_buffer_line(int32_t steps_x, int32_t steps_y, int32_t steps_z, uint32_
((settings.acceleration*60.0)/(ACCELERATION_TICKS_PER_SECOND))/ // acceleration mm/sec/sec per acceleration_tick
travel_per_step); // convert to: acceleration steps/min/acceleration_tick
if (acceleration_management) {
calculate_trapezoid_for_block(block,0,0); // compute a conservative acceleration trapezoid for now
double safe_speed_factor = factor_for_safe_speed(block);
calculate_trapezoid_for_block(block, safe_speed_factor, safe_speed_factor); // compute a conservative acceleration trapezoid for now
} else {
block->initial_rate = block->nominal_rate;
block->accelerate_until = 0;

5
stepper_plan.h
View File

@ -37,7 +37,7 @@ typedef struct {
// Fields used by the motion planner to manage acceleration
double speed_x, speed_y, speed_z; // Nominal mm/minute for each axis
double nominal_speed; // The nominal speed for this block in mm/min
double nominal_speed; // The nominal speed for this block in mm/min
double millimeters; // The total travel of this block in mm
double entry_factor; // The factor representing the change in speed at the start of this trapezoid.
// (The end of the curren speed trapezoid is defined by the entry_factor of the
@ -48,6 +48,9 @@ typedef struct {
int32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive)
uint32_t accelerate_until; // The index of the step event on which to stop acceleration
uint32_t decelerate_after; // The index of the step event on which to start decelerating
// Debug fields, PLEASE REMOVE
uint32_t exit_rate;
} block_t;
extern block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions