My Marlin configs for Fabrikator Mini and CTC i3 Pro B
Du kannst nicht mehr als 25 Themen auswählen Themen müssen mit entweder einem Buchstaben oder einer Ziffer beginnen. Sie können Bindestriche („-“) enthalten und bis zu 35 Zeichen lang sein.

ubl_G29.cpp 72KB

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