// Re-initialize buffer plan with a partially completed block, assumed to exist at the buffer tail.
// Called after a steppers have come to a complete stop for a feed hold and the cycle is stopped.
void plan_cycle_reinitialize()
{
// Re-plan from a complete stop. Reset planner entry speeds and buffer planned pointer.
st_update_plan_block_parameters();
block_buffer_planned = block_buffer_tail;
planner_recalculate();
}
// Re-initialize buffer plan with a partially completed block, assumed to exist at the buffer tail.
// Called after a steppers have come to a complete stop for a feed hold and the cycle is stopped.
void plan_cycle_reinitialize(int32_t step_events_remaining)
{
block_t *block = &block_buffer[block_buffer_tail]; // Point to partially completed block
// Only remaining millimeters and step_event_count need to be updated for planner recalculate.
// Other variables (step_x, step_y, step_z, rate_delta, etc.) all need to remain the same to
// ensure the original planned motion is resumed exactly.
block->millimeters = (block->millimeters*step_events_remaining)/block->step_event_count;
block->step_event_count = step_events_remaining;
// Re-plan from a complete stop. Reset planner entry speeds and flags.
block->entry_speed_sqr = 0.0;
block->max_entry_speed_sqr = 0.0;
block->recalculate_flag = true;
planner_recalculate();
}
void plan_buffer_wait (tActionRequest *pAction)
{
// Calculate the buffer head after we push this block
int next_buffer_head = next_block_index( block_buffer_head );
// 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];
//TODO
block->action_type = pAction->ActionType;
// every 50ms
block->millimeters = 10;
block->nominal_speed = 600;
block->nominal_rate = 20*60;
block->step_event_count = 1000;
// Acceleration planner disabled. Set minimum that is required.
block->entry_speed = block->nominal_speed;
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;
// Move buffer head
block_buffer_head = next_buffer_head;
if (acceleration_manager_enabled) { planner_recalculate(); }
st_wake_up();
}
// Add a new linear movement to the buffer. x, y and z is
// the signed, absolute target position in millimeters. Feed rate specifies the speed of the motion.
inline void planner_line(double x, double y, double z, double feed_rate, uint8_t nominal_laser_intensity, double pixel_width) {
// calculate target position in absolute steps
int32_t target[3];
target[X_AXIS] = lround(x*CONFIG_X_STEPS_PER_MM);
target[Y_AXIS] = lround(y*CONFIG_Y_STEPS_PER_MM);
target[Z_AXIS] = lround(z*CONFIG_Z_STEPS_PER_MM);
// calculate the buffer head and check for space
int next_buffer_head = next_block_index( block_buffer_head );
while(block_buffer_tail == next_buffer_head) { // buffer full condition
// good! We are well ahead of the robot. Rest here until buffer has room.
// sleep_mode();
protocol_idle();
}
// prepare to set up new block
block_t *block = &block_buffer[block_buffer_head];
// set block type to line command
if (pixel_width != 0.0) {
block->type = TYPE_RASTER_LINE;
block->pixel_steps_x1024 = lround(pixel_width*CONFIG_X_STEPS_PER_MM*1024);
} else {
block->type = TYPE_LINE;
}
// set nominal laser intensity
block->nominal_laser_intensity = nominal_laser_intensity;
// 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); }
// 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));
if (block->step_event_count == 0) { return; }; // bail if this is a zero-length block
// compute path vector in terms of absolute step target and current positions
double delta_mm[3];
delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/CONFIG_X_STEPS_PER_MM;
delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/CONFIG_Y_STEPS_PER_MM;
delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/CONFIG_Z_STEPS_PER_MM;
block->millimeters = sqrt( (delta_mm[X_AXIS]*delta_mm[X_AXIS]) +
(delta_mm[Y_AXIS]*delta_mm[Y_AXIS]) +
(delta_mm[Z_AXIS]*delta_mm[Z_AXIS]) );
double inverse_millimeters = 1.0/block->millimeters; // store for efficency
// calculate nominal_speed (mm/min) and nominal_rate (step/min)
// minimum stepper speed is limited by MINIMUM_STEPS_PER_MINUTE in stepper.c
double inverse_minute = feed_rate * inverse_millimeters;
block->nominal_speed = feed_rate; // always > 0
block->nominal_rate = ceil(block->step_event_count * inverse_minute); // always > 0
// compute the acceleration rate for this block. (step/min/acceleration_tick)
block->rate_delta = ceil( block->step_event_count * inverse_millimeters
* CONFIG_ACCELERATION / (60 * ACCELERATION_TICKS_PER_SECOND) );
//// acceleeration manager calculations
// Compute path unit vector
double unit_vec[3];
unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;
// Compute max junction speed by centripetal acceleration approximation.
// Let a circle be tangent to both previous and current path line segments, where the junction
// deviation is defined as the distance from the junction to the closest edge of the circle,
// colinear with the circle center. The circular segment joining the two paths represents the
// path of centripetal acceleration. Solve for max velocity based on max acceleration about the
// radius of the circle, defined indirectly by junction deviation. This may be also viewed as
// path width or max_jerk in the previous grbl version. This approach does not actually deviate
// from path, but used as a robust way to compute cornering speeds, as it takes into account the
// nonlinearities of both the junction angle and junction velocity.
double vmax_junction = ZERO_SPEED; // prime for junctions close to 0 degree
if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
// Compute cosine of angle between previous and current path.
// vmax_junction is computed without sin() or acos() by trig half angle identity.
double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
- previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
- previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
if (cos_theta < 0.95) {
// any junction *not* close to 0 degree
vmax_junction = min(previous_nominal_speed, block->nominal_speed); // prime for close to 180
if (cos_theta > -0.95) {
// any junction not close to neither 0 and 180 degree -> compute vmax
double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
vmax_junction = min( vmax_junction, sqrt( CONFIG_ACCELERATION * CONFIG_JUNCTION_DEVIATION
* sin_theta_d2/(1.0-sin_theta_d2) ) );
}
}
}
block->vmax_junction = vmax_junction;
// Initialize entry_speed. Compute based on deceleration to zero.
// This will be updated in the forward and reverse planner passes.
double v_allowable = max_allowable_speed(-CONFIG_ACCELERATION, ZERO_SPEED, block->millimeters);
block->entry_speed = min(vmax_junction, v_allowable);
// Set nominal_length_flag for more efficiency.
// If a block can de/ac-celerate from nominal speed to zero within the length of
// the block, then the speed will always be at the the maximum junction speed and
// may always be ignored for any speed reduction checks.
if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = true; }
else { block->nominal_length_flag = false; }
block->recalculate_flag = true; // always calculate trapezoid for new block
// update previous unit_vector and nominal speed
memcpy(previous_unit_vec, unit_vec, sizeof(unit_vec)); // previous_unit_vec[] = unit_vec[]
previous_nominal_speed = block->nominal_speed;
//// end of acceleeration manager calculations
// move buffer head and update position
block_buffer_head = next_buffer_head;
memcpy(position, target, sizeof(target)); // position[] = target[]
planner_recalculate();
// make sure the stepper interrupt is processing
stepper_start_processing();
}
// 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(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder)
{
// Calculate the buffer head after we push this byte
int next_buffer_head = next_block_index(block_buffer_head);
// 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)
{
manage_heater();
manage_inactivity();
lcd_update();
}
// The target position of the tool in absolute steps
// Calculate target position in absolute steps
//this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
long target[4];
target[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);
#ifdef PREVENT_DANGEROUS_EXTRUDE
if(target[E_AXIS]!=position[E_AXIS])
{
if(degHotend(active_extruder)<extrude_min_temp)
{
position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
}
#ifdef PREVENT_LENGTHY_EXTRUDE
if(labs(target[E_AXIS]-position[E_AXIS])>axis_steps_per_unit[E_AXIS]*EXTRUDE_MAXLENGTH)
{
#ifdef EASY_LOAD
if (!allow_lengthy_extrude_once) {
#endif
position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
#ifdef EASY_LOAD
}
allow_lengthy_extrude_once = false;
#endif
}
#endif // PREVENT_LENGTHY_EXTRUDE
}
#endif
// Prepare to set up new block
block_t *block = &block_buffer[block_buffer_head];
// Mark block as not busy (Not executed by the stepper interrupt)
block->busy = false;
// Number of steps for each axis
#ifndef COREXY
// default non-h-bot planning
block->steps_x = labs(target[X_AXIS]-position[X_AXIS]);
block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]);
#else
// corexy planning
// these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
block->steps_x = labs((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]));
block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]));
#endif
block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]);
block->steps_e = labs(target[E_AXIS]-position[E_AXIS]);
block->steps_e *= extrudemultiply;
block->steps_e /= 100;
block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_e)));
// Bail if this is a zero-length block
if (block->step_event_count <= dropsegments)
{
return;
}
block->fan_speed = fanSpeed;
#ifdef BARICUDA
block->valve_pressure = ValvePressure;
block->e_to_p_pressure = EtoPPressure;
#endif
// Compute direction bits for this block
block->direction_bits = 0;
#ifndef COREXY
if (target[X_AXIS] < position[X_AXIS])
{
block->direction_bits |= (1<<X_AXIS);
}
if (target[Y_AXIS] < position[Y_AXIS])
{
block->direction_bits |= (1<<Y_AXIS);
}
#else
if ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]) < 0)
{
block->direction_bits |= (1<<X_AXIS);
}
if ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]) < 0)
{
block->direction_bits |= (1<<Y_AXIS);
}
#endif
if (target[Z_AXIS] < position[Z_AXIS])
{
block->direction_bits |= (1<<Z_AXIS);
}
if (target[E_AXIS] < position[E_AXIS])
{
block->direction_bits |= (1<<E_AXIS);
}
block->active_extruder = extruder;
//enable active axes
#ifdef COREXY
if((block->steps_x != 0) || (block->steps_y != 0))
{
enable_x();
enable_y();
}
#else
if(block->steps_x != 0) enable_x();
if(block->steps_y != 0) enable_y();
#endif
#ifndef Z_LATE_ENABLE
if(block->steps_z != 0) enable_z();
#endif
// Enable all
if(block->steps_e != 0)
{
enable_e0();
enable_e1();
enable_e2();
}
if (block->steps_e == 0)
{
if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate;
}
else
{
if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate;
}
float delta_mm[4];
#ifndef COREXY
delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];
delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
#else
delta_mm[X_AXIS] = ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[X_AXIS];
delta_mm[Y_AXIS] = ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[Y_AXIS];
#endif
delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS];
delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS])*extrudemultiply/100.0;
if ( block->steps_x <=dropsegments && block->steps_y <=dropsegments && block->steps_z <=dropsegments )
{
block->millimeters = fabs(delta_mm[E_AXIS]);
}
else
{
block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS]));
}
float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides
// Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
float inverse_second = feed_rate * inverse_millimeters;
int moves_queued=(block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1);
// slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill
#ifdef OLD_SLOWDOWN
if(moves_queued < (BLOCK_BUFFER_SIZE * 0.5) && moves_queued > 1)
feed_rate = feed_rate*moves_queued / (BLOCK_BUFFER_SIZE * 0.5);
#endif
#ifdef SLOWDOWN
// segment time im micro seconds
unsigned long segment_time = lround(1000000.0/inverse_second);
if ((moves_queued > 1) && (moves_queued < (BLOCK_BUFFER_SIZE * 0.5)))
{
if (segment_time < minsegmenttime)
{ // buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
inverse_second=1000000.0/(segment_time+lround(2*(minsegmenttime-segment_time)/moves_queued));
#ifdef XY_FREQUENCY_LIMIT
segment_time = lround(1000000.0/inverse_second);
#endif
}
}
#endif
// END OF SLOW DOWN SECTION
block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0
block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0
// Calculate and limit speed in mm/sec for each axis
float current_speed[4];
float speed_factor = 1.0; //factor <=1 do decrease speed
for(int i=0; i < 4; i++)
{
current_speed[i] = delta_mm[i] * inverse_second;
if(fabs(current_speed[i]) > max_feedrate[i])
speed_factor = min(speed_factor, max_feedrate[i] / fabs(current_speed[i]));
}
// Max segement time in us.
#ifdef XY_FREQUENCY_LIMIT
#define MAX_FREQ_TIME (1000000.0/XY_FREQUENCY_LIMIT)
// Check and limit the xy direction change frequency
unsigned char direction_change = block->direction_bits ^ old_direction_bits;
old_direction_bits = block->direction_bits;
segment_time = lround((float)segment_time / speed_factor);
if((direction_change & (1<<X_AXIS)) == 0)
{
x_segment_time[0] += segment_time;
}
else
{
x_segment_time[2] = x_segment_time[1];
x_segment_time[1] = x_segment_time[0];
x_segment_time[0] = segment_time;
}
if((direction_change & (1<<Y_AXIS)) == 0)
{
y_segment_time[0] += segment_time;
}
else
{
y_segment_time[2] = y_segment_time[1];
y_segment_time[1] = y_segment_time[0];
y_segment_time[0] = segment_time;
}
long max_x_segment_time = max(x_segment_time[0], max(x_segment_time[1], x_segment_time[2]));
long max_y_segment_time = max(y_segment_time[0], max(y_segment_time[1], y_segment_time[2]));
long min_xy_segment_time =min(max_x_segment_time, max_y_segment_time);
if(min_xy_segment_time < MAX_FREQ_TIME)
speed_factor = min(speed_factor, speed_factor * (float)min_xy_segment_time / (float)MAX_FREQ_TIME);
#endif
// Correct the speed
if( speed_factor < 1.0)
{
for(unsigned char i=0; i < 4; i++)
{
current_speed[i] *= speed_factor;
}
block->nominal_speed *= speed_factor;
block->nominal_rate *= speed_factor;
}
// Compute and limit the acceleration rate for the trapezoid generator.
float steps_per_mm = block->step_event_count/block->millimeters;
if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)
{
block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
}
else
{
block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
// Limit acceleration per axis
if(((float)block->acceleration_st * (float)block->steps_x / (float)block->step_event_count) > axis_steps_per_sqr_second[X_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[X_AXIS];
if(((float)block->acceleration_st * (float)block->steps_y / (float)block->step_event_count) > axis_steps_per_sqr_second[Y_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS];
if(((float)block->acceleration_st * (float)block->steps_e / (float)block->step_event_count) > axis_steps_per_sqr_second[E_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[E_AXIS];
if(((float)block->acceleration_st * (float)block->steps_z / (float)block->step_event_count ) > axis_steps_per_sqr_second[Z_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS];
}
block->acceleration = block->acceleration_st / steps_per_mm;
block->acceleration_rate = (long)((float)block->acceleration_st * (16777216.0 / (F_CPU / 8.0)));
#if 0 // Use old jerk for now
// Compute path unit vector
double unit_vec[3];
unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;
// Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
// Let a circle be tangent to both previous and current path line segments, where the junction
// deviation is defined as the distance from the junction to the closest edge of the circle,
// colinear with the circle center. The circular segment joining the two paths represents the
// path of centripetal acceleration. Solve for max velocity based on max acceleration about the
// radius of the circle, defined indirectly by junction deviation. This may be also viewed as
// path width or max_jerk in the previous grbl version. This approach does not actually deviate
// from path, but used as a robust way to compute cornering speeds, as it takes into account the
// nonlinearities of both the junction angle and junction velocity.
double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
- previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
- previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
// Skip and use default max junction speed for 0 degree acute junction.
if (cos_theta < 0.95) {
vmax_junction = min(previous_nominal_speed,block->nominal_speed);
// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
if (cos_theta > -0.95) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation
double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
vmax_junction = min(vmax_junction,
sqrt(block->acceleration * junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
}
}
}
#endif
// Start with a safe speed
float vmax_junction = max_xy_jerk/2;
float vmax_junction_factor = 1.0;
if(fabs(current_speed[Z_AXIS]) > max_z_jerk/2)
vmax_junction = min(vmax_junction, max_z_jerk/2);
if(fabs(current_speed[E_AXIS]) > max_e_jerk/2)
vmax_junction = min(vmax_junction, max_e_jerk/2);
vmax_junction = min(vmax_junction, block->nominal_speed);
float safe_speed = vmax_junction;
if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2));
// if((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
vmax_junction = block->nominal_speed;
// }
if (jerk > max_xy_jerk) {
vmax_junction_factor = (max_xy_jerk/jerk);
}
if(fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]) > max_z_jerk) {
vmax_junction_factor= min(vmax_junction_factor, (max_z_jerk/fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS])));
}
if(fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]) > max_e_jerk) {
vmax_junction_factor = min(vmax_junction_factor, (max_e_jerk/fabs(current_speed[E_AXIS] - previous_speed[E_AXIS])));
}
vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
}
block->max_entry_speed = vmax_junction;
// Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
double v_allowable = max_allowable_speed(-block->acceleration,MINIMUM_PLANNER_SPEED,block->millimeters);
block->entry_speed = min(vmax_junction, v_allowable);
// Initialize planner efficiency flags
// Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
// If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
// the current block and next block junction speeds are guaranteed to always be at their maximum
// junction speeds in deceleration and acceleration, respectively. This is due to how the current
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
// the reverse and forward planners, the corresponding block junction speed will always be at the
// the maximum junction speed and may always be ignored for any speed reduction checks.
if (block->nominal_speed <= v_allowable) {
block->nominal_length_flag = true;
}
else {
block->nominal_length_flag = false;
}
block->recalculate_flag = true; // Always calculate trapezoid for new block
// Update previous path unit_vector and nominal speed
memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[]
previous_nominal_speed = block->nominal_speed;
#ifdef ADVANCE
// Calculate advance rate
if((block->steps_e == 0) || (block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)) {
block->advance_rate = 0;
block->advance = 0;
}
else {
long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st);
float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) *
(current_speed[E_AXIS] * current_speed[E_AXIS] * EXTRUTION_AREA * EXTRUTION_AREA)*256;
block->advance = advance;
if(acc_dist == 0) {
block->advance_rate = 0;
}
else {
block->advance_rate = advance / (float)acc_dist;
}
}
/*
SERIAL_ECHO_START;
SERIAL_ECHOPGM("advance :");
SERIAL_ECHO(block->advance/256.0);
SERIAL_ECHOPGM("advance rate :");
SERIAL_ECHOLN(block->advance_rate/256.0);
*/
#endif // ADVANCE
calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed,
safe_speed/block->nominal_speed);
// Move buffer head
block_buffer_head = next_buffer_head;
// Update position
memcpy(position, target, sizeof(target)); // position[] = target[]
planner_recalculate();
st_wake_up();
}
void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t extruder)
#endif // AUTO_BED_LEVELING_FEATURE
{
// Calculate the buffer head after we push this byte
int next_buffer_head = next_block_index(block_buffer_head);
// 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) idle();
#if ENABLED(MESH_BED_LEVELING)
if (mbl.active) z += mbl.get_z(x, y);
#elif ENABLED(AUTO_BED_LEVELING_FEATURE)
apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
#endif
// The target position of the tool in absolute steps
// Calculate target position in absolute steps
//this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
long target[NUM_AXIS];
target[X_AXIS] = lround(x * axis_steps_per_unit[X_AXIS]);
target[Y_AXIS] = lround(y * axis_steps_per_unit[Y_AXIS]);
target[Z_AXIS] = lround(z * axis_steps_per_unit[Z_AXIS]);
target[E_AXIS] = lround(e * axis_steps_per_unit[E_AXIS]);
float dx = target[X_AXIS] - position[X_AXIS],
dy = target[Y_AXIS] - position[Y_AXIS],
dz = target[Z_AXIS] - position[Z_AXIS];
// DRYRUN ignores all temperature constraints and assures that the extruder is instantly satisfied
if (marlin_debug_flags & DEBUG_DRYRUN)
position[E_AXIS] = target[E_AXIS];
float de = target[E_AXIS] - position[E_AXIS];
#if ENABLED(PREVENT_DANGEROUS_EXTRUDE)
if (de) {
if (degHotend(extruder) < extrude_min_temp) {
position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part
de = 0; // no difference
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
}
#if ENABLED(PREVENT_LENGTHY_EXTRUDE)
if (labs(de) > axis_steps_per_unit[E_AXIS] * EXTRUDE_MAXLENGTH) {
position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part
de = 0; // no difference
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
}
#endif
}
#endif
// Prepare to set up new block
block_t *block = &block_buffer[block_buffer_head];
// Mark block as not busy (Not executed by the stepper interrupt)
block->busy = false;
// Number of steps for each axis
#if ENABLED(COREXY)
// corexy planning
// these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
block->steps[A_AXIS] = labs(dx + dy);
block->steps[B_AXIS] = labs(dx - dy);
block->steps[Z_AXIS] = labs(dz);
#elif ENABLED(COREXZ)
// corexz planning
block->steps[A_AXIS] = labs(dx + dz);
block->steps[Y_AXIS] = labs(dy);
block->steps[C_AXIS] = labs(dx - dz);
#else
// default non-h-bot planning
block->steps[X_AXIS] = labs(dx);
block->steps[Y_AXIS] = labs(dy);
block->steps[Z_AXIS] = labs(dz);
#endif
block->steps[E_AXIS] = labs(de);
block->steps[E_AXIS] *= volumetric_multiplier[extruder];
block->steps[E_AXIS] *= extruder_multiplier[extruder];
block->steps[E_AXIS] /= 100;
block->step_event_count = max(block->steps[X_AXIS], max(block->steps[Y_AXIS], max(block->steps[Z_AXIS], block->steps[E_AXIS])));
// Bail if this is a zero-length block
if (block->step_event_count <= dropsegments) return;
block->fan_speed = fanSpeed;
#if ENABLED(BARICUDA)
block->valve_pressure = ValvePressure;
block->e_to_p_pressure = EtoPPressure;
#endif
// Compute direction bits for this block
uint8_t db = 0;
#if ENABLED(COREXY)
if (dx < 0) db |= BIT(X_HEAD); // Save the real Extruder (head) direction in X Axis
if (dy < 0) db |= BIT(Y_HEAD); // ...and Y
if (dz < 0) db |= BIT(Z_AXIS);
if (dx + dy < 0) db |= BIT(A_AXIS); // Motor A direction
if (dx - dy < 0) db |= BIT(B_AXIS); // Motor B direction
#elif ENABLED(COREXZ)
if (dx < 0) db |= BIT(X_HEAD); // Save the real Extruder (head) direction in X Axis
if (dy < 0) db |= BIT(Y_AXIS);
if (dz < 0) db |= BIT(Z_HEAD); // ...and Z
if (dx + dz < 0) db |= BIT(A_AXIS); // Motor A direction
if (dx - dz < 0) db |= BIT(C_AXIS); // Motor B direction
#else
if (dx < 0) db |= BIT(X_AXIS);
if (dy < 0) db |= BIT(Y_AXIS);
if (dz < 0) db |= BIT(Z_AXIS);
#endif
if (de < 0) db |= BIT(E_AXIS);
block->direction_bits = db;
block->active_extruder = extruder;
//enable active axes
#if ENABLED(COREXY)
if (block->steps[A_AXIS] || block->steps[B_AXIS]) {
enable_x();
enable_y();
}
#if DISABLED(Z_LATE_ENABLE)
if (block->steps[Z_AXIS]) enable_z();
#endif
#elif ENABLED(COREXZ)
if (block->steps[A_AXIS] || block->steps[C_AXIS]) {
enable_x();
enable_z();
}
if (block->steps[Y_AXIS]) enable_y();
#else
if (block->steps[X_AXIS]) enable_x();
if (block->steps[Y_AXIS]) enable_y();
#if DISABLED(Z_LATE_ENABLE)
if (block->steps[Z_AXIS]) enable_z();
#endif
#endif
// Enable extruder(s)
if (block->steps[E_AXIS]) {
if (DISABLE_INACTIVE_EXTRUDER) { //enable only selected extruder
for (int i=0; i<EXTRUDERS; i++)
if (g_uc_extruder_last_move[i] > 0) g_uc_extruder_last_move[i]--;
switch(extruder) {
case 0:
enable_e0();
g_uc_extruder_last_move[0] = BLOCK_BUFFER_SIZE * 2;
#if EXTRUDERS > 1
if (g_uc_extruder_last_move[1] == 0) disable_e1();
#if EXTRUDERS > 2
if (g_uc_extruder_last_move[2] == 0) disable_e2();
#if EXTRUDERS > 3
if (g_uc_extruder_last_move[3] == 0) disable_e3();
#endif
#endif
#endif
break;
#if EXTRUDERS > 1
case 1:
enable_e1();
g_uc_extruder_last_move[1] = BLOCK_BUFFER_SIZE * 2;
if (g_uc_extruder_last_move[0] == 0) disable_e0();
#if EXTRUDERS > 2
if (g_uc_extruder_last_move[2] == 0) disable_e2();
#if EXTRUDERS > 3
if (g_uc_extruder_last_move[3] == 0) disable_e3();
#endif
#endif
break;
#if EXTRUDERS > 2
case 2:
enable_e2();
g_uc_extruder_last_move[2] = BLOCK_BUFFER_SIZE * 2;
if (g_uc_extruder_last_move[0] == 0) disable_e0();
if (g_uc_extruder_last_move[1] == 0) disable_e1();
#if EXTRUDERS > 3
if (g_uc_extruder_last_move[3] == 0) disable_e3();
#endif
break;
#if EXTRUDERS > 3
case 3:
enable_e3();
g_uc_extruder_last_move[3] = BLOCK_BUFFER_SIZE * 2;
if (g_uc_extruder_last_move[0] == 0) disable_e0();
if (g_uc_extruder_last_move[1] == 0) disable_e1();
if (g_uc_extruder_last_move[2] == 0) disable_e2();
break;
#endif // EXTRUDERS > 3
#endif // EXTRUDERS > 2
#endif // EXTRUDERS > 1
}
}
else { // enable all
enable_e0();
enable_e1();
enable_e2();
enable_e3();
}
}
if (block->steps[E_AXIS])
NOLESS(feed_rate, minimumfeedrate);
else
NOLESS(feed_rate, mintravelfeedrate);
/**
* This part of the code calculates the total length of the movement.
* For cartesian bots, the X_AXIS is the real X movement and same for Y_AXIS.
* But for corexy bots, that is not true. The "X_AXIS" and "Y_AXIS" motors (that should be named to A_AXIS
* and B_AXIS) cannot be used for X and Y length, because A=X+Y and B=X-Y.
* So we need to create other 2 "AXIS", named X_HEAD and Y_HEAD, meaning the real displacement of the Head.
* Having the real displacement of the head, we can calculate the total movement length and apply the desired speed.
*/
#if ENABLED(COREXY)
float delta_mm[6];
delta_mm[X_HEAD] = dx / axis_steps_per_unit[A_AXIS];
delta_mm[Y_HEAD] = dy / axis_steps_per_unit[B_AXIS];
delta_mm[Z_AXIS] = dz / axis_steps_per_unit[Z_AXIS];
delta_mm[A_AXIS] = (dx + dy) / axis_steps_per_unit[A_AXIS];
delta_mm[B_AXIS] = (dx - dy) / axis_steps_per_unit[B_AXIS];
#elif ENABLED(COREXZ)
float delta_mm[6];
delta_mm[X_HEAD] = dx / axis_steps_per_unit[A_AXIS];
delta_mm[Y_AXIS] = dy / axis_steps_per_unit[Y_AXIS];
delta_mm[Z_HEAD] = dz / axis_steps_per_unit[C_AXIS];
delta_mm[A_AXIS] = (dx + dz) / axis_steps_per_unit[A_AXIS];
delta_mm[C_AXIS] = (dx - dz) / axis_steps_per_unit[C_AXIS];
#else
float delta_mm[4];
delta_mm[X_AXIS] = dx / axis_steps_per_unit[X_AXIS];
delta_mm[Y_AXIS] = dy / axis_steps_per_unit[Y_AXIS];
delta_mm[Z_AXIS] = dz / axis_steps_per_unit[Z_AXIS];
#endif
delta_mm[E_AXIS] = (de / axis_steps_per_unit[E_AXIS]) * volumetric_multiplier[extruder] * extruder_multiplier[extruder] / 100.0;
if (block->steps[X_AXIS] <= dropsegments && block->steps[Y_AXIS] <= dropsegments && block->steps[Z_AXIS] <= dropsegments) {
block->millimeters = fabs(delta_mm[E_AXIS]);
}
else {
block->millimeters = sqrt(
#if ENABLED(COREXY)
square(delta_mm[X_HEAD]) + square(delta_mm[Y_HEAD]) + square(delta_mm[Z_AXIS])
#elif ENABLED(COREXZ)
square(delta_mm[X_HEAD]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_HEAD])
#else
square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS])
#endif
);
}
float inverse_millimeters = 1.0 / block->millimeters; // Inverse millimeters to remove multiple divides
// Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
float inverse_second = feed_rate * inverse_millimeters;
int moves_queued = movesplanned();
// Slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
#if ENABLED(OLD_SLOWDOWN) || ENABLED(SLOWDOWN)
bool mq = moves_queued > 1 && moves_queued < BLOCK_BUFFER_SIZE / 2;
#if ENABLED(OLD_SLOWDOWN)
if (mq) feed_rate *= 2.0 * moves_queued / BLOCK_BUFFER_SIZE;
#endif
#if ENABLED(SLOWDOWN)
// segment time im micro seconds
unsigned long segment_time = lround(1000000.0/inverse_second);
if (mq) {
if (segment_time < minsegmenttime) {
// buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
inverse_second = 1000000.0 / (segment_time + lround(2 * (minsegmenttime - segment_time) / moves_queued));
#ifdef XY_FREQUENCY_LIMIT
segment_time = lround(1000000.0 / inverse_second);
#endif
}
}
#endif
#endif
block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0
block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0
#if ENABLED(FILAMENT_SENSOR)
//FMM update ring buffer used for delay with filament measurements
if (extruder == FILAMENT_SENSOR_EXTRUDER_NUM && delay_index2 > -1) { //only for extruder with filament sensor and if ring buffer is initialized
const int MMD = MAX_MEASUREMENT_DELAY + 1, MMD10 = MMD * 10;
delay_dist += delta_mm[E_AXIS]; // increment counter with next move in e axis
while (delay_dist >= MMD10) delay_dist -= MMD10; // loop around the buffer
while (delay_dist < 0) delay_dist += MMD10;
delay_index1 = delay_dist / 10.0; // calculate index
delay_index1 = constrain(delay_index1, 0, MAX_MEASUREMENT_DELAY); // (already constrained above)
if (delay_index1 != delay_index2) { // moved index
meas_sample = widthFil_to_size_ratio() - 100; // Subtract 100 to reduce magnitude - to store in a signed char
while (delay_index1 != delay_index2) {
// Increment and loop around buffer
if (++delay_index2 >= MMD) delay_index2 -= MMD;
delay_index2 = constrain(delay_index2, 0, MAX_MEASUREMENT_DELAY);
measurement_delay[delay_index2] = meas_sample;
}
}
}
#endif
// Calculate and limit speed in mm/sec for each axis
float current_speed[NUM_AXIS];
float speed_factor = 1.0; //factor <=1 do decrease speed
for (int i = 0; i < NUM_AXIS; i++) {
current_speed[i] = delta_mm[i] * inverse_second;
float cs = fabs(current_speed[i]), mf = max_feedrate[i];
if (cs > mf) speed_factor = min(speed_factor, mf / cs);
}
// Max segement time in us.
#ifdef XY_FREQUENCY_LIMIT
#define MAX_FREQ_TIME (1000000.0 / XY_FREQUENCY_LIMIT)
// Check and limit the xy direction change frequency
unsigned char direction_change = block->direction_bits ^ old_direction_bits;
old_direction_bits = block->direction_bits;
segment_time = lround((float)segment_time / speed_factor);
long xs0 = axis_segment_time[X_AXIS][0],
xs1 = axis_segment_time[X_AXIS][1],
xs2 = axis_segment_time[X_AXIS][2],
ys0 = axis_segment_time[Y_AXIS][0],
ys1 = axis_segment_time[Y_AXIS][1],
ys2 = axis_segment_time[Y_AXIS][2];
if ((direction_change & BIT(X_AXIS)) != 0) {
xs2 = axis_segment_time[X_AXIS][2] = xs1;
xs1 = axis_segment_time[X_AXIS][1] = xs0;
xs0 = 0;
}
xs0 = axis_segment_time[X_AXIS][0] = xs0 + segment_time;
if ((direction_change & BIT(Y_AXIS)) != 0) {
ys2 = axis_segment_time[Y_AXIS][2] = axis_segment_time[Y_AXIS][1];
ys1 = axis_segment_time[Y_AXIS][1] = axis_segment_time[Y_AXIS][0];
ys0 = 0;
}
ys0 = axis_segment_time[Y_AXIS][0] = ys0 + segment_time;
long max_x_segment_time = max(xs0, max(xs1, xs2)),
max_y_segment_time = max(ys0, max(ys1, ys2)),
min_xy_segment_time = min(max_x_segment_time, max_y_segment_time);
if (min_xy_segment_time < MAX_FREQ_TIME) {
float low_sf = speed_factor * min_xy_segment_time / MAX_FREQ_TIME;
speed_factor = min(speed_factor, low_sf);
}
#endif // XY_FREQUENCY_LIMIT
// Correct the speed
if (speed_factor < 1.0) {
for (unsigned char i = 0; i < NUM_AXIS; i++) current_speed[i] *= speed_factor;
block->nominal_speed *= speed_factor;
block->nominal_rate *= speed_factor;
}
// Compute and limit the acceleration rate for the trapezoid generator.
float steps_per_mm = block->step_event_count / block->millimeters;
long bsx = block->steps[X_AXIS], bsy = block->steps[Y_AXIS], bsz = block->steps[Z_AXIS], bse = block->steps[E_AXIS];
if (bsx == 0 && bsy == 0 && bsz == 0) {
block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
}
else if (bse == 0) {
block->acceleration_st = ceil(travel_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
}
else {
block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
}
// Limit acceleration per axis
unsigned long acc_st = block->acceleration_st,
xsteps = axis_steps_per_sqr_second[X_AXIS],
ysteps = axis_steps_per_sqr_second[Y_AXIS],
zsteps = axis_steps_per_sqr_second[Z_AXIS],
esteps = axis_steps_per_sqr_second[E_AXIS];
if ((float)acc_st * bsx / block->step_event_count > xsteps) acc_st = xsteps;
if ((float)acc_st * bsy / block->step_event_count > ysteps) acc_st = ysteps;
if ((float)acc_st * bsz / block->step_event_count > zsteps) acc_st = zsteps;
if ((float)acc_st * bse / block->step_event_count > esteps) acc_st = esteps;
block->acceleration_st = acc_st;
block->acceleration = acc_st / steps_per_mm;
block->acceleration_rate = (long)(acc_st * 16777216.0 / (F_CPU / 8.0));
#if 0 // Use old jerk for now
// Compute path unit vector
double unit_vec[3];
unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;
// Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
// Let a circle be tangent to both previous and current path line segments, where the junction
// deviation is defined as the distance from the junction to the closest edge of the circle,
// colinear with the circle center. The circular segment joining the two paths represents the
// path of centripetal acceleration. Solve for max velocity based on max acceleration about the
// radius of the circle, defined indirectly by junction deviation. This may be also viewed as
// path width or max_jerk in the previous grbl version. This approach does not actually deviate
// from path, but used as a robust way to compute cornering speeds, as it takes into account the
// nonlinearities of both the junction angle and junction velocity.
double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
- previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
- previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
// Skip and use default max junction speed for 0 degree acute junction.
if (cos_theta < 0.95) {
vmax_junction = min(previous_nominal_speed,block->nominal_speed);
// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
if (cos_theta > -0.95) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation
double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
vmax_junction = min(vmax_junction,
sqrt(block->acceleration * junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
}
}
}
#endif
// Start with a safe speed
float vmax_junction = max_xy_jerk / 2;
float vmax_junction_factor = 1.0;
float mz2 = max_z_jerk / 2, me2 = max_e_jerk / 2;
float csz = current_speed[Z_AXIS], cse = current_speed[E_AXIS];
if (fabs(csz) > mz2) vmax_junction = min(vmax_junction, mz2);
if (fabs(cse) > me2) vmax_junction = min(vmax_junction, me2);
vmax_junction = min(vmax_junction, block->nominal_speed);
float safe_speed = vmax_junction;
if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
float dx = current_speed[X_AXIS] - previous_speed[X_AXIS],
dy = current_speed[Y_AXIS] - previous_speed[Y_AXIS],
dz = fabs(csz - previous_speed[Z_AXIS]),
de = fabs(cse - previous_speed[E_AXIS]),
jerk = sqrt(dx * dx + dy * dy);
// if ((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
vmax_junction = block->nominal_speed;
// }
if (jerk > max_xy_jerk) vmax_junction_factor = max_xy_jerk / jerk;
if (dz > max_z_jerk) vmax_junction_factor = min(vmax_junction_factor, max_z_jerk / dz);
if (de > max_e_jerk) vmax_junction_factor = min(vmax_junction_factor, max_e_jerk / de);
vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
}
block->max_entry_speed = vmax_junction;
// Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
double v_allowable = max_allowable_speed(-block->acceleration, MINIMUM_PLANNER_SPEED, block->millimeters);
block->entry_speed = min(vmax_junction, v_allowable);
// Initialize planner efficiency flags
// Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
// If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
// the current block and next block junction speeds are guaranteed to always be at their maximum
// junction speeds in deceleration and acceleration, respectively. This is due to how the current
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
// the reverse and forward planners, the corresponding block junction speed will always be at the
// the maximum junction speed and may always be ignored for any speed reduction checks.
block->nominal_length_flag = (block->nominal_speed <= v_allowable);
block->recalculate_flag = true; // Always calculate trapezoid for new block
// Update previous path unit_vector and nominal speed
for (int i = 0; i < NUM_AXIS; i++) previous_speed[i] = current_speed[i];
previous_nominal_speed = block->nominal_speed;
#if ENABLED(ADVANCE)
// Calculate advance rate
if (!bse || (!bsx && !bsy && !bsz)) {
block->advance_rate = 0;
block->advance = 0;
}
else {
long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st);
float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) * (cse * cse * EXTRUSION_AREA * EXTRUSION_AREA) * 256;
block->advance = advance;
block->advance_rate = acc_dist ? advance / (float)acc_dist : 0;
}
/*
SERIAL_ECHO_START;
SERIAL_ECHOPGM("advance :");
SERIAL_ECHO(block->advance/256.0);
SERIAL_ECHOPGM("advance rate :");
SERIAL_ECHOLN(block->advance_rate/256.0);
*/
#endif // ADVANCE
calculate_trapezoid_for_block(block, block->entry_speed / block->nominal_speed, safe_speed / block->nominal_speed);
// Move buffer head
block_buffer_head = next_buffer_head;
// Update position
for (int i = 0; i < NUM_AXIS; i++) position[i] = target[i];
planner_recalculate();
st_wake_up();
} // plan_buffer_line()
// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in
// millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed
// rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes.
// All position data passed to the planner must be in terms of machine position to keep the planner
// independent of any coordinate system changes and offsets, which are handled by the g-code parser.
// NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control.
void plan_buffer_line(double x, double y, double z, double c, double feed_rate, uint8_t invert_feed_rate)
{
// Prepare to set up new block
block_t *block = &block_buffer[block_buffer_head];
// Calculate target position in absolute steps
int32_t target[4];
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]);
target[C_AXIS] = lround(c*settings.steps_per_mm[C_AXIS]);
// Compute direction bits for this block
block->direction_bits = 0;
if (target[X_AXIS] < pl.position[X_AXIS]) { block->direction_bits |= (1<<X_DIRECTION_BIT); }
if (target[Y_AXIS] < pl.position[Y_AXIS]) { block->direction_bits |= (1<<Y_DIRECTION_BIT); }
if (target[Z_AXIS] < pl.position[Z_AXIS]) { block->direction_bits |= (1<<Z_DIRECTION_BIT); }
if (target[C_AXIS] < pl.position[C_AXIS]) { block->direction_bits |= (1<<C_DIRECTION_BIT); }
// Number of steps for each axis
block->steps_x = labs(target[X_AXIS]-pl.position[X_AXIS]);
block->steps_y = labs(target[Y_AXIS]-pl.position[Y_AXIS]);
block->steps_z = labs(target[Z_AXIS]-pl.position[Z_AXIS]);
block->steps_c = labs(target[C_AXIS]-pl.position[C_AXIS]);
block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_c)));
// Bail if this is a zero-length block
if (block->step_event_count == 0) { return; };
// Compute path vector in terms of absolute step target and current positions
double delta_mm[4];
delta_mm[X_AXIS] = (target[X_AXIS]-pl.position[X_AXIS])/settings.steps_per_mm[X_AXIS];
delta_mm[Y_AXIS] = (target[Y_AXIS]-pl.position[Y_AXIS])/settings.steps_per_mm[Y_AXIS];
delta_mm[Z_AXIS] = (target[Z_AXIS]-pl.position[Z_AXIS])/settings.steps_per_mm[Z_AXIS];
delta_mm[C_AXIS] = (target[C_AXIS]-pl.position[C_AXIS])/settings.steps_per_mm[C_AXIS];
block->millimeters = sqrt(delta_mm[X_AXIS]*delta_mm[X_AXIS] + delta_mm[Y_AXIS]*delta_mm[Y_AXIS] +
delta_mm[Z_AXIS]*delta_mm[Z_AXIS] + delta_mm[C_AXIS]*delta_mm[C_AXIS]);
double inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides
// Calculate speed in mm/minute for each axis. No divide by zero due to previous checks.
// NOTE: Minimum stepper speed is limited by MINIMUM_STEPS_PER_MINUTE in stepper.c
double inverse_minute;
if (!invert_feed_rate) {
inverse_minute = feed_rate * inverse_millimeters;
} else {
inverse_minute = 1.0 / feed_rate;
}
block->nominal_speed = block->millimeters * inverse_minute; // (mm/min) Always > 0
block->nominal_rate = ceil(block->step_event_count * inverse_minute); // (step/min) Always > 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:
// Convert universal acceleration for direction-dependent stepper rate change parameter
block->rate_delta = ceil( block->step_event_count*inverse_millimeters *
settings.acceleration / (60 * ACCELERATION_TICKS_PER_SECOND )); // (step/min/acceleration_tick)
// Compute path unit vector
double unit_vec[4];
unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;
unit_vec[C_AXIS] = delta_mm[C_AXIS]*inverse_millimeters;
// Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
// Let a circle be tangent to both previous and current path line segments, where the junction
// deviation is defined as the distance from the junction to the closest edge of the circle,
// colinear with the circle center. The circular segment joining the two paths represents the
// path of centripetal acceleration. Solve for max velocity based on max acceleration about the
// radius of the circle, defined indirectly by junction deviation. This may be also viewed as
// path width or max_jerk in the previous grbl version. This approach does not actually deviate
// from path, but used as a robust way to compute cornering speeds, as it takes into account the
// nonlinearities of both the junction angle and junction velocity.
double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
if ((block_buffer_head != block_buffer_tail) && (pl.previous_nominal_speed > 0.0)) {
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
double cos_theta = - pl.previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
- pl.previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
- pl.previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS]
- pl.previous_unit_vec[C_AXIS] * unit_vec[C_AXIS] ;
// Skip and use default max junction speed for 0 degree acute junction.
if (cos_theta < 0.95) {
vmax_junction = min(pl.previous_nominal_speed,block->nominal_speed);
// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
if (cos_theta > -0.95) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation
double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
vmax_junction = min(vmax_junction,
sqrt(settings.acceleration * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
}
}
}
block->max_entry_speed = vmax_junction;
// Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
double v_allowable = max_allowable_speed(-settings.acceleration,MINIMUM_PLANNER_SPEED,block->millimeters);
block->entry_speed = min(vmax_junction, v_allowable);
// Initialize planner efficiency flags
// Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
// If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
// the current block and next block junction speeds are guaranteed to always be at their maximum
// junction speeds in deceleration and acceleration, respectively. This is due to how the current
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
// the reverse and forward planners, the corresponding block junction speed will always be at the
// the maximum junction speed and may always be ignored for any speed reduction checks.
if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = true; }
else { block->nominal_length_flag = false; }
block->recalculate_flag = true; // Always calculate trapezoid for new block
// Update previous path unit_vector and nominal speed
memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[]
pl.previous_nominal_speed = block->nominal_speed;
// Update buffer head and next buffer head indices
block_buffer_head = next_buffer_head;
next_buffer_head = next_block_index(block_buffer_head);
// Update planner position
memcpy(pl.position, target, sizeof(target)); // pl.position[] = target[]
planner_recalculate();
}
void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate)
#endif
{
// Prepare and initialize new block
plan_block_t *block = &block_buffer[block_buffer_head];
block->step_event_count = 0;
block->millimeters = 0;
block->direction_bits = 0;
block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration later
#ifdef USE_LINE_NUMBERS
block->line_number = line_number;
#endif
// Compute and store initial move distance data.
// TODO: After this for-loop, we don't touch the stepper algorithm data. Might be a good idea
// to try to keep these types of things completely separate from the planner for portability.
int32_t target_steps[N_AXIS];
float unit_vec[N_AXIS], delta_mm;
uint8_t idx;
for (idx=0; idx<N_AXIS; idx++) {
// Calculate target position in absolute steps. This conversion should be consistent throughout.
target_steps[idx] = lround(target[idx]*settings.steps_per_mm[idx]);
// Number of steps for each axis and determine max step events
block->steps[idx] = labs(target_steps[idx]-pl.position[idx]);
block->step_event_count = max(block->step_event_count, block->steps[idx]);
// Compute individual axes distance for move and prep unit vector calculations.
// NOTE: Computes true distance from converted step values.
delta_mm = (target_steps[idx] - pl.position[idx])/settings.steps_per_mm[idx];
unit_vec[idx] = delta_mm; // Store unit vector numerator. Denominator computed later.
// Set direction bits. Bit enabled always means direction is negative.
if (delta_mm < 0 ) { block->direction_bits |= get_direction_mask(idx); }
// Incrementally compute total move distance by Euclidean norm. First add square of each term.
block->millimeters += delta_mm*delta_mm;
}
block->millimeters = sqrt(block->millimeters); // Complete millimeters calculation with sqrt()
// Bail if this is a zero-length block. Highly unlikely to occur.
if (block->step_event_count == 0) { return; }
// Adjust feed_rate value to mm/min depending on type of rate input (normal, inverse time, or rapids)
// TODO: Need to distinguish a rapids vs feed move for overrides. Some flag of some sort.
if (feed_rate < 0) { feed_rate = SOME_LARGE_VALUE; } // Scaled down to absolute max/rapids rate later
else if (invert_feed_rate) { feed_rate = block->millimeters/feed_rate; }
// Calculate the unit vector of the line move and the block maximum feed rate and acceleration scaled
// down such that no individual axes maximum values are exceeded with respect to the line direction.
// NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes,
// if they are also orthogonal/independent. Operates on the absolute value of the unit vector.
float inverse_unit_vec_value;
float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple float divides
float junction_cos_theta = 0;
for (idx=0; idx<N_AXIS; idx++) {
if (unit_vec[idx] != 0) { // Avoid divide by zero.
unit_vec[idx] *= inverse_millimeters; // Complete unit vector calculation
inverse_unit_vec_value = fabs(1.0/unit_vec[idx]); // Inverse to remove multiple float divides.
// Check and limit feed rate against max individual axis velocities and accelerations
feed_rate = min(feed_rate,settings.max_rate[idx]*inverse_unit_vec_value);
block->acceleration = min(block->acceleration,settings.acceleration[idx]*inverse_unit_vec_value);
// Incrementally compute cosine of angle between previous and current path. Cos(theta) of the junction
// between the current move and the previous move is simply the dot product of the two unit vectors,
// where prev_unit_vec is negative. Used later to compute maximum junction speed.
junction_cos_theta -= pl.previous_unit_vec[idx] * unit_vec[idx];
}
}
// TODO: Need to check this method handling zero junction speeds when starting from rest.
if (block_buffer_head == block_buffer_tail) {
// Initialize block entry speed as zero. Assume it will be starting from rest. Planner will correct this later.
block->entry_speed_sqr = 0.0;
block->max_junction_speed_sqr = 0.0; // Starting from rest. Enforce start from zero velocity.
} else {
/*
Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
Let a circle be tangent to both previous and current path line segments, where the junction
deviation is defined as the distance from the junction to the closest edge of the circle,
colinear with the circle center. The circular segment joining the two paths represents the
path of centripetal acceleration. Solve for max velocity based on max acceleration about the
radius of the circle, defined indirectly by junction deviation. This may be also viewed as
path width or max_jerk in the previous grbl version. This approach does not actually deviate
from path, but used as a robust way to compute cornering speeds, as it takes into account the
nonlinearities of both the junction angle and junction velocity.
NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path
mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact
stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here
is exactly the same. Instead of motioning all the way to junction point, the machine will
just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform
a continuous mode path, but ARM-based microcontrollers most certainly do.
NOTE: The max junction speed is a fixed value, since machine acceleration limits cannot be
changed dynamically during operation nor can the line move geometry. This must be kept in
memory in the event of a feedrate override changing the nominal speeds of blocks, which can
change the overall maximum entry speed conditions of all blocks.
*/
// NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta).
float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // Trig half angle identity. Always positive.
// TODO: Technically, the acceleration used in calculation needs to be limited by the minimum of the
// two junctions. However, this shouldn't be a significant problem except in extreme circumstances.
block->max_junction_speed_sqr = max( MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED,
(block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2) );
}
// Store block nominal speed
block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min). Always > 0
// Compute the junction maximum entry based on the minimum of the junction speed and neighboring nominal speeds.
block->max_entry_speed_sqr = min(block->max_junction_speed_sqr,
min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr));
// Update previous path unit_vector and nominal speed (squared)
memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[]
pl.previous_nominal_speed_sqr = block->nominal_speed_sqr;
// Update planner position
memcpy(pl.position, target_steps, sizeof(target_steps)); // pl.position[] = target_steps[]
// New block is all set. Update buffer head and next buffer head indices.
block_buffer_head = next_buffer_head;
next_buffer_head = plan_next_block_index(block_buffer_head);
// Finish up by recalculating the plan with the new block.
planner_recalculate();
}
// 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();
}
void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder)
#endif //LEVEL_SENSOR
{
// Calculate the buffer head after we push this byte
next_buffer_head = next_block_index(block_buffer_head);
// If the buffer is full: good! That means we are well ahead of the robot.
// Rest here until there is room in the buffer.
#ifndef DOGLCD
buffer_recursivity++;
#endif // DOGLCD
while(block_buffer_tail == next_buffer_head)
{
next_buffer_head = next_block_index(block_buffer_head);
temp::TemperatureManager::single::instance().manageTemperatureControl();
#ifndef DOGLCD
manage_inactivity();
#endif //DOGLCD
lcd_update();
#ifdef DOGLCD
if (stop_planner_buffer == true)
{
stop_planner_buffer = false;
planner_buffer_stopped = true;
return;
}
#endif // DOGLCD
}
#ifndef DOGLCD
buffer_recursivity--;
#endif // DOGLCD
#ifdef LEVEL_SENSOR
apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
#endif // LEVEL_SENSOR
// The target position of the tool in absolute steps
// Calculate target position in absolute steps
//this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
long target[4];
target[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);
#ifdef PREVENT_DANGEROUS_EXTRUDE
if(target[E_AXIS]!=position[E_AXIS])
{
if(degHotend(active_extruder)<extrude_min_temp)
{
position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
}
#ifdef PREVENT_LENGTHY_EXTRUDE
if(labs(target[E_AXIS]-position[E_AXIS])>axis_steps_per_unit[E_AXIS]*EXTRUDE_MAXLENGTH)
{
position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
}
#endif
}
#endif
// Prepare to set up new block
block_t *block = &block_buffer[block_buffer_head];
// Mark block as not busy (Not executed by the stepper interrupt)
block->busy = false;
// Number of steps for each axis
#ifndef COREXY
// default non-h-bot planning
block->steps_x = labs(target[X_AXIS]-position[X_AXIS]);
block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]);
#else
// corexy planning
// these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
block->steps_x = labs((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]));
block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]));
#endif
block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]);
block->steps_e = labs(target[E_AXIS]-position[E_AXIS]);
block->steps_e *= volumetric_multiplier[active_extruder];
block->steps_e *= extrudemultiply;
block->steps_e /= 100;
block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_e)));
// Bail if this is a zero-length block
if (block->step_event_count <= dropsegments)
{
return;
}
block->fan_speed = fanSpeed;
#ifdef BARICUDA
block->valve_pressure = ValvePressure;
block->e_to_p_pressure = EtoPPressure;
#endif
// Compute direction bits for this block
block->direction_bits = 0;
#ifndef COREXY
if (target[X_AXIS] < position[X_AXIS])
{
block->direction_bits |= (1<<X_AXIS);
}
if (target[Y_AXIS] < position[Y_AXIS])
{
block->direction_bits |= (1<<Y_AXIS);
}
#else
if (target[X_AXIS] < position[X_AXIS])
{
block->direction_bits |= (1<<X_HEAD); //AlexBorro: Save the real Extruder (head) direction in X Axis
}
if (target[Y_AXIS] < position[Y_AXIS])
{
block->direction_bits |= (1<<Y_HEAD); //AlexBorro: Save the real Extruder (head) direction in Y Axis
}
if ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]) < 0)
{
block->direction_bits |= (1<<X_AXIS); //AlexBorro: Motor A direction (Incorrectly implemented as X_AXIS)
}
if ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]) < 0)
{
block->direction_bits |= (1<<Y_AXIS); //AlexBorro: Motor B direction (Incorrectly implemented as Y_AXIS)
}
#endif
if (target[Z_AXIS] < position[Z_AXIS])
{
block->direction_bits |= (1<<Z_AXIS);
}
if (target[E_AXIS] < position[E_AXIS])
{
block->direction_bits |= (1<<E_AXIS);
}
block->active_extruder = extruder;
//enable active axes
#ifdef COREXY
if((block->steps_x != 0) || (block->steps_y != 0))
{
enable_x();
enable_y();
}
#else
if(block->steps_x != 0) enable_x();
if(block->steps_y != 0) enable_y();
#endif
#ifndef Z_LATE_ENABLE
if(block->steps_z != 0) enable_z();
#endif
// Enable extruder(s)
if(block->steps_e != 0)
{
if (DISABLE_INACTIVE_EXTRUDER) //enable only selected extruder
{
if(g_uc_extruder_last_move[0] > 0) g_uc_extruder_last_move[0]--;
if(g_uc_extruder_last_move[1] > 0) g_uc_extruder_last_move[1]--;
if(g_uc_extruder_last_move[2] > 0) g_uc_extruder_last_move[2]--;
if(g_uc_extruder_last_move[3] > 0) g_uc_extruder_last_move[3]--;
switch(extruder)
{
case 0:
enable_e0();
g_uc_extruder_last_move[0] = BLOCK_BUFFER_SIZE*2;
if(g_uc_extruder_last_move[1] == 0) disable_e1();
if(g_uc_extruder_last_move[2] == 0) disable_e2();
if(g_uc_extruder_last_move[3] == 0) disable_e3();
break;
case 1:
enable_e1();
g_uc_extruder_last_move[1] = BLOCK_BUFFER_SIZE*2;
if(g_uc_extruder_last_move[0] == 0) disable_e0();
if(g_uc_extruder_last_move[2] == 0) disable_e2();
if(g_uc_extruder_last_move[3] == 0) disable_e3();
break;
case 2:
enable_e2();
g_uc_extruder_last_move[2] = BLOCK_BUFFER_SIZE*2;
if(g_uc_extruder_last_move[0] == 0) disable_e0();
if(g_uc_extruder_last_move[1] == 0) disable_e1();
if(g_uc_extruder_last_move[3] == 0) disable_e3();
break;
case 3:
enable_e3();
g_uc_extruder_last_move[3] = BLOCK_BUFFER_SIZE*2;
if(g_uc_extruder_last_move[0] == 0) disable_e0();
if(g_uc_extruder_last_move[1] == 0) disable_e1();
if(g_uc_extruder_last_move[2] == 0) disable_e2();
break;
}
}
else //enable all
{
enable_e0();
enable_e1();
enable_e2();
enable_e3();
}
}
if (block->steps_e == 0)
{
if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate;
}
else
{
if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate;
}
/* This part of the code calculates the total length of the movement.
For cartesian bots, the X_AXIS is the real X movement and same for Y_AXIS.
But for corexy bots, that is not true. The "X_AXIS" and "Y_AXIS" motors (that should be named to A_AXIS
and B_AXIS) cannot be used for X and Y length, because A=X+Y and B=X-Y.
So we need to create other 2 "AXIS", named X_HEAD and Y_HEAD, meaning the real displacement of the Head.
Having the real displacement of the head, we can calculate the total movement length and apply the desired speed.
*/
#ifndef COREXY
float delta_mm[4];
delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];
delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
#else
float delta_mm[6];
delta_mm[X_HEAD] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];
delta_mm[Y_HEAD] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
delta_mm[X_AXIS] = ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[X_AXIS];
delta_mm[Y_AXIS] = ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[Y_AXIS];
#endif
delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS];
delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS])*volumetric_multiplier[active_extruder]*extrudemultiply/100.0;
if ( block->steps_x <=dropsegments && block->steps_y <=dropsegments && block->steps_z <=dropsegments )
{
block->millimeters = fabs(delta_mm[E_AXIS]);
}
else
{
#ifndef COREXY
block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS]));
#else
block->millimeters = sqrt(square(delta_mm[X_HEAD]) + square(delta_mm[Y_HEAD]) + square(delta_mm[Z_AXIS]));
#endif
}
float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides
// Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
float inverse_second = feed_rate * inverse_millimeters;
int moves_queued=(block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1);
// slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill
#ifdef OLD_SLOWDOWN
if(moves_queued < (BLOCK_BUFFER_SIZE * 0.5) && moves_queued > 1)
feed_rate = feed_rate*moves_queued / (BLOCK_BUFFER_SIZE * 0.5);
#endif
#ifdef SLOWDOWN
// segment time im micro seconds
unsigned long segment_time = lround(1000000.0/inverse_second);
if ((moves_queued > 1) && (moves_queued < (BLOCK_BUFFER_SIZE * 0.5)))
{
if (segment_time < minsegmenttime)
{ // buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
inverse_second=1000000.0/(segment_time+lround(2*(minsegmenttime-segment_time)/moves_queued));
#ifdef XY_FREQUENCY_LIMIT
segment_time = lround(1000000.0/inverse_second);
#endif
}
}
#endif
// END OF SLOW DOWN SECTION
block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0
block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0
#ifdef FILAMENT_SENSOR
//FMM update ring buffer used for delay with filament measurements
if((extruder==FILAMENT_SENSOR_EXTRUDER_NUM) && (delay_index2 > -1)) //only for extruder with filament sensor and if ring buffer is initialized
{
delay_dist = delay_dist + delta_mm[E_AXIS]; //increment counter with next move in e axis
while (delay_dist >= (10*(MAX_MEASUREMENT_DELAY+1))) //check if counter is over max buffer size in mm
delay_dist = delay_dist - 10*(MAX_MEASUREMENT_DELAY+1); //loop around the buffer
while (delay_dist<0)
delay_dist = delay_dist + 10*(MAX_MEASUREMENT_DELAY+1); //loop around the buffer
delay_index1=delay_dist/10.0; //calculate index
//ensure the number is within range of the array after converting from floating point
if(delay_index1<0)
delay_index1=0;
else if (delay_index1>MAX_MEASUREMENT_DELAY)
delay_index1=MAX_MEASUREMENT_DELAY;
if(delay_index1 != delay_index2) //moved index
{
meas_sample=widthFil_to_size_ratio()-100; //subtract off 100 to reduce magnitude - to store in a signed char
}
while( delay_index1 != delay_index2)
{
delay_index2 = delay_index2 + 1;
if(delay_index2>MAX_MEASUREMENT_DELAY)
delay_index2=delay_index2-(MAX_MEASUREMENT_DELAY+1); //loop around buffer when incrementing
if(delay_index2<0)
delay_index2=0;
else if (delay_index2>MAX_MEASUREMENT_DELAY)
delay_index2=MAX_MEASUREMENT_DELAY;
measurement_delay[delay_index2]=meas_sample;
}
}
#endif
// Calculate and limit speed in mm/sec for each axis
float current_speed[4];
float speed_factor = 1.0; //factor <=1 do decrease speed
for(int i=0; i < 4; i++)
{
current_speed[i] = delta_mm[i] * inverse_second;
if(fabs(current_speed[i]) > max_feedrate[i])
speed_factor = min(speed_factor, max_feedrate[i] / fabs(current_speed[i]));
}
// Max segement time in us.
#ifdef XY_FREQUENCY_LIMIT
#define MAX_FREQ_TIME (1000000.0/XY_FREQUENCY_LIMIT)
// Check and limit the xy direction change frequency
unsigned char direction_change = block->direction_bits ^ old_direction_bits;
old_direction_bits = block->direction_bits;
segment_time = lround((float)segment_time / speed_factor);
if((direction_change & (1<<X_AXIS)) == 0)
{
x_segment_time[0] += segment_time;
}
else
{
x_segment_time[2] = x_segment_time[1];
x_segment_time[1] = x_segment_time[0];
x_segment_time[0] = segment_time;
}
if((direction_change & (1<<Y_AXIS)) == 0)
{
y_segment_time[0] += segment_time;
}
else
{
y_segment_time[2] = y_segment_time[1];
y_segment_time[1] = y_segment_time[0];
y_segment_time[0] = segment_time;
}
long max_x_segment_time = max(x_segment_time[0], max(x_segment_time[1], x_segment_time[2]));
long max_y_segment_time = max(y_segment_time[0], max(y_segment_time[1], y_segment_time[2]));
long min_xy_segment_time =min(max_x_segment_time, max_y_segment_time);
if(min_xy_segment_time < MAX_FREQ_TIME)
speed_factor = min(speed_factor, speed_factor * (float)min_xy_segment_time / (float)MAX_FREQ_TIME);
#endif // XY_FREQUENCY_LIMIT
// Correct the speed
if (speed_factor < 1.0) {
for (unsigned char i = 0; i < NUM_AXIS; i++) current_speed[i] *= speed_factor;
block->nominal_speed *= speed_factor;
block->nominal_rate *= speed_factor;
}
// Compute and limit the acceleration rate for the trapezoid generator.
float steps_per_mm = block->step_event_count / block->millimeters;
long bsx = block->steps_x, bsy = block->steps_y, bsz = block->steps_z, bse = block->steps_e;
if (bsx == 0 && bsy == 0 && bsz == 0) {
block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
}
else if (bse == 0) {
block->acceleration_st = ceil(travel_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
}
else {
block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
}
// Limit acceleration per axis
unsigned long acc_st = block->acceleration_st,
xsteps = axis_steps_per_sqr_second[X_AXIS],
ysteps = axis_steps_per_sqr_second[Y_AXIS],
zsteps = axis_steps_per_sqr_second[Z_AXIS],
esteps = axis_steps_per_sqr_second[E_AXIS];
if ((float)acc_st * bsx / block->step_event_count > xsteps) acc_st = xsteps;
if ((float)acc_st * bsy / block->step_event_count > ysteps) acc_st = ysteps;
if ((float)acc_st * bsz / block->step_event_count > zsteps) acc_st = zsteps;
if ((float)acc_st * bse / block->step_event_count > esteps) acc_st = esteps;
block->acceleration_st = acc_st;
block->acceleration = acc_st / steps_per_mm;
block->acceleration_rate = (long)(acc_st * 16777216.0 / (F_CPU / 8.0));
#if 0 // Use old jerk for now
// Compute path unit vector
double unit_vec[3];
unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;
// Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
// Let a circle be tangent to both previous and current path line segments, where the junction
// deviation is defined as the distance from the junction to the closest edge of the circle,
// colinear with the circle center. The circular segment joining the two paths represents the
// path of centripetal acceleration. Solve for max velocity based on max acceleration about the
// radius of the circle, defined indirectly by junction deviation. This may be also viewed as
// path width or max_jerk in the previous grbl version. This approach does not actually deviate
// from path, but used as a robust way to compute cornering speeds, as it takes into account the
// nonlinearities of both the junction angle and junction velocity.
double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
- previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
- previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
// Skip and use default max junction speed for 0 degree acute junction.
if (cos_theta < 0.95) {
vmax_junction = min(previous_nominal_speed,block->nominal_speed);
// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
if (cos_theta > -0.95) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation
double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
vmax_junction = min(vmax_junction,
sqrt(block->acceleration * junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
}
}
}
#endif
// Start with a safe speed
float vmax_junction = max_xy_jerk / 2;
float vmax_junction_factor = 1.0;
float mz2 = max_z_jerk / 2, me2 = max_e_jerk / 2;
float csz = current_speed[Z_AXIS], cse = current_speed[E_AXIS];
if (fabs(csz) > mz2) vmax_junction = min(vmax_junction, mz2);
if (fabs(cse) > me2) vmax_junction = min(vmax_junction, me2);
vmax_junction = min(vmax_junction, block->nominal_speed);
float safe_speed = vmax_junction;
if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
float dx = current_speed[X_AXIS] - previous_speed[X_AXIS],
dy = current_speed[Y_AXIS] - previous_speed[Y_AXIS],
dz = fabs(csz - previous_speed[Z_AXIS]),
de = fabs(cse - previous_speed[E_AXIS]),
jerk = sqrt(dx * dx + dy * dy);
// if ((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
vmax_junction = block->nominal_speed;
// }
if (jerk > max_xy_jerk) vmax_junction_factor = max_xy_jerk / jerk;
if (dz > max_z_jerk) vmax_junction_factor = min(vmax_junction_factor, max_z_jerk / dz);
if (de > max_e_jerk) vmax_junction_factor = min(vmax_junction_factor, max_e_jerk / de);
vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
}
block->max_entry_speed = vmax_junction;
// Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
double v_allowable = max_allowable_speed(-block->acceleration, MINIMUM_PLANNER_SPEED, block->millimeters);
block->entry_speed = min(vmax_junction, v_allowable);
// Initialize planner efficiency flags
// Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
// If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
// the current block and next block junction speeds are guaranteed to always be at their maximum
// junction speeds in deceleration and acceleration, respectively. This is due to how the current
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
// the reverse and forward planners, the corresponding block junction speed will always be at the
// the maximum junction speed and may always be ignored for any speed reduction checks.
block->nominal_length_flag = (block->nominal_speed <= v_allowable);
block->recalculate_flag = true; // Always calculate trapezoid for new block
// Update previous path unit_vector and nominal speed
for (int i = 0; i < NUM_AXIS; i++) previous_speed[i] = current_speed[i];
previous_nominal_speed = block->nominal_speed;
#ifdef ADVANCE
// Calculate advance rate
if((block->steps_e == 0) || (block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)) {
block->advance_rate = 0;
block->advance = 0;
}
else {
long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st);
float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) *
(current_speed[E_AXIS] * current_speed[E_AXIS] * EXTRUSION_AREA * EXTRUSION_AREA)*256;
block->advance = advance;
if(acc_dist == 0) {
block->advance_rate = 0;
}
else {
block->advance_rate = advance / (float)acc_dist;
}
}
/*
SERIAL_ECHO_START;
SERIAL_ECHOPGM("advance :");
SERIAL_ECHO(block->advance/256.0);
SERIAL_ECHOPGM("advance rate :");
SERIAL_ECHOLN(block->advance_rate/256.0);
*/
#endif // ADVANCE
calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed, safe_speed/block->nominal_speed);
// Move buffer head
block_buffer_head = next_buffer_head;
// Update position
for (int i = 0; i < NUM_AXIS; i++) position[i] = target[i];
planner_recalculate();
st_wake_up();
}
// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in
// millimaters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed
// rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes.
//void plan_buffer_line(double x, double y, double z, double feed_rate, uint8_t invert_feed_rate)
void plan_buffer_line (tActionRequest *pAction)
{
double x;
double y;
double z;
double feed_rate;
uint8_t invert_feed_rate;
bool e_only = false;
double speed_x, speed_y, speed_z, speed_e; // Nominal mm/minute for each axis
x = pAction->target.x;
y = pAction->target.y;
z = pAction->target.z;
feed_rate = pAction->target.feed_rate;
invert_feed_rate = pAction->target.invert_feed_rate;
// Calculate target position in absolute steps
int32_t target[NUM_AXES];
target[X_AXIS] = lround(x*(double)config.steps_per_mm_x);
target[Y_AXIS] = lround(y*(double)config.steps_per_mm_y);
target[Z_AXIS] = lround(z*(double)config.steps_per_mm_z);
target[E_AXIS] = lround(pAction->target.e*(double)config.steps_per_mm_e);
// Calculate the buffer head after we push this byte
int next_buffer_head = next_block_index( block_buffer_head );
// 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];
block->action_type = AT_MOVE;
// Compute direction bits for this block
block->direction_bits = 0;
if (target[X_AXIS] < position[X_AXIS]) { block->direction_bits |= X_DIR_BIT; }
if (target[Y_AXIS] < position[Y_AXIS]) { block->direction_bits |= Y_DIR_BIT; }
if (target[Z_AXIS] < position[Z_AXIS]) { block->direction_bits |= Z_DIR_BIT; }
if (target[E_AXIS] < position[E_AXIS]) { block->direction_bits |= E_DIR_BIT; }
// 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->steps_e = labs(target[E_AXIS]-position[E_AXIS]);
block->step_event_count = max(block->steps_x, max(block->steps_y, block->steps_z));
block->step_event_count = max(block->step_event_count, block->steps_e);
// Bail if this is a zero-length block
if (block->step_event_count == 0) { return; };
// Compute path vector in terms of absolute step target and current positions
double delta_mm[NUM_AXES];
delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/(double)config.steps_per_mm_x;
delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/(double)config.steps_per_mm_y;
delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/(double)config.steps_per_mm_z;
delta_mm[E_AXIS] = (target[E_AXIS]-position[E_AXIS])/(double)config.steps_per_mm_e;
block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) +
square(delta_mm[Z_AXIS]));
if (block->millimeters == 0)
{
e_only = true;
block->millimeters = fabs(delta_mm[E_AXIS]);
}
double inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides
//
// Speed limit code from Marlin firmware
//
//TODO: handle invert_feed_rate
double microseconds;
//if(feedrate<minimumfeedrate)
// feedrate=minimumfeedrate;
microseconds = lround((block->millimeters/feed_rate*60.0)*1000000.0);
// Calculate speed in mm/minute for each axis
double multiplier = 60.0*1000000.0/(double)microseconds;
speed_x = delta_mm[X_AXIS] * multiplier;
speed_y = delta_mm[Y_AXIS] * multiplier;
speed_z = delta_mm[Z_AXIS] * multiplier;
speed_e = delta_mm[E_AXIS] * multiplier;
// Limit speed per axis
double speed_factor = 1; //factor <=1 do decrease speed
if(fabs(speed_x) > config.maximum_feedrate_x)
{
speed_factor = (double)config.maximum_feedrate_x / fabs(speed_x);
}
if(fabs(speed_y) > config.maximum_feedrate_y)
{
double tmp_speed_factor = (double)config.maximum_feedrate_y / fabs(speed_y);
if(speed_factor > tmp_speed_factor) speed_factor = tmp_speed_factor;
}
if(fabs(speed_z) > config.maximum_feedrate_z)
{
double tmp_speed_factor = (double)config.maximum_feedrate_z / fabs(speed_z);
if(speed_factor > tmp_speed_factor) speed_factor = tmp_speed_factor;
}
if(fabs(speed_e) > config.maximum_feedrate_e)
{
double tmp_speed_factor = (double)config.maximum_feedrate_e / fabs(speed_e);
if(speed_factor > tmp_speed_factor) speed_factor = tmp_speed_factor;
}
multiplier = multiplier * speed_factor;
speed_x = delta_mm[X_AXIS] * multiplier;
speed_y = delta_mm[Y_AXIS] * multiplier;
speed_z = delta_mm[Z_AXIS] * multiplier;
speed_e = delta_mm[E_AXIS] * multiplier;
block->nominal_speed = block->millimeters * multiplier; // mm per min
block->nominal_rate = ceil(block->step_event_count * multiplier); // steps per minute
//---
#if 0
// Calculate speed in mm/minute for each axis. No divide by zero due to previous checks.
// NOTE: Minimum stepper speed is limited by MINIMUM_STEPS_PER_MINUTE in stepper.c
double inverse_minute;
if (!invert_feed_rate) {
inverse_minute = feed_rate * inverse_millimeters;
} else {
inverse_minute = 1.0 / feed_rate;
}
block->nominal_speed = block->millimeters * inverse_minute; // (mm/min) Always > 0
block->nominal_rate = ceil(block->step_event_count * inverse_minute); // (step/min) Always > 0
#endif
#if 0
double axis_speed;
axis_speed = delta_mm[Z_AXIS] * inverse_minute;
if (axis_speed > config.maximum_feedrate_z)
{
inverse_millimeters = 1.0 / delta_mm[Z_AXIS];
inverse_minute = calc_inverse_minute (false, config.maximum_feedrate_z, inverse_millimeters);
block->nominal_speed = delta_mm[Z_AXIS] * inverse_minute; // (mm/min) Always > 0
block->nominal_rate = ceil(block->step_event_count * inverse_minute); // (step/min) Always > 0
}
#endif
// 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:
// Convert universal acceleration for direction-dependent stepper rate change parameter
block->rate_delta = ceil( block->step_event_count*inverse_millimeters *
config.acceleration*60.0 / ACCELERATION_TICKS_PER_SECOND ); // (step/min/acceleration_tick)
#if 0
double rate_calc;
if (delta_mm[Z_AXIS] > 0)
{
rate_calc = ceil( block->step_event_count / delta_mm[Z_AXIS] *
50*60.0 / ACCELERATION_TICKS_PER_SECOND ); // (step/min/acceleration_tick)
if (rate_calc < block->rate_delta)
block->rate_delta = rate_calc;
}
#endif
// Perform planner-enabled calculations
if (acceleration_manager_enabled /*&& !e_only*/ ) {
// Compute path unit vector
double unit_vec[NUM_AXES];
unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;
// Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
// Let a circle be tangent to both previous and current path line segments, where the junction
// deviation is defined as the distance from the junction to the closest edge of the circle,
// colinear with the circle center. The circular segment joining the two paths represents the
// path of centripetal acceleration. Solve for max velocity based on max acceleration about the
// radius of the circle, defined indirectly by junction deviation. This may be also viewed as
// path width or max_jerk in the previous grbl version. This approach does not actually deviate
// from path, but used as a robust way to compute cornering speeds, as it takes into account the
// nonlinearities of both the junction angle and junction velocity.
double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
- previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
- previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
// Skip and use default max junction speed for 0 degree acute junction.
if (cos_theta < 0.95) {
vmax_junction = min(previous_nominal_speed,block->nominal_speed);
// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
if (cos_theta > -0.95) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation
double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
vmax_junction = min(vmax_junction,
sqrt(config.acceleration*60*60 * config.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
}
}
}
block->max_entry_speed = vmax_junction;
// Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
double v_allowable = max_allowable_speed(-config.acceleration,MINIMUM_PLANNER_SPEED,block->millimeters);
block->entry_speed = min(vmax_junction, v_allowable);
// Initialize planner efficiency flags
// Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
// If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
// the current block and next block junction speeds are guaranteed to always be at their maximum
// junction speeds in deceleration and acceleration, respectively. This is due to how the current
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
// the reverse and forward planners, the corresponding block junction speed will always be at the
// the maximum junction speed and may always be ignored for any speed reduction checks.
if (block->nominal_speed <= v_allowable) { block->nominal_length_flag = true; }
else { block->nominal_length_flag = false; }
block->recalculate_flag = true; // Always calculate trapezoid for new block
// Update previous path unit_vector and nominal speed
memcpy(previous_unit_vec, unit_vec, sizeof(unit_vec)); // previous_unit_vec[] = unit_vec[]
previous_nominal_speed = block->nominal_speed;
} else {
// Acceleration planner disabled. Set minimum that is required.
// block->entry_speed = block->nominal_speed;
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;
}
if (pAction->ActionType == AT_MOVE)
block->check_endstops = false;
else
block->check_endstops = true;
pAction->ActionType = AT_MOVE;
// Move buffer head
block_buffer_head = next_buffer_head;
// Update position
memcpy(position, target, sizeof(target)); // position[] = target[]
startpoint = pAction->target;
if (acceleration_manager_enabled) { planner_recalculate(); }
st_wake_up();
}
// Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in
// millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed
// rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes.
// All position data passed to the planner must be in terms of machine position to keep the planner
// independent of any coordinate system changes and offsets, which are handled by the g-code parser.
// NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control.
// Also the feed rate input value is used in three ways: as a normal feed rate if invert_feed_rate
// is false, as inverse time if invert_feed_rate is true, or as seek/rapids rate if the feed_rate
// value is negative (and invert_feed_rate always false).
void plan_buffer_line(float x, float y, float z, float feed_rate, uint8_t invert_feed_rate)
{
// Prepare to set up new block
block_t *block = &block_buffer[block_buffer_head];
// Calculate target position in absolute steps
int32_t target[N_AXIS];
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]);
// Number of steps for each axis
block->steps_x = labs(target[X_AXIS]-pl.position[X_AXIS]);
block->steps_y = labs(target[Y_AXIS]-pl.position[Y_AXIS]);
block->steps_z = labs(target[Z_AXIS]-pl.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; };
// Compute path vector in terms of absolute step target and current positions
float delta_mm[N_AXIS];
delta_mm[X_AXIS] = x-pl.last_x;
delta_mm[Y_AXIS] = y-pl.last_y;
delta_mm[Z_AXIS] = z-pl.last_z;
block->millimeters = sqrt(delta_mm[X_AXIS]*delta_mm[X_AXIS] + delta_mm[Y_AXIS]*delta_mm[Y_AXIS] +
delta_mm[Z_AXIS]*delta_mm[Z_AXIS]);
// Adjust feed_rate value to mm/min depending on type of rate input (normal, inverse time, or rapids)
// TODO: Need to distinguish a rapids vs feed move for overrides. Some flag of some sort.
if (feed_rate < 0) { feed_rate = SOME_LARGE_VALUE; } // Scaled down to absolute max/rapids rate later
else if (invert_feed_rate) { feed_rate = block->millimeters/feed_rate; }
// Calculate the unit vector of the line move and the block maximum feed rate and acceleration limited
// by the maximum possible values. Block rapids rates are computed or feed rates are scaled down so
// they don't exceed the maximum axes velocities. The block acceleration is maximized based on direction
// and axes properties as well.
// NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes,
// if they are also orthogonal/independent. Operates on the absolute value of the unit vector.
uint8_t i;
float unit_vec[N_AXIS], inverse_unit_vec_value;
float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple float divides
block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration in loop
for (i=0; i<N_AXIS; i++) {
if (delta_mm[i] == 0) {
unit_vec[i] = 0; // Store zero value. And avoid divide by zero.
} else {
// Compute unit vector and its absolute inverse value
unit_vec[i] = delta_mm[i]*inverse_millimeters;
inverse_unit_vec_value = abs(1.0/unit_vec[i]);
// Check and limit feed rate against max axis velocities and scale accelerations to maximums
feed_rate = min(feed_rate,settings.max_velocity[i]*inverse_unit_vec_value);
block->acceleration = min(block->acceleration,settings.acceleration[i]*inverse_unit_vec_value);
}
}
// Compute nominal speed and rates
block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min)^2. Always > 0
block->nominal_rate = ceil(feed_rate*(RANADE_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
// Compute the acceleration and distance traveled per step event for the stepper algorithm.
block->rate_delta = ceil(block->acceleration*
((RANADE_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic)
block->d_next = ceil((block->millimeters*RANADE_MULTIPLIER)/block->step_event_count); // (mult*mm/step)
// Compute direction bits. Bit enabled always means direction is negative.
block->direction_bits = 0;
if (unit_vec[X_AXIS] < 0) { block->direction_bits |= (1<<X_DIRECTION_BIT); }
if (unit_vec[Y_AXIS] < 0) { block->direction_bits |= (1<<Y_DIRECTION_BIT); }
if (unit_vec[Z_AXIS] < 0) { block->direction_bits |= (1<<Z_DIRECTION_BIT); }
// Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
// Let a circle be tangent to both previous and current path line segments, where the junction
// deviation is defined as the distance from the junction to the closest edge of the circle,
// colinear with the circle center. The circular segment joining the two paths represents the
// path of centripetal acceleration. Solve for max velocity based on max acceleration about the
// radius of the circle, defined indirectly by junction deviation. This may be also viewed as
// path width or max_jerk in the previous grbl version. This approach does not actually deviate
// from path, but used as a robust way to compute cornering speeds, as it takes into account the
// nonlinearities of both the junction angle and junction velocity.
// NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path
// mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact
// stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here
// is exactly the same. Instead of motioning all the way to junction point, the machine will
// just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform
// a continuous mode path, but ARM-based microcontrollers most certainly do.
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
block->max_entry_speed_sqr = MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED;
if ((block_buffer_head != block_buffer_tail) && (pl.previous_nominal_speed_sqr > 0.0)) {
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
float cos_theta = - pl.previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
- pl.previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
- pl.previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
// Skip and use default max junction speed for 0 degree acute junction.
if (cos_theta < 0.95) {
block->max_entry_speed_sqr = min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr);
// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
if (cos_theta > -0.95) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation
float sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
block->max_entry_speed_sqr = min(block->max_entry_speed_sqr,
block->acceleration * settings.junction_deviation * sin_theta_d2/(1.0-sin_theta_d2));
}
}
}
// Initialize block entry speed. Compute block entry velocity backwards from user-defined MINIMUM_PLANNER_SPEED.
// TODO: This could be moved to the planner recalculate function.
block->entry_speed_sqr = min( block->max_entry_speed_sqr,
MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED + 2*block->acceleration*block->millimeters);
// Set new block to be recalculated for conversion to stepper data.
block->recalculate_flag = true;
// Update previous path unit_vector and nominal speed (squared)
memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[]
pl.previous_nominal_speed_sqr = block->nominal_speed_sqr;
// Update planner position
memcpy(pl.position, target, sizeof(target)); // pl.position[] = target[]
pl.last_x = x;
pl.last_y = y;
pl.last_z = z;
// Update buffer head and next buffer head indices
block_buffer_head = next_buffer_head;
next_buffer_head = next_block_index(block_buffer_head);
planner_recalculate();
}
// 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(float x, float y, float z, float e, float feed_rate) {
int current_temp;
// Calculate the buffer head after we push this byte
int next_buffer_head = next_block_index(block_buffer_head);
// 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) {
manage_inactivity(1);
#if (MINIMUM_FAN_START_SPEED > 0)
manage_fan_start_speed();
#endif
}
// The target position of the tool in absolute steps
// Calculate target position in absolute steps
//this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
long target[4];
target[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);
current_temp = analog2temp( current_raw );
#if PREVENT_DANGEROUS_EXTRUDE > 0
if(target[E_AXIS]!=position[E_AXIS]) {
if(current_temp < EXTRUDE_MINTEMP && prevent_cold_extrude) {
position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
serial_send(TXT_COLD_EXTRUSION_PREVENTED_CRLF);
}
#if PREVENT_LENGTHY_EXTRUDE > 0
if(labs(target[E_AXIS]-position[E_AXIS]) > axis_steps_per_unit[E_AXIS] * EXTRUDE_MAXLENGTH) {
position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
serial_send(TXT_LONG_EXTRUSION_PREVENTED_CRLF);
}
#endif
}
#endif
// Prepare to set up new block
block_t *block = &block_buffer[block_buffer_head];
// Mark block as not busy (Not executed by the stepper interrupt)
block->busy = 0;
// 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->steps_e = labs(target[E_AXIS]-position[E_AXIS]);
block->steps_e = (long)(block->steps_e * extrudemultiply / 100.0);
block->step_event_count = MAX(block->steps_x, MAX(block->steps_y, MAX(block->steps_z, block->steps_e)));
// Bail if this is a zero-length block
if (block->step_event_count <= DROP_SEGMENTS) return;
// Compute direction bits for this block
block->direction_bits = 0;
if (target[X_AXIS] < position[X_AXIS]) block->direction_bits |= (1<<X_AXIS);
if (target[Y_AXIS] < position[Y_AXIS]) block->direction_bits |= (1<<Y_AXIS);
if (target[Z_AXIS] < position[Z_AXIS]) block->direction_bits |= (1<<Z_AXIS);
if (target[E_AXIS] < position[E_AXIS]) {
block->direction_bits |= (1<<E_AXIS);
//High Feedrate for retract
max_E_feedrate_calc = MAX_RETRACT_FEEDRATE;
retract_feedrate_aktiv = 1;
}
else {
if(retract_feedrate_aktiv) {
if(block->steps_e > 0) retract_feedrate_aktiv = 0;
}
else max_E_feedrate_calc = max_feedrate[E_AXIS];
}
#ifdef DELAY_ENABLE
if(block->steps_x != 0) {
enable_x();
delayMicroseconds(DELAY_ENABLE);
}
if(block->steps_y != 0) {
enable_y();
delayMicroseconds(DELAY_ENABLE);
}
if(block->steps_z != 0) {
enable_z();
delayMicroseconds(DELAY_ENABLE);
}
if(block->steps_e != 0) {
enable_e();
delayMicroseconds(DELAY_ENABLE);
}
#else
//enable active axes
if(block->steps_x != 0) enable_x();
if(block->steps_y != 0) enable_y();
if(block->steps_z != 0) enable_z();
if(block->steps_e != 0) enable_e();
#endif
if (block->steps_e == 0) {
if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate;
}
else {
if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate;
}
// slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
int moves_queued=(block_buffer_head-block_buffer_tail + block_buffer_size) & block_buffer_mask;
#ifdef SLOWDOWN
if(moves_queued < (block_buffer_size * 0.5) && moves_queued > MIN_MOVES_QUEUED_FOR_SLOWDOWN)
feed_rate = feed_rate*moves_queued / (float)(block_buffer_size * 0.5);
#endif
float delta_mm[4];
delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/(float)(axis_steps_per_unit[X_AXIS]);
delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/(float)(axis_steps_per_unit[Y_AXIS]);
delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/(float)(axis_steps_per_unit[Z_AXIS]);
//delta_mm[E_AXIS] = (target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS];
delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/(float)(axis_steps_per_unit[E_AXIS]))*extrudemultiply/100.0;
if ( block->steps_x <= DROP_SEGMENTS && block->steps_y <= DROP_SEGMENTS && block->steps_z <= DROP_SEGMENTS )
block->millimeters = fabs(delta_mm[E_AXIS]);
else
block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS]));
float inverse_millimeters = 1.0/(float)(block->millimeters); // Inverse millimeters to remove multiple divides
// Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
float inverse_second = feed_rate * inverse_millimeters;
block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0
block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0
/*
#ifdef SLOWDOWN
// segment time im micro seconds
long segment_time = lround(1000000.0/inverse_second);
if ((moves_queued>0) && (moves_queued < (BLOCK_BUFFER_SIZE - 4))) {
if (segment_time<minsegmenttime) { // buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
segment_time=segment_time+lround(2*(minsegmenttime-segment_time)/moves_queued);
}
}
else {
if (segment_time<minsegmenttime) segment_time=minsegmenttime;
}
#endif
// END OF SLOW DOWN SECTION
*/
// Calculate and limit speed in mm/sec for each axis
float current_speed[4];
float speed_factor = 1.0; //factor <=1 do decrease speed
for(int i=0; i < 3; i++) {
current_speed[i] = delta_mm[i] * inverse_second;
if(fabs(current_speed[i]) > max_feedrate[i])
speed_factor = fmin(speed_factor, max_feedrate[i] / fabs(current_speed[i]));
}
current_speed[E_AXIS] = delta_mm[E_AXIS] * inverse_second;
if(fabs(current_speed[E_AXIS]) > max_E_feedrate_calc)
speed_factor = fmin(speed_factor, max_E_feedrate_calc / fabs(current_speed[E_AXIS]));
// Correct the speed
if( speed_factor < 1.0) {
for(unsigned char i=0; i < 4; i++) current_speed[i] *= speed_factor;
block->nominal_speed *= speed_factor;
block->nominal_rate *= speed_factor;
}
// Compute and limit the acceleration rate for the trapezoid generator.
float steps_per_mm = block->step_event_count/(float)(block->millimeters);
if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)
block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
else {
block->acceleration_st = ceil(move_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
// Limit acceleration per axis
if(((float)block->acceleration_st * (float)block->steps_x / (float)block->step_event_count) > axis_steps_per_sqr_second[X_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[X_AXIS];
if(((float)block->acceleration_st * (float)block->steps_y / (float)block->step_event_count) > axis_steps_per_sqr_second[Y_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS];
if(((float)block->acceleration_st * (float)block->steps_e / (float)block->step_event_count) > axis_steps_per_sqr_second[E_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[E_AXIS];
if(((float)block->acceleration_st * (float)block->steps_z / (float)block->step_event_count ) > axis_steps_per_sqr_second[Z_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS];
}
block->acceleration = block->acceleration_st / (float)(steps_per_mm);
block->acceleration_rate = (long)((float)block->acceleration_st * 8.388608);
#if 0 // Use old jerk for now
// Compute path unit vector
double unit_vec[3];
unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;
// Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
// Let a circle be tangent to both previous and current path line segments, where the junction
// deviation is defined as the distance from the junction to the closest edge of the circle,
// colinear with the circle center. The circular segment joining the two paths represents the
// path of centripetal acceleration. Solve for max velocity based on max acceleration about the
// radius of the circle, defined indirectly by junction deviation. This may be also viewed as
// path width or max_jerk in the previous grbl version. This approach does not actually deviate
// from path, but used as a robust way to compute cornering speeds, as it takes into account the
// nonlinearities of both the junction angle and junction velocity.
double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
- previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
- previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
// Skip and use default max junction speed for 0 degree acute junction.
if (cos_theta < 0.95) {
vmax_junction = min(previous_nominal_speed,block->nominal_speed);
// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
if (cos_theta > -0.95) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation
double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
vmax_junction = min(vmax_junction,
sqrt(block->acceleration * junction_deviation * sin_theta_d2/(float)(1.0-sin_theta_d2)) );
}
}
}
#endif
// Start with a safe speed
float vmax_junction = max_xy_jerk/2.0;
float vmax_junction_factor = 1.0;
if(fabs(current_speed[Z_AXIS]) > max_z_jerk/2.0) vmax_junction = fmin(vmax_junction, max_z_jerk/2.0);
if(fabs(current_speed[E_AXIS]) > max_e_jerk/2.0) vmax_junction = fmin(vmax_junction, max_e_jerk/2.0);
if(G92_reset_previous_speed == 1) {
vmax_junction = 0.1;
G92_reset_previous_speed = 0;
}
vmax_junction = fmin(vmax_junction, block->nominal_speed);
float safe_speed = vmax_junction;
if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2));
// if((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
vmax_junction = block->nominal_speed;
// }
if (jerk > max_xy_jerk) vmax_junction_factor = (max_xy_jerk/(float)(jerk));
if(fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]) > max_z_jerk)
vmax_junction_factor= fmin(vmax_junction_factor, (max_z_jerk/fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS])));
if(fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]) > max_e_jerk)
vmax_junction_factor = fmin(vmax_junction_factor, (max_e_jerk/fabs(current_speed[E_AXIS] - previous_speed[E_AXIS])));
vmax_junction = fmin(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
}
block->max_entry_speed = vmax_junction;
// Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
double v_allowable = max_allowable_speed(-block->acceleration,MINIMUM_PLANNER_SPEED,block->millimeters);
block->entry_speed = fmin(vmax_junction, v_allowable);
// Initialize planner efficiency flags
// Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
// If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
// the current block and next block junction speeds are guaranteed to always be at their maximum
// junction speeds in deceleration and acceleration, respectively. This is due to how the current
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
// the reverse and forward planners, the corresponding block junction speed will always be at the
// the maximum junction speed and may always be ignored for any speed reduction checks.
if (block->nominal_speed <= v_allowable) block->nominal_length_flag = 1;
else block->nominal_length_flag = 0;
block->recalculate_flag = 1; // Always calculate trapezoid for new block
// Update previous path unit_vector and nominal speed
memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[]
previous_nominal_speed = block->nominal_speed;
calculate_trapezoid_for_block(block, block->entry_speed/(float)(block->nominal_speed),
safe_speed/(float)(block->nominal_speed));
// Move buffer head
CRITICAL_SECTION_START;
block_buffer_head = next_buffer_head;
CRITICAL_SECTION_END;
// Update position
memcpy(position, target, sizeof(target)); // position[] = target[]
planner_recalculate();
#ifdef AUTOTEMP
getHighESpeed();
#endif
st_wake_up();
}
