// 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(); }