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