void MTWLEDLogic() // called from main loop
{
patterncode pattern = MTWLED_lastpattern;
int swing;
if(MTWLED_control==1 || MTWLED_control==255) return;
if(MTWLEDEndstop(false)) return;
if(pattern.value==mtwled_nochange) return;
if(MTWLED_timer > millis()) return;
if(MTWLED_control==-1) { // if this is first time display endstop status before clearing to ready
MTWLEDEndstop(true);
MTWLED_control=0;
return;
}
if((degTargetHotend(0) == 0)) {
if((degHotend(0) > MTWLED_cool)) // heater is off but still warm
pattern.value=MTWLEDConvert(mtwled_heateroff);
else
pattern.value=MTWLEDConvert(mtwled_ready);
MTWLEDUpdate(pattern);
} else {
swing=abs(degTargetHotend(0) - degHotend(0)); // how far off from target temp we are
if(swing < MTWLED_swing*2) { // if within double the swing threshold
if(isHeatingHotend(0)) pattern.value=MTWLEDConvert(mtwled_templow); // heater is on so temp must be low
if(isCoolingHotend(0)) pattern.value=MTWLEDConvert(mtwled_temphigh); // heater is off so temp must be high
if(swing < MTWLED_swing) pattern.value=MTWLEDConvert(mtwled_temphit); // close to target temp, so consider us 'at temp'
MTWLEDUpdate(pattern);
}
}
}
void lcd_update() {
if (!NextionON) return;
nexLoop(nex_listen_list);
millis_t ms = millis();
if (ms > next_lcd_update_ms && PageInfo) {
if (fanSpeed > 0) fantimer.enable();
else fantimer.disable();
#if HAS_TEMP_0
temptoLCD(0, degHotend(0), degTargetHotend(0));
#endif
#if HAS_TEMP_1
temptoLCD(1, degHotend(1), degTargetHotend(1));
#endif
#if HAS_TEMP_2
temptoLCD(2, degHotend(2), degTargetHotend(2));
#elif HAS_TEMP_BED
temptoLCD(2, degBed(), degTargetBed());
#endif
coordtoLCD();
#if ENABLED(SDSUPPORT)
if (card.cardOK) {
MSD.setPic(7);
NPlay.setPic(38);
NStop.setPic(41);
}
else {
MSD.setPic(6);
NPlay.setPic(39);
NStop.setPic(42);
}
if (card.isFileOpen()) {
if (card.sdprinting) {
// Progress bar solid part
sdbar.setValue(card.percentDone());
NPlay.setPic(40);
}
else {
NPlay.setPic(38);
}
}
#endif
next_lcd_update_ms = ms + LCD_UPDATE_INTERVAL;
}
}
void MTWLEDTemp() // called from inside heater function while heater is on to to the percentile display
{
byte pattern;
if(MTWLED_control==255) return;
if(abs(degTargetHotend(0) - degHotend(0)) > MTWLED_swing*2) {
pattern = 90 + ((degHotend(0) / degTargetHotend(0)) * 10);
if(pattern > 99) pattern = 99;
MTWLEDUpdate(pattern);
}
}
static void lcd_menu_insert_material_preheat()
{
setTargetHotend(material[active_extruder].temperature, active_extruder);
int16_t temp = degHotend(active_extruder) - 20;
int16_t target = degTargetHotend(active_extruder) - 20 - 10;
if (temp < 0) temp = 0;
if (temp > target && !is_command_queued())
{
set_extrude_min_temp(0);
for(uint8_t e=0; e<EXTRUDERS; e++)
volume_to_filament_length[e] = 1.0;//Set the extrusion to 1mm per given value, so we can move the filament a set distance.
currentMenu = lcd_menu_change_material_insert_wait_user;
temp = target;
}
uint8_t progress = uint8_t(temp * 125 / target);
if (progress < minProgress)
progress = minProgress;
else
minProgress = progress;
lcd_info_screen(lcd_menu_material_main, cancelMaterialInsert);
lcd_lib_draw_stringP(3, 10, PSTR("Heating printhead for"));
lcd_lib_draw_stringP(3, 20, PSTR("material insertion"));
lcd_progressbar(progress);
lcd_lib_update_screen();
}
static void lcd_menu_first_run_material_load_heatup()
{
setTargetHotend(230, 0);
int16_t temp = degHotend(0) - 20;
int16_t target = degTargetHotend(0) - 10 - 20;
if (temp < 0) temp = 0;
if (temp > target)
{
for(uint8_t e=0; e<EXTRUDERS; e++)
volume_to_filament_length[e] = 1.0;//Set the extrusion to 1mm per given value, so we can move the filament a set distance.
currentMenu = lcd_menu_first_run_material_load_insert;
temp = target;
}
uint8_t progress = uint8_t(temp * 125 / target);
if (progress < minProgress)
progress = minProgress;
else
minProgress = progress;
lcd_basic_screen();
DRAW_PROGRESS_NR(12);
lcd_lib_draw_string_centerP(10, PSTR("Please wait,"));
lcd_lib_draw_string_centerP(20, PSTR("printhead heating for"));
lcd_lib_draw_string_centerP(30, PSTR("material loading"));
lcd_progressbar(progress);
lcd_lib_update_screen();
}
static void lcd_menu_change_material_preheat()
{
#ifdef USE_CHANGE_TEMPERATURE
setTargetHotend(material[active_extruder].change_temperature, active_extruder);
#else
setTargetHotend(material[active_extruder].temperature, active_extruder);
#endif
int16_t temp = degHotend(active_extruder) - 20;
int16_t target = degTargetHotend(active_extruder) - 20;
if (temp < 0) temp = 0;
if (temp > target - 5 && temp < target + 5)
{
if ((signed long)(millis() - preheat_end_time) > 0)
{
set_extrude_min_temp(0);
plan_set_e_position(0);
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], 20.0 / volume_to_filament_length[active_extruder], retract_feedrate/60.0, active_extruder);
float old_max_feedrate_e = max_feedrate[E_AXIS];
float old_retract_acceleration = retract_acceleration;
max_feedrate[E_AXIS] = FILAMENT_REVERSAL_SPEED;
retract_acceleration = FILAMENT_LONG_MOVE_ACCELERATION;
plan_set_e_position(0);
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], -1.0 / volume_to_filament_length[active_extruder], FILAMENT_REVERSAL_SPEED, active_extruder);
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], -FILAMENT_REVERSAL_LENGTH / volume_to_filament_length[active_extruder], FILAMENT_REVERSAL_SPEED, active_extruder);
max_feedrate[E_AXIS] = old_max_feedrate_e;
retract_acceleration = old_retract_acceleration;
currentMenu = lcd_menu_change_material_remove;
temp = target;
}
}
else
{
#ifdef USE_CHANGE_TEMPERATURE
preheat_end_time = millis() + (unsigned long)material[active_extruder].change_preheat_wait_time * 1000L;
#else
preheat_end_time = millis();
#endif
}
uint8_t progress = uint8_t(temp * 125 / target);
if (progress < minProgress)
progress = minProgress;
else
minProgress = progress;
lcd_info_screen(post_change_material_menu, cancelMaterialInsert);
lcd_lib_draw_stringP(3, 10, PSTR("Heating printhead"));
lcd_lib_draw_stringP(3, 20, PSTR("for material removal"));
lcd_progressbar(progress);
lcd_lib_update_screen();
}
static void lcd_menu_change_material_preheat()
{
run_history = true;
setTargetHotend(material[active_extruder].temperature, active_extruder);
int16_t temp = degHotend(active_extruder) - 20;
int16_t target = degTargetHotend(active_extruder) - 20 - 10;
if (temp < 0) temp = 0;
if (temp > target && !is_command_queued())
{
set_extrude_min_temp(0);
for(uint8_t e=0; e<EXTRUDERS; e++)
volume_to_filament_length[e] = 1.0;//Set the extrusion to 1mm per given value, so we can move the filament a set distance.
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], 20.0, retract_feedrate/60.0, active_extruder);
float old_max_feedrate_e = max_feedrate[E_AXIS];
float old_retract_acceleration = retract_acceleration;
max_feedrate[E_AXIS] = FILAMENT_REVERSAL_SPEED;
retract_acceleration = FILAMENT_LONG_MOVE_ACCELERATION;
current_position[E_AXIS] = 0;
plan_set_e_position(current_position[E_AXIS]);
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], -1.0, FILAMENT_REVERSAL_SPEED, active_extruder);
for(uint8_t n=0; n<6; n++)
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], (n+1)*-FILAMENT_REVERSAL_LENGTH/6, FILAMENT_REVERSAL_SPEED, active_extruder);
max_feedrate[E_AXIS] = old_max_feedrate_e;
retract_acceleration = old_retract_acceleration;
currentMenu = lcd_menu_change_material_remove;
temp = target;
}
uint8_t progress = uint8_t(temp * 125 / target);
if (progress < minProgress)
progress = minProgress;
else
minProgress = progress;
lcd_info_screen(lcd_menu_material_main, cancelMaterialInsert);
lcd_lib_draw_stringP(3, 0, PSTR("Heating printhead"));
lcd_lib_draw_stringP(3, 10, PSTR("for material removal"));
char buffer[20];
memset (buffer,0,sizeof(buffer));
char* c;
c = int_to_string(temp, buffer/*, PSTR( DEGREE_C_SYMBOL )*/);
*c++ = TEMPERATURE_SEPARATOR;
c = int_to_string(target, c, PSTR( DEGREE_C_SYMBOL ));
lcd_lib_draw_string_center(20, buffer);
lcd_progressbar(progress);
LED_HEAT();
lcd_lib_update_screen();
}
void MTWLEDLogic() // called from main loop
{
patterncode pattern = MTWLED_lastpattern;
if(MTWLED_control==1 || MTWLED_control==255) return;
if(MTWLEDEndstop(false)) return;
// if(pattern.value==mtwled_nochange) return;
if(MTWLED_timer > millis()) return;
if(MTWLED_control==-1) { // if this is first time display endstop status before clearing to ready
MTWLEDEndstop(true);
MTWLED_control=0;
return;
}
if((degTargetHotend(active_extruder) == 0)) { // assume not printing since target temp is zero
if((degHotend(active_extruder) > MTWLED_cool)) // heater is off but still warm
pattern.value=MTWLEDConvert(mtwled_heateroff);
else {
pattern.value=MTWLEDConvert(mtwled_ready);
MTWLED_heated=false;
}
MTWLEDUpdate(pattern);
} else {
if(MTWLED_heated)
switch(MTWLED_mode) {
// case 2: // display XYZ position colors separately needs pattern 8 implemented in controller
// MTWLEDUpdate(8,(current_position[X_AXIS]/X_MAX_POS)*50+5,(current_position[Z_AXIS]/Z_MAX_POS)*100+5,(current_position[Y_AXIS]/Y_MAX_POS)*50+5,1);
// break;
case 1: // XYZ position used as RBG
MTWLEDUpdate(10,(current_position[X_AXIS]/X_MAX_POS)*50+5,(current_position[Z_AXIS]/Z_MAX_POS)*100+5,(current_position[Y_AXIS]/Y_MAX_POS)*50+5,1);
break;
case 0:
default: // show solid color based on XYZ=RGB color values
if(abs(degTargetHotend(active_extruder) - degHotend(active_extruder)) > MTWLED_swing) { // temp is not close to target
if(isHeatingHotend(active_extruder)) pattern.value=MTWLEDConvert(mtwled_templow); // heater is on so temp must be low
if(isCoolingHotend(active_extruder)) pattern.value=MTWLEDConvert(mtwled_temphigh); // heater is off so temp must be high
} else { // temp is close to target
pattern.value=MTWLEDConvert(mtwled_temphit); // close to target temp, so consider us 'at temp'
}
MTWLEDUpdate(pattern);
break;
}
}
}
void MTWLEDTemp() // called from inside heater function while heater is on to do the percentile display
{
byte percent;
if(MTWLED_heated) return;
if(MTWLED_control==255) return;
if((degTargetHotend(active_extruder) == 0)) return;
percent = ((degHotend(active_extruder) / (degTargetHotend(active_extruder))) * 100);
if(percent >= 100) {
percent = 100;
MTWLED_heated=true;
if(MTWLED_feedback) {
SERIAL_PROTOCOLLN("LED heated. Entering printmode");
}
}
MTWLEDUpdate(9,percent,MTWLED_heatmode,0);
}
// Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in
// mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
// calculation the caller must also provide the physical length of the line in millimeters.
void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder)
{
// Calculate the buffer head after we push this byte
int next_buffer_head = next_block_index(block_buffer_head);
// If the buffer is full: good! That means we are well ahead of the robot.
// Rest here until there is room in the buffer.
while(block_buffer_tail == next_buffer_head)
{
manage_heater();
manage_inactivity();
lcd_update();
}
// The target position of the tool in absolute steps
// Calculate target position in absolute steps
//this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
long target[4];
target[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);
#ifdef PREVENT_DANGEROUS_EXTRUDE
if(target[E_AXIS]!=position[E_AXIS])
{
if(degHotend(active_extruder)<extrude_min_temp)
{
position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
}
#ifdef PREVENT_LENGTHY_EXTRUDE
if(labs(target[E_AXIS]-position[E_AXIS])>axis_steps_per_unit[E_AXIS]*EXTRUDE_MAXLENGTH)
{
#ifdef EASY_LOAD
if (!allow_lengthy_extrude_once) {
#endif
position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
#ifdef EASY_LOAD
}
allow_lengthy_extrude_once = false;
#endif
}
#endif // PREVENT_LENGTHY_EXTRUDE
}
#endif
// Prepare to set up new block
block_t *block = &block_buffer[block_buffer_head];
// Mark block as not busy (Not executed by the stepper interrupt)
block->busy = false;
// Number of steps for each axis
#ifndef COREXY
// default non-h-bot planning
block->steps_x = labs(target[X_AXIS]-position[X_AXIS]);
block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]);
#else
// corexy planning
// these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
block->steps_x = labs((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]));
block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]));
#endif
block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]);
block->steps_e = labs(target[E_AXIS]-position[E_AXIS]);
block->steps_e *= extrudemultiply;
block->steps_e /= 100;
block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_e)));
// Bail if this is a zero-length block
if (block->step_event_count <= dropsegments)
{
return;
}
block->fan_speed = fanSpeed;
#ifdef BARICUDA
block->valve_pressure = ValvePressure;
block->e_to_p_pressure = EtoPPressure;
#endif
// Compute direction bits for this block
block->direction_bits = 0;
#ifndef COREXY
if (target[X_AXIS] < position[X_AXIS])
{
block->direction_bits |= (1<<X_AXIS);
}
if (target[Y_AXIS] < position[Y_AXIS])
{
block->direction_bits |= (1<<Y_AXIS);
}
#else
if ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]) < 0)
{
block->direction_bits |= (1<<X_AXIS);
}
if ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]) < 0)
{
block->direction_bits |= (1<<Y_AXIS);
}
#endif
if (target[Z_AXIS] < position[Z_AXIS])
{
block->direction_bits |= (1<<Z_AXIS);
}
if (target[E_AXIS] < position[E_AXIS])
{
block->direction_bits |= (1<<E_AXIS);
}
block->active_extruder = extruder;
//enable active axes
#ifdef COREXY
if((block->steps_x != 0) || (block->steps_y != 0))
{
enable_x();
enable_y();
}
#else
if(block->steps_x != 0) enable_x();
if(block->steps_y != 0) enable_y();
#endif
#ifndef Z_LATE_ENABLE
if(block->steps_z != 0) enable_z();
#endif
// Enable all
if(block->steps_e != 0)
{
enable_e0();
enable_e1();
enable_e2();
}
if (block->steps_e == 0)
{
if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate;
}
else
{
if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate;
}
float delta_mm[4];
#ifndef COREXY
delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];
delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
#else
delta_mm[X_AXIS] = ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[X_AXIS];
delta_mm[Y_AXIS] = ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[Y_AXIS];
#endif
delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS];
delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS])*extrudemultiply/100.0;
if ( block->steps_x <=dropsegments && block->steps_y <=dropsegments && block->steps_z <=dropsegments )
{
block->millimeters = fabs(delta_mm[E_AXIS]);
}
else
{
block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS]));
}
float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides
// Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
float inverse_second = feed_rate * inverse_millimeters;
int moves_queued=(block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1);
// slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill
#ifdef OLD_SLOWDOWN
if(moves_queued < (BLOCK_BUFFER_SIZE * 0.5) && moves_queued > 1)
feed_rate = feed_rate*moves_queued / (BLOCK_BUFFER_SIZE * 0.5);
#endif
#ifdef SLOWDOWN
// segment time im micro seconds
unsigned long segment_time = lround(1000000.0/inverse_second);
if ((moves_queued > 1) && (moves_queued < (BLOCK_BUFFER_SIZE * 0.5)))
{
if (segment_time < minsegmenttime)
{ // buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
inverse_second=1000000.0/(segment_time+lround(2*(minsegmenttime-segment_time)/moves_queued));
#ifdef XY_FREQUENCY_LIMIT
segment_time = lround(1000000.0/inverse_second);
#endif
}
}
#endif
// END OF SLOW DOWN SECTION
block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0
block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0
// Calculate and limit speed in mm/sec for each axis
float current_speed[4];
float speed_factor = 1.0; //factor <=1 do decrease speed
for(int i=0; i < 4; i++)
{
current_speed[i] = delta_mm[i] * inverse_second;
if(fabs(current_speed[i]) > max_feedrate[i])
speed_factor = min(speed_factor, max_feedrate[i] / fabs(current_speed[i]));
}
// Max segement time in us.
#ifdef XY_FREQUENCY_LIMIT
#define MAX_FREQ_TIME (1000000.0/XY_FREQUENCY_LIMIT)
// Check and limit the xy direction change frequency
unsigned char direction_change = block->direction_bits ^ old_direction_bits;
old_direction_bits = block->direction_bits;
segment_time = lround((float)segment_time / speed_factor);
if((direction_change & (1<<X_AXIS)) == 0)
{
x_segment_time[0] += segment_time;
}
else
{
x_segment_time[2] = x_segment_time[1];
x_segment_time[1] = x_segment_time[0];
x_segment_time[0] = segment_time;
}
if((direction_change & (1<<Y_AXIS)) == 0)
{
y_segment_time[0] += segment_time;
}
else
{
y_segment_time[2] = y_segment_time[1];
y_segment_time[1] = y_segment_time[0];
y_segment_time[0] = segment_time;
}
long max_x_segment_time = max(x_segment_time[0], max(x_segment_time[1], x_segment_time[2]));
long max_y_segment_time = max(y_segment_time[0], max(y_segment_time[1], y_segment_time[2]));
long min_xy_segment_time =min(max_x_segment_time, max_y_segment_time);
if(min_xy_segment_time < MAX_FREQ_TIME)
speed_factor = min(speed_factor, speed_factor * (float)min_xy_segment_time / (float)MAX_FREQ_TIME);
#endif
// Correct the speed
if( speed_factor < 1.0)
{
for(unsigned char i=0; i < 4; i++)
{
current_speed[i] *= speed_factor;
}
block->nominal_speed *= speed_factor;
block->nominal_rate *= speed_factor;
}
// Compute and limit the acceleration rate for the trapezoid generator.
float steps_per_mm = block->step_event_count/block->millimeters;
if(block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)
{
block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
}
else
{
block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
// Limit acceleration per axis
if(((float)block->acceleration_st * (float)block->steps_x / (float)block->step_event_count) > axis_steps_per_sqr_second[X_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[X_AXIS];
if(((float)block->acceleration_st * (float)block->steps_y / (float)block->step_event_count) > axis_steps_per_sqr_second[Y_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[Y_AXIS];
if(((float)block->acceleration_st * (float)block->steps_e / (float)block->step_event_count) > axis_steps_per_sqr_second[E_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[E_AXIS];
if(((float)block->acceleration_st * (float)block->steps_z / (float)block->step_event_count ) > axis_steps_per_sqr_second[Z_AXIS])
block->acceleration_st = axis_steps_per_sqr_second[Z_AXIS];
}
block->acceleration = block->acceleration_st / steps_per_mm;
block->acceleration_rate = (long)((float)block->acceleration_st * (16777216.0 / (F_CPU / 8.0)));
#if 0 // Use old jerk for now
// Compute path unit vector
double unit_vec[3];
unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;
// Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
// Let a circle be tangent to both previous and current path line segments, where the junction
// deviation is defined as the distance from the junction to the closest edge of the circle,
// colinear with the circle center. The circular segment joining the two paths represents the
// path of centripetal acceleration. Solve for max velocity based on max acceleration about the
// radius of the circle, defined indirectly by junction deviation. This may be also viewed as
// path width or max_jerk in the previous grbl version. This approach does not actually deviate
// from path, but used as a robust way to compute cornering speeds, as it takes into account the
// nonlinearities of both the junction angle and junction velocity.
double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
- previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
- previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
// Skip and use default max junction speed for 0 degree acute junction.
if (cos_theta < 0.95) {
vmax_junction = min(previous_nominal_speed,block->nominal_speed);
// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
if (cos_theta > -0.95) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation
double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
vmax_junction = min(vmax_junction,
sqrt(block->acceleration * junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
}
}
}
#endif
// Start with a safe speed
float vmax_junction = max_xy_jerk/2;
float vmax_junction_factor = 1.0;
if(fabs(current_speed[Z_AXIS]) > max_z_jerk/2)
vmax_junction = min(vmax_junction, max_z_jerk/2);
if(fabs(current_speed[E_AXIS]) > max_e_jerk/2)
vmax_junction = min(vmax_junction, max_e_jerk/2);
vmax_junction = min(vmax_junction, block->nominal_speed);
float safe_speed = vmax_junction;
if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
float jerk = sqrt(pow((current_speed[X_AXIS]-previous_speed[X_AXIS]), 2)+pow((current_speed[Y_AXIS]-previous_speed[Y_AXIS]), 2));
// if((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
vmax_junction = block->nominal_speed;
// }
if (jerk > max_xy_jerk) {
vmax_junction_factor = (max_xy_jerk/jerk);
}
if(fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]) > max_z_jerk) {
vmax_junction_factor= min(vmax_junction_factor, (max_z_jerk/fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS])));
}
if(fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]) > max_e_jerk) {
vmax_junction_factor = min(vmax_junction_factor, (max_e_jerk/fabs(current_speed[E_AXIS] - previous_speed[E_AXIS])));
}
vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
}
block->max_entry_speed = vmax_junction;
// Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
double v_allowable = max_allowable_speed(-block->acceleration,MINIMUM_PLANNER_SPEED,block->millimeters);
block->entry_speed = min(vmax_junction, v_allowable);
// Initialize planner efficiency flags
// Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
// If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
// the current block and next block junction speeds are guaranteed to always be at their maximum
// junction speeds in deceleration and acceleration, respectively. This is due to how the current
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
// the reverse and forward planners, the corresponding block junction speed will always be at the
// the maximum junction speed and may always be ignored for any speed reduction checks.
if (block->nominal_speed <= v_allowable) {
block->nominal_length_flag = true;
}
else {
block->nominal_length_flag = false;
}
block->recalculate_flag = true; // Always calculate trapezoid for new block
// Update previous path unit_vector and nominal speed
memcpy(previous_speed, current_speed, sizeof(previous_speed)); // previous_speed[] = current_speed[]
previous_nominal_speed = block->nominal_speed;
#ifdef ADVANCE
// Calculate advance rate
if((block->steps_e == 0) || (block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)) {
block->advance_rate = 0;
block->advance = 0;
}
else {
long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st);
float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) *
(current_speed[E_AXIS] * current_speed[E_AXIS] * EXTRUTION_AREA * EXTRUTION_AREA)*256;
block->advance = advance;
if(acc_dist == 0) {
block->advance_rate = 0;
}
else {
block->advance_rate = advance / (float)acc_dist;
}
}
/*
SERIAL_ECHO_START;
SERIAL_ECHOPGM("advance :");
SERIAL_ECHO(block->advance/256.0);
SERIAL_ECHOPGM("advance rate :");
SERIAL_ECHOLN(block->advance_rate/256.0);
*/
#endif // ADVANCE
calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed,
safe_speed/block->nominal_speed);
// Move buffer head
block_buffer_head = next_buffer_head;
// Update position
memcpy(position, target, sizeof(target)); // position[] = target[]
planner_recalculate();
st_wake_up();
}
void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t extruder)
#endif // AUTO_BED_LEVELING_FEATURE
{
// Calculate the buffer head after we push this byte
int next_buffer_head = next_block_index(block_buffer_head);
// If the buffer is full: good! That means we are well ahead of the robot.
// Rest here until there is room in the buffer.
while (block_buffer_tail == next_buffer_head) idle();
#if ENABLED(MESH_BED_LEVELING)
if (mbl.active) z += mbl.get_z(x, y);
#elif ENABLED(AUTO_BED_LEVELING_FEATURE)
apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
#endif
// The target position of the tool in absolute steps
// Calculate target position in absolute steps
//this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
long target[NUM_AXIS];
target[X_AXIS] = lround(x * axis_steps_per_unit[X_AXIS]);
target[Y_AXIS] = lround(y * axis_steps_per_unit[Y_AXIS]);
target[Z_AXIS] = lround(z * axis_steps_per_unit[Z_AXIS]);
target[E_AXIS] = lround(e * axis_steps_per_unit[E_AXIS]);
float dx = target[X_AXIS] - position[X_AXIS],
dy = target[Y_AXIS] - position[Y_AXIS],
dz = target[Z_AXIS] - position[Z_AXIS];
// DRYRUN ignores all temperature constraints and assures that the extruder is instantly satisfied
if (marlin_debug_flags & DEBUG_DRYRUN)
position[E_AXIS] = target[E_AXIS];
float de = target[E_AXIS] - position[E_AXIS];
#if ENABLED(PREVENT_DANGEROUS_EXTRUDE)
if (de) {
if (degHotend(extruder) < extrude_min_temp) {
position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part
de = 0; // no difference
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
}
#if ENABLED(PREVENT_LENGTHY_EXTRUDE)
if (labs(de) > axis_steps_per_unit[E_AXIS] * EXTRUDE_MAXLENGTH) {
position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part
de = 0; // no difference
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
}
#endif
}
#endif
// Prepare to set up new block
block_t *block = &block_buffer[block_buffer_head];
// Mark block as not busy (Not executed by the stepper interrupt)
block->busy = false;
// Number of steps for each axis
#if ENABLED(COREXY)
// corexy planning
// these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
block->steps[A_AXIS] = labs(dx + dy);
block->steps[B_AXIS] = labs(dx - dy);
block->steps[Z_AXIS] = labs(dz);
#elif ENABLED(COREXZ)
// corexz planning
block->steps[A_AXIS] = labs(dx + dz);
block->steps[Y_AXIS] = labs(dy);
block->steps[C_AXIS] = labs(dx - dz);
#else
// default non-h-bot planning
block->steps[X_AXIS] = labs(dx);
block->steps[Y_AXIS] = labs(dy);
block->steps[Z_AXIS] = labs(dz);
#endif
block->steps[E_AXIS] = labs(de);
block->steps[E_AXIS] *= volumetric_multiplier[extruder];
block->steps[E_AXIS] *= extruder_multiplier[extruder];
block->steps[E_AXIS] /= 100;
block->step_event_count = max(block->steps[X_AXIS], max(block->steps[Y_AXIS], max(block->steps[Z_AXIS], block->steps[E_AXIS])));
// Bail if this is a zero-length block
if (block->step_event_count <= dropsegments) return;
block->fan_speed = fanSpeed;
#if ENABLED(BARICUDA)
block->valve_pressure = ValvePressure;
block->e_to_p_pressure = EtoPPressure;
#endif
// Compute direction bits for this block
uint8_t db = 0;
#if ENABLED(COREXY)
if (dx < 0) db |= BIT(X_HEAD); // Save the real Extruder (head) direction in X Axis
if (dy < 0) db |= BIT(Y_HEAD); // ...and Y
if (dz < 0) db |= BIT(Z_AXIS);
if (dx + dy < 0) db |= BIT(A_AXIS); // Motor A direction
if (dx - dy < 0) db |= BIT(B_AXIS); // Motor B direction
#elif ENABLED(COREXZ)
if (dx < 0) db |= BIT(X_HEAD); // Save the real Extruder (head) direction in X Axis
if (dy < 0) db |= BIT(Y_AXIS);
if (dz < 0) db |= BIT(Z_HEAD); // ...and Z
if (dx + dz < 0) db |= BIT(A_AXIS); // Motor A direction
if (dx - dz < 0) db |= BIT(C_AXIS); // Motor B direction
#else
if (dx < 0) db |= BIT(X_AXIS);
if (dy < 0) db |= BIT(Y_AXIS);
if (dz < 0) db |= BIT(Z_AXIS);
#endif
if (de < 0) db |= BIT(E_AXIS);
block->direction_bits = db;
block->active_extruder = extruder;
//enable active axes
#if ENABLED(COREXY)
if (block->steps[A_AXIS] || block->steps[B_AXIS]) {
enable_x();
enable_y();
}
#if DISABLED(Z_LATE_ENABLE)
if (block->steps[Z_AXIS]) enable_z();
#endif
#elif ENABLED(COREXZ)
if (block->steps[A_AXIS] || block->steps[C_AXIS]) {
enable_x();
enable_z();
}
if (block->steps[Y_AXIS]) enable_y();
#else
if (block->steps[X_AXIS]) enable_x();
if (block->steps[Y_AXIS]) enable_y();
#if DISABLED(Z_LATE_ENABLE)
if (block->steps[Z_AXIS]) enable_z();
#endif
#endif
// Enable extruder(s)
if (block->steps[E_AXIS]) {
if (DISABLE_INACTIVE_EXTRUDER) { //enable only selected extruder
for (int i=0; i<EXTRUDERS; i++)
if (g_uc_extruder_last_move[i] > 0) g_uc_extruder_last_move[i]--;
switch(extruder) {
case 0:
enable_e0();
g_uc_extruder_last_move[0] = BLOCK_BUFFER_SIZE * 2;
#if EXTRUDERS > 1
if (g_uc_extruder_last_move[1] == 0) disable_e1();
#if EXTRUDERS > 2
if (g_uc_extruder_last_move[2] == 0) disable_e2();
#if EXTRUDERS > 3
if (g_uc_extruder_last_move[3] == 0) disable_e3();
#endif
#endif
#endif
break;
#if EXTRUDERS > 1
case 1:
enable_e1();
g_uc_extruder_last_move[1] = BLOCK_BUFFER_SIZE * 2;
if (g_uc_extruder_last_move[0] == 0) disable_e0();
#if EXTRUDERS > 2
if (g_uc_extruder_last_move[2] == 0) disable_e2();
#if EXTRUDERS > 3
if (g_uc_extruder_last_move[3] == 0) disable_e3();
#endif
#endif
break;
#if EXTRUDERS > 2
case 2:
enable_e2();
g_uc_extruder_last_move[2] = BLOCK_BUFFER_SIZE * 2;
if (g_uc_extruder_last_move[0] == 0) disable_e0();
if (g_uc_extruder_last_move[1] == 0) disable_e1();
#if EXTRUDERS > 3
if (g_uc_extruder_last_move[3] == 0) disable_e3();
#endif
break;
#if EXTRUDERS > 3
case 3:
enable_e3();
g_uc_extruder_last_move[3] = BLOCK_BUFFER_SIZE * 2;
if (g_uc_extruder_last_move[0] == 0) disable_e0();
if (g_uc_extruder_last_move[1] == 0) disable_e1();
if (g_uc_extruder_last_move[2] == 0) disable_e2();
break;
#endif // EXTRUDERS > 3
#endif // EXTRUDERS > 2
#endif // EXTRUDERS > 1
}
}
else { // enable all
enable_e0();
enable_e1();
enable_e2();
enable_e3();
}
}
if (block->steps[E_AXIS])
NOLESS(feed_rate, minimumfeedrate);
else
NOLESS(feed_rate, mintravelfeedrate);
/**
* This part of the code calculates the total length of the movement.
* For cartesian bots, the X_AXIS is the real X movement and same for Y_AXIS.
* But for corexy bots, that is not true. The "X_AXIS" and "Y_AXIS" motors (that should be named to A_AXIS
* and B_AXIS) cannot be used for X and Y length, because A=X+Y and B=X-Y.
* So we need to create other 2 "AXIS", named X_HEAD and Y_HEAD, meaning the real displacement of the Head.
* Having the real displacement of the head, we can calculate the total movement length and apply the desired speed.
*/
#if ENABLED(COREXY)
float delta_mm[6];
delta_mm[X_HEAD] = dx / axis_steps_per_unit[A_AXIS];
delta_mm[Y_HEAD] = dy / axis_steps_per_unit[B_AXIS];
delta_mm[Z_AXIS] = dz / axis_steps_per_unit[Z_AXIS];
delta_mm[A_AXIS] = (dx + dy) / axis_steps_per_unit[A_AXIS];
delta_mm[B_AXIS] = (dx - dy) / axis_steps_per_unit[B_AXIS];
#elif ENABLED(COREXZ)
float delta_mm[6];
delta_mm[X_HEAD] = dx / axis_steps_per_unit[A_AXIS];
delta_mm[Y_AXIS] = dy / axis_steps_per_unit[Y_AXIS];
delta_mm[Z_HEAD] = dz / axis_steps_per_unit[C_AXIS];
delta_mm[A_AXIS] = (dx + dz) / axis_steps_per_unit[A_AXIS];
delta_mm[C_AXIS] = (dx - dz) / axis_steps_per_unit[C_AXIS];
#else
float delta_mm[4];
delta_mm[X_AXIS] = dx / axis_steps_per_unit[X_AXIS];
delta_mm[Y_AXIS] = dy / axis_steps_per_unit[Y_AXIS];
delta_mm[Z_AXIS] = dz / axis_steps_per_unit[Z_AXIS];
#endif
delta_mm[E_AXIS] = (de / axis_steps_per_unit[E_AXIS]) * volumetric_multiplier[extruder] * extruder_multiplier[extruder] / 100.0;
if (block->steps[X_AXIS] <= dropsegments && block->steps[Y_AXIS] <= dropsegments && block->steps[Z_AXIS] <= dropsegments) {
block->millimeters = fabs(delta_mm[E_AXIS]);
}
else {
block->millimeters = sqrt(
#if ENABLED(COREXY)
square(delta_mm[X_HEAD]) + square(delta_mm[Y_HEAD]) + square(delta_mm[Z_AXIS])
#elif ENABLED(COREXZ)
square(delta_mm[X_HEAD]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_HEAD])
#else
square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS])
#endif
);
}
float inverse_millimeters = 1.0 / block->millimeters; // Inverse millimeters to remove multiple divides
// Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
float inverse_second = feed_rate * inverse_millimeters;
int moves_queued = movesplanned();
// Slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
#if ENABLED(OLD_SLOWDOWN) || ENABLED(SLOWDOWN)
bool mq = moves_queued > 1 && moves_queued < BLOCK_BUFFER_SIZE / 2;
#if ENABLED(OLD_SLOWDOWN)
if (mq) feed_rate *= 2.0 * moves_queued / BLOCK_BUFFER_SIZE;
#endif
#if ENABLED(SLOWDOWN)
// segment time im micro seconds
unsigned long segment_time = lround(1000000.0/inverse_second);
if (mq) {
if (segment_time < minsegmenttime) {
// buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
inverse_second = 1000000.0 / (segment_time + lround(2 * (minsegmenttime - segment_time) / moves_queued));
#ifdef XY_FREQUENCY_LIMIT
segment_time = lround(1000000.0 / inverse_second);
#endif
}
}
#endif
#endif
block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0
block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0
#if ENABLED(FILAMENT_SENSOR)
//FMM update ring buffer used for delay with filament measurements
if (extruder == FILAMENT_SENSOR_EXTRUDER_NUM && delay_index2 > -1) { //only for extruder with filament sensor and if ring buffer is initialized
const int MMD = MAX_MEASUREMENT_DELAY + 1, MMD10 = MMD * 10;
delay_dist += delta_mm[E_AXIS]; // increment counter with next move in e axis
while (delay_dist >= MMD10) delay_dist -= MMD10; // loop around the buffer
while (delay_dist < 0) delay_dist += MMD10;
delay_index1 = delay_dist / 10.0; // calculate index
delay_index1 = constrain(delay_index1, 0, MAX_MEASUREMENT_DELAY); // (already constrained above)
if (delay_index1 != delay_index2) { // moved index
meas_sample = widthFil_to_size_ratio() - 100; // Subtract 100 to reduce magnitude - to store in a signed char
while (delay_index1 != delay_index2) {
// Increment and loop around buffer
if (++delay_index2 >= MMD) delay_index2 -= MMD;
delay_index2 = constrain(delay_index2, 0, MAX_MEASUREMENT_DELAY);
measurement_delay[delay_index2] = meas_sample;
}
}
}
#endif
// Calculate and limit speed in mm/sec for each axis
float current_speed[NUM_AXIS];
float speed_factor = 1.0; //factor <=1 do decrease speed
for (int i = 0; i < NUM_AXIS; i++) {
current_speed[i] = delta_mm[i] * inverse_second;
float cs = fabs(current_speed[i]), mf = max_feedrate[i];
if (cs > mf) speed_factor = min(speed_factor, mf / cs);
}
// Max segement time in us.
#ifdef XY_FREQUENCY_LIMIT
#define MAX_FREQ_TIME (1000000.0 / XY_FREQUENCY_LIMIT)
// Check and limit the xy direction change frequency
unsigned char direction_change = block->direction_bits ^ old_direction_bits;
old_direction_bits = block->direction_bits;
segment_time = lround((float)segment_time / speed_factor);
long xs0 = axis_segment_time[X_AXIS][0],
xs1 = axis_segment_time[X_AXIS][1],
xs2 = axis_segment_time[X_AXIS][2],
ys0 = axis_segment_time[Y_AXIS][0],
ys1 = axis_segment_time[Y_AXIS][1],
ys2 = axis_segment_time[Y_AXIS][2];
if ((direction_change & BIT(X_AXIS)) != 0) {
xs2 = axis_segment_time[X_AXIS][2] = xs1;
xs1 = axis_segment_time[X_AXIS][1] = xs0;
xs0 = 0;
}
xs0 = axis_segment_time[X_AXIS][0] = xs0 + segment_time;
if ((direction_change & BIT(Y_AXIS)) != 0) {
ys2 = axis_segment_time[Y_AXIS][2] = axis_segment_time[Y_AXIS][1];
ys1 = axis_segment_time[Y_AXIS][1] = axis_segment_time[Y_AXIS][0];
ys0 = 0;
}
ys0 = axis_segment_time[Y_AXIS][0] = ys0 + segment_time;
long max_x_segment_time = max(xs0, max(xs1, xs2)),
max_y_segment_time = max(ys0, max(ys1, ys2)),
min_xy_segment_time = min(max_x_segment_time, max_y_segment_time);
if (min_xy_segment_time < MAX_FREQ_TIME) {
float low_sf = speed_factor * min_xy_segment_time / MAX_FREQ_TIME;
speed_factor = min(speed_factor, low_sf);
}
#endif // XY_FREQUENCY_LIMIT
// Correct the speed
if (speed_factor < 1.0) {
for (unsigned char i = 0; i < NUM_AXIS; i++) current_speed[i] *= speed_factor;
block->nominal_speed *= speed_factor;
block->nominal_rate *= speed_factor;
}
// Compute and limit the acceleration rate for the trapezoid generator.
float steps_per_mm = block->step_event_count / block->millimeters;
long bsx = block->steps[X_AXIS], bsy = block->steps[Y_AXIS], bsz = block->steps[Z_AXIS], bse = block->steps[E_AXIS];
if (bsx == 0 && bsy == 0 && bsz == 0) {
block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
}
else if (bse == 0) {
block->acceleration_st = ceil(travel_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
}
else {
block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
}
// Limit acceleration per axis
unsigned long acc_st = block->acceleration_st,
xsteps = axis_steps_per_sqr_second[X_AXIS],
ysteps = axis_steps_per_sqr_second[Y_AXIS],
zsteps = axis_steps_per_sqr_second[Z_AXIS],
esteps = axis_steps_per_sqr_second[E_AXIS];
if ((float)acc_st * bsx / block->step_event_count > xsteps) acc_st = xsteps;
if ((float)acc_st * bsy / block->step_event_count > ysteps) acc_st = ysteps;
if ((float)acc_st * bsz / block->step_event_count > zsteps) acc_st = zsteps;
if ((float)acc_st * bse / block->step_event_count > esteps) acc_st = esteps;
block->acceleration_st = acc_st;
block->acceleration = acc_st / steps_per_mm;
block->acceleration_rate = (long)(acc_st * 16777216.0 / (F_CPU / 8.0));
#if 0 // Use old jerk for now
// Compute path unit vector
double unit_vec[3];
unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;
// Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
// Let a circle be tangent to both previous and current path line segments, where the junction
// deviation is defined as the distance from the junction to the closest edge of the circle,
// colinear with the circle center. The circular segment joining the two paths represents the
// path of centripetal acceleration. Solve for max velocity based on max acceleration about the
// radius of the circle, defined indirectly by junction deviation. This may be also viewed as
// path width or max_jerk in the previous grbl version. This approach does not actually deviate
// from path, but used as a robust way to compute cornering speeds, as it takes into account the
// nonlinearities of both the junction angle and junction velocity.
double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
- previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
- previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
// Skip and use default max junction speed for 0 degree acute junction.
if (cos_theta < 0.95) {
vmax_junction = min(previous_nominal_speed,block->nominal_speed);
// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
if (cos_theta > -0.95) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation
double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
vmax_junction = min(vmax_junction,
sqrt(block->acceleration * junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
}
}
}
#endif
// Start with a safe speed
float vmax_junction = max_xy_jerk / 2;
float vmax_junction_factor = 1.0;
float mz2 = max_z_jerk / 2, me2 = max_e_jerk / 2;
float csz = current_speed[Z_AXIS], cse = current_speed[E_AXIS];
if (fabs(csz) > mz2) vmax_junction = min(vmax_junction, mz2);
if (fabs(cse) > me2) vmax_junction = min(vmax_junction, me2);
vmax_junction = min(vmax_junction, block->nominal_speed);
float safe_speed = vmax_junction;
if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
float dx = current_speed[X_AXIS] - previous_speed[X_AXIS],
dy = current_speed[Y_AXIS] - previous_speed[Y_AXIS],
dz = fabs(csz - previous_speed[Z_AXIS]),
de = fabs(cse - previous_speed[E_AXIS]),
jerk = sqrt(dx * dx + dy * dy);
// if ((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
vmax_junction = block->nominal_speed;
// }
if (jerk > max_xy_jerk) vmax_junction_factor = max_xy_jerk / jerk;
if (dz > max_z_jerk) vmax_junction_factor = min(vmax_junction_factor, max_z_jerk / dz);
if (de > max_e_jerk) vmax_junction_factor = min(vmax_junction_factor, max_e_jerk / de);
vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
}
block->max_entry_speed = vmax_junction;
// Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
double v_allowable = max_allowable_speed(-block->acceleration, MINIMUM_PLANNER_SPEED, block->millimeters);
block->entry_speed = min(vmax_junction, v_allowable);
// Initialize planner efficiency flags
// Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
// If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
// the current block and next block junction speeds are guaranteed to always be at their maximum
// junction speeds in deceleration and acceleration, respectively. This is due to how the current
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
// the reverse and forward planners, the corresponding block junction speed will always be at the
// the maximum junction speed and may always be ignored for any speed reduction checks.
block->nominal_length_flag = (block->nominal_speed <= v_allowable);
block->recalculate_flag = true; // Always calculate trapezoid for new block
// Update previous path unit_vector and nominal speed
for (int i = 0; i < NUM_AXIS; i++) previous_speed[i] = current_speed[i];
previous_nominal_speed = block->nominal_speed;
#if ENABLED(ADVANCE)
// Calculate advance rate
if (!bse || (!bsx && !bsy && !bsz)) {
block->advance_rate = 0;
block->advance = 0;
}
else {
long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st);
float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) * (cse * cse * EXTRUSION_AREA * EXTRUSION_AREA) * 256;
block->advance = advance;
block->advance_rate = acc_dist ? advance / (float)acc_dist : 0;
}
/*
SERIAL_ECHO_START;
SERIAL_ECHOPGM("advance :");
SERIAL_ECHO(block->advance/256.0);
SERIAL_ECHOPGM("advance rate :");
SERIAL_ECHOLN(block->advance_rate/256.0);
*/
#endif // ADVANCE
calculate_trapezoid_for_block(block, block->entry_speed / block->nominal_speed, safe_speed / block->nominal_speed);
// Move buffer head
block_buffer_head = next_buffer_head;
// Update position
for (int i = 0; i < NUM_AXIS; i++) position[i] = target[i];
planner_recalculate();
st_wake_up();
} // plan_buffer_line()
void process()
{
switch (buff_obj[0]) {
case 'V':
writeString((char *)"{VER:008}");
return;
case 'S':
if (buff_value[0]=='E') writeString((char *)"{SYS:echo}");
/* else if (buff_value[0]=='H')
{
uint8_t i,itemCount;
itemCount=countFiles(false);
//if (file_from_wifi!=0)
{
writeString((char *)"{WIFI:");
writeInt(file_from_wifi,3);
put('/');
writeInt(itemCount,3);
put('}');
}
}*/
else if (buff_value[0]=='L')
{
uint8_t i;
uint8_t itemCount;
if ( card.isFileOpen() ) {
writeString((char *)"{SYS:BUSY}");
return;
}
//card.initsd();
//card.getWorkDirName();
itemCount = countFiles(true);
if (itemCount==0)
{
card.initsd();
card.setroot();
itemCount = countFiles(true);
}
for (i=0;i<itemCount;i++)
{
ListFile(i,itemCount);
}
if (itemCount==0)
{
if (card.cardOK) i=101;
else i=102;
writeString((char *)"{ERR:");
writeInt(i,3);
put('}');
}
else writeString((char *)"{SYS:OK}");
}
else if (buff_value[0]=='I')
{
int16_t t;
writeString((char *)"{T0:");
t=degHotend(0);
if (t>999) t=999;
writeInt(t,3);
put('/');
t=degTargetHotend(0);
writeInt(t,3);
put('}');
writeString((char *)"{T1:");
t=degHotend(1);
if (t>999) t=999;
writeInt(t,3);
put('/');
t=degTargetHotend(1);
writeInt(t,3);
put('}');
writeString((char *)"{TP:");
t=degBed();
if (t>999) t=999;
writeInt(t,3);
put('/');
t=degTargetBed();
writeInt(t,3);
put('}');
}
else if (buff_value[0]=='F')
{
/*if (buff_value[1]=='X')
{
eeprom::setEepromInt64(eeprom_offsets::FILAMENT_TRIP, eeprom::getEepromInt64(eeprom_offsets::FILAMENT_LIFETIME, 0));
eeprom::setEepromInt64(eeprom_offsets::FILAMENT_TRIP + sizeof(int64_t), eeprom::getEepromInt64(eeprom_offsets::FILAMENT_LIFETIME + sizeof(int64_t), 0));
}*/
writeString((char *)"{TU:");
uint16_t total_hours;
uint8_t total_minutes;
//eeprom::getBuildTime(&total_hours, &total_minutes);
total_hours=0;
total_minutes=0;
writeInt(total_hours,5);
put('.');
writeInt(total_minutes,2);
put('/');
uint8_t build_hours;
uint8_t build_minutes;
//host::getPrintTime(build_hours, build_minutes);
build_hours=0;
build_minutes=0;
writeInt(build_hours,3);
put('.');
writeInt(build_minutes,2);
put('/');
uint32_t filamentUsedA,filamentUsedB,filamentUsed;
char str[11];
//filamentUsedA=stepperAxisStepsToMM(eeprom::getEepromInt64(eeprom_offsets::FILAMENT_LIFETIME, 0), A_AXIS);
//filamentUsedB=stepperAxisStepsToMM(eeprom::getEepromInt64(eeprom_offsets::FILAMENT_LIFETIME + sizeof(int64_t),0), B_AXIS);
//filamentUsed=filamentUsedA+filamentUsedB;
filamentUsed=0;
itoa(filamentUsed,str,10);
writeString((char *)str);
put('/');
//filamentUsedA -= stepperAxisStepsToMM(eeprom::getEepromInt64(eeprom_offsets::FILAMENT_TRIP, 0), A_AXIS);
//filamentUsedB -= stepperAxisStepsToMM(eeprom::getEepromInt64(eeprom_offsets::FILAMENT_TRIP + sizeof(int64_t),0), B_AXIS);
//filamentUsed=filamentUsedA+filamentUsedB;
filamentUsed=0;
itoa(filamentUsed,str,10);
writeString((char *)str);
put('}');
}
else if (buff_value[0]=='R' &&
buff_value[1]=='E' &&
buff_value[2]=='S' &&
buff_value[3]=='E' &&
buff_value[4]=='T')
{
//Motherboard::getBoard().reset(true);
}
/*
else if (buff_value[0]=='S')
{
writeString((char *)"{SYS:P");
uint8_t i = command::pauseState();
writeInt(i,3);
put('/');
put('H');
i=host::getHostState();
writeInt(i,3);
put('}');
}
break;
*/
case 'C':
if (buff_value[0]=='P')
{
uint16_t t;
t=atoi((const char*)buff_value+1);
if (t<0 || t>150) return;
setTargetBed(t);
}
else if (buff_value[0]=='T')
{
int16_t t;
t=atoi((const char*)buff_value+2);
if (t<0 || t>280) return;
if (buff_value[1] == '0')
{
setTargetHotend(t,0);
}
else
{
setTargetHotend(t,1);
}
}
else if (buff_value[0]=='S')
{
int16_t t;
uint8_t i;
t=atoi((const char*)buff_value+1);
if (t<1) t=1;
else if (t>50) t=50;
feedmultiply=t*10;
}
break;
case 'P':
uint8_t i;
if (buff_value[0]=='H')
{
enquecommand_P(PSTR("G28"));
}
else if (buff_value[0]=='C')
{
//host::startOnboardBuild(utility::TOOLHEAD_CALIBRATE);
}
else if (buff_value[0]=='X')
{
extern bool cancel_heatup;
writeString((char *)"{SYS:CANCELING}");
//card.pauseSDPrint();
//disable_heater();
card.sdprinting = false;
card.closefile();
quickStop();
if(SD_FINISHED_STEPPERRELEASE)
{
enquecommand_P(PSTR(SD_FINISHED_RELEASECOMMAND));
}
autotempShutdown();
cancel_heatup = true;
writeString((char *)"{SYS:STARTED}");
writeString((char *)"{U:RG1R180180120P0L1S0D0O1E1H0C0X1Y1Z1A2B2N3M0}");
}
else if (buff_value[0]=='P')
{
writeString((char *)"{SYS:PAUSE}");
card.pauseSDPrint();
writeString((char *)"{SYS:PAUSED}");
}
else if (buff_value[0]=='R')
{
writeString((char *)"{SYS:RESUME}");
card.startFileprint();
writeString((char *)"{SYS:RESUMED}");
}
else if (buff_value[0]=='Z')
{
/*i=(buff_value[1]-'0')*100 + (buff_value[2]-'0')*10 + (buff_value[3]-'0');
float pauseAtZPos = i;
command::pauseAtZPos(stepperAxisMMToSteps(pauseAtZPos, Z_AXIS));*/
}
else
{
i=(buff_value[0]-'0')*100 + (buff_value[1]-'0')*10 + (buff_value[2]-'0');
card.getfilename(i);
if (card.filenameIsDir)
{
writeString((char *)"{SYS:DIR}");
card.chdir(card.filename);
}
else
{
char cmd[30];
char* c;
writeString((char *)"{PRINTFILE:");
if (card.longFilename[0]!=0) writeString(card.longFilename);
else writeString(card.filename);
put('}');
sprintf_P(cmd, PSTR("M23 %s"), card.filename);
for(c = &cmd[4]; *c; c++)
*c = tolower(*c);
enquecommand(cmd);
enquecommand_P(PSTR("M24"));
}
}
break;
case 'B':
PrintingStatus();
break;
/*
case 'J':
switch (buff_value[0])
{
case 'S':
BOARD_STATUS_SET(Motherboard::STATUS_MANUAL_MODE);
jog_speed=atoi((const char*)buff_value+1);
break;
case 'E':
steppers::enableAxes(0xff, false);
BOARD_STATUS_CLEAR(Motherboard::STATUS_MANUAL_MODE);
break;
case 'X':
case 'Y':
case 'Z':
case 'A':
case 'B':
steppers::abort();
uint8_t dummy;
Point position = steppers::getStepperPosition(&dummy);
int32_t t;
t=atoi((const char*)buff_value+1);
if (buff_value[0]<='B') position[buff_value[0]-'A'+3] += (t<<4);
else position[buff_value[0]-'X'] += (t<<4);
steppers::setTargetNew(position, jog_speed, 0, 0);
break;
}
break;
*/
/*
case 'H':
if (buff_value[0]=='R')
{
extern uint32_t homePosition[PROFILES_HOME_POSITIONS_STORED];
writeString((char *)"{H:R");
eeprom_read_block(homePosition, (void *)eeprom_offsets::AXIS_HOME_POSITIONS_STEPS, PROFILES_HOME_POSITIONS_STORED * sizeof(uint32_t));
writeInt(homePosition[0],5);
put('/');
writeInt(homePosition[1],5);
put('/');
writeInt(homePosition[2],5);
put('/');
writeInt((int32_t)(eeprom::getEeprom32(eeprom_offsets::TOOLHEAD_OFFSET_SETTINGS, 0)),5);
put('/');
writeInt((int32_t)(eeprom::getEeprom32(eeprom_offsets::TOOLHEAD_OFFSET_SETTINGS + sizeof(int32_t), 0)),5);
put('}');
}
else if (buff_value[0]=='W')
{
extern uint32_t homePosition[PROFILES_HOME_POSITIONS_STORED];
int32_t offset[2],t;
uint8_t axis;
axis=buff_value[1]-'X';
if (axis>=0 && axis<=2)
{
homePosition[axis]=atoi((const char*)buff_value+2);
cli();
eeprom_write_block((void *)&homePosition[axis],
(void*)(eeprom_offsets::AXIS_HOME_POSITIONS_STEPS + sizeof(uint32_t) * axis) ,
sizeof(uint32_t));
sei();
}
axis=buff_value[1]-'x';
if (axis>=0 && axis<=1)
{
t=atoi((const char*)buff_value+2);
int32_t offset[2];
bool smallOffsets;
offset[0] = (int32_t)(eeprom::getEeprom32(eeprom_offsets::TOOLHEAD_OFFSET_SETTINGS, 0));
offset[1] = (int32_t)(eeprom::getEeprom32(eeprom_offsets::TOOLHEAD_OFFSET_SETTINGS + sizeof(int32_t), 0));
smallOffsets = abs(offset[0]) < ((int32_t)stepperAxisStepsPerMM(0) << 2);
int32_t delta = stepperAxisMMToSteps((float)(t - 7) * 0.1f, axis);
if ( !smallOffsets ) delta = -delta;
int32_t new_offset = offset[axis] + delta;
eeprom_write_block((uint8_t *)&new_offset,
(uint8_t *)eeprom_offsets::TOOLHEAD_OFFSET_SETTINGS + axis * sizeof(int32_t),
sizeof(int32_t));
}
}
break;
*/
/*
case 'U':
if (buff_value[0]=='R')
{
int temp;
writeString((char *)"{U:RG");
if (eeprom::getEeprom8(eeprom_offsets::OVERRIDE_GCODE_TEMP, 0) != 0) put('1');
else put('0');
put('R');
temp=eeprom::getEeprom16(eeprom_offsets::PREHEAT_SETTINGS + preheat_eeprom_offsets::PREHEAT_LEFT_OFFSET, DEFAULT_PREHEAT_TEMP);
writeInt(temp,3);
//put('/');
temp=eeprom::getEeprom16(eeprom_offsets::PREHEAT_SETTINGS + preheat_eeprom_offsets::PREHEAT_RIGHT_OFFSET, DEFAULT_PREHEAT_TEMP);
writeInt(temp,3);
//put('/');
temp=eeprom::getEeprom16(eeprom_offsets::PREHEAT_SETTINGS + preheat_eeprom_offsets::PREHEAT_PLATFORM_OFFSET, DEFAULT_PREHEAT_TEMP);
writeInt(temp,3);
put('P');
if (eeprom::hasHBP() != 0) put('1');
else put('0');
put('L');
if (eeprom::getEeprom8(eeprom_offsets::ACCELERATION_SETTINGS + acceleration_eeprom_offsets::ACCELERATION_ACTIVE, 0x01) != 0) put('1');
else put('0');
put('S');
if (eeprom::getEeprom8(eeprom_offsets::COOL_PLAT, 0) != 0) put('1');
else put('0');
put('D');
if (eeprom::getEeprom8(eeprom_offsets::DITTO_PRINT_ENABLED, 0) != 0) put('1');
else put('0');
put('O');
if (eeprom::getEeprom8(eeprom_offsets::TOOLHEAD_OFFSET_SYSTEM,
DEFAULT_TOOLHEAD_OFFSET_SYSTEM) != 0) put('1');
else put('0');
put('E');
if (eeprom::getEeprom8(eeprom_offsets::EXTRUDER_HOLD,
DEFAULT_EXTRUDER_HOLD) != 0) put('1');
else put('0');
put('H');
if (eeprom::getEeprom8(eeprom_offsets::HEAT_DURING_PAUSE, DEFAULT_HEAT_DURING_PAUSE) != 0) put('1');
else put('0');
put('C');
if (eeprom::getEeprom8(eeprom_offsets::SD_USE_CRC, DEFAULT_SD_USE_CRC) != 0) put('1');
else put('0');
put('X');
put ('0' + eeprom::getEeprom8(eeprom_offsets::STEPPER_X_CURRENT, 0));
put('Y');
put ('0' + eeprom::getEeprom8(eeprom_offsets::STEPPER_Y_CURRENT, 0));
put('Z');
put ('0' + eeprom::getEeprom8(eeprom_offsets::STEPPER_Z_CURRENT, 0));
put('A');
put ('0' + eeprom::getEeprom8(eeprom_offsets::STEPPER_A_CURRENT, 0));
put('B');
put ('0' + eeprom::getEeprom8(eeprom_offsets::STEPPER_B_CURRENT, 0));
put('N');
put ('0' + eeprom::getEeprom8(eeprom_offsets::LANGUAGE, 0));
put('M');
put ('0' + eeprom::getEeprom8(eeprom_offsets::WIFI_SD, 0));
put('}');
}
else if (buff_value[0]=='W')
{
uint8_t *c;
uint8_t cmd=0;
c=buff_value;
while (*++c!=0)
{
if (*c<='9' && *c>='0')
{
uint8_t value;
value = *c - '0';
switch (cmd)
{
case 'G':
eeprom_write_byte((uint8_t *)eeprom_offsets::OVERRIDE_GCODE_TEMP,value);
break;
case 'P':
eeprom_write_byte((uint8_t*)eeprom_offsets::HBP_PRESENT, value);
break;
case 'L':
eeprom_write_byte((uint8_t*)eeprom_offsets::ACCELERATION_SETTINGS +
acceleration_eeprom_offsets::ACCELERATION_ACTIVE,
value);
break;
case 'S':
eeprom_write_byte((uint8_t*)eeprom_offsets::COOL_PLAT, value);
break;
case 'D':
eeprom_write_byte((uint8_t*)eeprom_offsets::DITTO_PRINT_ENABLED, value);
break;
case 'O':
eeprom_write_byte((uint8_t*)eeprom_offsets::TOOLHEAD_OFFSET_SYSTEM, value);
break;
case 'E':
eeprom_write_byte((uint8_t*)eeprom_offsets::EXTRUDER_HOLD, value);
break;
case 'H':
eeprom_write_byte((uint8_t*)eeprom_offsets::HEAT_DURING_PAUSE, value);
break;
case 'C':
eeprom_write_byte((uint8_t*)eeprom_offsets::SD_USE_CRC, value);
break;
case 'T':
eeprom_write_byte((uint8_t*)eeprom_offsets::PSTOP_ENABLE, value);
break;
case 'X':
eeprom_write_byte((uint8_t*)eeprom_offsets::STEPPER_X_CURRENT, value);
break;
case 'Y':
eeprom_write_byte((uint8_t*)eeprom_offsets::STEPPER_Y_CURRENT, value);
break;
case 'Z':
eeprom_write_byte((uint8_t*)eeprom_offsets::STEPPER_Z_CURRENT, value);
break;
case 'A':
eeprom_write_byte((uint8_t*)eeprom_offsets::STEPPER_A_CURRENT, value);
break;
case 'B':
eeprom_write_byte((uint8_t*)eeprom_offsets::STEPPER_B_CURRENT, value);
break;
case 'N':
eeprom_write_byte((uint8_t*)eeprom_offsets::LANGUAGE, value);
break;
case 'M':
eeprom_write_byte((uint8_t*)eeprom_offsets::WIFI_SD, value);
break;
default:
break;
}
}
else if (*c<='Z' && *c>='A') cmd = *c;
}
}
else if (buff_value[0] == 'U' &&
buff_value[1] == 'P' &&
buff_value[2] == 'D' &&
buff_value[3] == 'A' &&
buff_value[4] == 'T' &&
buff_value[5] == 'E')
{
char r;
cli();
wdt_disable();
while (1)
{
if (UCSR0A & (1<<RXC0))
{
r = UDR0;
UDR3 = r;
}
if (UCSR3A & (1<<RXC3))
{
r = UDR3;
UDR0 = r;
}
}
}
else {
if (buff_value[0]=='E' && buff_value[1]=='R' && buff_value[2]=='A' && buff_value[3]=='S' && buff_value[4]=='E')
{
eeprom::factoryResetEEPROM();
Motherboard::getBoard().reset(true);
}
else if (buff_value[0]=='F' && buff_value[1]=='U' && buff_value[2]=='L' && buff_value[3]=='L' && buff_value[4]=='E' && buff_value[5]=='R' && buff_value[6]=='A' && buff_value[7]=='S' && buff_value[8]=='E')
{
eeprom::erase();
host::stopBuildNow();
}
}
break;
*/
default:
break;
}
}
void PrintingStatus()
{
int16_t t;
int32_t t32;
writeString((char *)"{T0:");
t=degHotend(0);
if (t>999) t=999;
writeInt(t,3);
put('/');
t=degTargetHotend(0);
writeInt(t,3);
put('}');
writeString((char *)"{T1:");
t=degHotend(1);
if (t>999) t=999;
writeInt(t,3);
put('/');
t=degTargetHotend(1);
writeInt(t,3);
put('}');
writeString((char *)"{TP:");
t=degBed();
if (t>999) t=999;
writeInt(t,3);
put('/');
t=degTargetBed();
writeInt(t,3);
put('}');
writeString((char *)"{TQ:");
t=card.percentDone();
writeInt(t,3);
if (card.sdprinting) put('P');
else put('C');
/*switch(host::getHostState())
{
case host::HOST_STATE_BUILDING_ONBOARD:
case host::HOST_STATE_BUILDING:
case host::HOST_STATE_BUILDING_FROM_SD:
put('P');
break;
case host::HOST_STATE_HEAT_SHUTDOWN:
put('S');
break;
default:
put('C');
break;
}*/
put('}');
writeString((char *)"{TT:");
if(starttime != 0) t32 = millis()/60000 - starttime/60000;
else t32=0;
writeInt32(t32,6);
put('}');
writeString((char *)"{TR:");
//t32=command::estimatedTimeLeftInSeconds();
t32=0;
writeInt32(t32,6);
put('}');
writeString((char *)"{TF:");
//t32=command::filamentUsed();
t32=0;
writeInt32(t32,6);
put('}');
}
void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder)
#endif //LEVEL_SENSOR
{
// Calculate the buffer head after we push this byte
next_buffer_head = next_block_index(block_buffer_head);
// If the buffer is full: good! That means we are well ahead of the robot.
// Rest here until there is room in the buffer.
#ifndef DOGLCD
buffer_recursivity++;
#endif // DOGLCD
while(block_buffer_tail == next_buffer_head)
{
next_buffer_head = next_block_index(block_buffer_head);
temp::TemperatureManager::single::instance().manageTemperatureControl();
#ifndef DOGLCD
manage_inactivity();
#endif //DOGLCD
lcd_update();
#ifdef DOGLCD
if (stop_planner_buffer == true)
{
stop_planner_buffer = false;
planner_buffer_stopped = true;
return;
}
#endif // DOGLCD
}
#ifndef DOGLCD
buffer_recursivity--;
#endif // DOGLCD
#ifdef LEVEL_SENSOR
apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
#endif // LEVEL_SENSOR
// The target position of the tool in absolute steps
// Calculate target position in absolute steps
//this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
long target[4];
target[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
target[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);
target[Z_AXIS] = lround(z*axis_steps_per_unit[Z_AXIS]);
target[E_AXIS] = lround(e*axis_steps_per_unit[E_AXIS]);
#ifdef PREVENT_DANGEROUS_EXTRUDE
if(target[E_AXIS]!=position[E_AXIS])
{
if(degHotend(active_extruder)<extrude_min_temp)
{
position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
}
#ifdef PREVENT_LENGTHY_EXTRUDE
if(labs(target[E_AXIS]-position[E_AXIS])>axis_steps_per_unit[E_AXIS]*EXTRUDE_MAXLENGTH)
{
position[E_AXIS]=target[E_AXIS]; //behave as if the move really took place, but ignore E part
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
}
#endif
}
#endif
// Prepare to set up new block
block_t *block = &block_buffer[block_buffer_head];
// Mark block as not busy (Not executed by the stepper interrupt)
block->busy = false;
// Number of steps for each axis
#ifndef COREXY
// default non-h-bot planning
block->steps_x = labs(target[X_AXIS]-position[X_AXIS]);
block->steps_y = labs(target[Y_AXIS]-position[Y_AXIS]);
#else
// corexy planning
// these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
block->steps_x = labs((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]));
block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]));
#endif
block->steps_z = labs(target[Z_AXIS]-position[Z_AXIS]);
block->steps_e = labs(target[E_AXIS]-position[E_AXIS]);
block->steps_e *= volumetric_multiplier[active_extruder];
block->steps_e *= extrudemultiply;
block->steps_e /= 100;
block->step_event_count = max(block->steps_x, max(block->steps_y, max(block->steps_z, block->steps_e)));
// Bail if this is a zero-length block
if (block->step_event_count <= dropsegments)
{
return;
}
block->fan_speed = fanSpeed;
#ifdef BARICUDA
block->valve_pressure = ValvePressure;
block->e_to_p_pressure = EtoPPressure;
#endif
// Compute direction bits for this block
block->direction_bits = 0;
#ifndef COREXY
if (target[X_AXIS] < position[X_AXIS])
{
block->direction_bits |= (1<<X_AXIS);
}
if (target[Y_AXIS] < position[Y_AXIS])
{
block->direction_bits |= (1<<Y_AXIS);
}
#else
if (target[X_AXIS] < position[X_AXIS])
{
block->direction_bits |= (1<<X_HEAD); //AlexBorro: Save the real Extruder (head) direction in X Axis
}
if (target[Y_AXIS] < position[Y_AXIS])
{
block->direction_bits |= (1<<Y_HEAD); //AlexBorro: Save the real Extruder (head) direction in Y Axis
}
if ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]) < 0)
{
block->direction_bits |= (1<<X_AXIS); //AlexBorro: Motor A direction (Incorrectly implemented as X_AXIS)
}
if ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]) < 0)
{
block->direction_bits |= (1<<Y_AXIS); //AlexBorro: Motor B direction (Incorrectly implemented as Y_AXIS)
}
#endif
if (target[Z_AXIS] < position[Z_AXIS])
{
block->direction_bits |= (1<<Z_AXIS);
}
if (target[E_AXIS] < position[E_AXIS])
{
block->direction_bits |= (1<<E_AXIS);
}
block->active_extruder = extruder;
//enable active axes
#ifdef COREXY
if((block->steps_x != 0) || (block->steps_y != 0))
{
enable_x();
enable_y();
}
#else
if(block->steps_x != 0) enable_x();
if(block->steps_y != 0) enable_y();
#endif
#ifndef Z_LATE_ENABLE
if(block->steps_z != 0) enable_z();
#endif
// Enable extruder(s)
if(block->steps_e != 0)
{
if (DISABLE_INACTIVE_EXTRUDER) //enable only selected extruder
{
if(g_uc_extruder_last_move[0] > 0) g_uc_extruder_last_move[0]--;
if(g_uc_extruder_last_move[1] > 0) g_uc_extruder_last_move[1]--;
if(g_uc_extruder_last_move[2] > 0) g_uc_extruder_last_move[2]--;
if(g_uc_extruder_last_move[3] > 0) g_uc_extruder_last_move[3]--;
switch(extruder)
{
case 0:
enable_e0();
g_uc_extruder_last_move[0] = BLOCK_BUFFER_SIZE*2;
if(g_uc_extruder_last_move[1] == 0) disable_e1();
if(g_uc_extruder_last_move[2] == 0) disable_e2();
if(g_uc_extruder_last_move[3] == 0) disable_e3();
break;
case 1:
enable_e1();
g_uc_extruder_last_move[1] = BLOCK_BUFFER_SIZE*2;
if(g_uc_extruder_last_move[0] == 0) disable_e0();
if(g_uc_extruder_last_move[2] == 0) disable_e2();
if(g_uc_extruder_last_move[3] == 0) disable_e3();
break;
case 2:
enable_e2();
g_uc_extruder_last_move[2] = BLOCK_BUFFER_SIZE*2;
if(g_uc_extruder_last_move[0] == 0) disable_e0();
if(g_uc_extruder_last_move[1] == 0) disable_e1();
if(g_uc_extruder_last_move[3] == 0) disable_e3();
break;
case 3:
enable_e3();
g_uc_extruder_last_move[3] = BLOCK_BUFFER_SIZE*2;
if(g_uc_extruder_last_move[0] == 0) disable_e0();
if(g_uc_extruder_last_move[1] == 0) disable_e1();
if(g_uc_extruder_last_move[2] == 0) disable_e2();
break;
}
}
else //enable all
{
enable_e0();
enable_e1();
enable_e2();
enable_e3();
}
}
if (block->steps_e == 0)
{
if(feed_rate<mintravelfeedrate) feed_rate=mintravelfeedrate;
}
else
{
if(feed_rate<minimumfeedrate) feed_rate=minimumfeedrate;
}
/* This part of the code calculates the total length of the movement.
For cartesian bots, the X_AXIS is the real X movement and same for Y_AXIS.
But for corexy bots, that is not true. The "X_AXIS" and "Y_AXIS" motors (that should be named to A_AXIS
and B_AXIS) cannot be used for X and Y length, because A=X+Y and B=X-Y.
So we need to create other 2 "AXIS", named X_HEAD and Y_HEAD, meaning the real displacement of the Head.
Having the real displacement of the head, we can calculate the total movement length and apply the desired speed.
*/
#ifndef COREXY
float delta_mm[4];
delta_mm[X_AXIS] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];
delta_mm[Y_AXIS] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
#else
float delta_mm[6];
delta_mm[X_HEAD] = (target[X_AXIS]-position[X_AXIS])/axis_steps_per_unit[X_AXIS];
delta_mm[Y_HEAD] = (target[Y_AXIS]-position[Y_AXIS])/axis_steps_per_unit[Y_AXIS];
delta_mm[X_AXIS] = ((target[X_AXIS]-position[X_AXIS]) + (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[X_AXIS];
delta_mm[Y_AXIS] = ((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-position[Y_AXIS]))/axis_steps_per_unit[Y_AXIS];
#endif
delta_mm[Z_AXIS] = (target[Z_AXIS]-position[Z_AXIS])/axis_steps_per_unit[Z_AXIS];
delta_mm[E_AXIS] = ((target[E_AXIS]-position[E_AXIS])/axis_steps_per_unit[E_AXIS])*volumetric_multiplier[active_extruder]*extrudemultiply/100.0;
if ( block->steps_x <=dropsegments && block->steps_y <=dropsegments && block->steps_z <=dropsegments )
{
block->millimeters = fabs(delta_mm[E_AXIS]);
}
else
{
#ifndef COREXY
block->millimeters = sqrt(square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS]));
#else
block->millimeters = sqrt(square(delta_mm[X_HEAD]) + square(delta_mm[Y_HEAD]) + square(delta_mm[Z_AXIS]));
#endif
}
float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple divides
// Calculate speed in mm/second for each axis. No divide by zero due to previous checks.
float inverse_second = feed_rate * inverse_millimeters;
int moves_queued=(block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1);
// slow down when de buffer starts to empty, rather than wait at the corner for a buffer refill
#ifdef OLD_SLOWDOWN
if(moves_queued < (BLOCK_BUFFER_SIZE * 0.5) && moves_queued > 1)
feed_rate = feed_rate*moves_queued / (BLOCK_BUFFER_SIZE * 0.5);
#endif
#ifdef SLOWDOWN
// segment time im micro seconds
unsigned long segment_time = lround(1000000.0/inverse_second);
if ((moves_queued > 1) && (moves_queued < (BLOCK_BUFFER_SIZE * 0.5)))
{
if (segment_time < minsegmenttime)
{ // buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
inverse_second=1000000.0/(segment_time+lround(2*(minsegmenttime-segment_time)/moves_queued));
#ifdef XY_FREQUENCY_LIMIT
segment_time = lround(1000000.0/inverse_second);
#endif
}
}
#endif
// END OF SLOW DOWN SECTION
block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0
block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0
#ifdef FILAMENT_SENSOR
//FMM update ring buffer used for delay with filament measurements
if((extruder==FILAMENT_SENSOR_EXTRUDER_NUM) && (delay_index2 > -1)) //only for extruder with filament sensor and if ring buffer is initialized
{
delay_dist = delay_dist + delta_mm[E_AXIS]; //increment counter with next move in e axis
while (delay_dist >= (10*(MAX_MEASUREMENT_DELAY+1))) //check if counter is over max buffer size in mm
delay_dist = delay_dist - 10*(MAX_MEASUREMENT_DELAY+1); //loop around the buffer
while (delay_dist<0)
delay_dist = delay_dist + 10*(MAX_MEASUREMENT_DELAY+1); //loop around the buffer
delay_index1=delay_dist/10.0; //calculate index
//ensure the number is within range of the array after converting from floating point
if(delay_index1<0)
delay_index1=0;
else if (delay_index1>MAX_MEASUREMENT_DELAY)
delay_index1=MAX_MEASUREMENT_DELAY;
if(delay_index1 != delay_index2) //moved index
{
meas_sample=widthFil_to_size_ratio()-100; //subtract off 100 to reduce magnitude - to store in a signed char
}
while( delay_index1 != delay_index2)
{
delay_index2 = delay_index2 + 1;
if(delay_index2>MAX_MEASUREMENT_DELAY)
delay_index2=delay_index2-(MAX_MEASUREMENT_DELAY+1); //loop around buffer when incrementing
if(delay_index2<0)
delay_index2=0;
else if (delay_index2>MAX_MEASUREMENT_DELAY)
delay_index2=MAX_MEASUREMENT_DELAY;
measurement_delay[delay_index2]=meas_sample;
}
}
#endif
// Calculate and limit speed in mm/sec for each axis
float current_speed[4];
float speed_factor = 1.0; //factor <=1 do decrease speed
for(int i=0; i < 4; i++)
{
current_speed[i] = delta_mm[i] * inverse_second;
if(fabs(current_speed[i]) > max_feedrate[i])
speed_factor = min(speed_factor, max_feedrate[i] / fabs(current_speed[i]));
}
// Max segement time in us.
#ifdef XY_FREQUENCY_LIMIT
#define MAX_FREQ_TIME (1000000.0/XY_FREQUENCY_LIMIT)
// Check and limit the xy direction change frequency
unsigned char direction_change = block->direction_bits ^ old_direction_bits;
old_direction_bits = block->direction_bits;
segment_time = lround((float)segment_time / speed_factor);
if((direction_change & (1<<X_AXIS)) == 0)
{
x_segment_time[0] += segment_time;
}
else
{
x_segment_time[2] = x_segment_time[1];
x_segment_time[1] = x_segment_time[0];
x_segment_time[0] = segment_time;
}
if((direction_change & (1<<Y_AXIS)) == 0)
{
y_segment_time[0] += segment_time;
}
else
{
y_segment_time[2] = y_segment_time[1];
y_segment_time[1] = y_segment_time[0];
y_segment_time[0] = segment_time;
}
long max_x_segment_time = max(x_segment_time[0], max(x_segment_time[1], x_segment_time[2]));
long max_y_segment_time = max(y_segment_time[0], max(y_segment_time[1], y_segment_time[2]));
long min_xy_segment_time =min(max_x_segment_time, max_y_segment_time);
if(min_xy_segment_time < MAX_FREQ_TIME)
speed_factor = min(speed_factor, speed_factor * (float)min_xy_segment_time / (float)MAX_FREQ_TIME);
#endif // XY_FREQUENCY_LIMIT
// Correct the speed
if (speed_factor < 1.0) {
for (unsigned char i = 0; i < NUM_AXIS; i++) current_speed[i] *= speed_factor;
block->nominal_speed *= speed_factor;
block->nominal_rate *= speed_factor;
}
// Compute and limit the acceleration rate for the trapezoid generator.
float steps_per_mm = block->step_event_count / block->millimeters;
long bsx = block->steps_x, bsy = block->steps_y, bsz = block->steps_z, bse = block->steps_e;
if (bsx == 0 && bsy == 0 && bsz == 0) {
block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
}
else if (bse == 0) {
block->acceleration_st = ceil(travel_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
}
else {
block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
}
// Limit acceleration per axis
unsigned long acc_st = block->acceleration_st,
xsteps = axis_steps_per_sqr_second[X_AXIS],
ysteps = axis_steps_per_sqr_second[Y_AXIS],
zsteps = axis_steps_per_sqr_second[Z_AXIS],
esteps = axis_steps_per_sqr_second[E_AXIS];
if ((float)acc_st * bsx / block->step_event_count > xsteps) acc_st = xsteps;
if ((float)acc_st * bsy / block->step_event_count > ysteps) acc_st = ysteps;
if ((float)acc_st * bsz / block->step_event_count > zsteps) acc_st = zsteps;
if ((float)acc_st * bse / block->step_event_count > esteps) acc_st = esteps;
block->acceleration_st = acc_st;
block->acceleration = acc_st / steps_per_mm;
block->acceleration_rate = (long)(acc_st * 16777216.0 / (F_CPU / 8.0));
#if 0 // Use old jerk for now
// Compute path unit vector
double unit_vec[3];
unit_vec[X_AXIS] = delta_mm[X_AXIS]*inverse_millimeters;
unit_vec[Y_AXIS] = delta_mm[Y_AXIS]*inverse_millimeters;
unit_vec[Z_AXIS] = delta_mm[Z_AXIS]*inverse_millimeters;
// Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
// Let a circle be tangent to both previous and current path line segments, where the junction
// deviation is defined as the distance from the junction to the closest edge of the circle,
// colinear with the circle center. The circular segment joining the two paths represents the
// path of centripetal acceleration. Solve for max velocity based on max acceleration about the
// radius of the circle, defined indirectly by junction deviation. This may be also viewed as
// path width or max_jerk in the previous grbl version. This approach does not actually deviate
// from path, but used as a robust way to compute cornering speeds, as it takes into account the
// nonlinearities of both the junction angle and junction velocity.
double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
- previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
- previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
// Skip and use default max junction speed for 0 degree acute junction.
if (cos_theta < 0.95) {
vmax_junction = min(previous_nominal_speed,block->nominal_speed);
// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
if (cos_theta > -0.95) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation
double sin_theta_d2 = sqrt(0.5*(1.0-cos_theta)); // Trig half angle identity. Always positive.
vmax_junction = min(vmax_junction,
sqrt(block->acceleration * junction_deviation * sin_theta_d2/(1.0-sin_theta_d2)) );
}
}
}
#endif
// Start with a safe speed
float vmax_junction = max_xy_jerk / 2;
float vmax_junction_factor = 1.0;
float mz2 = max_z_jerk / 2, me2 = max_e_jerk / 2;
float csz = current_speed[Z_AXIS], cse = current_speed[E_AXIS];
if (fabs(csz) > mz2) vmax_junction = min(vmax_junction, mz2);
if (fabs(cse) > me2) vmax_junction = min(vmax_junction, me2);
vmax_junction = min(vmax_junction, block->nominal_speed);
float safe_speed = vmax_junction;
if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
float dx = current_speed[X_AXIS] - previous_speed[X_AXIS],
dy = current_speed[Y_AXIS] - previous_speed[Y_AXIS],
dz = fabs(csz - previous_speed[Z_AXIS]),
de = fabs(cse - previous_speed[E_AXIS]),
jerk = sqrt(dx * dx + dy * dy);
// if ((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
vmax_junction = block->nominal_speed;
// }
if (jerk > max_xy_jerk) vmax_junction_factor = max_xy_jerk / jerk;
if (dz > max_z_jerk) vmax_junction_factor = min(vmax_junction_factor, max_z_jerk / dz);
if (de > max_e_jerk) vmax_junction_factor = min(vmax_junction_factor, max_e_jerk / de);
vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
}
block->max_entry_speed = vmax_junction;
// Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
double v_allowable = max_allowable_speed(-block->acceleration, MINIMUM_PLANNER_SPEED, block->millimeters);
block->entry_speed = min(vmax_junction, v_allowable);
// Initialize planner efficiency flags
// Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
// If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
// the current block and next block junction speeds are guaranteed to always be at their maximum
// junction speeds in deceleration and acceleration, respectively. This is due to how the current
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
// the reverse and forward planners, the corresponding block junction speed will always be at the
// the maximum junction speed and may always be ignored for any speed reduction checks.
block->nominal_length_flag = (block->nominal_speed <= v_allowable);
block->recalculate_flag = true; // Always calculate trapezoid for new block
// Update previous path unit_vector and nominal speed
for (int i = 0; i < NUM_AXIS; i++) previous_speed[i] = current_speed[i];
previous_nominal_speed = block->nominal_speed;
#ifdef ADVANCE
// Calculate advance rate
if((block->steps_e == 0) || (block->steps_x == 0 && block->steps_y == 0 && block->steps_z == 0)) {
block->advance_rate = 0;
block->advance = 0;
}
else {
long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st);
float advance = (STEPS_PER_CUBIC_MM_E * EXTRUDER_ADVANCE_K) *
(current_speed[E_AXIS] * current_speed[E_AXIS] * EXTRUSION_AREA * EXTRUSION_AREA)*256;
block->advance = advance;
if(acc_dist == 0) {
block->advance_rate = 0;
}
else {
block->advance_rate = advance / (float)acc_dist;
}
}
/*
SERIAL_ECHO_START;
SERIAL_ECHOPGM("advance :");
SERIAL_ECHO(block->advance/256.0);
SERIAL_ECHOPGM("advance rate :");
SERIAL_ECHOLN(block->advance_rate/256.0);
*/
#endif // ADVANCE
calculate_trapezoid_for_block(block, block->entry_speed/block->nominal_speed, safe_speed/block->nominal_speed);
// Move buffer head
block_buffer_head = next_buffer_head;
// Update position
for (int i = 0; i < NUM_AXIS; i++) position[i] = target[i];
planner_recalculate();
st_wake_up();
}
void lcd_update() {
if (!NextionON) return;
nexLoop(nex_listen_list);
millis_t ms = millis();
if (ms > next_lcd_update_ms) {
sendCurrentPageId(&NextionPage);
if (NextionPage == 1) {
if (fanSpeed > 0) fantimer.enable();
else fantimer.disable();
uint32_t temp_feedrate = 0;
VSpeed.getValue(&temp_feedrate);
feedrate_multiplier = (int)temp_feedrate;
#if HAS(TEMP_0)
temptoLCD(0, degHotend(0), degTargetHotend(0));
#endif
#if HAS(TEMP_1)
temptoLCD(1, degHotend(1), degTargetHotend(1));
#endif
#if HAS(TEMP_2)
temptoLCD(2, degHotend(2), degTargetHotend(2));
#elif HAS(TEMP_BED)
temptoLCD(2, degBed(), degTargetBed());
#endif
coordtoLCD();
#if ENABLED(SDSUPPORT)
if (card.isFileOpen()) {
if (SDstatus != 2) {
SDstatus = 2;
SD.setValue(2);
NPlay.setShow();
NStop.setShow();
}
if(IS_SD_PRINTING) {
// Progress bar solid part
sdbar.setValue(card.percentDone());
NPlay.setPic(17);
// Estimate End Time
uint16_t time = print_job_timer.duration() / 60;
uint16_t end_time = (time * (100 - card.percentDone())) / card.percentDone();
if (end_time > (60 * 23)) {
lcd_setstatus("End --:--");
}
else if (end_time >= 0) {
char temp[30];
sprintf_P(temp, PSTR("End %i:%i"), end_time / 60, end_time%60);
lcd_setstatus(temp);
}
}
else {
NPlay.setPic(16);
}
}
else if (card.cardOK && SDstatus != 1) {
SDstatus = 1;
SD.setValue(1);
MSD1.setShow();
NPlay.setHide();
NStop.setHide();
}
else if (!card.cardOK && SDstatus != 0) {
SDstatus = 0;
SD.setValue(0);
MSD1.setHide();
NPlay.setHide();
NStop.setHide();
}
#endif
}
else if (NextionPage == 6) {
coordtoLCD();
}
next_lcd_update_ms = ms + LCD_UPDATE_INTERVAL;
}
}
