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