// 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(1); LCD_STATUS; } // 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]); // 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 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, max(block->steps_z, block->steps_e))); // Bail if this is a zero-length block if (block->step_event_count <=dropsegments) { 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); } //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(); 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]; 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]; block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS]) + square(delta_mm[E_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; 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 // segment time im micro seconds long segment_time = lround(1000000.0/inverse_second); if (block->steps_e == 0) { if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate; } else { if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate; } #ifdef SLOWDOWN // slow down when de 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_SIZE - 1); if(moves_queued < (BLOCK_BUFFER_SIZE * 0.5) && moves_queued > 1) feed_rate = feed_rate*moves_queued / (BLOCK_BUFFER_SIZE * 0.5); #endif /* if ((blockcount>0) && (blockcount < (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)/blockcount); } } else { if (segment_time<minsegmenttime) segment_time=minsegmenttime; } // END OF SLOW DOWN SECTION */ // Calculate speed in mm/sec for each axis float current_speed[4]; for(int i=0; i < 4; i++) { current_speed[i] = delta_mm[i] * inverse_second; } // Limit speed per axis float speed_factor = 1.0; //factor <=1 do decrease speed for(int i=0; i < 4; i++) { if(abs(current_speed[i]) > max_feedrate[i]) speed_factor = min(speed_factor, max_feedrate[i] / abs(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; 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) { // Serial.print("speed factor : "); Serial.println(speed_factor); for(int i=0; i < 4; i++) { if(abs(current_speed[i]) > max_feedrate[i]) speed_factor = min(speed_factor, max_feedrate[i] / abs(current_speed[i])); /* if(speed_factor < 0.1) { Serial.print("speed factor : "); Serial.println(speed_factor); Serial.print("current_speed"); Serial.print(i); Serial.print(" : "); Serial.println(current_speed[i]); } */ } 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 * 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/(1.0-sin_theta_d2)) ); } } } #endif // Start with a safe speed float vmax_junction = max_xy_jerk/2; if(abs(current_speed[Z_AXIS]) > max_z_jerk/2) vmax_junction = max_z_jerk/2; vmax_junction = min(vmax_junction, block->nominal_speed); if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) { float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2)); if((previous_speed[X_AXIS] != 0.0) || (previous_speed[Y_AXIS] != 0.0)) { vmax_junction = block->nominal_speed; } if (jerk > max_xy_jerk) { vmax_junction *= (max_xy_jerk/jerk); } if(abs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]) > max_z_jerk) { vmax_junction *= (max_z_jerk/abs(current_speed[Z_AXIS] - previous_speed[Z_AXIS])); } } 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) * (block->speed_e * block->speed_e * EXTRUTION_AREA * EXTRUTION_AREA / 3600.0)*65536; block->advance = advance; if(acc_dist == 0) { block->advance_rate = 0; } else { block->advance_rate = advance / (float)acc_dist; } } #endif // ADVANCE calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed, MINIMUM_PLANNER_SPEED/block->nominal_speed); // Move buffer head block_buffer_head = next_buffer_head; // Update position memcpy(position, target, sizeof(target)); // position[] = target[] planner_recalculate(); #ifdef AUTOTEMP getHighESpeed(); #endif st_wake_up(); }
void linear_move() { // make linear move with preset speeds and destinations, see G0 and G1 //Determine direction of movement if (destination_x > current_x) { p_X_dir = !INVERT_X_DIR; } else { p_X_dir = INVERT_X_DIR; } if (destination_y > current_y) { p_Y_dir = !INVERT_Y_DIR; } else { p_Y_dir = INVERT_Y_DIR; } if (destination_z > current_z) { p_Z_dir = !INVERT_Z_DIR; } else { p_Z_dir = INVERT_Z_DIR; } if (destination_e > current_e) { p_E_dir = !INVERT_E_DIR; } else { p_E_dir = INVERT_E_DIR; } //Only enable axis that are moving. If the axis doesn't need to move then it can stay disabled depending on configuration. if (x_steps_remaining) enable_x(); if (y_steps_remaining) enable_y(); if (z_steps_remaining) enable_z(); if (e_steps_remaining) enable_e(); check_x_min_endstop(); check_y_min_endstop(); check_z_min_endstop(); previous_millis_heater = millis(); while (x_steps_remaining + y_steps_remaining + z_steps_remaining + e_steps_remaining > 0) { // move until no more steps remain if (x_steps_remaining>0) { if ((micros()-previous_micros_x) >= x_interval) { do_x_step(); x_steps_remaining--; } check_x_min_endstop(); led1 = 1; } else { led1 = 0; wait_us(2); } if (y_steps_remaining>0) { if ((micros()-previous_micros_y) >= y_interval) { do_y_step(); y_steps_remaining--; } check_y_min_endstop(); led2=1; } else { led2=0; wait_us(2); } if (z_steps_remaining>0) { if ((micros()-previous_micros_z) >= z_interval) { do_z_step(); z_steps_remaining--; } check_z_min_endstop(); led3=1; } else { led3=0; wait_us(2); } if (e_steps_remaining>0) { if ((micros()-previous_micros_e) >= e_interval) { do_e_step(); e_steps_remaining--; led4=1; } } else { led4=0; wait_us(2); } if ( (millis() - previous_millis_heater) >= 500 ) { manage_heater(); previous_millis_heater = millis(); manage_inactivity(2); } wait_us(2); } led1=0; led2=0; led3=0; led4=0; if (DISABLE_X) disable_x(); if (DISABLE_Y) disable_y(); if (DISABLE_Z) disable_z(); if (DISABLE_E) disable_e(); // Update current position partly based on direction, we probably can combine this with the direction code above... if (destination_x > current_x) current_x = current_x + x_steps_to_take/x_steps_per_unit; else current_x = current_x - x_steps_to_take/x_steps_per_unit; if (destination_y > current_y) current_y = current_y + y_steps_to_take/y_steps_per_unit; else current_y = current_y - y_steps_to_take/y_steps_per_unit; if (destination_z > current_z) current_z = current_z + z_steps_to_take/z_steps_per_unit; else current_z = current_z - z_steps_to_take/z_steps_per_unit; if (destination_e > current_e) current_e = current_e + e_steps_to_take/e_steps_per_unit; else current_e = current_e - e_steps_to_take/e_steps_per_unit; }
/** * Adds movement to position to buffer if there is still space available in buffer. Position is * given in axis coordinate system. * @param [in] target Position in steps to move to in axis coordinate system * @param [in] feedRate Feedrate in steps/sec for this move * @param [in] activeExtruder Extruder used during this move * @return true if position was added to the buffer, false if not because buffer is full right now * @note Part of former plan_buffer_line from planner.cpp */ bool Planner::addMovement(Kinematics_AxisCoordinates_t target, uint32_t feedRate, uint8_t activeExtruder) { static uint32_t previousNominalSpeed; // Calculate the buffer head after we push this byte Planner_blockIndex_t nextBufferHead = getNextBlockIndex(blockBufferHead); if (blockBufferTail != nextBufferHead) { // Prepare to set up new block Planner_Block_t *block = &(blockBuffer[blockBufferHead]); // Mark block as not busy (Not executed by the stepper interrupt) block->busy = false; block->steps.x = labs(target.x-lastGivenPosition.x); block->steps.y = labs(target.y-lastGivenPosition.y); block->steps.z = labs(target.z-lastGivenPosition.z); block->steps.e = labs(target.e-lastGivenPosition.e); block->step_event_count = max(block->steps.x, max(block->steps.y, max(block->steps.z, block->steps.e))); // Only continue if this is a non zero-length block, if not move will be merged with next move if (block->step_event_count >= dropsegments) { // Compute direction bits for this block block->direction.value = 0; if (target.x < lastGivenPosition.x) { block->direction.bits.x = 1; } if (target.y < lastGivenPosition.y) { block->direction.bits.y = 1; } if (target.z < lastGivenPosition.z) { block->direction.bits.z = 1; } if (target.e < lastGivenPosition.e) { block->direction.bits.e = 1; } block->active_extruder = activeExtruder; //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(); /* TODO: Add DISABLE_INACTIVE_EXTRUDER stuff here */ if ( block->steps.x <= dropsegments && block->steps.y <= dropsegments && block->steps.z <= dropsegments ) { block->totalTravelSteps = fabs(block->steps.e); } else { block->totalTravelSteps = sqrt(sq(block->steps.x) + sq(block->steps.y) + sq(block->steps.z)); } float inverseSteps = 1.0/block->totalTravelSteps; // Inverse total steps to remove multiple divides // Calculate speed in mm/second for each axis. No divide by zero due to previous checks. float inverse_second = feedRate * inverseSteps; int moves_queued = (blockBufferHead - blockBufferTail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1); #ifdef SLOWDOWN // segment time in 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 // #ifdef SLOWDOWN block->nominal_speed = feedRate; // Set nominal speed to given, possibly corrected, feedRate block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0 // Calculate and limit speed in steps/sec for each axis Kinematics_AxisCoordinates_t current_speed; float speedFactor = 1.0; //factor <=1 do decrease speed current_speed.x = block->steps.x * inverse_second; if (current_speed.x > configuration.maxFeedrate.x) speedFactor = min(speedFactor, configuration.maxFeedrate.x / current_speed.x); current_speed.y = block->steps.y * inverse_second; if (current_speed.y > configuration.maxFeedrate.y) speedFactor = min(speedFactor, configuration.maxFeedrate.y / current_speed.y); current_speed.z = block->steps.z * inverse_second; if (current_speed.z > configuration.maxFeedrate.z) speedFactor = min(speedFactor, configuration.maxFeedrate.z / current_speed.z); current_speed.e = block->steps.e * inverse_second; if (current_speed.e > configuration.maxFeedrate.e) speedFactor = min(speedFactor, configuration.maxFeedrate.e / current_speed.e); /* TODO: Add #ifdef XY_FREQUENCY_LIMIT from planner.cpp here */ /* Correct all speeds by speedFactor */ if( speedFactor < 1.0) { current_speed.x *= speedFactor; current_speed.y *= speedFactor; current_speed.z *= speedFactor; current_speed.e *= speedFactor; block->nominal_speed *= speedFactor; block->nominal_rate *= speedFactor; } /* TODO: Add acceleration limiter from planner.cpp here */ block->acceleration_st = configuration.maxAcceleration; //block->acceleration = block->acceleration_st / steps_per_mm; block->acceleration_rate = (long)((float)block->acceleration_st * (16777216.0 / (F_CPU / 8.0))); /* Jerk control will control the jerk between two moves. Value of maxEntrySpeed for this block * is set depending on the nominalSpeed (aka feedRate) or the last block */ /* TODO: Add jerk control from planner.cpp here. For now we set maxEntrySpeed to half max allowed jerk */ block->max_entry_speed = configuration.max_xy_jerk/2; /* Compute maximum allowed entry speed based on deceleration over complete travel distance down * to user-defined MINIMUM_PLANNER_SPEED */ uint32_t maxAllowableSpeed = maxStartSpeed(-block->acceleration, MINIMUM_PLANNER_SPEED,block->totalTravelSteps); block->entry_speed = min(block->max_entry_speed, maxAllowableSpeed); // 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 <= maxAllowableSpeed) { block->nominal_length_flag = true; } else { block->nominal_length_flag = false; } block->recalculate_flag = true; // Always calculate trapezoid for new block calculateTrapezoidForBlock(block); // Move buffer head blockBufferHead = nextBufferHead; /* update lastGivenPosition and previousNominalSpeed variable */ memcpy(&lastGivenPosition, &target, sizeof(Kinematics_AxisCoordinates_t)); // lastGivenPosition[] = target[] previousNominalSpeed = block->nominal_speed; recalculate(); st_wake_up(); } return true; } else return false; }
// Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in // mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration // calculation the caller must also provide the physical length of the line in millimeters. void plan_buffer_line(float x, float y, float z, float e, float feed_rate) { int current_temp; // Calculate the buffer head after we push this byte int next_buffer_head = next_block_index(block_buffer_head); // If the buffer is full: good! That means we are well ahead of the robot. // Rest here until there is room in the buffer. while(block_buffer_tail == next_buffer_head) { manage_inactivity(1); #if (MINIMUM_FAN_START_SPEED > 0) manage_fan_start_speed(); #endif } // The target position of the tool in absolute steps // Calculate target position in absolute steps //this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow long target[4]; target[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]); target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]); target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]); target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]); current_temp = analog2temp( current_raw ); #if PREVENT_DANGEROUS_EXTRUDE > 0 if(target[E_AXIS]!=position[E_AXIS]) { if(current_temp < EXTRUDE_MINTEMP && prevent_cold_extrude) { position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part serial_send(TXT_COLD_EXTRUSION_PREVENTED_CRLF); } #if PREVENT_LENGTHY_EXTRUDE > 0 if(labs(target[E_AXIS]-position[E_AXIS]) > axis_steps_per_unit[E_AXIS] * EXTRUDE_MAXLENGTH) { position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part serial_send(TXT_LONG_EXTRUSION_PREVENTED_CRLF); } #endif } #endif // Prepare to set up new block block_t *block = &block_buffer[block_buffer_head]; // Mark block as not busy (Not executed by the stepper interrupt) block->busy = 0; // Number of steps for each axis block->steps_x = labs(target[X_AXIS]-position[X_AXIS]); block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]); block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]); block->steps_e = labs(target[E_AXIS]-position[E_AXIS]); block->steps_e = (long)(block->steps_e * extrudemultiply / 100.0); block->step_event_count = MAX(block->steps_x, MAX(block->steps_y, MAX(block->steps_z, block->steps_e))); // Bail if this is a zero-length block if (block->step_event_count <= DROP_SEGMENTS) return; // Compute direction bits for this block block->direction_bits = 0; if (target[X_AXIS] < position[X_AXIS]) block->direction_bits |= (1<<X_AXIS); if (target[Y_AXIS] < position[Y_AXIS]) block->direction_bits |= (1<<Y_AXIS); if (target[Z_AXIS] < position[Z_AXIS]) block->direction_bits |= (1<<Z_AXIS); if (target[E_AXIS] < position[E_AXIS]) { block->direction_bits |= (1<<E_AXIS); //High Feedrate for retract max_E_feedrate_calc = MAX_RETRACT_FEEDRATE; retract_feedrate_aktiv = 1; } else { if(retract_feedrate_aktiv) { if(block->steps_e > 0) retract_feedrate_aktiv = 0; } else max_E_feedrate_calc = max_feedrate[E_AXIS]; } #ifdef DELAY_ENABLE if(block->steps_x != 0) { enable_x(); delayMicroseconds(DELAY_ENABLE); } if(block->steps_y != 0) { enable_y(); delayMicroseconds(DELAY_ENABLE); } if(block->steps_z != 0) { enable_z(); delayMicroseconds(DELAY_ENABLE); } if(block->steps_e != 0) { enable_e(); delayMicroseconds(DELAY_ENABLE); } #else //enable active axes if(block->steps_x != 0) enable_x(); if(block->steps_y != 0) enable_y(); if(block->steps_z != 0) enable_z(); if(block->steps_e != 0) enable_e(); #endif if (block->steps_e == 0) { if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate; } else { if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate; } // slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill int moves_queued=(block_buffer_head-block_buffer_tail + block_buffer_size) & block_buffer_mask; #ifdef SLOWDOWN if(moves_queued < (block_buffer_size * 0.5) && moves_queued > MIN_MOVES_QUEUED_FOR_SLOWDOWN) feed_rate = feed_rate*moves_queued / (float)(block_buffer_size * 0.5); #endif float delta_mm[4]; delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/(float)(axis_steps_per_unit[X_AXIS]); delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/(float)(axis_steps_per_unit[Y_AXIS]); delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/(float)(axis_steps_per_unit[Z_AXIS]); //delta_mm[E_AXIS] = (target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS]; delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/(float)(axis_steps_per_unit[E_AXIS]))*extrudemultiply/100.0; if ( block->steps_x <= DROP_SEGMENTS && block->steps_y <= DROP_SEGMENTS && block->steps_z <= DROP_SEGMENTS ) block->millimeters = fabs(delta_mm[E_AXIS]); else block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS])); float inverse_millimeters = 1.0/(float)(block->millimeters); // Inverse millimeters to remove multiple divides // Calculate speed in mm/second for each axis. No divide by zero due to previous checks. float inverse_second = feed_rate * inverse_millimeters; block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0 block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0 /* #ifdef SLOWDOWN // segment time im micro seconds long segment_time = lround(1000000.0/inverse_second); if ((moves_queued>0) && (moves_queued < (BLOCK_BUFFER_SIZE - 4))) { if (segment_time<minsegmenttime) { // buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more. segment_time=segment_time+lround(2*(minsegmenttime-segment_time)/moves_queued); } } else { if (segment_time<minsegmenttime) segment_time=minsegmenttime; } #endif // END OF SLOW DOWN SECTION */ // Calculate and limit speed in mm/sec for each axis float current_speed[4]; float speed_factor = 1.0; //factor <=1 do decrease speed for(int i=0; i < 3; i++) { current_speed[i] = delta_mm[i] * inverse_second; if(fabs(current_speed[i]) > max_feedrate[i]) speed_factor = fmin(speed_factor, max_feedrate[i] / fabs(current_speed[i])); } current_speed[E_AXIS] = delta_mm[E_AXIS] * inverse_second; if(fabs(current_speed[E_AXIS]) > max_E_feedrate_calc) speed_factor = fmin(speed_factor, max_E_feedrate_calc / fabs(current_speed[E_AXIS])); // Correct the speed if( speed_factor < 1.0) { for(unsigned char i=0; i < 4; i++) current_speed[i] *= speed_factor; block->nominal_speed *= speed_factor; block->nominal_rate *= speed_factor; } // Compute and limit the acceleration rate for the trapezoid generator. float steps_per_mm = block->step_event_count/(float)(block->millimeters); if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0) block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 else { block->acceleration_st = ceil(move_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 // Limit acceleration per axis if(((float)block->acceleration_st * (float)block->steps_x / (float)block->step_event_count) > axis_steps_per_sqr_second[X_AXIS]) block->acceleration_st = axis_steps_per_sqr_second[X_AXIS]; if(((float)block->acceleration_st * (float)block->steps_y / (float)block->step_event_count) > axis_steps_per_sqr_second[Y_AXIS]) block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS]; if(((float)block->acceleration_st * (float)block->steps_e / (float)block->step_event_count) > axis_steps_per_sqr_second[E_AXIS]) block->acceleration_st = axis_steps_per_sqr_second[E_AXIS]; if(((float)block->acceleration_st * (float)block->steps_z / (float)block->step_event_count ) > axis_steps_per_sqr_second[Z_AXIS]) block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS]; } block->acceleration = block->acceleration_st / (float)(steps_per_mm); block->acceleration_rate = (long)((float)block->acceleration_st * 8.388608); #if 0 // Use old jerk for now // Compute path unit vector double unit_vec[3]; unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters; unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters; unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters; // Compute maximum allowable entry speed at junction by centripetal acceleration approximation. // Let a circle be tangent to both previous and current path line segments, where the junction // deviation is defined as the distance from the junction to the closest edge of the circle, // colinear with the circle center. The circular segment joining the two paths represents the // path of centripetal acceleration. Solve for max velocity based on max acceleration about the // radius of the circle, defined indirectly by junction deviation. This may be also viewed as // path width or max_jerk in the previous grbl version. This approach does not actually deviate // from path, but used as a robust way to compute cornering speeds, as it takes into account the // nonlinearities of both the junction angle and junction velocity. double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles. if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) { // Compute cosine of angle between previous and current path. (prev_unit_vec is negative) // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity. double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ; // Skip and use default max junction speed for 0 degree acute junction. if (cos_theta < 0.95) { vmax_junction = min(previous_nominal_speed,block->nominal_speed); // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds. if (cos_theta > -0.95) { // Compute maximum junction velocity based on maximum acceleration and junction deviation double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive. vmax_junction = min(vmax_junction, sqrt(block->acceleration * junction_deviation * sin_theta_d2/(float)(1.0-sin_theta_d2)) ); } } } #endif // Start with a safe speed float vmax_junction = max_xy_jerk/2.0; float vmax_junction_factor = 1.0; if(fabs(current_speed[Z_AXIS]) > max_z_jerk/2.0) vmax_junction = fmin(vmax_junction, max_z_jerk/2.0); if(fabs(current_speed[E_AXIS]) > max_e_jerk/2.0) vmax_junction = fmin(vmax_junction, max_e_jerk/2.0); if(G92_reset_previous_speed == 1) { vmax_junction = 0.1; G92_reset_previous_speed = 0; } vmax_junction = fmin(vmax_junction, block->nominal_speed); float safe_speed = vmax_junction; if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) { float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2)); // if((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) { vmax_junction = block->nominal_speed; // } if (jerk > max_xy_jerk) vmax_junction_factor = (max_xy_jerk/(float)(jerk)); if(fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]) > max_z_jerk) vmax_junction_factor= fmin(vmax_junction_factor, (max_z_jerk/fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]))); if(fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]) > max_e_jerk) vmax_junction_factor = fmin(vmax_junction_factor, (max_e_jerk/fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]))); vmax_junction = fmin(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed } block->max_entry_speed = vmax_junction; // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED. double v_allowable = max_allowable_speed(-block->acceleration,MINIMUM_PLANNER_SPEED,block->millimeters); block->entry_speed = fmin(vmax_junction, v_allowable); // Initialize planner efficiency flags // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds. // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then // the current block and next block junction speeds are guaranteed to always be at their maximum // junction speeds in deceleration and acceleration, respectively. This is due to how the current // block nominal speed limits both the current and next maximum junction speeds. Hence, in both // the reverse and forward planners, the corresponding block junction speed will always be at the // the maximum junction speed and may always be ignored for any speed reduction checks. if (block->nominal_speed <= v_allowable) block->nominal_length_flag = 1; else block->nominal_length_flag = 0; block->recalculate_flag = 1; // Always calculate trapezoid for new block // Update previous path unit_vector and nominal speed memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[] previous_nominal_speed = block->nominal_speed; calculate_trapezoid_for_block(block, block->entry_speed/(float)(block->nominal_speed), safe_speed/(float)(block->nominal_speed)); // Move buffer head CRITICAL_SECTION_START; block_buffer_head = next_buffer_head; CRITICAL_SECTION_END; // Update position memcpy(position, target, sizeof(target)); // position[] = target[] planner_recalculate(); #ifdef AUTOTEMP getHighESpeed(); #endif st_wake_up(); }