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_G29.cpp 71KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284128512861287128812891290129112921293129412951296129712981299130013011302130313041305130613071308130913101311131213131314131513161317131813191320132113221323132413251326132713281329133013311332133313341335133613371338133913401341134213431344134513461347134813491350135113521353135413551356135713581359136013611362136313641365136613671368136913701371137213731374137513761377137813791380138113821383138413851386138713881389139013911392139313941395139613971398139914001401140214031404140514061407140814091410141114121413141414151416141714181419142014211422142314241425142614271428142914301431143214331434143514361437143814391440144114421443144414451446144714481449145014511452145314541455145614571458145914601461146214631464146514661467146814691470147114721473147414751476147714781479148014811482148314841485148614871488148914901491149214931494149514961497149814991500150115021503150415051506150715081509151015111512151315141515151615171518151915201521152215231524152515261527152815291530153115321533153415351536153715381539154015411542154315441545154615471548154915501551155215531554155515561557155815591560156115621563156415651566156715681569157015711572157315741575157615771578157915801581158215831584158515861587158815891590159115921593159415951596159715981599160016011602160316041605160616071608160916101611161216131614161516161617161816191620162116221623162416251626162716281629163016311632163316341635163616371638163916401641164216431644164516461647164816491650165116521653165416551656165716581659166016611662166316641665166616671668166916701671167216731674167516761677
  1. /**
  2. * Marlin 3D Printer Firmware
  3. * Copyright (C) 2016 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. #include "MarlinConfig.h"
  23. #if ENABLED(AUTO_BED_LEVELING_UBL)
  24. //#include "vector_3.h"
  25. //#include "qr_solve.h"
  26. #include "ubl.h"
  27. #include "Marlin.h"
  28. #include "hex_print_routines.h"
  29. #include "configuration_store.h"
  30. #include "ultralcd.h"
  31. #include "stepper.h"
  32. #include <math.h>
  33. #include "least_squares_fit.h"
  34. extern float destination[XYZE];
  35. extern float current_position[XYZE];
  36. void lcd_return_to_status();
  37. bool lcd_clicked();
  38. void lcd_implementation_clear();
  39. void lcd_mesh_edit_setup(float initial);
  40. float lcd_mesh_edit();
  41. void lcd_z_offset_edit_setup(float);
  42. float lcd_z_offset_edit();
  43. extern float meshedit_done;
  44. extern long babysteps_done;
  45. extern float code_value_float();
  46. extern uint8_t code_value_byte();
  47. extern bool code_value_bool();
  48. extern bool code_has_value();
  49. extern float probe_pt(float x, float y, bool, int);
  50. extern bool set_probe_deployed(bool);
  51. void smart_fill_mesh();
  52. float measure_business_card_thickness(float &in_height);
  53. void manually_probe_remaining_mesh(const float&, const float&, const float&, const float&, const bool);
  54. bool ProbeStay = true;
  55. #define SIZE_OF_LITTLE_RAISE 1
  56. #define BIG_RAISE_NOT_NEEDED 0
  57. extern void lcd_status_screen();
  58. typedef void (*screenFunc_t)();
  59. extern void lcd_goto_screen(screenFunc_t screen, const uint32_t encoder = 0);
  60. /**
  61. * G29: Unified Bed Leveling by Roxy
  62. *
  63. * Parameters understood by this leveling system:
  64. *
  65. * A Activate Activate the Unified Bed Leveling system.
  66. *
  67. * B # Business Use the 'Business Card' mode of the Manual Probe subsystem. This is invoked as
  68. * G29 P2 B. The mode of G29 P2 allows you to use a business card or recipe card
  69. * as a shim that the nozzle will pinch as it is lowered. The idea is that you
  70. * can easily feel the nozzle getting to the same height by the amount of resistance
  71. * the business card exhibits to movement. You should try to achieve the same amount
  72. * of resistance on each probed point to facilitate accurate and repeatable measurements.
  73. * You should be very careful not to drive the nozzle into the business card with a
  74. * lot of force as it is very possible to cause damage to your printer if your are
  75. * careless. If you use the B option with G29 P2 B you can omit the numeric value
  76. * on first use to measure the business card's thickness. Subsequent usage of 'B'
  77. * will apply the previously-measured thickness as the default.
  78. * Note: A non-compressible Spark Gap feeler gauge is recommended over a Business Card.
  79. *
  80. * C Continue Continue, Constant, Current Location. This is not a primary command. C is used to
  81. * further refine the behaviour of several other commands. Issuing a G29 P1 C will
  82. * continue the generation of a partially constructed Mesh without invalidating what has
  83. * been done. Issuing a G29 P2 C will tell the Manual Probe subsystem to use the current
  84. * location in its search for the closest unmeasured Mesh Point. When used with a G29 Z C
  85. * it indicates to use the current location instead of defaulting to the center of the print bed.
  86. *
  87. * D Disable Disable the Unified Bed Leveling system.
  88. *
  89. * E Stow_probe Stow the probe after each sampled point.
  90. *
  91. * F # Fade Fade the amount of Mesh Based Compensation over a specified height. At the
  92. * specified height, no correction is applied and natural printer kenimatics take over. If no
  93. * number is specified for the command, 10mm is assumed to be reasonable.
  94. *
  95. * H # Height Specify the Height to raise the nozzle after each manual probe of the bed. The
  96. * default is 5mm.
  97. *
  98. * I # Invalidate Invalidate specified number of Mesh Points. The nozzle location is used unless
  99. * the X and Y parameter are used. If no number is specified, only the closest Mesh
  100. * point to the location is invalidated. The 'T' parameter is also available to produce
  101. * a map after the operation. This command is useful to invalidate a portion of the
  102. * Mesh so it can be adjusted using other tools in the Unified Bed Leveling System. When
  103. * attempting to invalidate an isolated bad point in the mesh, the 'T' option will indicate
  104. * where the nozzle is positioned in the Mesh with (#). You can move the nozzle around on
  105. * the bed and use this feature to select the center of the area (or cell) you want to
  106. * invalidate.
  107. *
  108. * J # Grid Perform a Grid Based Leveling of the current Mesh using a grid with n points on a side.
  109. * Not specifying a grid size will invoke the 3-Point leveling function.
  110. *
  111. * K # Kompare Kompare current Mesh with stored Mesh # replacing current Mesh with the result. This
  112. * command literally performs a diff between two Meshes.
  113. *
  114. * L Load Load Mesh from the previously activated location in the EEPROM.
  115. *
  116. * L # Load Load Mesh from the specified location in the EEPROM. Set this location as activated
  117. * for subsequent Load and Store operations.
  118. *
  119. * The P or Phase commands are used for the bulk of the work to setup a Mesh. In general, your Mesh will
  120. * start off being initialized with a G29 P0 or a G29 P1. Further refinement of the Mesh happens with
  121. * each additional Phase that processes it.
  122. *
  123. * P0 Phase 0 Zero Mesh Data and turn off the Mesh Compensation System. This reverts the
  124. * 3D Printer to the same state it was in before the Unified Bed Leveling Compensation
  125. * was turned on. Setting the entire Mesh to Zero is a special case that allows
  126. * a subsequent G or T leveling operation for backward compatibility.
  127. *
  128. * P1 Phase 1 Invalidate entire Mesh and continue with automatic generation of the Mesh data using
  129. * the Z-Probe. Usually the probe can not reach all areas that the nozzle can reach.
  130. * In Cartesian printers, mesh points within the X_OFFSET_FROM_EXTRUDER and Y_OFFSET_FROM_EXTRUDER
  131. * area can not be automatically probed. For Delta printers the area in which DELTA_PROBEABLE_RADIUS
  132. * and DELTA_PRINTABLE_RADIUS do not overlap will not be automatically probed.
  133. *
  134. * These points will be handled in Phase 2 and Phase 3. If the Phase 1 command is given the
  135. * C (Continue) parameter it does not invalidate the Mesh prior to automatically
  136. * probing needed locations. This allows you to invalidate portions of the Mesh but still
  137. * use the automatic probing capabilities of the Unified Bed Leveling System. An X and Y
  138. * parameter can be given to prioritize where the command should be trying to measure points.
  139. * If the X and Y parameters are not specified the current probe position is used.
  140. * P1 accepts a 'T' (Topology) parameter so you can observe mesh generation.
  141. * P1 also watches for the LCD Panel Encoder Switch to be held down, and will suspend
  142. * generation of the Mesh in that case. (Note: This check is only done between probe points,
  143. * so you must press and hold the switch until the Phase 1 command detects it.)
  144. *
  145. * P2 Phase 2 Probe areas of the Mesh that can't be automatically handled. Phase 2 respects an H
  146. * parameter to control the height between Mesh points. The default height for movement
  147. * between Mesh points is 5mm. A smaller number can be used to make this part of the
  148. * calibration less time consuming. You will be running the nozzle down until it just barely
  149. * touches the glass. You should have the nozzle clean with no plastic obstructing your view.
  150. * Use caution and move slowly. It is possible to damage your printer if you are careless.
  151. * Note that this command will use the configuration #define SIZE_OF_LITTLE_RAISE if the
  152. * nozzle is moving a distance of less than BIG_RAISE_NOT_NEEDED.
  153. *
  154. * The H parameter can be set negative if your Mesh dips in a large area. You can press
  155. * and hold the LCD Panel's encoder wheel to terminate the current Phase 2 command. You
  156. * can then re-issue the G29 P 2 command with an H parameter that is more suitable for the
  157. * area you are manually probing. Note that the command tries to start you in a corner
  158. * of the bed where movement will be predictable. You can force the location to be used in
  159. * the distance calculations by using the X and Y parameters. You may find it is helpful to
  160. * print out a Mesh Map (G29 T) to understand where the mesh is invalidated and where
  161. * the nozzle will need to move in order to complete the command. The C parameter is
  162. * available on the Phase 2 command also and indicates the search for points to measure should
  163. * be done based on the current location of the nozzle.
  164. *
  165. * A B parameter is also available for this command and described up above. It places the
  166. * manual probe subsystem into Business Card mode where the thickness of a business card is
  167. * measured and then used to accurately set the nozzle height in all manual probing for the
  168. * duration of the command. (S for Shim mode would be a better parameter name, but S is needed
  169. * for Save or Store of the Mesh to EEPROM) A Business card can be used, but you will have
  170. * better results if you use a flexible Shim that does not compress very much. That makes it
  171. * easier for you to get the nozzle to press with similar amounts of force against the shim so you
  172. * can get accurate measurements. As you are starting to touch the nozzle against the shim try
  173. * to get it to grasp the shim with the same force as when you measured the thickness of the
  174. * shim at the start of the command.
  175. *
  176. * Phase 2 allows the T (Map) parameter to be specified. This helps the user see the progression
  177. * of the Mesh being built.
  178. *
  179. * P3 Phase 3 Fill the unpopulated regions of the Mesh with a fixed value. There are two different paths the
  180. * user can go down. If the user specifies the value using the C parameter, the closest invalid
  181. * mesh points to the nozzle will be filled. The user can specify a repeat count using the R
  182. * parameter with the C version of the command.
  183. *
  184. * A second version of the fill command is available if no C constant is specified. Not
  185. * specifying a C constant will invoke the 'Smart Fill' algorithm. The G29 P3 command will search
  186. * from the edges of the mesh inward looking for invalid mesh points. It will look at the next
  187. * several mesh points to determine if the print bed is sloped up or down. If the bed is sloped
  188. * upward from the invalid mesh point, it will be replaced with the value of the nearest mesh point.
  189. * If the bed is sloped downward from the invalid mesh point, it will be replaced with a value that
  190. * puts all three points in a line. The second version of the G29 P3 command is a quick, easy and
  191. * usually safe way to populate the unprobed regions of your mesh so you can continue to the G26
  192. * Mesh Validation Pattern phase. Please note that you are populating your mesh with unverified
  193. * numbers. You should use some scrutiny and caution.
  194. *
  195. * P4 Phase 4 Fine tune the Mesh. The Delta Mesh Compensation System assume the existence of
  196. * an LCD Panel. It is possible to fine tune the mesh without the use of an LCD Panel.
  197. * (More work and details on doing this later!)
  198. * The System will search for the closest Mesh Point to the nozzle. It will move the
  199. * nozzle to this location. The user can use the LCD Panel to carefully adjust the nozzle
  200. * so it is just barely touching the bed. When the user clicks the control, the System
  201. * will lock in that height for that point in the Mesh Compensation System.
  202. *
  203. * Phase 4 has several additional parameters that the user may find helpful. Phase 4
  204. * can be started at a specific location by specifying an X and Y parameter. Phase 4
  205. * can be requested to continue the adjustment of Mesh Points by using the R(epeat)
  206. * parameter. If the Repetition count is not specified, it is assumed the user wishes
  207. * to adjust the entire matrix. The nozzle is moved to the Mesh Point being edited.
  208. * The command can be terminated early (or after the area of interest has been edited) by
  209. * pressing and holding the encoder wheel until the system recognizes the exit request.
  210. * Phase 4's general form is G29 P4 [R # of points] [X position] [Y position]
  211. *
  212. * Phase 4 is intended to be used with the G26 Mesh Validation Command. Using the
  213. * information left on the printer's bed from the G26 command it is very straight forward
  214. * and easy to fine tune the Mesh. One concept that is important to remember and that
  215. * will make using the Phase 4 command easy to use is this: You are editing the Mesh Points.
  216. * If you have too little clearance and not much plastic was extruded in an area, you want to
  217. * LOWER the Mesh Point at the location. If you did not get good adheasion, you want to
  218. * RAISE the Mesh Point at that location.
  219. *
  220. *
  221. * P5 Phase 5 Find Mean Mesh Height and Standard Deviation. Typically, it is easier to use and
  222. * work with the Mesh if it is Mean Adjusted. You can specify a C parameter to
  223. * Correct the Mesh to a 0.00 Mean Height. Adding a C parameter will automatically
  224. * execute a G29 P6 C <mean height>.
  225. *
  226. * P6 Phase 6 Shift Mesh height. The entire Mesh's height is adjusted by the height specified
  227. * with the C parameter. Being able to adjust the height of a Mesh is useful tool. It
  228. * can be used to compensate for poorly calibrated Z-Probes and other errors. Ideally,
  229. * you should have the Mesh adjusted for a Mean Height of 0.00 and the Z-Probe measuring
  230. * 0.000 at the Z Home location.
  231. *
  232. * Q Test Load specified Test Pattern to assist in checking correct operation of system. This
  233. * command is not anticipated to be of much value to the typical user. It is intended
  234. * for developers to help them verify correct operation of the Unified Bed Leveling System.
  235. *
  236. * R # Repeat Repeat this command the specified number of times. If no number is specified the
  237. * command will be repeated GRID_MAX_POINTS_X * GRID_MAX_POINTS_Y times.
  238. *
  239. * S Store Store the current Mesh in the Activated area of the EEPROM. It will also store the
  240. * current state of the Unified Bed Leveling system in the EEPROM.
  241. *
  242. * S # Store Store the current Mesh at the specified location in EEPROM. Activate this location
  243. * for subsequent Load and Store operations. Valid storage slot numbers begin at 0 and
  244. * extend to a limit related to the available EEPROM storage.
  245. *
  246. * S -1 Store Store the current Mesh as a print out that is suitable to be feed back into the system
  247. * at a later date. The GCode output can be saved and later replayed by the host software
  248. * to reconstruct the current mesh on another machine.
  249. *
  250. * T Topology Display the Mesh Map Topology.
  251. * 'T' can be used alone (e.g., G29 T) or in combination with most of the other commands.
  252. * This option works with all Phase commands (e.g., G29 P4 R 5 T X 50 Y100 C -.1 O)
  253. * This parameter can also specify a Map Type. T0 (the default) is user-readable. T1 can
  254. * is suitable to paste into a spreadsheet for a 3D graph of the mesh.
  255. *
  256. * U Unlevel Perform a probe of the outer perimeter to assist in physically leveling unlevel beds.
  257. * Only used for G29 P1 T U. This speeds up the probing of the edge of the bed. Useful
  258. * when the entire bed doesn't need to be probed because it will be adjusted.
  259. *
  260. * V # Verbosity Set the verbosity level (0-4) for extra details. (Default 0)
  261. *
  262. * W What? Display valuable Unified Bed Leveling System data.
  263. *
  264. * X # X Location for this command
  265. *
  266. * Y # Y Location for this command
  267. *
  268. *
  269. * Release Notes:
  270. * You MUST do M502, M500 to initialize the storage. Failure to do this will cause all
  271. * kinds of problems. Enabling EEPROM Storage is highly recommended. With EEPROM Storage
  272. * of the mesh, you are limited to 3-Point and Grid Leveling. (G29 P0 T and G29 P0 G
  273. * respectively.)
  274. *
  275. * When you do a G28 and then a G29 P1 to automatically build your first mesh, you are going to notice
  276. * the Unified Bed Leveling probes points further and further away from the starting location. (The
  277. * starting location defaults to the center of the bed.) The original Grid and Mesh leveling used
  278. * a Zig Zag pattern. The new pattern is better, especially for people with Delta printers. This
  279. * allows you to get the center area of the Mesh populated (and edited) quicker. This allows you to
  280. * perform a small print and check out your settings quicker. You do not need to populate the
  281. * entire mesh to use it. (You don't want to spend a lot of time generating a mesh only to realize
  282. * you don't have the resolution or zprobe_zoffset set correctly. The Mesh generation
  283. * gathers points closest to where the nozzle is located unless you specify an (X,Y) coordinate pair.
  284. *
  285. * The Unified Bed Leveling uses a lot of EEPROM storage to hold its data. And it takes some effort
  286. * to get this Mesh data correct for a user's printer. We do not want this data destroyed as
  287. * new versions of Marlin add or subtract to the items stored in EEPROM. So, for the benefit of
  288. * the users, we store the Mesh data at the end of the EEPROM and do not keep it contiguous with the
  289. * other data stored in the EEPROM. (For sure the developers are going to complain about this, but
  290. * this is going to be helpful to the users!)
  291. *
  292. * The foundation of this Bed Leveling System is built on Epatel's Mesh Bed Leveling code. A big
  293. * 'Thanks!' to him and the creators of 3-Point and Grid Based leveling. Combining their contributions
  294. * we now have the functionality and features of all three systems combined.
  295. */
  296. // The simple parameter flags and values are 'static' so parameter parsing can be in a support routine.
  297. static int g29_verbose_level, phase_value, repetition_cnt,
  298. storage_slot = 0, map_type, grid_size;
  299. static bool repeat_flag, c_flag, x_flag, y_flag;
  300. static float x_pos, y_pos, measured_z, card_thickness = 0.0, ubl_constant = 0.0;
  301. extern void lcd_setstatus(const char* message, const bool persist);
  302. extern void lcd_setstatuspgm(const char* message, const uint8_t level);
  303. void __attribute__((optimize("O0"))) gcode_G29() {
  304. if (!settings.calc_num_meshes()) {
  305. SERIAL_PROTOCOLLNPGM("?You need to enable your EEPROM and initialize it");
  306. SERIAL_PROTOCOLLNPGM("with M502, M500, M501 in that order.\n");
  307. return;
  308. }
  309. // Check for commands that require the printer to be homed.
  310. if (axis_unhomed_error()) {
  311. if (code_seen('J'))
  312. home_all_axes();
  313. else if (code_seen('P')) {
  314. if (code_has_value()) {
  315. const int p_val = code_value_int();
  316. if (p_val == 1 || p_val == 2 || p_val == 4)
  317. home_all_axes();
  318. }
  319. }
  320. }
  321. if (g29_parameter_parsing()) return; // abort if parsing the simple parameters causes a problem,
  322. // Invalidate Mesh Points. This command is a little bit asymetrical because
  323. // it directly specifies the repetition count and does not use the 'R' parameter.
  324. if (code_seen('I')) {
  325. uint8_t cnt = 0;
  326. repetition_cnt = code_has_value() ? code_value_int() : 1;
  327. while (repetition_cnt--) {
  328. if (cnt > 20) { cnt = 0; idle(); }
  329. const mesh_index_pair location = find_closest_mesh_point_of_type(REAL, x_pos, y_pos, USE_NOZZLE_AS_REFERENCE, NULL, false);
  330. if (location.x_index < 0) {
  331. SERIAL_PROTOCOLLNPGM("Entire Mesh invalidated.\n");
  332. break; // No more invalid Mesh Points to populate
  333. }
  334. ubl.z_values[location.x_index][location.y_index] = NAN;
  335. cnt++;
  336. }
  337. SERIAL_PROTOCOLLNPGM("Locations invalidated.\n");
  338. }
  339. if (code_seen('Q')) {
  340. const int test_pattern = code_has_value() ? code_value_int() : -99;
  341. if (!WITHIN(test_pattern, -1, 2)) {
  342. SERIAL_PROTOCOLLNPGM("Invalid test_pattern value. (0-2)\n");
  343. return;
  344. }
  345. SERIAL_PROTOCOLLNPGM("Loading test_pattern values.\n");
  346. switch (test_pattern) {
  347. case -1:
  348. g29_eeprom_dump();
  349. break;
  350. case 0:
  351. for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++) { // Create a bowl shape - similar to
  352. for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++) { // a poorly calibrated Delta.
  353. const float p1 = 0.5 * (GRID_MAX_POINTS_X) - x,
  354. p2 = 0.5 * (GRID_MAX_POINTS_Y) - y;
  355. ubl.z_values[x][y] += 2.0 * HYPOT(p1, p2);
  356. }
  357. }
  358. break;
  359. case 1:
  360. for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++) { // Create a diagonal line several Mesh cells thick that is raised
  361. ubl.z_values[x][x] += 9.999;
  362. ubl.z_values[x][x + (x < GRID_MAX_POINTS_Y - 1) ? 1 : -1] += 9.999; // We want the altered line several mesh points thick
  363. }
  364. break;
  365. case 2:
  366. // Allow the user to specify the height because 10mm is a little extreme in some cases.
  367. for (uint8_t x = (GRID_MAX_POINTS_X) / 3; x < 2 * (GRID_MAX_POINTS_X) / 3; x++) // Create a rectangular raised area in
  368. for (uint8_t y = (GRID_MAX_POINTS_Y) / 3; y < 2 * (GRID_MAX_POINTS_Y) / 3; y++) // the center of the bed
  369. ubl.z_values[x][y] += code_seen('C') ? ubl_constant : 9.99;
  370. break;
  371. }
  372. }
  373. if (code_seen('J')) {
  374. if (grid_size) { // if not 0 it is a normal n x n grid being probed
  375. ubl.save_ubl_active_state_and_disable();
  376. ubl.tilt_mesh_based_on_probed_grid(code_seen('T'));
  377. ubl.restore_ubl_active_state_and_leave();
  378. } else { // grid_size==0 which means a 3-Point leveling has been requested
  379. float z1 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_1_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_1_Y), false, g29_verbose_level),
  380. z2 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_2_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_2_Y), false, g29_verbose_level),
  381. z3 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_3_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_3_Y), true, g29_verbose_level);
  382. if ( isnan(z1) || isnan(z2) || isnan(z3)) { // probe_pt will return NAN if unreachable
  383. SERIAL_ERROR_START;
  384. SERIAL_ERRORLNPGM("Attempt to probe off the bed.");
  385. goto LEAVE;
  386. }
  387. // We need to adjust z1, z2, z3 by the Mesh Height at these points. Just because they are non-zero doesn't mean
  388. // the Mesh is tilted! (We need to compensate each probe point by what the Mesh says that location's height is)
  389. ubl.save_ubl_active_state_and_disable();
  390. z1 -= ubl.get_z_correction(LOGICAL_X_POSITION(UBL_PROBE_PT_1_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_1_Y)) /* + zprobe_zoffset */ ;
  391. z2 -= ubl.get_z_correction(LOGICAL_X_POSITION(UBL_PROBE_PT_2_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_2_Y)) /* + zprobe_zoffset */ ;
  392. z3 -= ubl.get_z_correction(LOGICAL_X_POSITION(UBL_PROBE_PT_3_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_3_Y)) /* + zprobe_zoffset */ ;
  393. do_blocking_move_to_xy(0.5 * (UBL_MESH_MAX_X - (UBL_MESH_MIN_X)), 0.5 * (UBL_MESH_MAX_Y - (UBL_MESH_MIN_Y)));
  394. ubl.tilt_mesh_based_on_3pts(z1, z2, z3);
  395. ubl.restore_ubl_active_state_and_leave();
  396. }
  397. }
  398. if (code_seen('P')) {
  399. if (WITHIN(phase_value, 0, 1) && ubl.state.storage_slot == -1) {
  400. ubl.state.storage_slot = 0;
  401. SERIAL_PROTOCOLLNPGM("Default storage slot 0 selected.");
  402. }
  403. switch (phase_value) {
  404. case 0:
  405. //
  406. // Zero Mesh Data
  407. //
  408. ubl.reset();
  409. SERIAL_PROTOCOLLNPGM("Mesh zeroed.");
  410. break;
  411. case 1:
  412. //
  413. // Invalidate Entire Mesh and Automatically Probe Mesh in areas that can be reached by the probe
  414. //
  415. if (!code_seen('C')) {
  416. ubl.invalidate();
  417. SERIAL_PROTOCOLLNPGM("Mesh invalidated. Probing mesh.");
  418. }
  419. if (g29_verbose_level > 1) {
  420. SERIAL_PROTOCOLPAIR("Probing Mesh Points Closest to (", x_pos);
  421. SERIAL_PROTOCOLCHAR(',');
  422. SERIAL_PROTOCOL(y_pos);
  423. SERIAL_PROTOCOLLNPGM(").\n");
  424. }
  425. ubl.probe_entire_mesh(x_pos + X_PROBE_OFFSET_FROM_EXTRUDER, y_pos + Y_PROBE_OFFSET_FROM_EXTRUDER,
  426. code_seen('T'), code_seen('E'), code_seen('U'));
  427. break;
  428. case 2: {
  429. //
  430. // Manually Probe Mesh in areas that can't be reached by the probe
  431. //
  432. SERIAL_PROTOCOLLNPGM("Manually probing unreachable mesh locations.");
  433. do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES);
  434. if (!x_flag && !y_flag) {
  435. /**
  436. * Use a good default location for the path.
  437. * The flipped > and < operators in these comparisons is intentional.
  438. * It should cause the probed points to follow a nice path on Cartesian printers.
  439. * It may make sense to have Delta printers default to the center of the bed.
  440. * Until that is decided, this can be forced with the X and Y parameters.
  441. */
  442. #if IS_KINEMATIC
  443. x_pos = X_HOME_POS;
  444. y_pos = Y_HOME_POS;
  445. #else // cartesian
  446. x_pos = X_PROBE_OFFSET_FROM_EXTRUDER > 0 ? X_MAX_POS : X_MIN_POS;
  447. y_pos = Y_PROBE_OFFSET_FROM_EXTRUDER < 0 ? Y_MAX_POS : Y_MIN_POS;
  448. #endif
  449. }
  450. if (code_seen('C')) {
  451. x_pos = current_position[X_AXIS];
  452. y_pos = current_position[Y_AXIS];
  453. }
  454. float height = Z_CLEARANCE_BETWEEN_PROBES;
  455. if (code_seen('B')) {
  456. card_thickness = code_has_value() ? code_value_float() : measure_business_card_thickness(height);
  457. if (fabs(card_thickness) > 1.5) {
  458. SERIAL_PROTOCOLLNPGM("?Error in Business Card measurement.");
  459. return;
  460. }
  461. }
  462. if (code_seen('H') && code_has_value()) height = code_value_float();
  463. if ( !position_is_reachable_xy( x_pos, y_pos )) {
  464. SERIAL_PROTOCOLLNPGM("(X,Y) outside printable radius.");
  465. return;
  466. }
  467. manually_probe_remaining_mesh(x_pos, y_pos, height, card_thickness, code_seen('T'));
  468. SERIAL_PROTOCOLLNPGM("G29 P2 finished.");
  469. } break;
  470. case 3: {
  471. /**
  472. * Populate invalid mesh areas. Proceed with caution.
  473. * Two choices are available:
  474. * - Specify a constant with the 'C' parameter.
  475. * - Allow 'G29 P3' to choose a 'reasonable' constant.
  476. */
  477. if (c_flag) {
  478. if (repetition_cnt >= GRID_MAX_POINTS) {
  479. for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++) {
  480. for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++) {
  481. ubl.z_values[x][y] = ubl_constant;
  482. }
  483. }
  484. }
  485. else {
  486. while (repetition_cnt--) { // this only populates reachable mesh points near
  487. const mesh_index_pair location = find_closest_mesh_point_of_type(INVALID, x_pos, y_pos, USE_NOZZLE_AS_REFERENCE, NULL, false);
  488. if (location.x_index < 0) break; // No more reachable invalid Mesh Points to populate
  489. ubl.z_values[location.x_index][location.y_index] = ubl_constant;
  490. }
  491. }
  492. } else {
  493. smart_fill_mesh(); // Do a 'Smart' fill using nearby known values
  494. }
  495. break;
  496. }
  497. case 4:
  498. //
  499. // Fine Tune (i.e., Edit) the Mesh
  500. //
  501. fine_tune_mesh(x_pos, y_pos, code_seen('T'));
  502. break;
  503. case 5: ubl.find_mean_mesh_height(); break;
  504. case 6: ubl.shift_mesh_height(); break;
  505. }
  506. }
  507. //
  508. // Much of the 'What?' command can be eliminated. But until we are fully debugged, it is
  509. // good to have the extra information. Soon... we prune this to just a few items
  510. //
  511. if (code_seen('W')) ubl.g29_what_command();
  512. //
  513. // When we are fully debugged, this may go away. But there are some valid
  514. // use cases for the users. So we can wait and see what to do with it.
  515. //
  516. if (code_seen('K')) // Kompare Current Mesh Data to Specified Stored Mesh
  517. g29_compare_current_mesh_to_stored_mesh();
  518. //
  519. // Load a Mesh from the EEPROM
  520. //
  521. if (code_seen('L')) { // Load Current Mesh Data
  522. storage_slot = code_has_value() ? code_value_int() : ubl.state.storage_slot;
  523. int16_t a = settings.calc_num_meshes();
  524. if (!a) {
  525. SERIAL_PROTOCOLLNPGM("?EEPROM storage not available.");
  526. return;
  527. }
  528. if (!WITHIN(storage_slot, 0, a - 1)) {
  529. SERIAL_PROTOCOLLNPGM("?Invalid storage slot.");
  530. SERIAL_PROTOCOLLNPAIR("?Use 0 to ", a - 1);
  531. return;
  532. }
  533. settings.load_mesh(storage_slot);
  534. ubl.state.storage_slot = storage_slot;
  535. SERIAL_PROTOCOLLNPGM("Done.");
  536. }
  537. //
  538. // Store a Mesh in the EEPROM
  539. //
  540. if (code_seen('S')) { // Store (or Save) Current Mesh Data
  541. storage_slot = code_has_value() ? code_value_int() : ubl.state.storage_slot;
  542. if (storage_slot == -1) { // Special case, we are going to 'Export' the mesh to the
  543. SERIAL_ECHOLNPGM("G29 I 999"); // host in a form it can be reconstructed on a different machine
  544. for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
  545. for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++)
  546. if (!isnan(ubl.z_values[x][y])) {
  547. SERIAL_ECHOPAIR("M421 I ", x);
  548. SERIAL_ECHOPAIR(" J ", y);
  549. SERIAL_ECHOPGM(" Z ");
  550. SERIAL_ECHO_F(ubl.z_values[x][y], 6);
  551. SERIAL_ECHOPAIR(" ; X ", LOGICAL_X_POSITION(pgm_read_float(&ubl.mesh_index_to_xpos[x])));
  552. SERIAL_ECHOPAIR(", Y ", LOGICAL_Y_POSITION(pgm_read_float(&ubl.mesh_index_to_ypos[y])));
  553. SERIAL_EOL;
  554. }
  555. return;
  556. }
  557. int16_t a = settings.calc_num_meshes();
  558. if (!a) {
  559. SERIAL_PROTOCOLLNPGM("?EEPROM storage not available.");
  560. goto LEAVE;
  561. }
  562. if (!WITHIN(storage_slot, 0, a - 1)) {
  563. SERIAL_PROTOCOLLNPGM("?Invalid storage slot.");
  564. SERIAL_PROTOCOLLNPAIR("?Use 0 to ", a - 1);
  565. goto LEAVE;
  566. }
  567. settings.store_mesh(storage_slot);
  568. ubl.state.storage_slot = storage_slot;
  569. SERIAL_PROTOCOLLNPGM("Done.");
  570. }
  571. if (code_seen('T'))
  572. ubl.display_map(code_has_value() ? code_value_int() : 0);
  573. /*
  574. * This code may not be needed... Prepare for its removal...
  575. *
  576. if (code_seen('Z')) {
  577. if (code_has_value())
  578. ubl.state.z_offset = code_value_float(); // do the simple case. Just lock in the specified value
  579. else {
  580. ubl.save_ubl_active_state_and_disable();
  581. //measured_z = probe_pt(x_pos + X_PROBE_OFFSET_FROM_EXTRUDER, y_pos + Y_PROBE_OFFSET_FROM_EXTRUDER, ProbeDeployAndStow, g29_verbose_level);
  582. ubl.has_control_of_lcd_panel = true; // Grab the LCD Hardware
  583. measured_z = 1.5;
  584. do_blocking_move_to_z(measured_z); // Get close to the bed, but leave some space so we don't damage anything
  585. // The user is not going to be locking in a new Z-Offset very often so
  586. // it won't be that painful to spin the Encoder Wheel for 1.5mm
  587. lcd_implementation_clear();
  588. lcd_z_offset_edit_setup(measured_z);
  589. KEEPALIVE_STATE(PAUSED_FOR_USER);
  590. do {
  591. measured_z = lcd_z_offset_edit();
  592. idle();
  593. do_blocking_move_to_z(measured_z);
  594. } while (!ubl_lcd_clicked());
  595. ubl.has_control_of_lcd_panel = true; // There is a race condition for the Encoder Wheel getting clicked.
  596. // It could get detected in lcd_mesh_edit (actually _lcd_mesh_fine_tune)
  597. // or here. So, until we are done looking for a long Encoder Wheel Press,
  598. // we need to take control of the panel
  599. KEEPALIVE_STATE(IN_HANDLER);
  600. lcd_return_to_status();
  601. const millis_t nxt = millis() + 1500UL;
  602. while (ubl_lcd_clicked()) { // debounce and watch for abort
  603. idle();
  604. if (ELAPSED(millis(), nxt)) {
  605. SERIAL_PROTOCOLLNPGM("\nZ-Offset Adjustment Stopped.");
  606. do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE);
  607. LCD_MESSAGEPGM("Z-Offset Stopped"); // TODO: Make translatable string
  608. ubl.restore_ubl_active_state_and_leave();
  609. goto LEAVE;
  610. }
  611. }
  612. ubl.has_control_of_lcd_panel = false;
  613. safe_delay(20); // We don't want any switch noise.
  614. ubl.state.z_offset = measured_z;
  615. lcd_implementation_clear();
  616. ubl.restore_ubl_active_state_and_leave();
  617. }
  618. }
  619. */
  620. LEAVE:
  621. lcd_reset_alert_level();
  622. LCD_MESSAGEPGM("");
  623. lcd_quick_feedback();
  624. ubl.has_control_of_lcd_panel = false;
  625. }
  626. void unified_bed_leveling::find_mean_mesh_height() {
  627. float sum = 0.0;
  628. int n = 0;
  629. for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
  630. for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++)
  631. if (!isnan(ubl.z_values[x][y])) {
  632. sum += ubl.z_values[x][y];
  633. n++;
  634. }
  635. const float mean = sum / n;
  636. //
  637. // Now do the sumation of the squares of difference from mean
  638. //
  639. float sum_of_diff_squared = 0.0;
  640. for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
  641. for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++)
  642. if (!isnan(ubl.z_values[x][y]))
  643. sum_of_diff_squared += sq(ubl.z_values[x][y] - mean);
  644. SERIAL_ECHOLNPAIR("# of samples: ", n);
  645. SERIAL_ECHOPGM("Mean Mesh Height: ");
  646. SERIAL_ECHO_F(mean, 6);
  647. SERIAL_EOL;
  648. const float sigma = sqrt(sum_of_diff_squared / (n + 1));
  649. SERIAL_ECHOPGM("Standard Deviation: ");
  650. SERIAL_ECHO_F(sigma, 6);
  651. SERIAL_EOL;
  652. if (c_flag)
  653. for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
  654. for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++)
  655. if (!isnan(ubl.z_values[x][y]))
  656. ubl.z_values[x][y] -= mean + ubl_constant;
  657. }
  658. void unified_bed_leveling::shift_mesh_height() {
  659. for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
  660. for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++)
  661. if (!isnan(ubl.z_values[x][y]))
  662. ubl.z_values[x][y] += ubl_constant;
  663. }
  664. /**
  665. * Probe all invalidated locations of the mesh that can be reached by the probe.
  666. * This attempts to fill in locations closest to the nozzle's start location first.
  667. */
  668. void unified_bed_leveling::probe_entire_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map, const bool stow_probe, bool close_or_far) {
  669. mesh_index_pair location;
  670. ubl.has_control_of_lcd_panel = true;
  671. ubl.save_ubl_active_state_and_disable(); // we don't do bed level correction because we want the raw data when we probe
  672. DEPLOY_PROBE();
  673. uint16_t max_iterations = GRID_MAX_POINTS;
  674. do {
  675. if (ubl_lcd_clicked()) {
  676. SERIAL_PROTOCOLLNPGM("\nMesh only partially populated.\n");
  677. lcd_quick_feedback();
  678. STOW_PROBE();
  679. while (ubl_lcd_clicked()) idle();
  680. ubl.has_control_of_lcd_panel = false;
  681. ubl.restore_ubl_active_state_and_leave();
  682. safe_delay(50); // Debounce the Encoder wheel
  683. return;
  684. }
  685. location = find_closest_mesh_point_of_type(INVALID, lx, ly, USE_PROBE_AS_REFERENCE, NULL, close_or_far);
  686. if (location.x_index >= 0) { // mesh point found and is reachable by probe
  687. const float rawx = pgm_read_float(&ubl.mesh_index_to_xpos[location.x_index]),
  688. rawy = pgm_read_float(&ubl.mesh_index_to_ypos[location.y_index]);
  689. const float measured_z = probe_pt(LOGICAL_X_POSITION(rawx), LOGICAL_Y_POSITION(rawy), stow_probe, g29_verbose_level);
  690. ubl.z_values[location.x_index][location.y_index] = measured_z;
  691. }
  692. if (do_ubl_mesh_map) ubl.display_map(map_type);
  693. } while ((location.x_index >= 0) && (--max_iterations));
  694. STOW_PROBE();
  695. ubl.restore_ubl_active_state_and_leave();
  696. do_blocking_move_to_xy(
  697. constrain(lx - (X_PROBE_OFFSET_FROM_EXTRUDER), UBL_MESH_MIN_X, UBL_MESH_MAX_X),
  698. constrain(ly - (Y_PROBE_OFFSET_FROM_EXTRUDER), UBL_MESH_MIN_Y, UBL_MESH_MAX_Y)
  699. );
  700. }
  701. void unified_bed_leveling::tilt_mesh_based_on_3pts(const float &z1, const float &z2, const float &z3) {
  702. matrix_3x3 rotation;
  703. vector_3 v1 = vector_3( (UBL_PROBE_PT_1_X - UBL_PROBE_PT_2_X),
  704. (UBL_PROBE_PT_1_Y - UBL_PROBE_PT_2_Y),
  705. (z1 - z2) ),
  706. v2 = vector_3( (UBL_PROBE_PT_3_X - UBL_PROBE_PT_2_X),
  707. (UBL_PROBE_PT_3_Y - UBL_PROBE_PT_2_Y),
  708. (z3 - z2) ),
  709. normal = vector_3::cross(v1, v2);
  710. normal = normal.get_normal();
  711. /**
  712. * This vector is normal to the tilted plane.
  713. * However, we don't know its direction. We need it to point up. So if
  714. * Z is negative, we need to invert the sign of all components of the vector
  715. */
  716. if (normal.z < 0.0) {
  717. normal.x = -normal.x;
  718. normal.y = -normal.y;
  719. normal.z = -normal.z;
  720. }
  721. rotation = matrix_3x3::create_look_at(vector_3(normal.x, normal.y, 1));
  722. if (g29_verbose_level > 2) {
  723. SERIAL_ECHOPGM("bed plane normal = [");
  724. SERIAL_PROTOCOL_F(normal.x, 7);
  725. SERIAL_PROTOCOLCHAR(',');
  726. SERIAL_PROTOCOL_F(normal.y, 7);
  727. SERIAL_PROTOCOLCHAR(',');
  728. SERIAL_PROTOCOL_F(normal.z, 7);
  729. SERIAL_ECHOLNPGM("]");
  730. rotation.debug(PSTR("rotation matrix:"));
  731. }
  732. //
  733. // All of 3 of these points should give us the same d constant
  734. //
  735. float t = normal.x * (UBL_PROBE_PT_1_X) + normal.y * (UBL_PROBE_PT_1_Y),
  736. d = t + normal.z * z1;
  737. if (g29_verbose_level>2) {
  738. SERIAL_ECHOPGM("D constant: ");
  739. SERIAL_PROTOCOL_F(d, 7);
  740. SERIAL_ECHOLNPGM(" ");
  741. }
  742. #if ENABLED(DEBUG_LEVELING_FEATURE)
  743. if (DEBUGGING(LEVELING)) {
  744. SERIAL_ECHOPGM("d from 1st point: ");
  745. SERIAL_ECHO_F(d, 6);
  746. SERIAL_EOL;
  747. t = normal.x * (UBL_PROBE_PT_2_X) + normal.y * (UBL_PROBE_PT_2_Y);
  748. d = t + normal.z * z2;
  749. SERIAL_ECHOPGM("d from 2nd point: ");
  750. SERIAL_ECHO_F(d, 6);
  751. SERIAL_EOL;
  752. t = normal.x * (UBL_PROBE_PT_3_X) + normal.y * (UBL_PROBE_PT_3_Y);
  753. d = t + normal.z * z3;
  754. SERIAL_ECHOPGM("d from 3rd point: ");
  755. SERIAL_ECHO_F(d, 6);
  756. SERIAL_EOL;
  757. }
  758. #endif
  759. for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
  760. for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) {
  761. float x_tmp = pgm_read_float(&ubl.mesh_index_to_xpos[i]),
  762. y_tmp = pgm_read_float(&ubl.mesh_index_to_ypos[j]),
  763. z_tmp = ubl.z_values[i][j];
  764. #if ENABLED(DEBUG_LEVELING_FEATURE)
  765. if (DEBUGGING(LEVELING)) {
  766. SERIAL_ECHOPGM("before rotation = [");
  767. SERIAL_PROTOCOL_F(x_tmp, 7);
  768. SERIAL_PROTOCOLCHAR(',');
  769. SERIAL_PROTOCOL_F(y_tmp, 7);
  770. SERIAL_PROTOCOLCHAR(',');
  771. SERIAL_PROTOCOL_F(z_tmp, 7);
  772. SERIAL_ECHOPGM("] ---> ");
  773. safe_delay(20);
  774. }
  775. #endif
  776. apply_rotation_xyz(rotation, x_tmp, y_tmp, z_tmp);
  777. #if ENABLED(DEBUG_LEVELING_FEATURE)
  778. if (DEBUGGING(LEVELING)) {
  779. SERIAL_ECHOPGM("after rotation = [");
  780. SERIAL_PROTOCOL_F(x_tmp, 7);
  781. SERIAL_PROTOCOLCHAR(',');
  782. SERIAL_PROTOCOL_F(y_tmp, 7);
  783. SERIAL_PROTOCOLCHAR(',');
  784. SERIAL_PROTOCOL_F(z_tmp, 7);
  785. SERIAL_ECHOLNPGM("]");
  786. safe_delay(55);
  787. }
  788. #endif
  789. ubl.z_values[i][j] += z_tmp - d;
  790. }
  791. }
  792. }
  793. float use_encoder_wheel_to_measure_point() {
  794. while (ubl_lcd_clicked()) delay(50); // wait for user to release encoder wheel
  795. delay(50); // debounce
  796. KEEPALIVE_STATE(PAUSED_FOR_USER);
  797. while (!ubl_lcd_clicked()) { // we need the loop to move the nozzle based on the encoder wheel here!
  798. idle();
  799. if (ubl.encoder_diff) {
  800. do_blocking_move_to_z(current_position[Z_AXIS] + 0.01 * float(ubl.encoder_diff));
  801. ubl.encoder_diff = 0;
  802. }
  803. }
  804. KEEPALIVE_STATE(IN_HANDLER);
  805. return current_position[Z_AXIS];
  806. }
  807. static void echo_and_take_a_measurement() {
  808. SERIAL_PROTOCOLLNPGM(" and take a measurement.");
  809. }
  810. float measure_business_card_thickness(float &in_height) {
  811. ubl.has_control_of_lcd_panel = true;
  812. ubl.save_ubl_active_state_and_disable(); // Disable bed level correction for probing
  813. do_blocking_move_to_z(in_height);
  814. do_blocking_move_to_xy(0.5 * (UBL_MESH_MAX_X - (UBL_MESH_MIN_X)), 0.5 * (UBL_MESH_MAX_Y - (UBL_MESH_MIN_Y)));
  815. //, min(planner.max_feedrate_mm_s[X_AXIS], planner.max_feedrate_mm_s[Y_AXIS]) / 2.0);
  816. stepper.synchronize();
  817. SERIAL_PROTOCOLPGM("Place shim under nozzle");
  818. LCD_MESSAGEPGM("Place shim & measure"); // TODO: Make translatable string
  819. lcd_goto_screen(lcd_status_screen);
  820. echo_and_take_a_measurement();
  821. const float z1 = use_encoder_wheel_to_measure_point();
  822. do_blocking_move_to_z(current_position[Z_AXIS] + SIZE_OF_LITTLE_RAISE);
  823. stepper.synchronize();
  824. SERIAL_PROTOCOLPGM("Remove shim");
  825. LCD_MESSAGEPGM("Remove & measure bed"); // TODO: Make translatable string
  826. echo_and_take_a_measurement();
  827. const float z2 = use_encoder_wheel_to_measure_point();
  828. do_blocking_move_to_z(current_position[Z_AXIS] + Z_CLEARANCE_BETWEEN_PROBES);
  829. const float thickness = abs(z1 - z2);
  830. if (g29_verbose_level > 1) {
  831. SERIAL_PROTOCOLPGM("Business Card is ");
  832. SERIAL_PROTOCOL_F(thickness, 4);
  833. SERIAL_PROTOCOLLNPGM("mm thick.");
  834. }
  835. in_height = current_position[Z_AXIS]; // do manual probing at lower height
  836. ubl.has_control_of_lcd_panel = false;
  837. ubl.restore_ubl_active_state_and_leave();
  838. return thickness;
  839. }
  840. 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) {
  841. ubl.has_control_of_lcd_panel = true;
  842. ubl.save_ubl_active_state_and_disable(); // we don't do bed level correction because we want the raw data when we probe
  843. do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES);
  844. do_blocking_move_to_xy(lx, ly);
  845. lcd_goto_screen(lcd_status_screen);
  846. mesh_index_pair location;
  847. do {
  848. location = find_closest_mesh_point_of_type(INVALID, lx, ly, USE_NOZZLE_AS_REFERENCE, NULL, false);
  849. // It doesn't matter if the probe can't reach the NAN location. This is a manual probe.
  850. if (location.x_index < 0 && location.y_index < 0) continue;
  851. const float rawx = pgm_read_float(&ubl.mesh_index_to_xpos[location.x_index]),
  852. rawy = pgm_read_float(&ubl.mesh_index_to_ypos[location.y_index]),
  853. xProbe = LOGICAL_X_POSITION(rawx),
  854. yProbe = LOGICAL_Y_POSITION(rawy);
  855. if (!position_is_reachable_raw_xy(rawx, rawy)) break; // SHOULD NOT OCCUR (find_closest_mesh_point only returns reachable points)
  856. do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES);
  857. LCD_MESSAGEPGM("Moving to next"); // TODO: Make translatable string
  858. do_blocking_move_to_xy(xProbe, yProbe);
  859. do_blocking_move_to_z(z_clearance);
  860. KEEPALIVE_STATE(PAUSED_FOR_USER);
  861. ubl.has_control_of_lcd_panel = true;
  862. if (do_ubl_mesh_map) ubl.display_map(map_type); // show user where we're probing
  863. if (code_seen('B'))
  864. LCD_MESSAGEPGM("Place shim & measure"); // TODO: Make translatable string
  865. else
  866. LCD_MESSAGEPGM("Measure"); // TODO: Make translatable string
  867. while (ubl_lcd_clicked()) delay(50); // wait for user to release encoder wheel
  868. delay(50); // debounce
  869. while (!ubl_lcd_clicked()) { // we need the loop to move the nozzle based on the encoder wheel here!
  870. idle();
  871. if (ubl.encoder_diff) {
  872. do_blocking_move_to_z(current_position[Z_AXIS] + float(ubl.encoder_diff) / 100.0);
  873. ubl.encoder_diff = 0;
  874. }
  875. }
  876. const millis_t nxt = millis() + 1500L;
  877. while (ubl_lcd_clicked()) { // debounce and watch for abort
  878. idle();
  879. if (ELAPSED(millis(), nxt)) {
  880. SERIAL_PROTOCOLLNPGM("\nMesh only partially populated.");
  881. do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE);
  882. lcd_quick_feedback();
  883. while (ubl_lcd_clicked()) idle();
  884. ubl.has_control_of_lcd_panel = false;
  885. KEEPALIVE_STATE(IN_HANDLER);
  886. ubl.restore_ubl_active_state_and_leave();
  887. return;
  888. }
  889. }
  890. ubl.z_values[location.x_index][location.y_index] = current_position[Z_AXIS] - card_thickness;
  891. if (g29_verbose_level > 2) {
  892. SERIAL_PROTOCOLPGM("Mesh Point Measured at: ");
  893. SERIAL_PROTOCOL_F(ubl.z_values[location.x_index][location.y_index], 6);
  894. SERIAL_EOL;
  895. }
  896. } while (location.x_index >= 0 && location.y_index >= 0);
  897. if (do_ubl_mesh_map) ubl.display_map(map_type);
  898. LEAVE:
  899. ubl.restore_ubl_active_state_and_leave();
  900. KEEPALIVE_STATE(IN_HANDLER);
  901. do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE);
  902. do_blocking_move_to_xy(lx, ly);
  903. }
  904. bool g29_parameter_parsing() {
  905. bool err_flag = false;
  906. LCD_MESSAGEPGM("Doing G29 UBL!"); // TODO: Make translatable string
  907. lcd_quick_feedback();
  908. ubl_constant = 0.0;
  909. repetition_cnt = 0;
  910. x_flag = code_seen('X') && code_has_value();
  911. x_pos = x_flag ? code_value_float() : current_position[X_AXIS];
  912. y_flag = code_seen('Y') && code_has_value();
  913. y_pos = y_flag ? code_value_float() : current_position[Y_AXIS];
  914. repeat_flag = code_seen('R');
  915. if (repeat_flag) {
  916. repetition_cnt = code_has_value() ? code_value_int() : GRID_MAX_POINTS;
  917. NOMORE(repetition_cnt, GRID_MAX_POINTS);
  918. if (repetition_cnt < 1) {
  919. SERIAL_PROTOCOLLNPGM("?(R)epetition count invalid (1+).\n");
  920. return UBL_ERR;
  921. }
  922. }
  923. g29_verbose_level = code_seen('V') ? code_value_int() : 0;
  924. if (!WITHIN(g29_verbose_level, 0, 4)) {
  925. SERIAL_PROTOCOLLNPGM("?(V)erbose level is implausible (0-4).\n");
  926. err_flag = true;
  927. }
  928. if (code_seen('P')) {
  929. phase_value = code_value_int();
  930. if (!WITHIN(phase_value, 0, 6)) {
  931. SERIAL_PROTOCOLLNPGM("?(P)hase value invalid (0-6).\n");
  932. err_flag = true;
  933. }
  934. }
  935. if (code_seen('J')) {
  936. grid_size = code_has_value() ? code_value_int() : 0;
  937. if (grid_size!=0 && !WITHIN(grid_size, 2, 9)) {
  938. SERIAL_PROTOCOLLNPGM("?Invalid grid size (J) specified (2-9).\n");
  939. err_flag = true;
  940. }
  941. }
  942. if (x_flag != y_flag) {
  943. SERIAL_PROTOCOLLNPGM("Both X & Y locations must be specified.\n");
  944. err_flag = true;
  945. }
  946. if (!WITHIN(RAW_X_POSITION(x_pos), X_MIN_POS, X_MAX_POS)) {
  947. SERIAL_PROTOCOLLNPGM("Invalid X location specified.\n");
  948. err_flag = true;
  949. }
  950. if (!WITHIN(RAW_Y_POSITION(y_pos), Y_MIN_POS, Y_MAX_POS)) {
  951. SERIAL_PROTOCOLLNPGM("Invalid Y location specified.\n");
  952. err_flag = true;
  953. }
  954. if (err_flag) return UBL_ERR;
  955. // Activate or deactivate UBL
  956. if (code_seen('A')) {
  957. if (code_seen('D')) {
  958. SERIAL_PROTOCOLLNPGM("?Can't activate and deactivate at the same time.\n");
  959. return UBL_ERR;
  960. }
  961. ubl.state.active = true;
  962. ubl.report_state();
  963. }
  964. else if (code_seen('D')) {
  965. ubl.state.active = false;
  966. ubl.report_state();
  967. }
  968. // Set global 'C' flag and its value
  969. if ((c_flag = code_seen('C')))
  970. ubl_constant = code_value_float();
  971. #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
  972. if (code_seen('F') && code_has_value()) {
  973. const float fh = code_value_float();
  974. if (!WITHIN(fh, 0.0, 100.0)) {
  975. SERIAL_PROTOCOLLNPGM("?(F)ade height for Bed Level Correction not plausible.\n");
  976. return UBL_ERR;
  977. }
  978. set_z_fade_height(fh);
  979. }
  980. #endif
  981. map_type = code_seen('T') && code_has_value() ? code_value_int() : 0;
  982. if (!WITHIN(map_type, 0, 1)) {
  983. SERIAL_PROTOCOLLNPGM("Invalid map type.\n");
  984. return UBL_ERR;
  985. }
  986. return UBL_OK;
  987. }
  988. static int ubl_state_at_invocation = 0,
  989. ubl_state_recursion_chk = 0;
  990. void unified_bed_leveling::save_ubl_active_state_and_disable() {
  991. ubl_state_recursion_chk++;
  992. if (ubl_state_recursion_chk != 1) {
  993. SERIAL_ECHOLNPGM("save_ubl_active_state_and_disabled() called multiple times in a row.");
  994. LCD_MESSAGEPGM("save_UBL_active() error"); // TODO: Make translatable string
  995. lcd_quick_feedback();
  996. return;
  997. }
  998. ubl_state_at_invocation = ubl.state.active;
  999. ubl.state.active = 0;
  1000. }
  1001. void unified_bed_leveling::restore_ubl_active_state_and_leave() {
  1002. if (--ubl_state_recursion_chk) {
  1003. SERIAL_ECHOLNPGM("restore_ubl_active_state_and_leave() called too many times.");
  1004. LCD_MESSAGEPGM("restore_UBL_active() error"); // TODO: Make translatable string
  1005. lcd_quick_feedback();
  1006. return;
  1007. }
  1008. ubl.state.active = ubl_state_at_invocation;
  1009. }
  1010. /**
  1011. * Much of the 'What?' command can be eliminated. But until we are fully debugged, it is
  1012. * good to have the extra information. Soon... we prune this to just a few items
  1013. */
  1014. void unified_bed_leveling::g29_what_command() {
  1015. report_state();
  1016. if (state.storage_slot == -1)
  1017. SERIAL_PROTOCOLPGM("No Mesh Loaded.");
  1018. else {
  1019. SERIAL_PROTOCOLPAIR("Mesh ", state.storage_slot);
  1020. SERIAL_PROTOCOLPGM(" Loaded.");
  1021. }
  1022. SERIAL_EOL;
  1023. safe_delay(50);
  1024. SERIAL_PROTOCOLLNPAIR("UBL object count: ", (int)ubl_cnt);
  1025. #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
  1026. SERIAL_PROTOCOL("planner.z_fade_height : ");
  1027. SERIAL_PROTOCOL_F(planner.z_fade_height, 4);
  1028. SERIAL_EOL;
  1029. #endif
  1030. SERIAL_PROTOCOLPGM("zprobe_zoffset: ");
  1031. SERIAL_PROTOCOL_F(zprobe_zoffset, 7);
  1032. SERIAL_EOL;
  1033. SERIAL_ECHOLNPAIR("UBL_MESH_MIN_X " STRINGIFY(UBL_MESH_MIN_X) "=", UBL_MESH_MIN_X);
  1034. SERIAL_ECHOLNPAIR("UBL_MESH_MIN_Y " STRINGIFY(UBL_MESH_MIN_Y) "=", UBL_MESH_MIN_Y);
  1035. safe_delay(25);
  1036. SERIAL_ECHOLNPAIR("UBL_MESH_MAX_X " STRINGIFY(UBL_MESH_MAX_X) "=", UBL_MESH_MAX_X);
  1037. SERIAL_ECHOLNPAIR("UBL_MESH_MAX_Y " STRINGIFY(UBL_MESH_MAX_Y) "=", UBL_MESH_MAX_Y);
  1038. safe_delay(25);
  1039. SERIAL_ECHOLNPAIR("GRID_MAX_POINTS_X ", GRID_MAX_POINTS_X);
  1040. SERIAL_ECHOLNPAIR("GRID_MAX_POINTS_Y ", GRID_MAX_POINTS_Y);
  1041. safe_delay(25);
  1042. SERIAL_ECHOLNPAIR("MESH_X_DIST ", MESH_X_DIST);
  1043. SERIAL_ECHOLNPAIR("MESH_Y_DIST ", MESH_Y_DIST);
  1044. safe_delay(25);
  1045. SERIAL_PROTOCOLPGM("X-Axis Mesh Points at: ");
  1046. for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
  1047. SERIAL_PROTOCOL_F(LOGICAL_X_POSITION(pgm_read_float(&mesh_index_to_xpos[i])), 3);
  1048. SERIAL_PROTOCOLPGM(" ");
  1049. safe_delay(25);
  1050. }
  1051. SERIAL_EOL;
  1052. SERIAL_PROTOCOLPGM("Y-Axis Mesh Points at: ");
  1053. for (uint8_t i = 0; i < GRID_MAX_POINTS_Y; i++) {
  1054. SERIAL_PROTOCOL_F(LOGICAL_Y_POSITION(pgm_read_float(&mesh_index_to_ypos[i])), 3);
  1055. SERIAL_PROTOCOLPGM(" ");
  1056. safe_delay(25);
  1057. }
  1058. SERIAL_EOL;
  1059. #if HAS_KILL
  1060. SERIAL_PROTOCOLPAIR("Kill pin on :", KILL_PIN);
  1061. SERIAL_PROTOCOLLNPAIR(" state:", READ(KILL_PIN));
  1062. #endif
  1063. SERIAL_EOL;
  1064. safe_delay(50);
  1065. SERIAL_PROTOCOLLNPAIR("ubl_state_at_invocation :", ubl_state_at_invocation);
  1066. SERIAL_EOL;
  1067. SERIAL_PROTOCOLLNPAIR("ubl_state_recursion_chk :", ubl_state_recursion_chk);
  1068. SERIAL_EOL;
  1069. safe_delay(50);
  1070. SERIAL_PROTOCOLPAIR("Meshes go from ", hex_address((void*)settings.get_start_of_meshes()));
  1071. SERIAL_PROTOCOLLNPAIR(" to ", hex_address((void*)settings.get_end_of_meshes()));
  1072. safe_delay(50);
  1073. SERIAL_PROTOCOLLNPAIR("sizeof(ubl) : ", (int)sizeof(ubl));
  1074. SERIAL_EOL;
  1075. SERIAL_PROTOCOLLNPAIR("z_value[][] size: ", (int)sizeof(z_values));
  1076. SERIAL_EOL;
  1077. safe_delay(25);
  1078. SERIAL_PROTOCOLLNPAIR("EEPROM free for UBL: ", hex_address((void*)(settings.get_end_of_meshes() - settings.get_start_of_meshes())));
  1079. safe_delay(50);
  1080. SERIAL_PROTOCOLPAIR("EEPROM can hold ", settings.calc_num_meshes());
  1081. SERIAL_PROTOCOLLNPGM(" meshes.\n");
  1082. safe_delay(25);
  1083. if (!sanity_check()) {
  1084. echo_name();
  1085. SERIAL_PROTOCOLLNPGM(" sanity checks passed.");
  1086. }
  1087. }
  1088. /**
  1089. * When we are fully debugged, the EEPROM dump command will get deleted also. But
  1090. * right now, it is good to have the extra information. Soon... we prune this.
  1091. */
  1092. void g29_eeprom_dump() {
  1093. unsigned char cccc;
  1094. uint16_t kkkk;
  1095. SERIAL_ECHO_START;
  1096. SERIAL_ECHOLNPGM("EEPROM Dump:");
  1097. for (uint16_t i = 0; i < E2END + 1; i += 16) {
  1098. if (!(i & 0x3)) idle();
  1099. print_hex_word(i);
  1100. SERIAL_ECHOPGM(": ");
  1101. for (uint16_t j = 0; j < 16; j++) {
  1102. kkkk = i + j;
  1103. eeprom_read_block(&cccc, (void *)kkkk, 1);
  1104. print_hex_byte(cccc);
  1105. SERIAL_ECHO(' ');
  1106. }
  1107. SERIAL_EOL;
  1108. }
  1109. SERIAL_EOL;
  1110. }
  1111. /**
  1112. * When we are fully debugged, this may go away. But there are some valid
  1113. * use cases for the users. So we can wait and see what to do with it.
  1114. */
  1115. void g29_compare_current_mesh_to_stored_mesh() {
  1116. int16_t a = settings.calc_num_meshes();
  1117. if (!a) {
  1118. SERIAL_PROTOCOLLNPGM("?EEPROM storage not available.");
  1119. return;
  1120. }
  1121. if (!code_has_value()) {
  1122. SERIAL_PROTOCOLLNPGM("?Storage slot # required.");
  1123. SERIAL_PROTOCOLLNPAIR("?Use 0 to ", a - 1);
  1124. return;
  1125. }
  1126. storage_slot = code_value_int();
  1127. if (!WITHIN(storage_slot, 0, a - 1)) {
  1128. SERIAL_PROTOCOLLNPGM("?Invalid storage slot.");
  1129. SERIAL_PROTOCOLLNPAIR("?Use 0 to ", a - 1);
  1130. return;
  1131. }
  1132. float tmp_z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
  1133. settings.load_mesh(storage_slot, &tmp_z_values);
  1134. SERIAL_PROTOCOLPAIR("Subtracting mesh in slot ", storage_slot);
  1135. SERIAL_PROTOCOLLNPGM(" from current mesh.");
  1136. for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
  1137. for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++)
  1138. ubl.z_values[x][y] -= tmp_z_values[x][y];
  1139. }
  1140. mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType type, const float &lx, const float &ly, const bool probe_as_reference, unsigned int bits[16], const bool far_flag) {
  1141. mesh_index_pair out_mesh;
  1142. out_mesh.x_index = out_mesh.y_index = -1;
  1143. // Get our reference position. Either the nozzle or probe location.
  1144. const float px = RAW_X_POSITION(lx) - (probe_as_reference == USE_PROBE_AS_REFERENCE ? X_PROBE_OFFSET_FROM_EXTRUDER : 0),
  1145. py = RAW_Y_POSITION(ly) - (probe_as_reference == USE_PROBE_AS_REFERENCE ? Y_PROBE_OFFSET_FROM_EXTRUDER : 0),
  1146. raw_x = RAW_CURRENT_POSITION(X), raw_y = RAW_CURRENT_POSITION(Y);
  1147. float closest = far_flag ? -99999.99 : 99999.99;
  1148. for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
  1149. for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) {
  1150. if ( (type == INVALID && isnan(ubl.z_values[i][j])) // Check to see if this location holds the right thing
  1151. || (type == REAL && !isnan(ubl.z_values[i][j]))
  1152. || (type == SET_IN_BITMAP && is_bit_set(bits, i, j))
  1153. ) {
  1154. // We only get here if we found a Mesh Point of the specified type
  1155. const float mx = pgm_read_float(&ubl.mesh_index_to_xpos[i]), // Check if we can probe this mesh location
  1156. my = pgm_read_float(&ubl.mesh_index_to_ypos[j]);
  1157. // If using the probe as the reference there are some unreachable locations.
  1158. // Also for round beds, there are grid points outside the bed that nozzle can't reach.
  1159. // Prune them from the list and ignore them till the next Phase (manual nozzle probing).
  1160. if ( ! (probe_as_reference ? position_is_reachable_by_probe_raw_xy(mx, my) : position_is_reachable_raw_xy(mx, my)) )
  1161. continue;
  1162. // Reachable. Check if it's the closest location to the nozzle.
  1163. // Add in a weighting factor that considers the current location of the nozzle.
  1164. float distance = HYPOT(px - mx, py - my) + HYPOT(raw_x - mx, raw_y - my) * 0.1;
  1165. /**
  1166. * If doing the far_flag action, we want to be as far as possible
  1167. * from the starting point and from any other probed points. We
  1168. * want the next point spread out and filling in any blank spaces
  1169. * in the mesh. So we add in some of the distance to every probed
  1170. * point we can find.
  1171. */
  1172. if (far_flag) {
  1173. for (uint8_t k = 0; k < GRID_MAX_POINTS_X; k++) {
  1174. for (uint8_t l = 0; l < GRID_MAX_POINTS_Y; l++) {
  1175. if (!isnan(ubl.z_values[k][l])) {
  1176. distance += sq(i - k) * (MESH_X_DIST) * .05
  1177. + sq(j - l) * (MESH_Y_DIST) * .05;
  1178. }
  1179. }
  1180. }
  1181. }
  1182. // if far_flag, look for farthest point
  1183. if (far_flag == (distance > closest) && distance != closest) {
  1184. closest = distance; // We found a closer/farther location with
  1185. out_mesh.x_index = i; // the specified type of mesh value.
  1186. out_mesh.y_index = j;
  1187. out_mesh.distance = closest;
  1188. }
  1189. }
  1190. } // for j
  1191. } // for i
  1192. return out_mesh;
  1193. }
  1194. void fine_tune_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map) {
  1195. if (!code_seen('R')) // fine_tune_mesh() is special. If no repetion count flag is specified
  1196. repetition_cnt = 1; // we know to do exactly one mesh location. Otherwise we use what the parser decided.
  1197. mesh_index_pair location;
  1198. uint16_t not_done[16];
  1199. int32_t round_off;
  1200. if ( ! position_is_reachable_xy( lx, ly )) {
  1201. SERIAL_PROTOCOLLNPGM("(X,Y) outside printable radius.");
  1202. return;
  1203. }
  1204. ubl.save_ubl_active_state_and_disable();
  1205. memset(not_done, 0xFF, sizeof(not_done));
  1206. LCD_MESSAGEPGM("Fine Tuning Mesh"); // TODO: Make translatable string
  1207. do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE);
  1208. do_blocking_move_to_xy(lx, ly);
  1209. do {
  1210. location = find_closest_mesh_point_of_type(SET_IN_BITMAP, lx, ly, USE_NOZZLE_AS_REFERENCE, not_done, false);
  1211. if (location.x_index < 0 ) break; // stop when we can't find any more reachable points.
  1212. bit_clear(not_done, location.x_index, location.y_index); // Mark this location as 'adjusted' so we will find a
  1213. // different location the next time through the loop
  1214. const float rawx = pgm_read_float(&ubl.mesh_index_to_xpos[location.x_index]),
  1215. rawy = pgm_read_float(&ubl.mesh_index_to_ypos[location.y_index]);
  1216. if ( ! position_is_reachable_raw_xy( rawx, rawy )) { // SHOULD NOT OCCUR because find_closest_mesh_point_of_type will only return reachable
  1217. break;
  1218. }
  1219. float new_z = ubl.z_values[location.x_index][location.y_index];
  1220. if (!isnan(new_z)) { //can't fine tune a point that hasn't been probed
  1221. do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE); // Move the nozzle to where we are going to edit
  1222. do_blocking_move_to_xy(LOGICAL_X_POSITION(rawx), LOGICAL_Y_POSITION(rawy));
  1223. round_off = (int32_t)(new_z * 1000.0); // we chop off the last digits just to be clean. We are rounding to the
  1224. new_z = float(round_off) / 1000.0;
  1225. KEEPALIVE_STATE(PAUSED_FOR_USER);
  1226. ubl.has_control_of_lcd_panel = true;
  1227. if (do_ubl_mesh_map) ubl.display_map(map_type); // show the user which point is being adjusted
  1228. lcd_implementation_clear();
  1229. lcd_mesh_edit_setup(new_z);
  1230. do {
  1231. new_z = lcd_mesh_edit();
  1232. idle();
  1233. } while (!ubl_lcd_clicked());
  1234. lcd_return_to_status();
  1235. // There is a race condition for the Encoder Wheel getting clicked.
  1236. // It could get detected in lcd_mesh_edit (actually _lcd_mesh_fine_tune)
  1237. // or here.
  1238. ubl.has_control_of_lcd_panel = true;
  1239. }
  1240. const millis_t nxt = millis() + 1500UL;
  1241. while (ubl_lcd_clicked()) { // debounce and watch for abort
  1242. idle();
  1243. if (ELAPSED(millis(), nxt)) {
  1244. lcd_return_to_status();
  1245. //SERIAL_PROTOCOLLNPGM("\nFine Tuning of Mesh Stopped.");
  1246. do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE);
  1247. LCD_MESSAGEPGM("Mesh Editing Stopped"); // TODO: Make translatable string
  1248. while (ubl_lcd_clicked()) idle();
  1249. goto FINE_TUNE_EXIT;
  1250. }
  1251. }
  1252. safe_delay(20); // We don't want any switch noise.
  1253. ubl.z_values[location.x_index][location.y_index] = new_z;
  1254. lcd_implementation_clear();
  1255. } while (( location.x_index >= 0 ) && (--repetition_cnt>0));
  1256. FINE_TUNE_EXIT:
  1257. ubl.has_control_of_lcd_panel = false;
  1258. KEEPALIVE_STATE(IN_HANDLER);
  1259. if (do_ubl_mesh_map) ubl.display_map(map_type);
  1260. ubl.restore_ubl_active_state_and_leave();
  1261. do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE);
  1262. do_blocking_move_to_xy(lx, ly);
  1263. LCD_MESSAGEPGM("Done Editing Mesh"); // TODO: Make translatable string
  1264. SERIAL_ECHOLNPGM("Done Editing Mesh");
  1265. }
  1266. /**
  1267. * 'Smart Fill': Scan from the outward edges of the mesh towards the center.
  1268. * If an invalid location is found, use the next two points (if valid) to
  1269. * calculate a 'reasonable' value for the unprobed mesh point.
  1270. */
  1271. bool smart_fill_one(const uint8_t x, const uint8_t y, const int8_t xdir, const int8_t ydir) {
  1272. const int8_t x1 = x + xdir, x2 = x1 + xdir,
  1273. y1 = y + ydir, y2 = y1 + ydir;
  1274. // A NAN next to a pair of real values?
  1275. if (isnan(ubl.z_values[x][y]) && !isnan(ubl.z_values[x1][y1]) && !isnan(ubl.z_values[x2][y2])) {
  1276. if (ubl.z_values[x1][y1] < ubl.z_values[x2][y2]) // Angled downward?
  1277. ubl.z_values[x][y] = ubl.z_values[x1][y1]; // Use nearest (maybe a little too high.)
  1278. else
  1279. ubl.z_values[x][y] = 2.0 * ubl.z_values[x1][y1] - ubl.z_values[x2][y2]; // Angled upward...
  1280. return true;
  1281. }
  1282. return false;
  1283. }
  1284. typedef struct { uint8_t sx, ex, sy, ey; bool yfirst; } smart_fill_info;
  1285. void smart_fill_mesh() {
  1286. const smart_fill_info info[] = {
  1287. { 0, GRID_MAX_POINTS_X, 0, GRID_MAX_POINTS_Y - 2, false }, // Bottom of the mesh looking up
  1288. { 0, GRID_MAX_POINTS_X, GRID_MAX_POINTS_Y - 1, 0, false }, // Top of the mesh looking down
  1289. { 0, GRID_MAX_POINTS_X - 2, 0, GRID_MAX_POINTS_Y, true }, // Left side of the mesh looking right
  1290. { GRID_MAX_POINTS_X - 1, 0, 0, GRID_MAX_POINTS_Y, true } // Right side of the mesh looking left
  1291. };
  1292. for (uint8_t i = 0; i < COUNT(info); ++i) {
  1293. const smart_fill_info &f = info[i];
  1294. if (f.yfirst) {
  1295. const int8_t dir = f.ex > f.sx ? 1 : -1;
  1296. for (uint8_t y = f.sy; y != f.ey; ++y)
  1297. for (uint8_t x = f.sx; x != f.ex; x += dir)
  1298. if (smart_fill_one(x, y, dir, 0)) break;
  1299. }
  1300. else {
  1301. const int8_t dir = f.ey > f.sy ? 1 : -1;
  1302. for (uint8_t x = f.sx; x != f.ex; ++x)
  1303. for (uint8_t y = f.sy; y != f.ey; y += dir)
  1304. if (smart_fill_one(x, y, 0, dir)) break;
  1305. }
  1306. }
  1307. }
  1308. void unified_bed_leveling::tilt_mesh_based_on_probed_grid(const bool do_ubl_mesh_map) {
  1309. constexpr int16_t x_min = max(MIN_PROBE_X, UBL_MESH_MIN_X),
  1310. x_max = min(MAX_PROBE_X, UBL_MESH_MAX_X),
  1311. y_min = max(MIN_PROBE_Y, UBL_MESH_MIN_Y),
  1312. y_max = min(MAX_PROBE_Y, UBL_MESH_MAX_Y);
  1313. const float dx = float(x_max - x_min) / (grid_size - 1.0),
  1314. dy = float(y_max - y_min) / (grid_size - 1.0);
  1315. struct linear_fit_data lsf_results;
  1316. incremental_LSF_reset(&lsf_results);
  1317. bool zig_zag = false;
  1318. for (uint8_t ix = 0; ix < grid_size; ix++) {
  1319. const float x = float(x_min) + ix * dx;
  1320. for (int8_t iy = 0; iy < grid_size; iy++) {
  1321. const float y = float(y_min) + dy * (zig_zag ? grid_size - 1 - iy : iy);
  1322. float measured_z = probe_pt(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y), code_seen('E'), g29_verbose_level);
  1323. #if ENABLED(DEBUG_LEVELING_FEATURE)
  1324. if (DEBUGGING(LEVELING)) {
  1325. SERIAL_CHAR('(');
  1326. SERIAL_PROTOCOL_F(x, 7);
  1327. SERIAL_CHAR(',');
  1328. SERIAL_PROTOCOL_F(y, 7);
  1329. SERIAL_ECHOPGM(") logical: ");
  1330. SERIAL_CHAR('(');
  1331. SERIAL_PROTOCOL_F(LOGICAL_X_POSITION(x), 7);
  1332. SERIAL_CHAR(',');
  1333. SERIAL_PROTOCOL_F(LOGICAL_X_POSITION(y), 7);
  1334. SERIAL_ECHOPGM(") measured: ");
  1335. SERIAL_PROTOCOL_F(measured_z, 7);
  1336. SERIAL_ECHOPGM(" correction: ");
  1337. SERIAL_PROTOCOL_F(get_z_correction(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y)), 7);
  1338. }
  1339. #endif
  1340. measured_z -= get_z_correction(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y)) /* + zprobe_zoffset */ ;
  1341. #if ENABLED(DEBUG_LEVELING_FEATURE)
  1342. if (DEBUGGING(LEVELING)) {
  1343. SERIAL_ECHOPGM(" final >>>---> ");
  1344. SERIAL_PROTOCOL_F(measured_z, 7);
  1345. SERIAL_EOL;
  1346. }
  1347. #endif
  1348. incremental_LSF(&lsf_results, x, y, measured_z);
  1349. }
  1350. zig_zag ^= true;
  1351. }
  1352. if (finish_incremental_LSF(&lsf_results)) {
  1353. SERIAL_ECHOPGM("Could not complete LSF!");
  1354. return;
  1355. }
  1356. if (g29_verbose_level > 3) {
  1357. SERIAL_ECHOPGM("LSF Results A=");
  1358. SERIAL_PROTOCOL_F(lsf_results.A, 7);
  1359. SERIAL_ECHOPGM(" B=");
  1360. SERIAL_PROTOCOL_F(lsf_results.B, 7);
  1361. SERIAL_ECHOPGM(" D=");
  1362. SERIAL_PROTOCOL_F(lsf_results.D, 7);
  1363. SERIAL_EOL;
  1364. }
  1365. vector_3 normal = vector_3(lsf_results.A, lsf_results.B, 1.0000).get_normal();
  1366. if (g29_verbose_level > 2) {
  1367. SERIAL_ECHOPGM("bed plane normal = [");
  1368. SERIAL_PROTOCOL_F(normal.x, 7);
  1369. SERIAL_PROTOCOLCHAR(',');
  1370. SERIAL_PROTOCOL_F(normal.y, 7);
  1371. SERIAL_PROTOCOLCHAR(',');
  1372. SERIAL_PROTOCOL_F(normal.z, 7);
  1373. SERIAL_ECHOLNPGM("]");
  1374. }
  1375. matrix_3x3 rotation = matrix_3x3::create_look_at(vector_3(lsf_results.A, lsf_results.B, 1));
  1376. for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
  1377. for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) {
  1378. float x_tmp = pgm_read_float(&mesh_index_to_xpos[i]),
  1379. y_tmp = pgm_read_float(&mesh_index_to_ypos[j]),
  1380. z_tmp = z_values[i][j];
  1381. #if ENABLED(DEBUG_LEVELING_FEATURE)
  1382. if (DEBUGGING(LEVELING)) {
  1383. SERIAL_ECHOPGM("before rotation = [");
  1384. SERIAL_PROTOCOL_F(x_tmp, 7);
  1385. SERIAL_PROTOCOLCHAR(',');
  1386. SERIAL_PROTOCOL_F(y_tmp, 7);
  1387. SERIAL_PROTOCOLCHAR(',');
  1388. SERIAL_PROTOCOL_F(z_tmp, 7);
  1389. SERIAL_ECHOPGM("] ---> ");
  1390. safe_delay(20);
  1391. }
  1392. #endif
  1393. apply_rotation_xyz(rotation, x_tmp, y_tmp, z_tmp);
  1394. #if ENABLED(DEBUG_LEVELING_FEATURE)
  1395. if (DEBUGGING(LEVELING)) {
  1396. SERIAL_ECHOPGM("after rotation = [");
  1397. SERIAL_PROTOCOL_F(x_tmp, 7);
  1398. SERIAL_PROTOCOLCHAR(',');
  1399. SERIAL_PROTOCOL_F(y_tmp, 7);
  1400. SERIAL_PROTOCOLCHAR(',');
  1401. SERIAL_PROTOCOL_F(z_tmp, 7);
  1402. SERIAL_ECHOLNPGM("]");
  1403. safe_delay(55);
  1404. }
  1405. #endif
  1406. z_values[i][j] += z_tmp - lsf_results.D;
  1407. }
  1408. }
  1409. #if ENABLED(DEBUG_LEVELING_FEATURE)
  1410. if (DEBUGGING(LEVELING)) {
  1411. rotation.debug(PSTR("rotation matrix:"));
  1412. SERIAL_ECHOPGM("LSF Results A=");
  1413. SERIAL_PROTOCOL_F(lsf_results.A, 7);
  1414. SERIAL_ECHOPGM(" B=");
  1415. SERIAL_PROTOCOL_F(lsf_results.B, 7);
  1416. SERIAL_ECHOPGM(" D=");
  1417. SERIAL_PROTOCOL_F(lsf_results.D, 7);
  1418. SERIAL_EOL;
  1419. safe_delay(55);
  1420. SERIAL_ECHOPGM("bed plane normal = [");
  1421. SERIAL_PROTOCOL_F(normal.x, 7);
  1422. SERIAL_PROTOCOLCHAR(',');
  1423. SERIAL_PROTOCOL_F(normal.y, 7);
  1424. SERIAL_PROTOCOLCHAR(',');
  1425. SERIAL_PROTOCOL_F(normal.z, 7);
  1426. SERIAL_ECHOPGM("]\n");
  1427. SERIAL_EOL;
  1428. }
  1429. #endif
  1430. }
  1431. #endif // AUTO_BED_LEVELING_UBL