My Marlin configs for Fabrikator Mini and CTC i3 Pro B
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

ubl.h 15KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352
  1. /**
  2. * Marlin 3D Printer Firmware
  3. * Copyright (C) 2016, 2017 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
  4. *
  5. * Based on Sprinter and grbl.
  6. * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
  7. *
  8. * This program is free software: you can redistribute it and/or modify
  9. * it under the terms of the GNU General Public License as published by
  10. * the Free Software Foundation, either version 3 of the License, or
  11. * (at your option) any later version.
  12. *
  13. * This program is distributed in the hope that it will be useful,
  14. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  15. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  16. * GNU General Public License for more details.
  17. *
  18. * You should have received a copy of the GNU General Public License
  19. * along with this program. If not, see <http://www.gnu.org/licenses/>.
  20. *
  21. */
  22. #ifndef UNIFIED_BED_LEVELING_H
  23. #define UNIFIED_BED_LEVELING_H
  24. #include "MarlinConfig.h"
  25. #if ENABLED(AUTO_BED_LEVELING_UBL)
  26. #include "Marlin.h"
  27. #include "planner.h"
  28. #include "math.h"
  29. #include "vector_3.h"
  30. #define UBL_VERSION "1.00"
  31. #define UBL_OK false
  32. #define UBL_ERR true
  33. typedef struct {
  34. int8_t x_index, y_index;
  35. float distance; // When populated, the distance from the search location
  36. } mesh_index_pair;
  37. // ubl.cpp
  38. void bit_clear(uint16_t bits[16], uint8_t x, uint8_t y);
  39. void bit_set(uint16_t bits[16], uint8_t x, uint8_t y);
  40. bool is_bit_set(uint16_t bits[16], uint8_t x, uint8_t y);
  41. // ubl_motion.cpp
  42. void debug_current_and_destination(const char * const title);
  43. void ubl_line_to_destination(const float&, uint8_t);
  44. // ubl_G29.cpp
  45. enum MeshPointType { INVALID, REAL, SET_IN_BITMAP };
  46. void dump(char * const str, const float &f);
  47. void probe_entire_mesh(const float&, const float&, const bool, const bool, const bool);
  48. void manually_probe_remaining_mesh(const float&, const float&, const float&, const float&, const bool);
  49. float measure_business_card_thickness(const float&);
  50. mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType, const float&, const float&, const bool, unsigned int[16], bool);
  51. void shift_mesh_height();
  52. void fine_tune_mesh(const float&, const float&, const bool);
  53. bool g29_parameter_parsing();
  54. void g29_what_command();
  55. void g29_eeprom_dump();
  56. void g29_compare_current_mesh_to_stored_mesh();
  57. // External references
  58. char *ftostr43sign(const float&, char);
  59. bool ubl_lcd_clicked();
  60. void home_all_axes();
  61. void gcode_G26();
  62. void gcode_G29();
  63. extern uint8_t ubl_cnt;
  64. ///////////////////////////////////////////////////////////////////////////////////////////////////////
  65. #if ENABLED(ULTRA_LCD)
  66. extern char lcd_status_message[];
  67. void lcd_quick_feedback();
  68. #endif
  69. #define MESH_X_DIST (float(UBL_MESH_MAX_X - (UBL_MESH_MIN_X)) / float(GRID_MAX_POINTS_X - 1))
  70. #define MESH_Y_DIST (float(UBL_MESH_MAX_Y - (UBL_MESH_MIN_Y)) / float(GRID_MAX_POINTS_Y - 1))
  71. typedef struct {
  72. bool active = false;
  73. float z_offset = 0.0;
  74. int8_t eeprom_storage_slot = -1;
  75. } ubl_state;
  76. class unified_bed_leveling {
  77. private:
  78. static float last_specified_z;
  79. public:
  80. void find_mean_mesh_height();
  81. void shift_mesh_height();
  82. void probe_entire_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map, const bool stow_probe, bool do_furthest);
  83. void tilt_mesh_based_on_3pts(const float &z1, const float &z2, const float &z3);
  84. void tilt_mesh_based_on_probed_grid(const bool do_ubl_mesh_map);
  85. void manually_probe_remaining_mesh(const float &lx, const float &ly, const float &z_clearance, const float &card_thickness, const bool do_ubl_mesh_map);
  86. void save_ubl_active_state_and_disable();
  87. void restore_ubl_active_state_and_leave();
  88. void g29_what_command();
  89. void g29_eeprom_dump() ;
  90. void g29_compare_current_mesh_to_stored_mesh();
  91. void fine_tune_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map);
  92. void smart_fill_mesh();
  93. void display_map(const int);
  94. void reset();
  95. void invalidate();
  96. void store_state();
  97. void load_state();
  98. void store_mesh(const int16_t);
  99. void load_mesh(const int16_t);
  100. bool sanity_check();
  101. static ubl_state state;
  102. static float z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
  103. // 15 is the maximum nubmer of grid points supported + 1 safety margin for now,
  104. // until determinism prevails
  105. constexpr static float mesh_index_to_xpos[16] PROGMEM = {
  106. UBL_MESH_MIN_X + 0 * (MESH_X_DIST), UBL_MESH_MIN_X + 1 * (MESH_X_DIST),
  107. UBL_MESH_MIN_X + 2 * (MESH_X_DIST), UBL_MESH_MIN_X + 3 * (MESH_X_DIST),
  108. UBL_MESH_MIN_X + 4 * (MESH_X_DIST), UBL_MESH_MIN_X + 5 * (MESH_X_DIST),
  109. UBL_MESH_MIN_X + 6 * (MESH_X_DIST), UBL_MESH_MIN_X + 7 * (MESH_X_DIST),
  110. UBL_MESH_MIN_X + 8 * (MESH_X_DIST), UBL_MESH_MIN_X + 9 * (MESH_X_DIST),
  111. UBL_MESH_MIN_X + 10 * (MESH_X_DIST), UBL_MESH_MIN_X + 11 * (MESH_X_DIST),
  112. UBL_MESH_MIN_X + 12 * (MESH_X_DIST), UBL_MESH_MIN_X + 13 * (MESH_X_DIST),
  113. UBL_MESH_MIN_X + 14 * (MESH_X_DIST), UBL_MESH_MIN_X + 15 * (MESH_X_DIST)
  114. };
  115. constexpr static float mesh_index_to_ypos[16] PROGMEM = {
  116. UBL_MESH_MIN_Y + 0 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 1 * (MESH_Y_DIST),
  117. UBL_MESH_MIN_Y + 2 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 3 * (MESH_Y_DIST),
  118. UBL_MESH_MIN_Y + 4 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 5 * (MESH_Y_DIST),
  119. UBL_MESH_MIN_Y + 6 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 7 * (MESH_Y_DIST),
  120. UBL_MESH_MIN_Y + 8 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 9 * (MESH_Y_DIST),
  121. UBL_MESH_MIN_Y + 10 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 11 * (MESH_Y_DIST),
  122. UBL_MESH_MIN_Y + 12 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 13 * (MESH_Y_DIST),
  123. UBL_MESH_MIN_Y + 14 * (MESH_Y_DIST), UBL_MESH_MIN_Y + 15 * (MESH_Y_DIST)
  124. };
  125. static bool g26_debug_flag, has_control_of_lcd_panel;
  126. static int16_t eeprom_start; // Please do no change this to 8 bits in size
  127. // It needs to hold values bigger than this.
  128. static volatile int encoder_diff; // Volatile because it's changed at interrupt time.
  129. unified_bed_leveling();
  130. FORCE_INLINE void set_z(const int8_t px, const int8_t py, const float &z) { z_values[px][py] = z; }
  131. int8_t get_cell_index_x(const float &x) {
  132. const int8_t cx = (x - (UBL_MESH_MIN_X)) * (1.0 / (MESH_X_DIST));
  133. return constrain(cx, 0, (GRID_MAX_POINTS_X) - 1); // -1 is appropriate if we want all movement to the X_MAX
  134. } // position. But with this defined this way, it is possible
  135. // to extrapolate off of this point even further out. Probably
  136. // that is OK because something else should be keeping that from
  137. // happening and should not be worried about at this level.
  138. int8_t get_cell_index_y(const float &y) {
  139. const int8_t cy = (y - (UBL_MESH_MIN_Y)) * (1.0 / (MESH_Y_DIST));
  140. return constrain(cy, 0, (GRID_MAX_POINTS_Y) - 1); // -1 is appropriate if we want all movement to the Y_MAX
  141. } // position. But with this defined this way, it is possible
  142. // to extrapolate off of this point even further out. Probably
  143. // that is OK because something else should be keeping that from
  144. // happening and should not be worried about at this level.
  145. int8_t find_closest_x_index(const float &x) {
  146. const int8_t px = (x - (UBL_MESH_MIN_X) + (MESH_X_DIST) * 0.5) * (1.0 / (MESH_X_DIST));
  147. return WITHIN(px, 0, GRID_MAX_POINTS_X - 1) ? px : -1;
  148. }
  149. int8_t find_closest_y_index(const float &y) {
  150. const int8_t py = (y - (UBL_MESH_MIN_Y) + (MESH_Y_DIST) * 0.5) * (1.0 / (MESH_Y_DIST));
  151. return WITHIN(py, 0, GRID_MAX_POINTS_Y - 1) ? py : -1;
  152. }
  153. /**
  154. * z2 --|
  155. * z0 | |
  156. * | | + (z2-z1)
  157. * z1 | | |
  158. * ---+-------------+--------+-- --|
  159. * a1 a0 a2
  160. * |<---delta_a---------->|
  161. *
  162. * calc_z0 is the basis for all the Mesh Based correction. It is used to
  163. * find the expected Z Height at a position between two known Z-Height locations.
  164. *
  165. * It is fairly expensive with its 4 floating point additions and 2 floating point
  166. * multiplications.
  167. */
  168. FORCE_INLINE float calc_z0(const float &a0, const float &a1, const float &z1, const float &a2, const float &z2) {
  169. return z1 + (z2 - z1) * (a0 - a1) / (a2 - a1);
  170. }
  171. /**
  172. * z_correction_for_x_on_horizontal_mesh_line is an optimization for
  173. * the rare occasion when a point lies exactly on a Mesh line (denoted by index yi).
  174. */
  175. inline float z_correction_for_x_on_horizontal_mesh_line(const float &lx0, const int x1_i, const int yi) {
  176. if (!WITHIN(x1_i, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(yi, 0, GRID_MAX_POINTS_Y - 1)) {
  177. SERIAL_ECHOPAIR("? in z_correction_for_x_on_horizontal_mesh_line(lx0=", lx0);
  178. SERIAL_ECHOPAIR(",x1_i=", x1_i);
  179. SERIAL_ECHOPAIR(",yi=", yi);
  180. SERIAL_CHAR(')');
  181. SERIAL_EOL;
  182. return NAN;
  183. }
  184. const float xratio = (RAW_X_POSITION(lx0) - pgm_read_float(&mesh_index_to_xpos[x1_i])) * (1.0 / (MESH_X_DIST)),
  185. z1 = z_values[x1_i][yi];
  186. return z1 + xratio * (z_values[x1_i + 1][yi] - z1);
  187. }
  188. //
  189. // See comments above for z_correction_for_x_on_horizontal_mesh_line
  190. //
  191. inline float z_correction_for_y_on_vertical_mesh_line(const float &ly0, const int xi, const int y1_i) {
  192. if (!WITHIN(xi, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(y1_i, 0, GRID_MAX_POINTS_Y - 1)) {
  193. SERIAL_ECHOPAIR("? in get_z_correction_along_vertical_mesh_line_at_specific_x(ly0=", ly0);
  194. SERIAL_ECHOPAIR(", x1_i=", xi);
  195. SERIAL_ECHOPAIR(", yi=", y1_i);
  196. SERIAL_CHAR(')');
  197. SERIAL_EOL;
  198. return NAN;
  199. }
  200. const float yratio = (RAW_Y_POSITION(ly0) - pgm_read_float(&mesh_index_to_ypos[y1_i])) * (1.0 / (MESH_Y_DIST)),
  201. z1 = z_values[xi][y1_i];
  202. return z1 + yratio * (z_values[xi][y1_i + 1] - z1);
  203. }
  204. /**
  205. * This is the generic Z-Correction. It works anywhere within a Mesh Cell. It first
  206. * does a linear interpolation along both of the bounding X-Mesh-Lines to find the
  207. * Z-Height at both ends. Then it does a linear interpolation of these heights based
  208. * on the Y position within the cell.
  209. */
  210. float get_z_correction(const float &lx0, const float &ly0) {
  211. const int8_t cx = get_cell_index_x(RAW_X_POSITION(lx0)),
  212. cy = get_cell_index_y(RAW_Y_POSITION(ly0));
  213. if (!WITHIN(cx, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(cy, 0, GRID_MAX_POINTS_Y - 1)) {
  214. SERIAL_ECHOPAIR("? in get_z_correction(lx0=", lx0);
  215. SERIAL_ECHOPAIR(", ly0=", ly0);
  216. SERIAL_CHAR(')');
  217. SERIAL_EOL;
  218. #if ENABLED(ULTRA_LCD)
  219. strcpy(lcd_status_message, "get_z_correction() indexes out of range.");
  220. lcd_quick_feedback();
  221. #endif
  222. return 0.0; // this used to return state.z_offset
  223. }
  224. const float z1 = calc_z0(RAW_X_POSITION(lx0),
  225. pgm_read_float(&mesh_index_to_xpos[cx]), z_values[cx][cy],
  226. pgm_read_float(&mesh_index_to_xpos[cx + 1]), z_values[cx + 1][cy]);
  227. const float z2 = calc_z0(RAW_X_POSITION(lx0),
  228. pgm_read_float(&mesh_index_to_xpos[cx]), z_values[cx][cy + 1],
  229. pgm_read_float(&mesh_index_to_xpos[cx + 1]), z_values[cx + 1][cy + 1]);
  230. float z0 = calc_z0(RAW_Y_POSITION(ly0),
  231. pgm_read_float(&mesh_index_to_ypos[cy]), z1,
  232. pgm_read_float(&mesh_index_to_ypos[cy + 1]), z2);
  233. #if ENABLED(DEBUG_LEVELING_FEATURE)
  234. if (DEBUGGING(MESH_ADJUST)) {
  235. SERIAL_ECHOPAIR(" raw get_z_correction(", lx0);
  236. SERIAL_CHAR(',');
  237. SERIAL_ECHO(ly0);
  238. SERIAL_ECHOPGM(") = ");
  239. SERIAL_ECHO_F(z0, 6);
  240. }
  241. #endif
  242. #if ENABLED(DEBUG_LEVELING_FEATURE)
  243. if (DEBUGGING(MESH_ADJUST)) {
  244. SERIAL_ECHOPGM(" >>>---> ");
  245. SERIAL_ECHO_F(z0, 6);
  246. SERIAL_EOL;
  247. }
  248. #endif
  249. if (isnan(z0)) { // if part of the Mesh is undefined, it will show up as NAN
  250. z0 = 0.0; // in ubl.z_values[][] and propagate through the
  251. // calculations. If our correction is NAN, we throw it out
  252. // because part of the Mesh is undefined and we don't have the
  253. // information we need to complete the height correction.
  254. #if ENABLED(DEBUG_LEVELING_FEATURE)
  255. if (DEBUGGING(MESH_ADJUST)) {
  256. SERIAL_ECHOPAIR("??? Yikes! NAN in get_z_correction(", lx0);
  257. SERIAL_CHAR(',');
  258. SERIAL_ECHO(ly0);
  259. SERIAL_CHAR(')');
  260. SERIAL_EOL;
  261. }
  262. #endif
  263. }
  264. return z0; // there used to be a +state.z_offset on this line
  265. }
  266. /**
  267. * This function sets the Z leveling fade factor based on the given Z height,
  268. * only re-calculating when necessary.
  269. *
  270. * Returns 1.0 if planner.z_fade_height is 0.0.
  271. * Returns 0.0 if Z is past the specified 'Fade Height'.
  272. */
  273. #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
  274. FORCE_INLINE float fade_scaling_factor_for_z(const float &lz) {
  275. if (planner.z_fade_height == 0.0) return 1.0;
  276. static float fade_scaling_factor = 1.0;
  277. const float rz = RAW_Z_POSITION(lz);
  278. if (last_specified_z != rz) {
  279. last_specified_z = rz;
  280. fade_scaling_factor =
  281. rz < planner.z_fade_height
  282. ? 1.0 - (rz * planner.inverse_z_fade_height)
  283. : 0.0;
  284. }
  285. return fade_scaling_factor;
  286. }
  287. #endif
  288. }; // class unified_bed_leveling
  289. extern unified_bed_leveling ubl;
  290. #define UBL_LAST_EEPROM_INDEX E2END
  291. #endif // AUTO_BED_LEVELING_UBL
  292. #endif // UNIFIED_BED_LEVELING_H