void check_axes_activity() { unsigned char x_active = 0; unsigned char y_active = 0; unsigned char z_active = 0; unsigned char e_active = 0; unsigned char fan_speed = 0; unsigned char tail_fan_speed = 0; block_t *block; if(block_buffer_tail != block_buffer_head) { uint8_t block_index = block_buffer_tail; tail_fan_speed = block_buffer[block_index].fan_speed; while(block_index != block_buffer_head) { block = &block_buffer[block_index]; if(block->steps_x != 0) x_active++; if(block->steps_y != 0) y_active++; if(block->steps_z != 0) z_active++; if(block->steps_e != 0) e_active++; if(block->fan_speed != 0) fan_speed++; block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1); } } else { #if FAN_PIN > -1 if (FanSpeed != 0){ analogWrite(FAN_PIN,FanSpeed); // If buffer is empty use current fan speed } #endif } if((DISABLE_X) && (x_active == 0)) disable_x(); if((DISABLE_Y) && (y_active == 0)) disable_y(); if((DISABLE_Z) && (z_active == 0)) disable_z(); if((DISABLE_E) && (e_active == 0)) { disable_e0(); disable_e1(); disable_e2(); } #if FAN_PIN > -1 if((FanSpeed == 0) && (fan_speed ==0)) { analogWrite(FAN_PIN, 0); } if (FanSpeed != 0 && tail_fan_speed !=0) { analogWrite(FAN_PIN,tail_fan_speed); } #endif #ifdef AUTOTEMP getHighESpeed(); #endif }
void check_axes_activity() { unsigned char x_active = 0; unsigned char y_active = 0; unsigned char z_active = 0; unsigned char e_active = 0; unsigned char tail_fan_speed = fanSpeed; block_t *block; if(block_buffer_tail != block_buffer_head) { uint8_t block_index = block_buffer_tail; tail_fan_speed = block_buffer[block_index].fan_speed; while(block_index != block_buffer_head) { block = &block_buffer[block_index]; if(block->steps_x != 0) x_active++; if(block->steps_y != 0) y_active++; if(block->steps_z != 0) z_active++; if(block->steps_e != 0) e_active++; block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1); } } if((DISABLE_X) && (x_active == 0)) disable_x(); if((DISABLE_Y) && (y_active == 0)) disable_y(); if((DISABLE_Z) && (z_active == 0)) disable_z(); if((DISABLE_E) && (e_active == 0)) { disable_e0(); disable_e1(); disable_e2(); } #if FAN_PIN > -1 #ifndef FAN_SOFT_PWM #ifdef FAN_KICKSTART_TIME static unsigned long fan_kick_end; if (tail_fan_speed) { if (fan_kick_end == 0) { // Just starting up fan - run at full power. fan_kick_end = millis() + FAN_KICKSTART_TIME; tail_fan_speed = 255; } else if (fan_kick_end > millis()) // Fan still spinning up. tail_fan_speed = 255; } else { fan_kick_end = 0; } #endif//FAN_KICKSTART_TIME analogWrite(FAN_PIN,tail_fan_speed); #endif//!FAN_SOFT_PWM #endif//FAN_PIN > -1 #ifdef AUTOTEMP getHighESpeed(); #endif }
void check_axes_activity() { unsigned char axis_active[NUM_AXIS] = { 0 }, tail_fan_speed = fanSpeed; #if ENABLED(BARICUDA) unsigned char tail_valve_pressure = ValvePressure, tail_e_to_p_pressure = EtoPPressure; #endif block_t *block; if (blocks_queued()) { uint8_t block_index = block_buffer_tail; tail_fan_speed = block_buffer[block_index].fan_speed; #if ENABLED(BARICUDA) block = &block_buffer[block_index]; tail_valve_pressure = block->valve_pressure; tail_e_to_p_pressure = block->e_to_p_pressure; #endif while (block_index != block_buffer_head) { block = &block_buffer[block_index]; for (int i=0; i<NUM_AXIS; i++) if (block->steps[i]) axis_active[i]++; block_index = next_block_index(block_index); } } if (DISABLE_X && !axis_active[X_AXIS]) disable_x(); if (DISABLE_Y && !axis_active[Y_AXIS]) disable_y(); if (DISABLE_Z && !axis_active[Z_AXIS]) disable_z(); if (DISABLE_E && !axis_active[E_AXIS]) { disable_e0(); disable_e1(); disable_e2(); disable_e3(); } #if HAS_FAN #ifdef FAN_KICKSTART_TIME static millis_t fan_kick_end; if (tail_fan_speed) { millis_t ms = millis(); if (fan_kick_end == 0) { // Just starting up fan - run at full power. fan_kick_end = ms + FAN_KICKSTART_TIME; tail_fan_speed = 255; } else if (fan_kick_end > ms) // Fan still spinning up. tail_fan_speed = 255; } else { fan_kick_end = 0; } #endif //FAN_KICKSTART_TIME #if ENABLED(FAN_MIN_PWM) #define CALC_FAN_SPEED (tail_fan_speed ? ( FAN_MIN_PWM + (tail_fan_speed * (255 - FAN_MIN_PWM)) / 255 ) : 0) #else #define CALC_FAN_SPEED tail_fan_speed #endif // FAN_MIN_PWM #if ENABLED(FAN_SOFT_PWM) fanSpeedSoftPwm = CALC_FAN_SPEED; #else analogWrite(FAN_PIN, CALC_FAN_SPEED); #endif // FAN_SOFT_PWM #endif // HAS_FAN #if ENABLED(AUTOTEMP) getHighESpeed(); #endif #if ENABLED(BARICUDA) #if HAS_HEATER_1 analogWrite(HEATER_1_PIN,tail_valve_pressure); #endif #if HAS_HEATER_2 analogWrite(HEATER_2_PIN,tail_e_to_p_pressure); #endif #endif }
void check_axes_activity() { unsigned char x_active = 0; unsigned char y_active = 0; unsigned char z_active = 0; unsigned char j_active = 0; unsigned char e_active = 0; unsigned char tail_fan_speed = fanSpeed; #ifdef BARICUDA unsigned char tail_valve_pressure = ValvePressure; unsigned char tail_e_to_p_pressure = EtoPPressure; #endif block_t *block; if(block_buffer_tail != block_buffer_head) { uint8_t block_index = block_buffer_tail; tail_fan_speed = block_buffer[block_index].fan_speed; #ifdef BARICUDA tail_valve_pressure = block_buffer[block_index].valve_pressure; tail_e_to_p_pressure = block_buffer[block_index].e_to_p_pressure; #endif while(block_index != block_buffer_head) { block = &block_buffer[block_index]; if(block->steps_x != 0) x_active++; if(block->steps_y != 0) y_active++; if(block->steps_z != 0) z_active++; if(block->steps_j != 0) j_active++; if(block->steps_e != 0) e_active++; block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1); } } if((DISABLE_X) && (x_active == 0)) disable_x(); if((DISABLE_Y) && (y_active == 0)) disable_y(); if((DISABLE_Z) && (z_active == 0)) disable_z(); if((DISABLE_J) && (j_active == 0)) disable_j(); if((DISABLE_E) && (e_active == 0)) { disable_e0(); disable_e1(); disable_e2(); } #if defined(FAN_PIN) && FAN_PIN > -1 #ifdef FAN_KICKSTART_TIME static unsigned long fan_kick_end; if (tail_fan_speed) { if (fan_kick_end == 0) { // Just starting up fan - run at full power. fan_kick_end = millis() + FAN_KICKSTART_TIME; tail_fan_speed = 255; } else if (fan_kick_end > millis()) // Fan still spinning up. tail_fan_speed = 255; } else { fan_kick_end = 0; } #endif//FAN_KICKSTART_TIME #ifdef FAN_SOFT_PWM fanSpeedSoftPwm = tail_fan_speed; #else analogWrite(FAN_PIN,tail_fan_speed); #endif//!FAN_SOFT_PWM #endif//FAN_PIN > -1 #ifdef AUTOTEMP getHighESpeed(); #endif #ifdef BARICUDA #if defined(HEATER_1_PIN) && HEATER_1_PIN > -1 analogWrite(HEATER_1_PIN,tail_valve_pressure); #endif #if defined(HEATER_2_PIN) && HEATER_2_PIN > -1 analogWrite(HEATER_2_PIN,tail_e_to_p_pressure); #endif #endif }
/** * Maintain fans, paste extruder pressure, */ void Planner::check_axes_activity() { unsigned char axis_active[NUM_AXIS] = { 0 }, tail_fan_speed[FAN_COUNT]; #if FAN_COUNT > 0 for (uint8_t i = 0; i < FAN_COUNT; i++) tail_fan_speed[i] = fanSpeeds[i]; #endif #if ENABLED(BARICUDA) unsigned char tail_valve_pressure = baricuda_valve_pressure, tail_e_to_p_pressure = baricuda_e_to_p_pressure; #endif if (blocks_queued()) { #if FAN_COUNT > 0 for (uint8_t i = 0; i < FAN_COUNT; i++) tail_fan_speed[i] = block_buffer[block_buffer_tail].fan_speed[i]; #endif block_t* block; #if ENABLED(BARICUDA) block = &block_buffer[block_buffer_tail]; tail_valve_pressure = block->valve_pressure; tail_e_to_p_pressure = block->e_to_p_pressure; #endif for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) { block = &block_buffer[b]; for (int i = 0; i < NUM_AXIS; i++) if (block->steps[i]) axis_active[i]++; } } #if ENABLED(DISABLE_X) if (!axis_active[X_AXIS]) disable_x(); #endif #if ENABLED(DISABLE_Y) if (!axis_active[Y_AXIS]) disable_y(); #endif #if ENABLED(DISABLE_Z) if (!axis_active[Z_AXIS]) disable_z(); #endif #if ENABLED(DISABLE_E) if (!axis_active[E_AXIS]) { disable_e0(); disable_e1(); disable_e2(); disable_e3(); } #endif #if FAN_COUNT > 0 #if defined(FAN_MIN_PWM) #define CALC_FAN_SPEED(f) (tail_fan_speed[f] ? ( FAN_MIN_PWM + (tail_fan_speed[f] * (255 - FAN_MIN_PWM)) / 255 ) : 0) #else #define CALC_FAN_SPEED(f) tail_fan_speed[f] #endif #ifdef FAN_KICKSTART_TIME static millis_t fan_kick_end[FAN_COUNT] = { 0 }; #define KICKSTART_FAN(f) \ if (tail_fan_speed[f]) { \ millis_t ms = millis(); \ if (fan_kick_end[f] == 0) { \ fan_kick_end[f] = ms + FAN_KICKSTART_TIME; \ tail_fan_speed[f] = 255; \ } else { \ if (PENDING(ms, fan_kick_end[f])) { \ tail_fan_speed[f] = 255; \ } \ } \ } else { \ fan_kick_end[f] = 0; \ } #if HAS_FAN0 KICKSTART_FAN(0); #endif #if HAS_FAN1 KICKSTART_FAN(1); #endif #if HAS_FAN2 KICKSTART_FAN(2); #endif #endif //FAN_KICKSTART_TIME #if ENABLED(FAN_SOFT_PWM) #if HAS_FAN0 thermalManager.fanSpeedSoftPwm[0] = CALC_FAN_SPEED(0); #endif #if HAS_FAN1 thermalManager.fanSpeedSoftPwm[1] = CALC_FAN_SPEED(1); #endif #if HAS_FAN2 thermalManager.fanSpeedSoftPwm[2] = CALC_FAN_SPEED(2); #endif #else #if HAS_FAN0 analogWrite(FAN_PIN, CALC_FAN_SPEED(0)); #endif #if HAS_FAN1 analogWrite(FAN1_PIN, CALC_FAN_SPEED(1)); #endif #if HAS_FAN2 analogWrite(FAN2_PIN, CALC_FAN_SPEED(2)); #endif #endif #endif // FAN_COUNT > 0 #if ENABLED(AUTOTEMP) getHighESpeed(); #endif #if ENABLED(BARICUDA) #if HAS_HEATER_1 analogWrite(HEATER_1_PIN, tail_valve_pressure); #endif #if HAS_HEATER_2 analogWrite(HEATER_2_PIN, tail_e_to_p_pressure); #endif #endif }
// 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 check_axes_activity() { unsigned char axis_active[NUM_AXIS], tail_fan_speed = fanSpeed; #ifdef BARICUDA unsigned char tail_valve_pressure = ValvePressure, tail_e_to_p_pressure = EtoPPressure; #endif block_t *block; if (blocks_queued()) { uint8_t block_index = block_buffer_tail; tail_fan_speed = block_buffer[block_index].fan_speed; #ifdef BARICUDA block = &block_buffer[block_index]; tail_valve_pressure = block->valve_pressure; tail_e_to_p_pressure = block->e_to_p_pressure; #endif while (block_index != block_buffer_head) { block = &block_buffer[block_index]; for (int i=0; i<NUM_AXIS; i++) if (block->steps[i]) axis_active[i]++; block_index = next_block_index(block_index); } } if (DISABLE_X && !axis_active[X_AXIS]) disable_x(); if (DISABLE_Y && !axis_active[Y_AXIS]) disable_y(); if (DISABLE_Z && !axis_active[Z_AXIS]) disable_z(); if (DISABLE_E && !axis_active[E_AXIS]) { disable_e0(); disable_e1(); disable_e2(); disable_e3(); } #if defined(FAN_PIN) && FAN_PIN > -1 // HAS_FAN #ifdef FAN_KICKSTART_TIME static unsigned long fan_kick_end; if (tail_fan_speed) { if (fan_kick_end == 0) { // Just starting up fan - run at full power. fan_kick_end = millis() + FAN_KICKSTART_TIME; tail_fan_speed = 255; } else if (fan_kick_end > millis()) // Fan still spinning up. tail_fan_speed = 255; } else { fan_kick_end = 0; } #endif//FAN_KICKSTART_TIME #ifdef FAN_SOFT_PWM fanSpeedSoftPwm = tail_fan_speed; #else analogWrite(FAN_PIN, tail_fan_speed); #endif //!FAN_SOFT_PWM #endif //FAN_PIN > -1 #ifdef AUTOTEMP getHighESpeed(); #endif #ifdef BARICUDA #if defined(HEATER_1_PIN) && HEATER_1_PIN > -1 // HAS_HEATER_1 analogWrite(HEATER_1_PIN,tail_valve_pressure); #endif #if defined(HEATER_2_PIN) && HEATER_2_PIN > -1 // HAS_HEATER_2 analogWrite(HEATER_2_PIN,tail_e_to_p_pressure); #endif #endif }
// 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(); }