void lcd_mixer_gradient_z_end_edit() {
ui.defer_status_screen(true);
ui.encoder_direction_normal();
ENCODER_RATE_MULTIPLY(true);
if (ui.encoderPosition != 0) {
mixer.gradient.end_z += float((int)ui.encoderPosition) * 0.1;
ui.encoderPosition = 0;
NOLESS(mixer.gradient.end_z, 0);
NOMORE(mixer.gradient.end_z, Z_MAX_POS);
}
if (ui.should_draw()) {
char tmp[21];
sprintf_P(tmp, PSTR(MSG_END_Z ": %4d.%d mm"), int(mixer.gradient.end_z), int(mixer.gradient.end_z * 10) % 10);
SETCURSOR(2, (LCD_HEIGHT - 1) / 2);
LCDPRINT(tmp);
}
if (ui.lcd_clicked) {
if (mixer.gradient.end_z < mixer.gradient.start_z)
mixer.gradient.start_z = mixer.gradient.end_z;
mixer.refresh_gradient();
ui.goto_previous_screen();
}
}
void getHighESpeed() {
static float oldt = 0;
if (!autotemp_enabled) return;
if (degTargetHotend0() + 2 < autotemp_min) return; // probably temperature set to zero.
float high = 0.0;
uint8_t block_index = block_buffer_tail;
while (block_index != block_buffer_head) {
block_t* block = &block_buffer[block_index];
if (block->steps[X_AXIS] || block->steps[Y_AXIS] || block->steps[Z_AXIS]) {
float se = (float)block->steps[E_AXIS] / block->step_event_count * block->nominal_speed; // mm/sec;
NOLESS(high, se);
}
block_index = next_block_index(block_index);
}
float t = autotemp_min + high * autotemp_factor;
t = constrain(t, autotemp_min, autotemp_max);
if (oldt > t) {
t *= (1 - (AUTOTEMP_OLDWEIGHT));
t += (AUTOTEMP_OLDWEIGHT) * oldt;
}
oldt = t;
setTargetHotend0(t);
}
void calculate_trapezoid_for_block(block_t *block, float entry_factor, float exit_factor) {
unsigned long initial_rate = ceil(block->nominal_rate * entry_factor); // (step/min)
unsigned long final_rate = ceil(block->nominal_rate * exit_factor); // (step/min)
// Limit minimal step rate (Otherwise the timer will overflow.)
NOLESS(initial_rate, 120);
NOLESS(final_rate, 120);
long acceleration = block->acceleration_st;
int32_t accelerate_steps = ceil(estimate_acceleration_distance(initial_rate, block->nominal_rate, acceleration));
int32_t decelerate_steps = floor(estimate_acceleration_distance(block->nominal_rate, final_rate, -acceleration));
// Calculate the size of Plateau of Nominal Rate.
int32_t plateau_steps = block->step_event_count - accelerate_steps - decelerate_steps;
// Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
// have to use intersection_distance() to calculate when to abort acceleration and start braking
// in order to reach the final_rate exactly at the end of this block.
if (plateau_steps < 0) {
accelerate_steps = ceil(intersection_distance(initial_rate, final_rate, acceleration, block->step_event_count));
accelerate_steps = max(accelerate_steps, 0); // Check limits due to numerical round-off
accelerate_steps = min((uint32_t)accelerate_steps, block->step_event_count);//(We can cast here to unsigned, because the above line ensures that we are above zero)
plateau_steps = 0;
}
#if ENABLED(ADVANCE)
volatile long initial_advance = block->advance * entry_factor * entry_factor;
volatile long final_advance = block->advance * exit_factor * exit_factor;
#endif // ADVANCE
// block->accelerate_until = accelerate_steps;
// block->decelerate_after = accelerate_steps+plateau_steps;
CRITICAL_SECTION_START; // Fill variables used by the stepper in a critical section
if (!block->busy) { // Don't update variables if block is busy.
block->accelerate_until = accelerate_steps;
block->decelerate_after = accelerate_steps+plateau_steps;
block->initial_rate = initial_rate;
block->final_rate = final_rate;
#if ENABLED(ADVANCE)
block->initial_advance = initial_advance;
block->final_advance = final_advance;
#endif
}
CRITICAL_SECTION_END;
}
void Laser::fire(float intensity/*=100.0*/){
laser.firing = LASER_ON;
laser.last_firing = micros(); // microseconds of last laser firing
NOMORE(intensity, 100.0);
NOLESS(intensity, 0.0);
#if ENABLED(LASER_PWM_INVERT)
intensity = 100 - intensity;
#endif
#if LASER_CONTROL == 1
#if LASER_PWR_PIN == 2
OCR3B = PWM_MAPPED_INTENSITY;
#elif LASER_PWR_PIN == 3
OCR3C = PWM_MAPPED_INTENSITY;
#elif LASER_PWR_PIN == 5
OCR3A = PWM_MAPPED_INTENSITY;
#elif LASER_PWR_PIN == 6
OCR4A = PWM_MAPPED_INTENSITY;
#elif LASER_PWR_PIN == 7
OCR4B = PWM_MAPPED_INTENSITY;
#elif LASER_PWR_PIN == 8
OCR4C = PWM_MAPPED_INTENSITY;
#endif
#elif LASER_CONTROL == 2
#if LASER_PWM_PIN == 2
OCR3B = PWM_MAPPED_INTENSITY;
#elif LASER_PWM_PIN == 3
OCR3C = PWM_MAPPED_INTENSITY;
#elif LASER_PWM_PIN == 5
OCR3A = PWM_MAPPED_INTENSITY;
#elif LASER_PWM_PIN == 6
OCR4A = PWM_MAPPED_INTENSITY;
#elif LASER_PWM_PIN == 7
OCR4B = PWM_MAPPED_INTENSITY;
#elif LASER_PWM_PIN == 8
OCR4C = PWM_MAPPED_INTENSITY;
#endif
WRITE(LASER_PWR_PIN, LASER_ARM);
#endif /* LASER_CONTROL */
if (laser.diagnostics) SERIAL_EM("Laser_byte fired");
}
void Mixer::update_gradient_for_z(const float z) {
if (z == prev_z) return;
prev_z = z;
const float slice = gradient.end_z - gradient.start_z;
float pct = (z - gradient.start_z) / slice;
NOLESS(pct, 0.0f); NOMORE(pct, 1.0f);
MIXER_STEPPER_LOOP(i) {
const mixer_perc_t sm = gradient.start_mix[i];
mix[i] = sm + (gradient.end_mix[i] - sm) * pct;
}
copy_mix_to_color(gradient.color);
}
void Mixer::normalize(const uint8_t tool_index) {
float cmax = 0;
#ifdef MIXER_NORMALIZER_DEBUG
float csum = 0;
#endif
MIXER_STEPPER_LOOP(i) {
const float v = collector[i];
NOLESS(cmax, v);
#ifdef MIXER_NORMALIZER_DEBUG
csum += v;
#endif
}
#ifdef MIXER_NORMALIZER_DEBUG
SERIAL_ECHOPGM("Mixer: Old relation : [ ");
MIXER_STEPPER_LOOP(i) {
SERIAL_ECHO_F(collector[i] / csum, 3);
SERIAL_CHAR(' ');
}
SERIAL_ECHOLNPGM("]");
#endif
// Scale all values so their maximum is COLOR_A_MASK
const float scale = float(COLOR_A_MASK) / cmax;
MIXER_STEPPER_LOOP(i) color[tool_index][i] = collector[i] * scale;
#ifdef MIXER_NORMALIZER_DEBUG
csum = 0;
SERIAL_ECHOPGM("Mixer: Normalize to : [ ");
MIXER_STEPPER_LOOP(i) {
SERIAL_ECHO(uint16_t(color[tool_index][i]));
SERIAL_CHAR(' ');
csum += color[tool_index][i];
}
SERIAL_ECHOLNPGM("]");
SERIAL_ECHOPGM("Mixer: New relation : [ ");
MIXER_STEPPER_LOOP(i) {
SERIAL_ECHO_F(uint16_t(color[tool_index][i]) / csum, 3);
SERIAL_CHAR(' ');
}
SERIAL_ECHOLNPGM("]");
#endif
#if ENABLED(GRADIENT_MIX)
refresh_gradient();
#endif
}
float r8mat_amax(int m, int n, float a[])
/******************************************************************************/
/**
Purpose:
R8MAT_AMAX returns the maximum absolute value entry of an R8MAT.
Discussion:
An R8MAT is a doubly dimensioned array of R8 values, stored as a vector
in column-major order.
Licensing:
This code is distributed under the GNU LGPL license.
Modified:
07 September 2012
Author:
John Burkardt
Parameters:
Input, int M, the number of rows in A.
Input, int N, the number of columns in A.
Input, float A[M*N], the M by N matrix.
Output, float R8MAT_AMAX, the maximum absolute value entry of A.
*/
{
float value = r8_abs(a[0 + 0 * m]);
for (int j = 0; j < n; j++) {
for (int i = 0; i < m; i++) {
NOLESS(value, r8_abs(a[i + j * m]));
}
}
return value;
}
void Planner::getHighESpeed() {
static float oldt = 0;
if (!autotemp_enabled) return;
if (thermalManager.degTargetHotend(0) + 2 < autotemp_min) return; // probably temperature set to zero.
float high = 0.0;
for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) {
block_t* block = &block_buffer[b];
if (block->steps[X_AXIS] || block->steps[Y_AXIS] || block->steps[Z_AXIS]) {
float se = (float)block->steps[E_AXIS] / block->step_event_count * block->nominal_speed; // mm/sec;
NOLESS(high, se);
}
}
float t = autotemp_min + high * autotemp_factor;
t = constrain(t, autotemp_min, autotemp_max);
if (oldt > t) {
t *= (1 - (AUTOTEMP_OLDWEIGHT));
t += (AUTOTEMP_OLDWEIGHT) * oldt;
}
oldt = t;
thermalManager.setTargetHotend(t, 0);
}
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()
// Get the Z adjustment for non-linear bed leveling
float bilinear_z_offset(const float raw[XYZ]) {
static float z1, d2, z3, d4, L, D, ratio_x, ratio_y,
last_x = -999.999, last_y = -999.999;
// Whole units for the grid line indices. Constrained within bounds.
static int8_t gridx, gridy, nextx, nexty,
last_gridx = -99, last_gridy = -99;
// XY relative to the probed area
const float rx = raw[X_AXIS] - bilinear_start[X_AXIS],
ry = raw[Y_AXIS] - bilinear_start[Y_AXIS];
#if ENABLED(EXTRAPOLATE_BEYOND_GRID)
// Keep using the last grid box
#define FAR_EDGE_OR_BOX 2
#else
// Just use the grid far edge
#define FAR_EDGE_OR_BOX 1
#endif
if (last_x != rx) {
last_x = rx;
ratio_x = rx * ABL_BG_FACTOR(X_AXIS);
const float gx = constrain(FLOOR(ratio_x), 0, ABL_BG_POINTS_X - (FAR_EDGE_OR_BOX));
ratio_x -= gx; // Subtract whole to get the ratio within the grid box
#if DISABLED(EXTRAPOLATE_BEYOND_GRID)
// Beyond the grid maintain height at grid edges
NOLESS(ratio_x, 0); // Never < 0.0. (> 1.0 is ok when nextx==gridx.)
#endif
gridx = gx;
nextx = MIN(gridx + 1, ABL_BG_POINTS_X - 1);
}
if (last_y != ry || last_gridx != gridx) {
if (last_y != ry) {
last_y = ry;
ratio_y = ry * ABL_BG_FACTOR(Y_AXIS);
const float gy = constrain(FLOOR(ratio_y), 0, ABL_BG_POINTS_Y - (FAR_EDGE_OR_BOX));
ratio_y -= gy;
#if DISABLED(EXTRAPOLATE_BEYOND_GRID)
// Beyond the grid maintain height at grid edges
NOLESS(ratio_y, 0); // Never < 0.0. (> 1.0 is ok when nexty==gridy.)
#endif
gridy = gy;
nexty = MIN(gridy + 1, ABL_BG_POINTS_Y - 1);
}
if (last_gridx != gridx || last_gridy != gridy) {
last_gridx = gridx;
last_gridy = gridy;
// Z at the box corners
z1 = ABL_BG_GRID(gridx, gridy); // left-front
d2 = ABL_BG_GRID(gridx, nexty) - z1; // left-back (delta)
z3 = ABL_BG_GRID(nextx, gridy); // right-front
d4 = ABL_BG_GRID(nextx, nexty) - z3; // right-back (delta)
}
// Bilinear interpolate. Needed since ry or gridx has changed.
L = z1 + d2 * ratio_y; // Linear interp. LF -> LB
const float R = z3 + d4 * ratio_y; // Linear interp. RF -> RB
D = R - L;
}
const float offset = L + ratio_x * D; // the offset almost always changes
/*
static float last_offset = 0;
if (ABS(last_offset - offset) > 0.2) {
SERIAL_ECHOPGM("Sudden Shift at ");
SERIAL_ECHOPAIR("x=", rx);
SERIAL_ECHOPAIR(" / ", bilinear_grid_spacing[X_AXIS]);
SERIAL_ECHOLNPAIR(" -> gridx=", gridx);
SERIAL_ECHOPAIR(" y=", ry);
SERIAL_ECHOPAIR(" / ", bilinear_grid_spacing[Y_AXIS]);
SERIAL_ECHOLNPAIR(" -> gridy=", gridy);
SERIAL_ECHOPAIR(" ratio_x=", ratio_x);
SERIAL_ECHOLNPAIR(" ratio_y=", ratio_y);
SERIAL_ECHOPAIR(" z1=", z1);
SERIAL_ECHOPAIR(" z2=", z2);
SERIAL_ECHOPAIR(" z3=", z3);
SERIAL_ECHOLNPAIR(" z4=", z4);
SERIAL_ECHOPAIR(" L=", L);
SERIAL_ECHOPAIR(" R=", R);
SERIAL_ECHOLNPAIR(" offset=", offset);
}
last_offset = offset;
//*/
return offset;
}
/**
* M48: Z probe repeatability measurement function.
*
* Usage:
* M48 <P#> <X#> <Y#> <V#> <E> <L#> <S>
* P = Number of sampled points (4-50, default 10)
* X = Sample X position
* Y = Sample Y position
* V = Verbose level (0-4, default=1)
* E = Engage Z probe for each reading
* L = Number of legs of movement before probe
* S = Schizoid (Or Star if you prefer)
*
* This function requires the machine to be homed before invocation.
*/
void GcodeSuite::M48() {
if (axis_unhomed_error()) return;
const int8_t verbose_level = parser.byteval('V', 1);
if (!WITHIN(verbose_level, 0, 4)) {
SERIAL_ECHOLNPGM("?(V)erbose level is implausible (0-4).");
return;
}
if (verbose_level > 0)
SERIAL_ECHOLNPGM("M48 Z-Probe Repeatability Test");
const int8_t n_samples = parser.byteval('P', 10);
if (!WITHIN(n_samples, 4, 50)) {
SERIAL_ECHOLNPGM("?Sample size not plausible (4-50).");
return;
}
const ProbePtRaise raise_after = parser.boolval('E') ? PROBE_PT_STOW : PROBE_PT_RAISE;
float X_current = current_position[X_AXIS],
Y_current = current_position[Y_AXIS];
const float X_probe_location = parser.linearval('X', X_current + X_PROBE_OFFSET_FROM_EXTRUDER),
Y_probe_location = parser.linearval('Y', Y_current + Y_PROBE_OFFSET_FROM_EXTRUDER);
if (!position_is_reachable_by_probe(X_probe_location, Y_probe_location)) {
SERIAL_ECHOLNPGM("? (X,Y) out of bounds.");
return;
}
bool seen_L = parser.seen('L');
uint8_t n_legs = seen_L ? parser.value_byte() : 0;
if (n_legs > 15) {
SERIAL_ECHOLNPGM("?Number of legs in movement not plausible (0-15).");
return;
}
if (n_legs == 1) n_legs = 2;
const bool schizoid_flag = parser.boolval('S');
if (schizoid_flag && !seen_L) n_legs = 7;
/**
* Now get everything to the specified probe point So we can safely do a
* probe to get us close to the bed. If the Z-Axis is far from the bed,
* we don't want to use that as a starting point for each probe.
*/
if (verbose_level > 2)
SERIAL_ECHOLNPGM("Positioning the probe...");
// Disable bed level correction in M48 because we want the raw data when we probe
#if HAS_LEVELING
const bool was_enabled = planner.leveling_active;
set_bed_leveling_enabled(false);
#endif
setup_for_endstop_or_probe_move();
float mean = 0.0, sigma = 0.0, min = 99999.9, max = -99999.9, sample_set[n_samples];
// Move to the first point, deploy, and probe
const float t = probe_pt(X_probe_location, Y_probe_location, raise_after, verbose_level);
bool probing_good = !isnan(t);
if (probing_good) {
randomSeed(millis());
for (uint8_t n = 0; n < n_samples; n++) {
if (n_legs) {
const int dir = (random(0, 10) > 5.0) ? -1 : 1; // clockwise or counter clockwise
float angle = random(0, 360);
const float radius = random(
#if ENABLED(DELTA)
(int) (0.1250000000 * (DELTA_PRINTABLE_RADIUS)),
(int) (0.3333333333 * (DELTA_PRINTABLE_RADIUS))
#else
(int) 5.0, (int) (0.125 * MIN(X_BED_SIZE, Y_BED_SIZE))
#endif
);
if (verbose_level > 3) {
SERIAL_ECHOPAIR("Starting radius: ", radius);
SERIAL_ECHOPAIR(" angle: ", angle);
SERIAL_ECHOPGM(" Direction: ");
if (dir > 0) SERIAL_ECHOPGM("Counter-");
SERIAL_ECHOLNPGM("Clockwise");
}
for (uint8_t l = 0; l < n_legs - 1; l++) {
float delta_angle;
if (schizoid_flag)
// The points of a 5 point star are 72 degrees apart. We need to
// skip a point and go to the next one on the star.
delta_angle = dir * 2.0 * 72.0;
else
// If we do this line, we are just trying to move further
// around the circle.
delta_angle = dir * (float) random(25, 45);
angle += delta_angle;
while (angle > 360.0) // We probably do not need to keep the angle between 0 and 2*PI, but the
angle -= 360.0; // Arduino documentation says the trig functions should not be given values
while (angle < 0.0) // outside of this range. It looks like they behave correctly with
angle += 360.0; // numbers outside of the range, but just to be safe we clamp them.
X_current = X_probe_location - (X_PROBE_OFFSET_FROM_EXTRUDER) + cos(RADIANS(angle)) * radius;
Y_current = Y_probe_location - (Y_PROBE_OFFSET_FROM_EXTRUDER) + sin(RADIANS(angle)) * radius;
#if DISABLED(DELTA)
X_current = constrain(X_current, X_MIN_POS, X_MAX_POS);
Y_current = constrain(Y_current, Y_MIN_POS, Y_MAX_POS);
#else
// If we have gone out too far, we can do a simple fix and scale the numbers
// back in closer to the origin.
while (!position_is_reachable_by_probe(X_current, Y_current)) {
X_current *= 0.8;
Y_current *= 0.8;
if (verbose_level > 3) {
SERIAL_ECHOPAIR("Pulling point towards center:", X_current);
SERIAL_ECHOLNPAIR(", ", Y_current);
}
}
#endif
if (verbose_level > 3) {
SERIAL_ECHOPGM("Going to:");
SERIAL_ECHOPAIR(" X", X_current);
SERIAL_ECHOPAIR(" Y", Y_current);
SERIAL_ECHOLNPAIR(" Z", current_position[Z_AXIS]);
}
do_blocking_move_to_xy(X_current, Y_current);
} // n_legs loop
} // n_legs
// Probe a single point
sample_set[n] = probe_pt(X_probe_location, Y_probe_location, raise_after, 0);
// Break the loop if the probe fails
probing_good = !isnan(sample_set[n]);
if (!probing_good) break;
/**
* Get the current mean for the data points we have so far
*/
float sum = 0.0;
for (uint8_t j = 0; j <= n; j++) sum += sample_set[j];
mean = sum / (n + 1);
NOMORE(min, sample_set[n]);
NOLESS(max, sample_set[n]);
/**
* Now, use that mean to calculate the standard deviation for the
* data points we have so far
*/
sum = 0.0;
for (uint8_t j = 0; j <= n; j++)
sum += sq(sample_set[j] - mean);
sigma = SQRT(sum / (n + 1));
if (verbose_level > 0) {
if (verbose_level > 1) {
SERIAL_ECHO(n + 1);
SERIAL_ECHOPAIR(" of ", (int)n_samples);
SERIAL_ECHOPAIR_F(": z: ", sample_set[n], 3);
if (verbose_level > 2) {
SERIAL_ECHOPAIR_F(" mean: ", mean, 4);
SERIAL_ECHOPAIR_F(" sigma: ", sigma, 6);
SERIAL_ECHOPAIR_F(" min: ", min, 3);
SERIAL_ECHOPAIR_F(" max: ", max, 3);
SERIAL_ECHOPAIR_F(" range: ", max-min, 3);
}
SERIAL_EOL();
}
}
} // n_samples loop
}
STOW_PROBE();
if (probing_good) {
SERIAL_ECHOLNPGM("Finished!");
if (verbose_level > 0) {
SERIAL_ECHOPAIR_F("Mean: ", mean, 6);
SERIAL_ECHOPAIR_F(" Min: ", min, 3);
SERIAL_ECHOPAIR_F(" Max: ", max, 3);
SERIAL_ECHOLNPAIR_F(" Range: ", max-min, 3);
}
SERIAL_ECHOLNPAIR_F("Standard Deviation: ", sigma, 6);
SERIAL_EOL();
}
clean_up_after_endstop_or_probe_move();
// Re-enable bed level correction if it had been on
#if HAS_LEVELING
set_bed_leveling_enabled(was_enabled);
#endif
report_current_position();
}
// Get the Z adjustment for non-linear bed leveling
float Bed_level::bilinear_z_offset(const float logical[XYZ]) {
static float z1, d2, z3, d4, L, D, ratio_x, ratio_y,
last_x = -999.999, last_y = -999.999;
// Whole units for the grid line indices. Constrained within bounds.
static int8_t gridx, gridy, nextx, nexty,
last_gridx = -99, last_gridy = -99;
// XY relative to the probed area
const float x = RAW_X_POSITION(logical[X_AXIS]) - bilinear_start[X_AXIS],
y = RAW_Y_POSITION(logical[Y_AXIS]) - bilinear_start[Y_AXIS];
if (last_x != x) {
last_x = x;
ratio_x = x * ABL_BG_FACTOR(X_AXIS);
const float gx = constrain(floor(ratio_x), 0, ABL_BG_POINTS_X - 1);
ratio_x -= gx; // Subtract whole to get the ratio within the grid box
NOLESS(ratio_x, 0); // Never < 0.0. (> 1.0 is ok when nextx==gridx.)
gridx = gx;
nextx = min(gridx + 1, ABL_BG_POINTS_X - 1);
}
if (last_y != y || last_gridx != gridx) {
if (last_y != y) {
last_y = y;
ratio_y = y * ABL_BG_FACTOR(Y_AXIS);
const float gy = constrain(floor(ratio_y), 0, ABL_BG_POINTS_Y - 1);
ratio_y -= gy;
NOLESS(ratio_y, 0);
gridy = gy;
nexty = min(gridy + 1, ABL_BG_POINTS_Y - 1);
}
if (last_gridx != gridx || last_gridy != gridy) {
last_gridx = gridx;
last_gridy = gridy;
// Z at the box corners
z1 = ABL_BG_GRID(gridx, gridy); // left-front
d2 = ABL_BG_GRID(gridx, nexty) - z1; // left-back (delta)
z3 = ABL_BG_GRID(nextx, gridy); // right-front
d4 = ABL_BG_GRID(nextx, nexty) - z3; // right-back (delta)
}
// Bilinear interpolate. Needed since y or gridx has changed.
L = z1 + d2 * ratio_y; // Linear interp. LF -> LB
const float R = z3 + d4 * ratio_y; // Linear interp. RF -> RB
D = R - L;
}
const float offset = L + ratio_x * D; // the offset almost always changes
/*
static float last_offset = 0;
if (FABS(last_offset - offset) > 0.2) {
SERIAL_MSG("Sudden Shift at ");
SERIAL_MV("x=", x);
SERIAL_MV(" / ", ABL_BG_SPACING(X_AXIS));
SERIAL_EMV(" -> gridx=", gridx);
SERIAL_MV(" y=", y);
SERIAL_MV(" / ", ABL_BG_SPACING(Y_AXIS));
SERIAL_EMV(" -> gridy=", gridy);
SERIAL_MV(" ratio_x=", ratio_x);
SERIAL_EMV(" ratio_y=", ratio_y);
SERIAL_MV(" z1=", z1);
SERIAL_MV(" d2=", d2);
SERIAL_MV(" z3=", z3);
SERIAL_EMV(" d4=", d4);
SERIAL_MV(" L=", L);
SERIAL_MV(" R=", R);
SERIAL_EMV(" offset=", offset);
}
last_offset = offset;
*/
return offset;
}
/**
* M420: Enable/Disable Bed Leveling and/or set the Z fade height.
*
* S[bool] Turns leveling on or off
* Z[height] Sets the Z fade height (0 or none to disable)
* V[bool] Verbose - Print the leveling grid
*
* With AUTO_BED_LEVELING_UBL only:
*
* L[index] Load UBL mesh from index (0 is default)
* T[map] 0:Human-readable 1:CSV 2:"LCD" 4:Compact
*
* With mesh-based leveling only:
*
* C Center mesh on the mean of the lowest and highest
*
* With MARLIN_DEV_MODE:
* S2 Create a simple random mesh and enable
*/
void GcodeSuite::M420() {
const bool seen_S = parser.seen('S'),
to_enable = seen_S ? parser.value_bool() : planner.leveling_active;
#if ENABLED(MARLIN_DEV_MODE)
if (parser.intval('S') == 2) {
#if ENABLED(AUTO_BED_LEVELING_BILINEAR)
bilinear_start[X_AXIS] = MIN_PROBE_X;
bilinear_start[Y_AXIS] = MIN_PROBE_Y;
bilinear_grid_spacing[X_AXIS] = (MAX_PROBE_X - (MIN_PROBE_X)) / (GRID_MAX_POINTS_X - 1);
bilinear_grid_spacing[Y_AXIS] = (MAX_PROBE_Y - (MIN_PROBE_Y)) / (GRID_MAX_POINTS_Y - 1);
#endif
for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++)
Z_VALUES(x, y) = 0.001 * random(-200, 200);
SERIAL_ECHOPGM("Simulated " STRINGIFY(GRID_MAX_POINTS_X) "x" STRINGIFY(GRID_MAX_POINTS_X) " mesh ");
SERIAL_ECHOPAIR(" (", MIN_PROBE_X);
SERIAL_CHAR(','); SERIAL_ECHO(MIN_PROBE_Y);
SERIAL_ECHOPAIR(")-(", MAX_PROBE_X);
SERIAL_CHAR(','); SERIAL_ECHO(MAX_PROBE_Y);
SERIAL_ECHOLNPGM(")");
}
#endif
// If disabling leveling do it right away
// (Don't disable for just M420 or M420 V)
if (seen_S && !to_enable) set_bed_leveling_enabled(false);
const float oldpos[] = { current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] };
#if ENABLED(AUTO_BED_LEVELING_UBL)
// L to load a mesh from the EEPROM
if (parser.seen('L')) {
set_bed_leveling_enabled(false);
#if ENABLED(EEPROM_SETTINGS)
const int8_t storage_slot = parser.has_value() ? parser.value_int() : ubl.storage_slot;
const int16_t a = settings.calc_num_meshes();
if (!a) {
SERIAL_ECHOLNPGM("?EEPROM storage not available.");
return;
}
if (!WITHIN(storage_slot, 0, a - 1)) {
SERIAL_ECHOLNPGM("?Invalid storage slot.");
SERIAL_ECHOLNPAIR("?Use 0 to ", a - 1);
return;
}
settings.load_mesh(storage_slot);
ubl.storage_slot = storage_slot;
#else
SERIAL_ECHOLNPGM("?EEPROM storage not available.");
return;
#endif
}
// L or V display the map info
if (parser.seen('L') || parser.seen('V')) {
ubl.display_map(parser.byteval('T'));
SERIAL_ECHOPGM("Mesh is ");
if (!ubl.mesh_is_valid()) SERIAL_ECHOPGM("in");
SERIAL_ECHOLNPAIR("valid\nStorage slot: ", ubl.storage_slot);
}
#endif // AUTO_BED_LEVELING_UBL
const bool seenV = parser.seen('V');
#if HAS_MESH
if (leveling_is_valid()) {
// Subtract the given value or the mean from all mesh values
if (parser.seen('C')) {
const float cval = parser.value_float();
#if ENABLED(AUTO_BED_LEVELING_UBL)
set_bed_leveling_enabled(false);
ubl.adjust_mesh_to_mean(true, cval);
#else
#if ENABLED(M420_C_USE_MEAN)
// Get the sum and average of all mesh values
float mesh_sum = 0;
for (uint8_t x = GRID_MAX_POINTS_X; x--;)
for (uint8_t y = GRID_MAX_POINTS_Y; y--;)
mesh_sum += Z_VALUES(x, y);
const float zmean = mesh_sum / float(GRID_MAX_POINTS);
#else
// Find the low and high mesh values
float lo_val = 100, hi_val = -100;
for (uint8_t x = GRID_MAX_POINTS_X; x--;)
for (uint8_t y = GRID_MAX_POINTS_Y; y--;) {
const float z = Z_VALUES(x, y);
NOMORE(lo_val, z);
NOLESS(hi_val, z);
}
// Take the mean of the lowest and highest
const float zmean = (lo_val + hi_val) / 2.0 + cval;
#endif
// If not very close to 0, adjust the mesh
if (!NEAR_ZERO(zmean)) {
set_bed_leveling_enabled(false);
// Subtract the mean from all values
for (uint8_t x = GRID_MAX_POINTS_X; x--;)
for (uint8_t y = GRID_MAX_POINTS_Y; y--;)
Z_VALUES(x, y) -= zmean;
#if ENABLED(ABL_BILINEAR_SUBDIVISION)
bed_level_virt_interpolate();
#endif
}
#endif
}
}
else if (to_enable || seenV) {
SERIAL_ECHO_MSG("Invalid mesh.");
goto EXIT_M420;
}
#endif // HAS_MESH
// V to print the matrix or mesh
if (seenV) {
#if ABL_PLANAR
planner.bed_level_matrix.debug(PSTR("Bed Level Correction Matrix:"));
#else
if (leveling_is_valid()) {
#if ENABLED(AUTO_BED_LEVELING_BILINEAR)
print_bilinear_leveling_grid();
#if ENABLED(ABL_BILINEAR_SUBDIVISION)
print_bilinear_leveling_grid_virt();
#endif
#elif ENABLED(MESH_BED_LEVELING)
SERIAL_ECHOLNPGM("Mesh Bed Level data:");
mbl.report_mesh();
#endif
}
#endif
}
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
if (parser.seen('Z')) set_z_fade_height(parser.value_linear_units(), false);
#endif
// Enable leveling if specified, or if previously active
set_bed_leveling_enabled(to_enable);
#if HAS_MESH
EXIT_M420:
#endif
// Error if leveling failed to enable or reenable
if (to_enable && !planner.leveling_active)
SERIAL_ERROR_MSG(MSG_ERR_M420_FAILED);
SERIAL_ECHO_START();
SERIAL_ECHOPGM("Bed Leveling ");
serialprintln_onoff(planner.leveling_active);
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
SERIAL_ECHO_START();
SERIAL_ECHOPGM("Fade Height ");
if (planner.z_fade_height > 0.0)
SERIAL_ECHOLN(planner.z_fade_height);
else
SERIAL_ECHOLNPGM(MSG_OFF);
#endif
// Report change in position
if (memcmp(oldpos, current_position, sizeof(oldpos)))
report_current_position();
}
bool _O2 unified_bed_leveling::prepare_segmented_line_to(const float (&rtarget)[XYZE], const float &feedrate) {
if (!position_is_reachable(rtarget[X_AXIS], rtarget[Y_AXIS])) // fail if moving outside reachable boundary
return true; // did not move, so current_position still accurate
const float total[XYZE] = {
rtarget[X_AXIS] - current_position[X_AXIS],
rtarget[Y_AXIS] - current_position[Y_AXIS],
rtarget[Z_AXIS] - current_position[Z_AXIS],
rtarget[E_AXIS] - current_position[E_AXIS]
};
const float cartesian_xy_mm = HYPOT(total[X_AXIS], total[Y_AXIS]); // total horizontal xy distance
#if IS_KINEMATIC
const float seconds = cartesian_xy_mm / feedrate; // seconds to move xy distance at requested rate
uint16_t segments = lroundf(delta_segments_per_second * seconds), // preferred number of segments for distance @ feedrate
seglimit = lroundf(cartesian_xy_mm * (1.0f / (DELTA_SEGMENT_MIN_LENGTH))); // number of segments at minimum segment length
NOMORE(segments, seglimit); // limit to minimum segment length (fewer segments)
#else
uint16_t segments = lroundf(cartesian_xy_mm * (1.0f / (DELTA_SEGMENT_MIN_LENGTH))); // cartesian fixed segment length
#endif
NOLESS(segments, 1U); // must have at least one segment
const float inv_segments = 1.0f / segments; // divide once, multiply thereafter
#if IS_SCARA // scale the feed rate from mm/s to degrees/s
scara_feed_factor = cartesian_xy_mm * inv_segments * feedrate;
scara_oldA = planner.get_axis_position_degrees(A_AXIS);
scara_oldB = planner.get_axis_position_degrees(B_AXIS);
#endif
const float diff[XYZE] = {
total[X_AXIS] * inv_segments,
total[Y_AXIS] * inv_segments,
total[Z_AXIS] * inv_segments,
total[E_AXIS] * inv_segments
};
// Note that E segment distance could vary slightly as z mesh height
// changes for each segment, but small enough to ignore.
float raw[XYZE] = {
current_position[X_AXIS],
current_position[Y_AXIS],
current_position[Z_AXIS],
current_position[E_AXIS]
};
// Only compute leveling per segment if ubl active and target below z_fade_height.
if (!planner.leveling_active || !planner.leveling_active_at_z(rtarget[Z_AXIS])) { // no mesh leveling
while (--segments) {
LOOP_XYZE(i) raw[i] += diff[i];
ubl_buffer_segment_raw(raw, feedrate);
}
ubl_buffer_segment_raw(rtarget, feedrate);
return false; // moved but did not set_current_from_destination();
}
// Otherwise perform per-segment leveling
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
const float fade_scaling_factor = planner.fade_scaling_factor_for_z(rtarget[Z_AXIS]);
#endif
// increment to first segment destination
LOOP_XYZE(i) raw[i] += diff[i];
for (;;) { // for each mesh cell encountered during the move
// Compute mesh cell invariants that remain constant for all segments within cell.
// Note for cell index, if point is outside the mesh grid (in MESH_INSET perimeter)
// the bilinear interpolation from the adjacent cell within the mesh will still work.
// Inner loop will exit each time (because out of cell bounds) but will come back
// in top of loop and again re-find same adjacent cell and use it, just less efficient
// for mesh inset area.
int8_t cell_xi = (raw[X_AXIS] - (MESH_MIN_X)) * (1.0f / (MESH_X_DIST)),
cell_yi = (raw[Y_AXIS] - (MESH_MIN_Y)) * (1.0f / (MESH_Y_DIST));
cell_xi = constrain(cell_xi, 0, (GRID_MAX_POINTS_X) - 1);
cell_yi = constrain(cell_yi, 0, (GRID_MAX_POINTS_Y) - 1);
const float x0 = mesh_index_to_xpos(cell_xi), // 64 byte table lookup avoids mul+add
y0 = mesh_index_to_ypos(cell_yi);
float z_x0y0 = z_values[cell_xi ][cell_yi ], // z at lower left corner
z_x1y0 = z_values[cell_xi+1][cell_yi ], // z at upper left corner
z_x0y1 = z_values[cell_xi ][cell_yi+1], // z at lower right corner
z_x1y1 = z_values[cell_xi+1][cell_yi+1]; // z at upper right corner
if (isnan(z_x0y0)) z_x0y0 = 0; // ideally activating planner.leveling_active (G29 A)
if (isnan(z_x1y0)) z_x1y0 = 0; // should refuse if any invalid mesh points
if (isnan(z_x0y1)) z_x0y1 = 0; // in order to avoid isnan tests per cell,
if (isnan(z_x1y1)) z_x1y1 = 0; // thus guessing zero for undefined points
float cx = raw[X_AXIS] - x0, // cell-relative x and y
cy = raw[Y_AXIS] - y0;
const float z_xmy0 = (z_x1y0 - z_x0y0) * (1.0f / (MESH_X_DIST)), // z slope per x along y0 (lower left to lower right)
z_xmy1 = (z_x1y1 - z_x0y1) * (1.0f / (MESH_X_DIST)); // z slope per x along y1 (upper left to upper right)
float z_cxy0 = z_x0y0 + z_xmy0 * cx; // z height along y0 at cx (changes for each cx in cell)
const float z_cxy1 = z_x0y1 + z_xmy1 * cx, // z height along y1 at cx
z_cxyd = z_cxy1 - z_cxy0; // z height difference along cx from y0 to y1
float z_cxym = z_cxyd * (1.0f / (MESH_Y_DIST)); // z slope per y along cx from y0 to y1 (changes for each cx in cell)
// float z_cxcy = z_cxy0 + z_cxym * cy; // interpolated mesh z height along cx at cy (do inside the segment loop)
// As subsequent segments step through this cell, the z_cxy0 intercept will change
// and the z_cxym slope will change, both as a function of cx within the cell, and
// each change by a constant for fixed segment lengths.
const float z_sxy0 = z_xmy0 * diff[X_AXIS], // per-segment adjustment to z_cxy0
z_sxym = (z_xmy1 - z_xmy0) * (1.0f / (MESH_Y_DIST)) * diff[X_AXIS]; // per-segment adjustment to z_cxym
for (;;) { // for all segments within this mesh cell
if (--segments == 0) // if this is last segment, use rtarget for exact
COPY(raw, rtarget);
const float z_cxcy = (z_cxy0 + z_cxym * cy) // interpolated mesh z height along cx at cy
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
* fade_scaling_factor // apply fade factor to interpolated mesh height
#endif
;
const float z = raw[Z_AXIS];
raw[Z_AXIS] += z_cxcy;
ubl_buffer_segment_raw(raw, feedrate);
raw[Z_AXIS] = z;
if (segments == 0) // done with last segment
return false; // did not set_current_from_destination()
LOOP_XYZE(i) raw[i] += diff[i];
cx += diff[X_AXIS];
cy += diff[Y_AXIS];
if (!WITHIN(cx, 0, MESH_X_DIST) || !WITHIN(cy, 0, MESH_Y_DIST)) // done within this cell, break to next
break;
// Next segment still within same mesh cell, adjust the per-segment
// slope and intercept to compute next z height.
z_cxy0 += z_sxy0; // adjust z_cxy0 by per-segment z_sxy0
z_cxym += z_sxym; // adjust z_cxym by per-segment z_sxym
} // segment loop
} // cell loop
return false; // caller will update current_position
}
FORCE_INLINE void MarlinSerial<Cfg>::store_rxd_char() {
static EmergencyParser::State emergency_state; // = EP_RESET
// Get the tail - Nothing can alter its value while this ISR is executing, but there's
// a chance that this ISR interrupted the main process while it was updating the index.
// The backup mechanism ensures the correct value is always returned.
const ring_buffer_pos_t t = atomic_read_rx_tail();
// Get the head pointer - This ISR is the only one that modifies its value, so it's safe to read here
ring_buffer_pos_t h = rx_buffer.head;
// Get the next element
ring_buffer_pos_t i = (ring_buffer_pos_t)(h + 1) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1);
// This must read the R_UCSRA register before reading the received byte to detect error causes
if (Cfg::DROPPED_RX && B_DOR && !++rx_dropped_bytes) --rx_dropped_bytes;
if (Cfg::RX_OVERRUNS && B_DOR && !++rx_buffer_overruns) --rx_buffer_overruns;
if (Cfg::RX_FRAMING_ERRORS && B_FE && !++rx_framing_errors) --rx_framing_errors;
// Read the character from the USART
uint8_t c = R_UDR;
if (Cfg::EMERGENCYPARSER) emergency_parser.update(emergency_state, c);
// If the character is to be stored at the index just before the tail
// (such that the head would advance to the current tail), the RX FIFO is
// full, so don't write the character or advance the head.
if (i != t) {
rx_buffer.buffer[h] = c;
h = i;
}
else if (Cfg::DROPPED_RX && !++rx_dropped_bytes)
--rx_dropped_bytes;
if (Cfg::MAX_RX_QUEUED) {
// Calculate count of bytes stored into the RX buffer
const ring_buffer_pos_t rx_count = (ring_buffer_pos_t)(h - t) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1);
// Keep track of the maximum count of enqueued bytes
NOLESS(rx_max_enqueued, rx_count);
}
if (Cfg::XONOFF) {
// If the last char that was sent was an XON
if ((xon_xoff_state & XON_XOFF_CHAR_MASK) == XON_CHAR) {
// Bytes stored into the RX buffer
const ring_buffer_pos_t rx_count = (ring_buffer_pos_t)(h - t) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1);
// If over 12.5% of RX buffer capacity, send XOFF before running out of
// RX buffer space .. 325 bytes @ 250kbits/s needed to let the host react
// and stop sending bytes. This translates to 13mS propagation time.
if (rx_count >= (Cfg::RX_SIZE) / 8) {
// At this point, definitely no TX interrupt was executing, since the TX ISR can't be preempted.
// Don't enable the TX interrupt here as a means to trigger the XOFF char, because if it happens
// to be in the middle of trying to disable the RX interrupt in the main program, eventually the
// enabling of the TX interrupt could be undone. The ONLY reliable thing this can do to ensure
// the sending of the XOFF char is to send it HERE AND NOW.
// About to send the XOFF char
xon_xoff_state = XOFF_CHAR | XON_XOFF_CHAR_SENT;
// Wait until the TX register becomes empty and send it - Here there could be a problem
// - While waiting for the TX register to empty, the RX register could receive a new
// character. This must also handle that situation!
while (!B_UDRE) {
if (B_RXC) {
// A char arrived while waiting for the TX buffer to be empty - Receive and process it!
i = (ring_buffer_pos_t)(h + 1) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1);
// Read the character from the USART
c = R_UDR;
if (Cfg::EMERGENCYPARSER) emergency_parser.update(emergency_state, c);
// If the character is to be stored at the index just before the tail
// (such that the head would advance to the current tail), the FIFO is
// full, so don't write the character or advance the head.
if (i != t) {
rx_buffer.buffer[h] = c;
h = i;
}
else if (Cfg::DROPPED_RX && !++rx_dropped_bytes)
--rx_dropped_bytes;
}
sw_barrier();
}
R_UDR = XOFF_CHAR;
// Clear the TXC bit -- "can be cleared by writing a one to its bit
// location". This makes sure flush() won't return until the bytes
// actually got written
B_TXC = 1;
// At this point there could be a race condition between the write() function
// and this sending of the XOFF char. This interrupt could happen between the
// wait to be empty TX buffer loop and the actual write of the character. Since
// the TX buffer is full because it's sending the XOFF char, the only way to be
// sure the write() function will succeed is to wait for the XOFF char to be
// completely sent. Since an extra character could be received during the wait
// it must also be handled!
while (!B_UDRE) {
if (B_RXC) {
// A char arrived while waiting for the TX buffer to be empty - Receive and process it!
i = (ring_buffer_pos_t)(h + 1) & (ring_buffer_pos_t)(Cfg::RX_SIZE - 1);
// Read the character from the USART
c = R_UDR;
if (Cfg::EMERGENCYPARSER)
emergency_parser.update(emergency_state, c);
// If the character is to be stored at the index just before the tail
// (such that the head would advance to the current tail), the FIFO is
// full, so don't write the character or advance the head.
if (i != t) {
rx_buffer.buffer[h] = c;
h = i;
}
else if (Cfg::DROPPED_RX && !++rx_dropped_bytes)
--rx_dropped_bytes;
}
sw_barrier();
}
// At this point everything is ready. The write() function won't
// have any issues writing to the UART TX register if it needs to!
}
}
}
// Store the new head value - The main loop will retry until the value is stable
rx_buffer.head = h;
}