/** * lx, ly, lz - Logical (cartesian, not delta) positions in mm */ void Bed_level::apply_leveling(float &lx, float &ly, float &lz) { #if HAS_ABL if (!abl_enabled) return; #endif #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) static float z_fade_factor = 1.0, last_raw_lz = -999.0; if (z_fade_height) { const float raw_lz = RAW_Z_POSITION(lz); if (raw_lz >= z_fade_height) return; if (last_raw_lz != raw_lz) { last_raw_lz = raw_lz; z_fade_factor = 1.0 - raw_lz * inverse_z_fade_height; } } else z_fade_factor = 1.0; #endif #if ENABLED(MESH_BED_LEVELING) if (mbl.active()) lz += mbl.get_z(RAW_X_POSITION(lx), RAW_Y_POSITION(ly) #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) , z_fade_factor #endif ); #elif ABL_PLANAR float dx = RAW_X_POSITION(lx) - (X_TILT_FULCRUM), dy = RAW_Y_POSITION(ly) - (Y_TILT_FULCRUM), dz = RAW_Z_POSITION(lz); apply_rotation_xyz(bed_level_matrix, dx, dy, dz); lx = LOGICAL_X_POSITION(dx + X_TILT_FULCRUM); ly = LOGICAL_Y_POSITION(dy + Y_TILT_FULCRUM); lz = LOGICAL_Z_POSITION(dz); #elif ENABLED(AUTO_BED_LEVELING_BILINEAR) float tmp[XYZ] = { lx, ly, 0 }; lz += bilinear_z_offset(tmp) #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) * z_fade_factor #endif ; #endif }
void Bed_level::unapply_leveling(float logical[XYZ]) { #if HAS_ABL if (!abl_enabled) return; #endif #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) if (z_fade_height && RAW_Z_POSITION(logical[Z_AXIS]) >= z_fade_height) return; #endif #if ENABLED(MESH_BED_LEVELING) if (mbl.active()) { #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) const float c = mbl.get_z(RAW_X_POSITION(logical[X_AXIS]), RAW_Y_POSITION(logical[Y_AXIS]), 1.0); logical[Z_AXIS] = (z_fade_height * (RAW_Z_POSITION(logical[Z_AXIS]) - c)) / (z_fade_height - c); #else logical[Z_AXIS] -= mbl.get_z(RAW_X_POSITION(logical[X_AXIS]), RAW_Y_POSITION(logical[Y_AXIS])); #endif } #elif ABL_PLANAR matrix_3x3 inverse = matrix_3x3::transpose(bed_level_matrix); float dx = RAW_X_POSITION(logical[X_AXIS]) - (X_TILT_FULCRUM), dy = RAW_Y_POSITION(logical[Y_AXIS]) - (Y_TILT_FULCRUM), dz = RAW_Z_POSITION(logical[Z_AXIS]); apply_rotation_xyz(inverse, dx, dy, dz); logical[X_AXIS] = LOGICAL_X_POSITION(dx + X_TILT_FULCRUM); logical[Y_AXIS] = LOGICAL_Y_POSITION(dy + Y_TILT_FULCRUM); logical[Z_AXIS] = LOGICAL_Z_POSITION(dz); #elif ENABLED(AUTO_BED_LEVELING_BILINEAR) #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) const float c = bilinear_z_offset(logical); logical[Z_AXIS] = (z_fade_height * (RAW_Z_POSITION(logical[Z_AXIS]) - c)) / (z_fade_height - c); #else logical[Z_AXIS] -= bilinear_z_offset(logical); #endif #endif }
void report_current_position_detail() { stepper.synchronize(); SERIAL_PROTOCOLPGM("\nLogical:"); report_xyze(current_position); SERIAL_PROTOCOLPGM("Raw: "); const float raw[XYZ] = { RAW_X_POSITION(current_position[X_AXIS]), RAW_Y_POSITION(current_position[Y_AXIS]), RAW_Z_POSITION(current_position[Z_AXIS]) }; report_xyz(raw); SERIAL_PROTOCOLPGM("Leveled:"); float leveled[XYZ] = { current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] }; planner.apply_leveling(leveled); report_xyz(leveled); SERIAL_PROTOCOLPGM("UnLevel:"); float unleveled[XYZ] = { leveled[X_AXIS], leveled[Y_AXIS], leveled[Z_AXIS] }; planner.unapply_leveling(unleveled); report_xyz(unleveled); #if IS_KINEMATIC #if IS_SCARA SERIAL_PROTOCOLPGM("ScaraK: "); #else SERIAL_PROTOCOLPGM("DeltaK: "); #endif inverse_kinematics(leveled); // writes delta[] report_xyz(delta); #endif SERIAL_PROTOCOLPGM("Stepper:"); const float step_count[XYZE] = { stepper.position(X_AXIS), stepper.position(Y_AXIS), stepper.position(Z_AXIS), stepper.position(E_AXIS) }; report_xyze(step_count, 4, 0); #if IS_SCARA const float deg[XYZ] = { stepper.get_axis_position_degrees(A_AXIS), stepper.get_axis_position_degrees(B_AXIS) }; SERIAL_PROTOCOLPGM("Degrees:"); report_xyze(deg, 2); #endif SERIAL_PROTOCOLPGM("FromStp:"); get_cartesian_from_steppers(); // writes cartes[XYZ] (with forward kinematics) const float from_steppers[XYZE] = { cartes[X_AXIS], cartes[Y_AXIS], cartes[Z_AXIS], stepper.get_axis_position_mm(E_AXIS) }; report_xyze(from_steppers); const float diff[XYZE] = { from_steppers[X_AXIS] - leveled[X_AXIS], from_steppers[Y_AXIS] - leveled[Y_AXIS], from_steppers[Z_AXIS] - leveled[Z_AXIS], from_steppers[E_AXIS] - current_position[E_AXIS] }; SERIAL_PROTOCOLPGM("Differ: "); report_xyze(diff); }