// 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; }
double entry_factor = 1.0;
double exit_factor;
if (next) {
exit_factor = next->entry_factor;
} else {
exit_factor = factor_for_safe_speed(current);
}
// Calculate the entry_factor for the current block.
if (previous) {
// Reduce speed so that junction_jerk is within the maximum allowed
double jerk = junction_jerk(previous, current);
if (jerk > settings.max_jerk) {
entry_factor = (settings.max_jerk/jerk);
}
// If the required deceleration across the block is too rapid, reduce the entry_factor accordingly.
if (entry_factor > exit_factor) {
double max_entry_speed = max_allowable_speed(-settings.acceleration,current->nominal_speed*exit_factor,
current->millimeters);
double max_entry_factor = max_entry_speed/current->nominal_speed;
if (max_entry_factor < entry_factor) {
entry_factor = max_entry_factor;
}
}
} else {
entry_factor = factor_for_safe_speed(current);
}
// Store result
current->entry_factor = entry_factor;
}
// Recalculates the trapezoid speed profiles for all blocks in the plan according to the
// entry_factor for each junction. Must be called by planner_recalculate() after
// updating the blocks.
void planner_recalculate_trapezoids() {
uint8_t block_index = block_buffer_tail;
block_t *current;
block_t *next = NULL;
while(block_index != block_buffer_head) {
current = next;
next = &block_buffer[block_index];
if (current) {
calculate_trapezoid_for_block(current, current->entry_factor, next->entry_factor);
}
block_index = (block_index+1);
block_index = block_index % BLOCK_BUFFER_SIZE;
}
calculate_trapezoid_for_block(next, next->entry_factor, factor_for_safe_speed(next));
}
// Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in
// mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
// calculation the caller must also provide the physical length of the line in millimeters.
void plan_buffer_line(double x, double y, double z, double feed_rate,
int invert_feed_rate)
{
// The target position of the tool in absolute steps
// Calculate target position in absolute steps
int32_t target[3];
target[X_AXIS] = lround(x * settings.steps_per_mm[X_AXIS]);
target[Y_AXIS] = lround(y * settings.steps_per_mm[Y_AXIS]);
target[Z_AXIS] = lround(z * settings.steps_per_mm[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.
// Rest here until there is room in the buffer.
while (block_buffer_tail == next_buffer_head) {
sleep_mode();
}
// Prepare to set up new block
block_t *block = &block_buffer[block_buffer_head];
// Number of steps for each 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);
} else {
microseconds = lround(ONE_MINUTE_OF_MICROSECONDS / feed_rate);
}
// Calculate speed in mm/minute for each axis
double multiplier = 60.0 * 1000000.0 / microseconds;
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
// average travel per step event changes. For a line along one axis the travel per step event
// is equal to the travel/step in the particular axis. For a 45 degree line the steppers of both
// axes might step for every step event. Travel per step event is then sqrt(travel_x^2+travel_y^2).
// To generate trapezoids with contant acceleration between blocks the rate_delta must be computed
// specifically for each line to compensate for this phenomenon:
double travel_per_step = block->millimeters / block->step_event_count;
block->rate_delta = ceil(((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_manager_enabled) {
// compute a preliminary conservative acceleration trapezoid
double safe_speed_factor = factor_for_safe_speed(block);
calculate_trapezoid_for_block(block, safe_speed_factor,
safe_speed_factor);
} else {
block->initial_rate = block->nominal_rate;
block->final_rate = block->nominal_rate;
block->accelerate_until = 0;
block->decelerate_after = block->step_event_count;
block->rate_delta = 0;
}
// Compute direction bits for this block
block->direction_bits = 0;
if (target[X_AXIS] < position[X_AXIS]) {
block->direction_bits |= (1 << X_DIRECTION_BIT);
}
if (target[Y_AXIS] < position[Y_AXIS]) {
block->direction_bits |= (1 << Y_DIRECTION_BIT);
}
if (target[Z_AXIS] < position[Z_AXIS]) {
block->direction_bits |= (1 << Z_DIRECTION_BIT);
}
// Move buffer head
block_buffer_head = next_buffer_head;
// Update position
memcpy(position, target, sizeof(target)); // position[] = target[]
if (acceleration_manager_enabled) {
planner_recalculate();
}
st_wake_up();
}
