// Recalculates the trapezoid speed profiles for all blocks in the plan according to the // entry_factor for each junction. Must be called by planner_recalculate() after // updating the blocks. void TimeEstimateCalculator::recalculate_trapezoids() { Block *current; Block *next = NULL; for(unsigned int n=0; n<blocks.size(); n--) { current = next; next = &blocks[n]; if (current) { // Recalculate if current block entry or exit junction speed has changed. if (current->recalculate_flag || next->recalculate_flag) { // NOTE: Entry and exit factors always > 0 by all previous logic operations. calculate_trapezoid_for_block(current, current->entry_speed/current->nominal_feedrate, next->entry_speed/current->nominal_feedrate); current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed } } } // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. if(next != NULL) { calculate_trapezoid_for_block(next, next->entry_speed/next->nominal_feedrate, MINIMUM_PLANNER_SPEED/next->nominal_feedrate); next->recalculate_flag = false; } }
// Recalculates the trapezoid speed profiles for all blocks in the plan according to the // entry_factor for each junction. Must be called by planner_recalculate() after // updating the blocks. void planner_recalculate_trapezoids() { int8_t block_index = block_buffer_tail; block_t *current; block_t *next = NULL; while(block_index != block_buffer_head) { current = next; next = &block_buffer[block_index]; if (current) { // Recalculate if current block entry or exit junction speed has changed. if (current->recalculate_flag || next->recalculate_flag) { // NOTE: Entry and exit factors always > 0 by all previous logic operations. calculate_trapezoid_for_block(current, current->entry_speed/current->nominal_speed, next->entry_speed/current->nominal_speed); current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed } } block_index = next_block_index( block_index ); } // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. if(next != NULL) { calculate_trapezoid_for_block(next, next->entry_speed/next->nominal_speed, MINIMUM_PLANNER_SPEED/next->nominal_speed); next->recalculate_flag = false; } }
// Recalculates the trapezoid speed profiles for all blocks in the plan according to the // entry_factor for each junction. Must be called by planner_recalculate() after // updating the blocks. void planner_recalculate_trapezoids() { uint8_t block_index = block_buffer_tail; block_t *current; block_t *next = NULL; while(block_index != block_buffer_head) { current = next; next = &block_buffer[block_index]; if (current) { calculate_trapezoid_for_block(current, current->entry_factor, next->entry_factor); } block_index = (block_index+1); block_index = block_index % BLOCK_BUFFER_SIZE; } calculate_trapezoid_for_block(next, next->entry_factor, factor_for_safe_speed(next)); }
// planner, called whenever a new block was added // All planner computations are performed with doubles (float on Arduinos) to minimize numerical round- // off errors. Only when planned values are converted to stepper rate parameters, these are integers. inline static void planner_recalculate() { //// reverse pass // Recalculate entry_speed to be (a) less or equal to vmax_junction and // (b) low enough so it can definitely reach the next entry_speed at fixed acceleration. int8_t block_index = block_buffer_head; block_t *previous = NULL; // block closer to tail (older) block_t *current = NULL; // block who's entry_speed to be adjusted block_t *next = NULL; // block closer to head (newer) while(block_index != block_buffer_tail) { block_index = prev_block_index( block_index ); next = current; current = previous; previous = &block_buffer[block_index]; if (current && next) { reduce_entry_speed_reverse(current, next); } } // skip tail/first block //// forward pass // Recalculate entry_speed to be low enough it can definitely // be reached from previous entry_speed at fixed acceleration. block_index = block_buffer_tail; previous = NULL; // block closer to tail (older) current = NULL; // block who's entry_speed to be adjusted next = NULL; // block closer to head (newer) while(block_index != block_buffer_head) { previous = current; current = next; next = &block_buffer[block_index]; if (previous && current) { reduce_entry_speed_forward(previous, current); } block_index = next_block_index(block_index); } if (current && next) { reduce_entry_speed_forward(current, next); } //// recalculate trapeziods for all flagged blocks // At this point all blocks have entry_speeds that that can be (a) reached from the prevous // entry_speed with the one and only acceleration from our settings and (b) have junction // speeds that do not exceed our limits for given direction change. // Now we only need to calculate the actual accelerate_until and decelerate_after values. block_index = block_buffer_tail; current = NULL; next = NULL; while(block_index != block_buffer_head) { current = next; next = &block_buffer[block_index]; if (current) { if (current->recalculate_flag || next->recalculate_flag) { calculate_trapezoid_for_block( current, current->entry_speed/current->nominal_speed, next->entry_speed/current->nominal_speed ); current->recalculate_flag = false; } } block_index = next_block_index( block_index ); } // always recalculate last (newest) block with zero exit speed calculate_trapezoid_for_block( next, next->entry_speed/next->nominal_speed, ZERO_SPEED/next->nominal_speed ); next->recalculate_flag = false; }
// 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()
void TimeEstimateCalculator::plan(Position newPos, double feedrate) { Block block; memset(&block, 0, sizeof(block)); block.maxTravel = 0; for(unsigned int n=0; n<NUM_AXIS; n++) { block.delta[n] = newPos[n] - currentPosition[n]; block.absDelta[n] = fabs(block.delta[n]); block.maxTravel = std::max(block.maxTravel, block.absDelta[n]); } if (block.maxTravel <= 0) return; if (feedrate < minimumfeedrate) feedrate = minimumfeedrate; block.distance = sqrtf(square(block.absDelta[0]) + square(block.absDelta[1]) + square(block.absDelta[2])); if (block.distance == 0.0) block.distance = block.absDelta[3]; block.nominal_feedrate = feedrate; Position current_feedrate; Position current_abs_feedrate; double feedrate_factor = 1.0; for(unsigned int n=0; n<NUM_AXIS; n++) { current_feedrate[n] = block.delta[n] * feedrate / block.distance; current_abs_feedrate[n] = abs(current_feedrate[n]); if (current_abs_feedrate[n] > max_feedrate[n]) feedrate_factor = std::min(feedrate_factor, max_feedrate[n] / current_abs_feedrate[n]); } //TODO: XY_FREQUENCY_LIMIT if(feedrate_factor < 1.0) { for(unsigned int n=0; n<NUM_AXIS; n++) { current_feedrate[n] *= feedrate_factor; current_abs_feedrate[n] *= feedrate_factor; } block.nominal_feedrate *= feedrate_factor; } block.acceleration = acceleration; for(unsigned int n=0; n<NUM_AXIS; n++) { if (block.acceleration * (block.absDelta[n] / block.distance) > max_acceleration[n]) block.acceleration = max_acceleration[n]; } double vmax_junction = max_xy_jerk/2; double vmax_junction_factor = 1.0; if(current_abs_feedrate[Z_AXIS] > max_z_jerk/2) vmax_junction = std::min(vmax_junction, max_z_jerk/2); if(current_abs_feedrate[E_AXIS] > max_e_jerk/2) vmax_junction = std::min(vmax_junction, max_e_jerk/2); vmax_junction = std::min(vmax_junction, block.nominal_feedrate); double safe_speed = vmax_junction; if ((blocks.size() > 0) && (previous_nominal_feedrate > 0.0001)) { double xy_jerk = sqrt(square(current_feedrate[X_AXIS]-previous_feedrate[X_AXIS])+square(current_feedrate[Y_AXIS]-previous_feedrate[Y_AXIS])); vmax_junction = block.nominal_feedrate; if (xy_jerk > max_xy_jerk) { vmax_junction_factor = (max_xy_jerk/xy_jerk); } if(fabs(current_feedrate[Z_AXIS] - previous_feedrate[Z_AXIS]) > max_z_jerk) { vmax_junction_factor = std::min(vmax_junction_factor, (max_z_jerk/fabs(current_feedrate[Z_AXIS] - previous_feedrate[Z_AXIS]))); } if(fabs(current_feedrate[E_AXIS] - previous_feedrate[E_AXIS]) > max_e_jerk) { vmax_junction_factor = std::min(vmax_junction_factor, (max_e_jerk/fabs(current_feedrate[E_AXIS] - previous_feedrate[E_AXIS]))); } vmax_junction = std::min(previous_nominal_feedrate, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed } block.max_entry_speed = vmax_junction; double v_allowable = max_allowable_speed(-block.acceleration, MINIMUM_PLANNER_SPEED, block.distance); block.entry_speed = std::min(vmax_junction, v_allowable); block.nominal_length_flag = block.nominal_feedrate <= v_allowable; block.recalculate_flag = true; // Always calculate trapezoid for new block previous_feedrate = current_feedrate; previous_nominal_feedrate = block.nominal_feedrate; currentPosition = newPos; calculate_trapezoid_for_block(&block, block.entry_speed/block.nominal_feedrate, safe_speed/block.nominal_feedrate); blocks.push_back(block); }
// 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(); }
/* PLANNER SPEED DEFINITION +--------+ <- current->nominal_speed / \ current->entry_speed -> + \ | + <- next->entry_speed +-------------+ time --> Recalculates the motion plan according to the following algorithm: 1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_speed) so that: a. The junction speed is equal to or less than the maximum junction speed limit b. No speed reduction within one block requires faster deceleration than the acceleration limits. c. The last (or newest appended) block is planned from a complete stop. 2. Go over every block in chronological (forward) order and dial down junction speed values if a. The speed increase within one block would require faster acceleration than the acceleration limits. When these stages are complete, all blocks have a junction entry speed that will allow all speed changes to be performed using the overall limiting acceleration value, and where no junction speed is greater than the max limit. In other words, it just computed the fastest possible velocity profile through all buffered blocks, where the final buffered block is planned to come to a full stop when the buffer is fully executed. Finally it will: 3. Convert the plan to data that the stepper algorithm needs. Only block trapezoids adjacent to a a planner-modified junction speed with be updated, the others are assumed ok as is. All planner computations(1)(2) are performed in floating point to minimize numerical round-off errors. Only when planned values are converted to stepper rate parameters(3), these are integers. If another motion block is added while executing, the planner will re-plan and update the stored optimal velocity profile as it goes. Conceptually, the planner works like blowing up a balloon, where the balloon is the velocity profile. It's constrained by the speeds at the beginning and end of the buffer, along with the maximum junction speeds and nominal speeds of each block. Once a plan is computed, or balloon filled, this is the optimal velocity profile through all of the motions in the buffer. Whenever a new block is added, this changes some of the limiting conditions, or how the balloon is filled, so it has to be re-calculated to get the new optimal velocity profile. Also, since the planner only computes on what's in the planner buffer, some motions with lots of short line segments, like arcs, may seem to move slow. This is because there simply isn't enough combined distance traveled in the entire buffer to accelerate up to the nominal speed and then decelerate to a stop at the end of the buffer. There are a few simple solutions to this: (1) Maximize the machine acceleration. The planner will be able to compute higher speed profiles within the same combined distance. (2) Increase line segment(s) distance. The more combined distance the planner has to use, the faster it can go. (3) Increase the MINIMUM_PLANNER_SPEED. Not recommended. This will change what speed the planner plans to at the end of the buffer. Can lead to lost steps when coming to a stop. (4) [BEST] Increase the planner buffer size. The more combined distance, the bigger the balloon, or faster it can go. But this is not possible for 328p Arduinos because its limited memory is already maxed out. Future ARM versions should not have this issue, with look-ahead planner blocks numbering up to a hundred or more. NOTE: Since this function is constantly re-calculating for every new incoming block, it must be as efficient as possible. For example, in situations like arc generation or complex curves, the short, rapid line segments can execute faster than new blocks can be added, and the planner buffer will then starve and empty, leading to weird hiccup-like jerky motions. */ static void planner_recalculate() { // float entry_speed_sqr; // uint8_t block_index = block_buffer_head; // block_t *previous = NULL; // block_t *current = NULL; // block_t *next; // while (block_index != block_buffer_tail) { // block_index = prev_block_index( block_index ); // next = current; // current = previous; // previous = &block_buffer[block_index]; // // if (next && current) { // if (next != block_buffer_planned) { // if (previous == block_buffer_tail) { block_buffer_planned = next; } // else { // // if (current->entry_speed_sqr != current->max_entry_speed_sqr) { // current->recalculate_flag = true; // Almost always changes. So force recalculate. // entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; // if (entry_speed_sqr < current->max_entry_speed_sqr) { // current->entry_speed_sqr = entry_speed_sqr; // } else { // current->entry_speed_sqr = current->max_entry_speed_sqr; // } // } else { // block_buffer_planned = current; // } // } // } else { // break; // } // } // } // // block_index = block_buffer_planned; // next = &block_buffer[block_index]; // current = prev_block_index(block_index); // while (block_index != block_buffer_head) { // // // If the current block is an acceleration block, but it is not long enough to complete the // // full speed change within the block, we need to adjust the exit speed accordingly. Entry // // speeds have already been reset, maximized, and reverse planned by reverse planner. // if (current->entry_speed_sqr < next->entry_speed_sqr) { // // Compute block exit speed based on the current block speed and distance // // Computes: v_exit^2 = v_entry^2 + 2*acceleration*distance // entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; // // // If it's less than the stored value, update the exit speed and set recalculate flag. // if (entry_speed_sqr < next->entry_speed_sqr) { // next->entry_speed_sqr = entry_speed_sqr; // next->recalculate_flag = true; // } // } // // // Recalculate if current block entry or exit junction speed has changed. // if (current->recalculate_flag || next->recalculate_flag) { // // NOTE: Entry and exit factors always > 0 by all previous logic operations. // calculate_trapezoid_for_block(current, current->entry_speed_sqr, next->entry_speed_sqr); // current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed // } // // current = next; // next = &block_buffer[block_index]; // block_index = next_block_index( block_index ); // } // // // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. // calculate_trapezoid_for_block(next, next->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); // next->recalculate_flag = false; // TODO: No over-write protection exists for the executing block. For most cases this has proven to be ok, but // for feed-rate overrides, something like this is essential. Place a request here to the stepper driver to // find out where in the planner buffer is the a safe place to begin re-planning from. // if (block_buffer_head != block_buffer_tail) { float entry_speed_sqr; // Perform reverse planner pass. Skip the head(end) block since it is already initialized, and skip the // tail(first) block to prevent over-writing of the initial entry speed. uint8_t block_index = prev_block_index( block_buffer_head ); // Assume buffer is not empty. block_t *current = &block_buffer[block_index]; // Head block-1 = Newly appended block block_t *next; if (block_index != block_buffer_tail) { block_index = prev_block_index( block_index ); } while (block_index != block_buffer_tail) { next = current; current = &block_buffer[block_index]; // TODO: Determine maximum entry speed at junction for feedrate overrides, since they can alter // the planner nominal speeds at any time. This calc could be done in the override handler, but // this could require an additional variable to be stored to differentiate the programmed nominal // speeds, max junction speed, and override speeds/scalar. // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising. // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and // check for maximum allowable speed reductions to ensure maximum possible planned speed. if (current->entry_speed_sqr != current->max_entry_speed_sqr) { current->entry_speed_sqr = current->max_entry_speed_sqr; current->recalculate_flag = true; // Almost always changes. So force recalculate. if (next->entry_speed_sqr < current->max_entry_speed_sqr) { // Computes: v_entry^2 = v_exit^2 + 2*acceleration*distance entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters; if (entry_speed_sqr < current->max_entry_speed_sqr) { current->entry_speed_sqr = entry_speed_sqr; } } } block_index = prev_block_index( block_index ); } // Perform forward planner pass. Begins junction speed adjustments after tail(first) block. // Also recalculate trapezoids, block by block, as the forward pass completes the plan. block_index = next_block_index(block_buffer_tail); next = &block_buffer[block_buffer_tail]; // Places tail(first) block into current while (block_index != block_buffer_head) { current = next; next = &block_buffer[block_index]; // If the current block is an acceleration block, but it is not long enough to complete the // full speed change within the block, we need to adjust the exit speed accordingly. Entry // speeds have already been reset, maximized, and reverse planned by reverse planner. if (current->entry_speed_sqr < next->entry_speed_sqr) { // Compute block exit speed based on the current block speed and distance // Computes: v_exit^2 = v_entry^2 + 2*acceleration*distance entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters; // If it's less than the stored value, update the exit speed and set recalculate flag. if (entry_speed_sqr < next->entry_speed_sqr) { next->entry_speed_sqr = entry_speed_sqr; next->recalculate_flag = true; } } // Recalculate if current block entry or exit junction speed has changed. if (current->recalculate_flag || next->recalculate_flag) { // NOTE: Entry and exit factors always > 0 by all previous logic operations. calculate_trapezoid_for_block(current, current->entry_speed_sqr, next->entry_speed_sqr); current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed } block_index = next_block_index( block_index ); } // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated. calculate_trapezoid_for_block(next, next->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED); next->recalculate_flag = false; // } }
// 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(); }