mesh_bed_calibration.cpp 100 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453145414551456145714581459146014611462146314641465146614671468146914701471147214731474147514761477147814791480148114821483148414851486148714881489149014911492149314941495149614971498149915001501150215031504150515061507150815091510151115121513151415151516151715181519152015211522152315241525152615271528152915301531153215331534153515361537153815391540154115421543154415451546154715481549155015511552155315541555155615571558155915601561156215631564156515661567156815691570157115721573157415751576157715781579158015811582158315841585158615871588158915901591159215931594159515961597159815991600160116021603160416051606160716081609161016111612161316141615161616171618161916201621162216231624162516261627162816291630163116321633163416351636163716381639164016411642164316441645164616471648164916501651165216531654165516561657165816591660166116621663166416651666166716681669167016711672167316741675167616771678167916801681168216831684168516861687168816891690169116921693169416951696169716981699170017011702170317041705170617071708170917101711171217131714171517161717171817191720172117221723172417251726172717281729173017311732173317341735173617371738173917401741174217431744174517461747174817491750175117521753175417551756175717581759176017611762176317641765176617671768176917701771177217731774177517761777177817791780178117821783178417851786178717881789179017911792179317941795179617971798179918001801180218031804180518061807180818091810181118121813181418151816181718181819182018211822182318241825182618271828182918301831183218331834183518361837183818391840184118421843184418451846184718481849185018511852185318541855185618571858185918601861186218631864186518661867186818691870187118721873187418751876187718781879188018811882188318841885188618871888188918901891189218931894189518961897189818991900190119021903190419051906190719081909191019111912191319141915191619171918191919201921192219231924192519261927192819291930193119321933193419351936193719381939194019411942194319441945194619471948194919501951195219531954195519561957195819591960196119621963196419651966196719681969197019711972197319741975197619771978197919801981198219831984198519861987198819891990199119921993199419951996199719981999200020012002200320042005200620072008200920102011201220132014201520162017201820192020202120222023202420252026202720282029203020312032203320342035203620372038203920402041204220432044204520462047204820492050205120522053205420552056205720582059206020612062206320642065206620672068206920702071207220732074207520762077207820792080208120822083208420852086208720882089209020912092209320942095209620972098209921002101210221032104210521062107210821092110211121122113211421152116211721182119212021212122212321242125212621272128212921302131213221332134213521362137213821392140214121422143214421452146214721482149215021512152215321542155215621572158215921602161216221632164216521662167216821692170217121722173217421752176217721782179218021812182218321842185218621872188218921902191219221932194219521962197219821992200220122022203220422052206220722082209221022112212221322142215221622172218221922202221222222232224222522262227222822292230223122322233223422352236223722382239224022412242224322442245224622472248224922502251225222532254225522562257225822592260226122622263226422652266226722682269227022712272227322742275227622772278227922802281228222832284228522862287228822892290229122922293229422952296229722982299230023012302230323042305230623072308230923102311231223132314231523162317231823192320232123222323232423252326232723282329233023312332233323342335233623372338233923402341234223432344234523462347234823492350235123522353235423552356235723582359236023612362236323642365236623672368236923702371237223732374237523762377237823792380238123822383238423852386238723882389239023912392239323942395239623972398239924002401240224032404240524062407240824092410241124122413241424152416241724182419242024212422242324242425242624272428242924302431243224332434243524362437243824392440244124422443244424452446244724482449245024512452245324542455245624572458245924602461246224632464
  1. #include "Marlin.h"
  2. #include "Configuration.h"
  3. #include "ConfigurationStore.h"
  4. #include "language_all.h"
  5. #include "mesh_bed_calibration.h"
  6. #include "mesh_bed_leveling.h"
  7. #include "stepper.h"
  8. #include "ultralcd.h"
  9. uint8_t world2machine_correction_mode;
  10. float world2machine_rotation_and_skew[2][2];
  11. float world2machine_rotation_and_skew_inv[2][2];
  12. float world2machine_shift[2];
  13. // Weight of the Y coordinate for the least squares fitting of the bed induction sensor targets.
  14. // Only used for the first row of the points, which may not befully in reach of the sensor.
  15. #define WEIGHT_FIRST_ROW_X_HIGH (1.f)
  16. #define WEIGHT_FIRST_ROW_X_LOW (0.35f)
  17. #define WEIGHT_FIRST_ROW_Y_HIGH (0.3f)
  18. #define WEIGHT_FIRST_ROW_Y_LOW (0.0f)
  19. #define BED_ZERO_REF_X (- 22.f + X_PROBE_OFFSET_FROM_EXTRUDER)
  20. #define BED_ZERO_REF_Y (- 0.6f + Y_PROBE_OFFSET_FROM_EXTRUDER)
  21. // Scaling of the real machine axes against the programmed dimensions in the firmware.
  22. // The correction is tiny, here around 0.5mm on 250mm length.
  23. //#define MACHINE_AXIS_SCALE_X ((250.f - 0.5f) / 250.f)
  24. //#define MACHINE_AXIS_SCALE_Y ((250.f - 0.5f) / 250.f)
  25. #define MACHINE_AXIS_SCALE_X 1.f
  26. #define MACHINE_AXIS_SCALE_Y 1.f
  27. #define BED_CALIBRATION_POINT_OFFSET_MAX_EUCLIDIAN (0.8f)
  28. #define BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_X (0.8f)
  29. #define BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_Y (1.5f)
  30. #define MIN_BED_SENSOR_POINT_RESPONSE_DMR (2.0f)
  31. //#define Y_MIN_POS_FOR_BED_CALIBRATION (MANUAL_Y_HOME_POS-0.2f)
  32. #define Y_MIN_POS_FOR_BED_CALIBRATION (Y_MIN_POS)
  33. // Distances toward the print bed edge may not be accurate.
  34. #define Y_MIN_POS_CALIBRATION_POINT_ACCURATE (Y_MIN_POS + 3.f)
  35. // When the measured point center is out of reach of the sensor, Y coordinate will be ignored
  36. // by the Least Squares fitting and the X coordinate will be weighted low.
  37. #define Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH (Y_MIN_POS - 0.5f)
  38. // 0.12 degrees equals to an offset of 0.5mm on 250mm length.
  39. const float bed_skew_angle_mild = (0.12f * M_PI / 180.f);
  40. // 0.25 degrees equals to an offset of 1.1mm on 250mm length.
  41. const float bed_skew_angle_extreme = (0.25f * M_PI / 180.f);
  42. // Positions of the bed reference points in the machine coordinates, referenced to the P.I.N.D.A sensor.
  43. // The points are ordered in a zig-zag fashion to speed up the calibration.
  44. const float bed_ref_points[] PROGMEM = {
  45. 13.f - BED_ZERO_REF_X, 8.4f - BED_ZERO_REF_Y,
  46. 115.f - BED_ZERO_REF_X, 8.4f - BED_ZERO_REF_Y,
  47. 216.f - BED_ZERO_REF_X, 8.4f - BED_ZERO_REF_Y,
  48. 216.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y,
  49. 115.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y,
  50. 13.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y,
  51. 13.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y,
  52. 115.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y,
  53. 216.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y
  54. };
  55. // Positions of the bed reference points in the machine coordinates, referenced to the P.I.N.D.A sensor.
  56. // The points are the following: center front, center right, center rear, center left.
  57. const float bed_ref_points_4[] PROGMEM = {
  58. 115.f - BED_ZERO_REF_X, 8.4f - BED_ZERO_REF_Y,
  59. 216.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y,
  60. 115.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y,
  61. 13.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y
  62. };
  63. static inline float sqr(float x) { return x * x; }
  64. static inline bool point_on_1st_row(const uint8_t i, const uint8_t npts)
  65. {
  66. if (npts == 4) return (i == 0);
  67. else return (i < 3);
  68. }
  69. // Weight of a point coordinate in a least squares optimization.
  70. // The first row of points may not be fully reachable
  71. // and the y values may be shortened a bit by the bed carriage
  72. // pulling the belt up.
  73. static inline float point_weight_x(const uint8_t i, const uint8_t npts, const float &y)
  74. {
  75. float w = 1.f;
  76. if (point_on_1st_row(i, npts)) {
  77. if (y >= Y_MIN_POS_CALIBRATION_POINT_ACCURATE) {
  78. w = WEIGHT_FIRST_ROW_X_HIGH;
  79. } else if (y < Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH) {
  80. // If the point is fully outside, give it some weight.
  81. w = WEIGHT_FIRST_ROW_X_LOW;
  82. } else {
  83. // Linearly interpolate the weight from 1 to WEIGHT_FIRST_ROW_X.
  84. float t = (y - Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH) / (Y_MIN_POS_CALIBRATION_POINT_ACCURATE - Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH);
  85. w = (1.f - t) * WEIGHT_FIRST_ROW_X_LOW + t * WEIGHT_FIRST_ROW_X_HIGH;
  86. }
  87. }
  88. return w;
  89. }
  90. // Weight of a point coordinate in a least squares optimization.
  91. // The first row of points may not be fully reachable
  92. // and the y values may be shortened a bit by the bed carriage
  93. // pulling the belt up.
  94. static inline float point_weight_y(const uint8_t i, const uint8_t npts, const float &y)
  95. {
  96. float w = 1.f;
  97. if (point_on_1st_row(i, npts)) {
  98. if (y >= Y_MIN_POS_CALIBRATION_POINT_ACCURATE) {
  99. w = WEIGHT_FIRST_ROW_Y_HIGH;
  100. } else if (y < Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH) {
  101. // If the point is fully outside, give it some weight.
  102. w = WEIGHT_FIRST_ROW_Y_LOW;
  103. } else {
  104. // Linearly interpolate the weight from 1 to WEIGHT_FIRST_ROW_X.
  105. float t = (y - Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH) / (Y_MIN_POS_CALIBRATION_POINT_ACCURATE - Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH);
  106. w = (1.f - t) * WEIGHT_FIRST_ROW_Y_LOW + t * WEIGHT_FIRST_ROW_Y_HIGH;
  107. }
  108. }
  109. return w;
  110. }
  111. // Non-Linear Least Squares fitting of the bed to the measured induction points
  112. // using the Gauss-Newton method.
  113. // This method will maintain a unity length of the machine axes,
  114. // which is the correct approach if the sensor points are not measured precisely.
  115. BedSkewOffsetDetectionResultType calculate_machine_skew_and_offset_LS(
  116. // Matrix of maximum 9 2D points (18 floats)
  117. const float *measured_pts,
  118. uint8_t npts,
  119. const float *true_pts,
  120. // Resulting correction matrix.
  121. float *vec_x,
  122. float *vec_y,
  123. float *cntr,
  124. // Temporary values, 49-18-(2*3)=25 floats
  125. // , float *temp
  126. int8_t verbosity_level
  127. )
  128. {
  129. if (verbosity_level >= 10) {
  130. SERIAL_ECHOLNPGM("calculate machine skew and offset LS");
  131. // Show the initial state, before the fitting.
  132. SERIAL_ECHOPGM("X vector, initial: ");
  133. MYSERIAL.print(vec_x[0], 5);
  134. SERIAL_ECHOPGM(", ");
  135. MYSERIAL.print(vec_x[1], 5);
  136. SERIAL_ECHOLNPGM("");
  137. SERIAL_ECHOPGM("Y vector, initial: ");
  138. MYSERIAL.print(vec_y[0], 5);
  139. SERIAL_ECHOPGM(", ");
  140. MYSERIAL.print(vec_y[1], 5);
  141. SERIAL_ECHOLNPGM("");
  142. SERIAL_ECHOPGM("center, initial: ");
  143. MYSERIAL.print(cntr[0], 5);
  144. SERIAL_ECHOPGM(", ");
  145. MYSERIAL.print(cntr[1], 5);
  146. SERIAL_ECHOLNPGM("");
  147. for (uint8_t i = 0; i < npts; ++i) {
  148. SERIAL_ECHOPGM("point #");
  149. MYSERIAL.print(int(i));
  150. SERIAL_ECHOPGM(" measured: (");
  151. MYSERIAL.print(measured_pts[i * 2], 5);
  152. SERIAL_ECHOPGM(", ");
  153. MYSERIAL.print(measured_pts[i * 2 + 1], 5);
  154. SERIAL_ECHOPGM("); target: (");
  155. MYSERIAL.print(pgm_read_float(true_pts + i * 2), 5);
  156. SERIAL_ECHOPGM(", ");
  157. MYSERIAL.print(pgm_read_float(true_pts + i * 2 + 1), 5);
  158. SERIAL_ECHOPGM("), error: ");
  159. MYSERIAL.print(sqrt(
  160. sqr(pgm_read_float(true_pts + i * 2) - measured_pts[i * 2]) +
  161. sqr(pgm_read_float(true_pts + i * 2 + 1) - measured_pts[i * 2 + 1])), 5);
  162. SERIAL_ECHOLNPGM("");
  163. }
  164. delay_keep_alive(100);
  165. }
  166. // Run some iterations of the Gauss-Newton method of non-linear least squares.
  167. // Initial set of parameters:
  168. // X,Y offset
  169. cntr[0] = 0.f;
  170. cntr[1] = 0.f;
  171. // Rotation of the machine X axis from the bed X axis.
  172. float a1 = 0;
  173. // Rotation of the machine Y axis from the bed Y axis.
  174. float a2 = 0;
  175. for (int8_t iter = 0; iter < 100; ++iter) {
  176. float c1 = cos(a1) * MACHINE_AXIS_SCALE_X;
  177. float s1 = sin(a1) * MACHINE_AXIS_SCALE_X;
  178. float c2 = cos(a2) * MACHINE_AXIS_SCALE_Y;
  179. float s2 = sin(a2) * MACHINE_AXIS_SCALE_Y;
  180. // Prepare the Normal equation for the Gauss-Newton method.
  181. float A[4][4] = { 0.f };
  182. float b[4] = { 0.f };
  183. float acc;
  184. for (uint8_t r = 0; r < 4; ++r) {
  185. for (uint8_t c = 0; c < 4; ++c) {
  186. acc = 0;
  187. // J^T times J
  188. for (uint8_t i = 0; i < npts; ++i) {
  189. // First for the residuum in the x axis:
  190. if (r != 1 && c != 1) {
  191. float a =
  192. (r == 0) ? 1.f :
  193. ((r == 2) ? (-s1 * measured_pts[2 * i]) :
  194. (-c2 * measured_pts[2 * i + 1]));
  195. float b =
  196. (c == 0) ? 1.f :
  197. ((c == 2) ? (-s1 * measured_pts[2 * i]) :
  198. (-c2 * measured_pts[2 * i + 1]));
  199. float w = point_weight_x(i, npts, measured_pts[2 * i + 1]);
  200. acc += a * b * w;
  201. }
  202. // Second for the residuum in the y axis.
  203. // The first row of the points have a low weight, because their position may not be known
  204. // with a sufficient accuracy.
  205. if (r != 0 && c != 0) {
  206. float a =
  207. (r == 1) ? 1.f :
  208. ((r == 2) ? ( c1 * measured_pts[2 * i]) :
  209. (-s2 * measured_pts[2 * i + 1]));
  210. float b =
  211. (c == 1) ? 1.f :
  212. ((c == 2) ? ( c1 * measured_pts[2 * i]) :
  213. (-s2 * measured_pts[2 * i + 1]));
  214. float w = point_weight_y(i, npts, measured_pts[2 * i + 1]);
  215. acc += a * b * w;
  216. }
  217. }
  218. A[r][c] = acc;
  219. }
  220. // J^T times f(x)
  221. acc = 0.f;
  222. for (uint8_t i = 0; i < npts; ++i) {
  223. {
  224. float j =
  225. (r == 0) ? 1.f :
  226. ((r == 1) ? 0.f :
  227. ((r == 2) ? (-s1 * measured_pts[2 * i]) :
  228. (-c2 * measured_pts[2 * i + 1])));
  229. float fx = c1 * measured_pts[2 * i] - s2 * measured_pts[2 * i + 1] + cntr[0] - pgm_read_float(true_pts + i * 2);
  230. float w = point_weight_x(i, npts, measured_pts[2 * i + 1]);
  231. acc += j * fx * w;
  232. }
  233. {
  234. float j =
  235. (r == 0) ? 0.f :
  236. ((r == 1) ? 1.f :
  237. ((r == 2) ? ( c1 * measured_pts[2 * i]) :
  238. (-s2 * measured_pts[2 * i + 1])));
  239. float fy = s1 * measured_pts[2 * i] + c2 * measured_pts[2 * i + 1] + cntr[1] - pgm_read_float(true_pts + i * 2 + 1);
  240. float w = point_weight_y(i, npts, measured_pts[2 * i + 1]);
  241. acc += j * fy * w;
  242. }
  243. }
  244. b[r] = -acc;
  245. }
  246. // Solve for h by a Gauss iteration method.
  247. float h[4] = { 0.f };
  248. for (uint8_t gauss_iter = 0; gauss_iter < 100; ++gauss_iter) {
  249. h[0] = (b[0] - A[0][1] * h[1] - A[0][2] * h[2] - A[0][3] * h[3]) / A[0][0];
  250. h[1] = (b[1] - A[1][0] * h[0] - A[1][2] * h[2] - A[1][3] * h[3]) / A[1][1];
  251. h[2] = (b[2] - A[2][0] * h[0] - A[2][1] * h[1] - A[2][3] * h[3]) / A[2][2];
  252. h[3] = (b[3] - A[3][0] * h[0] - A[3][1] * h[1] - A[3][2] * h[2]) / A[3][3];
  253. }
  254. // and update the current position with h.
  255. // It may be better to use the Levenberg-Marquart method here,
  256. // but because we are very close to the solution alread,
  257. // the simple Gauss-Newton non-linear Least Squares method works well enough.
  258. cntr[0] += h[0];
  259. cntr[1] += h[1];
  260. a1 += h[2];
  261. a2 += h[3];
  262. if (verbosity_level >= 20) {
  263. SERIAL_ECHOPGM("iteration: ");
  264. MYSERIAL.print(int(iter));
  265. SERIAL_ECHOPGM("; correction vector: ");
  266. MYSERIAL.print(h[0], 5);
  267. SERIAL_ECHOPGM(", ");
  268. MYSERIAL.print(h[1], 5);
  269. SERIAL_ECHOPGM(", ");
  270. MYSERIAL.print(h[2], 5);
  271. SERIAL_ECHOPGM(", ");
  272. MYSERIAL.print(h[3], 5);
  273. SERIAL_ECHOLNPGM("");
  274. SERIAL_ECHOPGM("corrected x/y: ");
  275. MYSERIAL.print(cntr[0], 5);
  276. SERIAL_ECHOPGM(", ");
  277. MYSERIAL.print(cntr[0], 5);
  278. SERIAL_ECHOLNPGM("");
  279. SERIAL_ECHOPGM("corrected angles: ");
  280. MYSERIAL.print(180.f * a1 / M_PI, 5);
  281. SERIAL_ECHOPGM(", ");
  282. MYSERIAL.print(180.f * a2 / M_PI, 5);
  283. SERIAL_ECHOLNPGM("");
  284. }
  285. }
  286. vec_x[0] = cos(a1) * MACHINE_AXIS_SCALE_X;
  287. vec_x[1] = sin(a1) * MACHINE_AXIS_SCALE_X;
  288. vec_y[0] = -sin(a2) * MACHINE_AXIS_SCALE_Y;
  289. vec_y[1] = cos(a2) * MACHINE_AXIS_SCALE_Y;
  290. BedSkewOffsetDetectionResultType result = BED_SKEW_OFFSET_DETECTION_PERFECT;
  291. {
  292. angleDiff = fabs(a2 - a1);
  293. if (angleDiff > bed_skew_angle_mild)
  294. result = (angleDiff > bed_skew_angle_extreme) ?
  295. BED_SKEW_OFFSET_DETECTION_SKEW_EXTREME :
  296. BED_SKEW_OFFSET_DETECTION_SKEW_MILD;
  297. if (fabs(a1) > bed_skew_angle_extreme ||
  298. fabs(a2) > bed_skew_angle_extreme)
  299. result = BED_SKEW_OFFSET_DETECTION_SKEW_EXTREME;
  300. }
  301. if (verbosity_level >= 1) {
  302. SERIAL_ECHOPGM("correction angles: ");
  303. MYSERIAL.print(180.f * a1 / M_PI, 5);
  304. SERIAL_ECHOPGM(", ");
  305. MYSERIAL.print(180.f * a2 / M_PI, 5);
  306. SERIAL_ECHOLNPGM("");
  307. }
  308. if (verbosity_level >= 10) {
  309. // Show the adjusted state, before the fitting.
  310. SERIAL_ECHOPGM("X vector new, inverted: ");
  311. MYSERIAL.print(vec_x[0], 5);
  312. SERIAL_ECHOPGM(", ");
  313. MYSERIAL.print(vec_x[1], 5);
  314. SERIAL_ECHOLNPGM("");
  315. SERIAL_ECHOPGM("Y vector new, inverted: ");
  316. MYSERIAL.print(vec_y[0], 5);
  317. SERIAL_ECHOPGM(", ");
  318. MYSERIAL.print(vec_y[1], 5);
  319. SERIAL_ECHOLNPGM("");
  320. SERIAL_ECHOPGM("center new, inverted: ");
  321. MYSERIAL.print(cntr[0], 5);
  322. SERIAL_ECHOPGM(", ");
  323. MYSERIAL.print(cntr[1], 5);
  324. SERIAL_ECHOLNPGM("");
  325. delay_keep_alive(100);
  326. SERIAL_ECHOLNPGM("Error after correction: ");
  327. }
  328. // Measure the error after correction.
  329. for (uint8_t i = 0; i < npts; ++i) {
  330. float x = vec_x[0] * measured_pts[i * 2] + vec_y[0] * measured_pts[i * 2 + 1] + cntr[0];
  331. float y = vec_x[1] * measured_pts[i * 2] + vec_y[1] * measured_pts[i * 2 + 1] + cntr[1];
  332. float errX = sqr(pgm_read_float(true_pts + i * 2) - x);
  333. float errY = sqr(pgm_read_float(true_pts + i * 2 + 1) - y);
  334. float err = sqrt(errX + errY);
  335. if (verbosity_level >= 10) {
  336. SERIAL_ECHOPGM("point #");
  337. MYSERIAL.print(int(i));
  338. SERIAL_ECHOLNPGM(":");
  339. }
  340. if (point_on_1st_row(i, npts)) {
  341. if(verbosity_level >= 20) SERIAL_ECHOPGM("Point on first row");
  342. float w = point_weight_y(i, npts, measured_pts[2 * i + 1]);
  343. if (sqrt(errX) > BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_X ||
  344. (w != 0.f && sqrt(errY) > BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_Y)) {
  345. result = BED_SKEW_OFFSET_DETECTION_FITTING_FAILED;
  346. if (verbosity_level >= 20) {
  347. SERIAL_ECHOPGM(", weigth Y: ");
  348. MYSERIAL.print(w);
  349. if (sqrt(errX) > BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_X) SERIAL_ECHOPGM(", error X > max. error X");
  350. if (w != 0.f && sqrt(errY) > BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_Y) SERIAL_ECHOPGM(", error Y > max. error Y");
  351. }
  352. }
  353. }
  354. else {
  355. if(verbosity_level >=20 ) SERIAL_ECHOPGM("Point not on first row");
  356. if (err > BED_CALIBRATION_POINT_OFFSET_MAX_EUCLIDIAN) {
  357. result = BED_SKEW_OFFSET_DETECTION_FITTING_FAILED;
  358. if(verbosity_level >= 20) SERIAL_ECHOPGM(", error > max. error euclidian");
  359. }
  360. }
  361. if (verbosity_level >= 10) {
  362. SERIAL_ECHOLNPGM("");
  363. SERIAL_ECHOPGM("measured: (");
  364. MYSERIAL.print(measured_pts[i * 2], 5);
  365. SERIAL_ECHOPGM(", ");
  366. MYSERIAL.print(measured_pts[i * 2 + 1], 5);
  367. SERIAL_ECHOPGM("); corrected: (");
  368. MYSERIAL.print(x, 5);
  369. SERIAL_ECHOPGM(", ");
  370. MYSERIAL.print(y, 5);
  371. SERIAL_ECHOPGM("); target: (");
  372. MYSERIAL.print(pgm_read_float(true_pts + i * 2), 5);
  373. SERIAL_ECHOPGM(", ");
  374. MYSERIAL.print(pgm_read_float(true_pts + i * 2 + 1), 5);
  375. SERIAL_ECHOLNPGM(")");
  376. SERIAL_ECHOPGM("error: ");
  377. MYSERIAL.print(err);
  378. SERIAL_ECHOPGM(", error X: ");
  379. MYSERIAL.print(sqrt(errX));
  380. SERIAL_ECHOPGM(", error Y: ");
  381. MYSERIAL.print(sqrt(errY));
  382. SERIAL_ECHOLNPGM("");
  383. SERIAL_ECHOLNPGM("");
  384. }
  385. }
  386. if (verbosity_level >= 20) {
  387. SERIAL_ECHOLNPGM("Max. errors:");
  388. SERIAL_ECHOPGM("Max. error X:");
  389. MYSERIAL.println(BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_X);
  390. SERIAL_ECHOPGM("Max. error Y:");
  391. MYSERIAL.println(BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_Y);
  392. SERIAL_ECHOPGM("Max. error euclidian:");
  393. MYSERIAL.println(BED_CALIBRATION_POINT_OFFSET_MAX_EUCLIDIAN);
  394. SERIAL_ECHOLNPGM("");
  395. }
  396. #if 0
  397. if (result == BED_SKEW_OFFSET_DETECTION_PERFECT && fabs(a1) < bed_skew_angle_mild && fabs(a2) < bed_skew_angle_mild) {
  398. if (verbosity_level > 0)
  399. SERIAL_ECHOLNPGM("Very little skew detected. Disabling skew correction.");
  400. // Just disable the skew correction.
  401. vec_x[0] = MACHINE_AXIS_SCALE_X;
  402. vec_x[1] = 0.f;
  403. vec_y[0] = 0.f;
  404. vec_y[1] = MACHINE_AXIS_SCALE_Y;
  405. }
  406. #else
  407. if (result == BED_SKEW_OFFSET_DETECTION_PERFECT) {
  408. if (verbosity_level > 0)
  409. SERIAL_ECHOLNPGM("Very little skew detected. Orthogonalizing the axes.");
  410. // Orthogonalize the axes.
  411. a1 = 0.5f * (a1 + a2);
  412. vec_x[0] = cos(a1) * MACHINE_AXIS_SCALE_X;
  413. vec_x[1] = sin(a1) * MACHINE_AXIS_SCALE_X;
  414. vec_y[0] = -sin(a1) * MACHINE_AXIS_SCALE_Y;
  415. vec_y[1] = cos(a1) * MACHINE_AXIS_SCALE_Y;
  416. // Refresh the offset.
  417. cntr[0] = 0.f;
  418. cntr[1] = 0.f;
  419. float wx = 0.f;
  420. float wy = 0.f;
  421. for (int8_t i = 0; i < npts; ++ i) {
  422. float x = vec_x[0] * measured_pts[i * 2] + vec_y[0] * measured_pts[i * 2 + 1];
  423. float y = vec_x[1] * measured_pts[i * 2] + vec_y[1] * measured_pts[i * 2 + 1];
  424. float w = point_weight_x(i, npts, y);
  425. cntr[0] += w * (pgm_read_float(true_pts + i * 2) - x);
  426. wx += w;
  427. if (verbosity_level >= 20) {
  428. MYSERIAL.print(i);
  429. SERIAL_ECHOLNPGM("");
  430. SERIAL_ECHOLNPGM("Weight_x:");
  431. MYSERIAL.print(w);
  432. SERIAL_ECHOLNPGM("");
  433. SERIAL_ECHOLNPGM("cntr[0]:");
  434. MYSERIAL.print(cntr[0]);
  435. SERIAL_ECHOLNPGM("");
  436. SERIAL_ECHOLNPGM("wx:");
  437. MYSERIAL.print(wx);
  438. }
  439. w = point_weight_y(i, npts, y);
  440. cntr[1] += w * (pgm_read_float(true_pts + i * 2 + 1) - y);
  441. wy += w;
  442. if (verbosity_level >= 20) {
  443. SERIAL_ECHOLNPGM("");
  444. SERIAL_ECHOLNPGM("Weight_y:");
  445. MYSERIAL.print(w);
  446. SERIAL_ECHOLNPGM("");
  447. SERIAL_ECHOLNPGM("cntr[1]:");
  448. MYSERIAL.print(cntr[1]);
  449. SERIAL_ECHOLNPGM("");
  450. SERIAL_ECHOLNPGM("wy:");
  451. MYSERIAL.print(wy);
  452. SERIAL_ECHOLNPGM("");
  453. SERIAL_ECHOLNPGM("");
  454. }
  455. }
  456. cntr[0] /= wx;
  457. cntr[1] /= wy;
  458. if (verbosity_level >= 20) {
  459. SERIAL_ECHOLNPGM("");
  460. SERIAL_ECHOLNPGM("Final cntr values:");
  461. SERIAL_ECHOLNPGM("cntr[0]:");
  462. MYSERIAL.print(cntr[0]);
  463. SERIAL_ECHOLNPGM("");
  464. SERIAL_ECHOLNPGM("cntr[1]:");
  465. MYSERIAL.print(cntr[1]);
  466. SERIAL_ECHOLNPGM("");
  467. }
  468. }
  469. #endif
  470. // Invert the transformation matrix made of vec_x, vec_y and cntr.
  471. {
  472. float d = vec_x[0] * vec_y[1] - vec_x[1] * vec_y[0];
  473. float Ainv[2][2] = {
  474. { vec_y[1] / d, -vec_y[0] / d },
  475. { -vec_x[1] / d, vec_x[0] / d }
  476. };
  477. float cntrInv[2] = {
  478. -Ainv[0][0] * cntr[0] - Ainv[0][1] * cntr[1],
  479. -Ainv[1][0] * cntr[0] - Ainv[1][1] * cntr[1]
  480. };
  481. vec_x[0] = Ainv[0][0];
  482. vec_x[1] = Ainv[1][0];
  483. vec_y[0] = Ainv[0][1];
  484. vec_y[1] = Ainv[1][1];
  485. cntr[0] = cntrInv[0];
  486. cntr[1] = cntrInv[1];
  487. }
  488. if (verbosity_level >= 1) {
  489. // Show the adjusted state, before the fitting.
  490. SERIAL_ECHOPGM("X vector, adjusted: ");
  491. MYSERIAL.print(vec_x[0], 5);
  492. SERIAL_ECHOPGM(", ");
  493. MYSERIAL.print(vec_x[1], 5);
  494. SERIAL_ECHOLNPGM("");
  495. SERIAL_ECHOPGM("Y vector, adjusted: ");
  496. MYSERIAL.print(vec_y[0], 5);
  497. SERIAL_ECHOPGM(", ");
  498. MYSERIAL.print(vec_y[1], 5);
  499. SERIAL_ECHOLNPGM("");
  500. SERIAL_ECHOPGM("center, adjusted: ");
  501. MYSERIAL.print(cntr[0], 5);
  502. SERIAL_ECHOPGM(", ");
  503. MYSERIAL.print(cntr[1], 5);
  504. SERIAL_ECHOLNPGM("");
  505. delay_keep_alive(100);
  506. }
  507. if (verbosity_level >= 2) {
  508. SERIAL_ECHOLNPGM("Difference after correction: ");
  509. for (uint8_t i = 0; i < npts; ++i) {
  510. float x = vec_x[0] * pgm_read_float(true_pts + i * 2) + vec_y[0] * pgm_read_float(true_pts + i * 2 + 1) + cntr[0];
  511. float y = vec_x[1] * pgm_read_float(true_pts + i * 2) + vec_y[1] * pgm_read_float(true_pts + i * 2 + 1) + cntr[1];
  512. SERIAL_ECHOPGM("point #");
  513. MYSERIAL.print(int(i));
  514. SERIAL_ECHOPGM("measured: (");
  515. MYSERIAL.print(measured_pts[i * 2], 5);
  516. SERIAL_ECHOPGM(", ");
  517. MYSERIAL.print(measured_pts[i * 2 + 1], 5);
  518. SERIAL_ECHOPGM("); measured-corrected: (");
  519. MYSERIAL.print(x, 5);
  520. SERIAL_ECHOPGM(", ");
  521. MYSERIAL.print(y, 5);
  522. SERIAL_ECHOPGM("); target: (");
  523. MYSERIAL.print(pgm_read_float(true_pts + i * 2), 5);
  524. SERIAL_ECHOPGM(", ");
  525. MYSERIAL.print(pgm_read_float(true_pts + i * 2 + 1), 5);
  526. SERIAL_ECHOPGM("), error: ");
  527. MYSERIAL.print(sqrt(sqr(measured_pts[i * 2] - x) + sqr(measured_pts[i * 2 + 1] - y)));
  528. SERIAL_ECHOLNPGM("");
  529. }
  530. if (verbosity_level >= 20) {
  531. SERIAL_ECHOLNPGM("");
  532. SERIAL_ECHOLNPGM("Calculate offset and skew returning result:");
  533. MYSERIAL.print(int(result));
  534. SERIAL_ECHOLNPGM("");
  535. SERIAL_ECHOLNPGM("");
  536. }
  537. delay_keep_alive(100);
  538. }
  539. return result;
  540. }
  541. void reset_bed_offset_and_skew()
  542. {
  543. eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_CENTER+0), 0x0FFFFFFFF);
  544. eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_CENTER+4), 0x0FFFFFFFF);
  545. eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_X +0), 0x0FFFFFFFF);
  546. eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_X +4), 0x0FFFFFFFF);
  547. eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_Y +0), 0x0FFFFFFFF);
  548. eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_Y +4), 0x0FFFFFFFF);
  549. // Reset the 8 16bit offsets.
  550. for (int8_t i = 0; i < 4; ++ i)
  551. eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_Z_JITTER+i*4), 0x0FFFFFFFF);
  552. }
  553. bool is_bed_z_jitter_data_valid()
  554. // offsets of the Z heiths of the calibration points from the first point are saved as 16bit signed int, scaled to tenths of microns
  555. {
  556. for (int8_t i = 0; i < 8; ++ i)
  557. if (eeprom_read_word((uint16_t*)(EEPROM_BED_CALIBRATION_Z_JITTER+i*2)) == 0x0FFFF)
  558. return false;
  559. return true;
  560. }
  561. static void world2machine_update(const float vec_x[2], const float vec_y[2], const float cntr[2])
  562. {
  563. world2machine_rotation_and_skew[0][0] = vec_x[0];
  564. world2machine_rotation_and_skew[1][0] = vec_x[1];
  565. world2machine_rotation_and_skew[0][1] = vec_y[0];
  566. world2machine_rotation_and_skew[1][1] = vec_y[1];
  567. world2machine_shift[0] = cntr[0];
  568. world2machine_shift[1] = cntr[1];
  569. // No correction.
  570. world2machine_correction_mode = WORLD2MACHINE_CORRECTION_NONE;
  571. if (world2machine_shift[0] != 0.f || world2machine_shift[1] != 0.f)
  572. // Shift correction.
  573. world2machine_correction_mode |= WORLD2MACHINE_CORRECTION_SHIFT;
  574. if (world2machine_rotation_and_skew[0][0] != 1.f || world2machine_rotation_and_skew[0][1] != 0.f ||
  575. world2machine_rotation_and_skew[1][0] != 0.f || world2machine_rotation_and_skew[1][1] != 1.f) {
  576. // Rotation & skew correction.
  577. world2machine_correction_mode |= WORLD2MACHINE_CORRECTION_SKEW;
  578. // Invert the world2machine matrix.
  579. float d = world2machine_rotation_and_skew[0][0] * world2machine_rotation_and_skew[1][1] - world2machine_rotation_and_skew[1][0] * world2machine_rotation_and_skew[0][1];
  580. world2machine_rotation_and_skew_inv[0][0] = world2machine_rotation_and_skew[1][1] / d;
  581. world2machine_rotation_and_skew_inv[0][1] = -world2machine_rotation_and_skew[0][1] / d;
  582. world2machine_rotation_and_skew_inv[1][0] = -world2machine_rotation_and_skew[1][0] / d;
  583. world2machine_rotation_and_skew_inv[1][1] = world2machine_rotation_and_skew[0][0] / d;
  584. } else {
  585. world2machine_rotation_and_skew_inv[0][0] = 1.f;
  586. world2machine_rotation_and_skew_inv[0][1] = 0.f;
  587. world2machine_rotation_and_skew_inv[1][0] = 0.f;
  588. world2machine_rotation_and_skew_inv[1][1] = 1.f;
  589. }
  590. }
  591. void world2machine_reset()
  592. {
  593. const float vx[] = { 1.f, 0.f };
  594. const float vy[] = { 0.f, 1.f };
  595. const float cntr[] = { 0.f, 0.f };
  596. world2machine_update(vx, vy, cntr);
  597. }
  598. void world2machine_revert_to_uncorrected()
  599. {
  600. if (world2machine_correction_mode != WORLD2MACHINE_CORRECTION_NONE) {
  601. // Reset the machine correction matrix.
  602. const float vx[] = { 1.f, 0.f };
  603. const float vy[] = { 0.f, 1.f };
  604. const float cntr[] = { 0.f, 0.f };
  605. world2machine_update(vx, vy, cntr);
  606. // Wait for the motors to stop and update the current position with the absolute values.
  607. st_synchronize();
  608. current_position[X_AXIS] = st_get_position_mm(X_AXIS);
  609. current_position[Y_AXIS] = st_get_position_mm(Y_AXIS);
  610. }
  611. }
  612. static inline bool vec_undef(const float v[2])
  613. {
  614. const uint32_t *vx = (const uint32_t*)v;
  615. return vx[0] == 0x0FFFFFFFF || vx[1] == 0x0FFFFFFFF;
  616. }
  617. void world2machine_initialize()
  618. {
  619. //SERIAL_ECHOLNPGM("world2machine_initialize");
  620. float cntr[2] = {
  621. eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_CENTER+0)),
  622. eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_CENTER+4))
  623. };
  624. float vec_x[2] = {
  625. eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +0)),
  626. eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +4))
  627. };
  628. float vec_y[2] = {
  629. eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +0)),
  630. eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +4))
  631. };
  632. bool reset = false;
  633. if (vec_undef(cntr) || vec_undef(vec_x) || vec_undef(vec_y)) {
  634. SERIAL_ECHOLNPGM("Undefined bed correction matrix.");
  635. reset = true;
  636. }
  637. else {
  638. // Length of the vec_x shall be close to unity.
  639. float l = sqrt(vec_x[0] * vec_x[0] + vec_x[1] * vec_x[1]);
  640. if (l < 0.9 || l > 1.1) {
  641. SERIAL_ECHOLNPGM("X vector length:");
  642. MYSERIAL.println(l);
  643. SERIAL_ECHOLNPGM("Invalid bed correction matrix. Length of the X vector out of range.");
  644. reset = true;
  645. }
  646. // Length of the vec_y shall be close to unity.
  647. l = sqrt(vec_y[0] * vec_y[0] + vec_y[1] * vec_y[1]);
  648. if (l < 0.9 || l > 1.1) {
  649. SERIAL_ECHOLNPGM("Y vector length:");
  650. MYSERIAL.println(l);
  651. SERIAL_ECHOLNPGM("Invalid bed correction matrix. Length of the Y vector out of range.");
  652. reset = true;
  653. }
  654. // Correction of the zero point shall be reasonably small.
  655. l = sqrt(cntr[0] * cntr[0] + cntr[1] * cntr[1]);
  656. if (l > 15.f) {
  657. SERIAL_ECHOLNPGM("Zero point correction:");
  658. MYSERIAL.println(l);
  659. SERIAL_ECHOLNPGM("Invalid bed correction matrix. Shift out of range.");
  660. reset = true;
  661. }
  662. // vec_x and vec_y shall be nearly perpendicular.
  663. l = vec_x[0] * vec_y[0] + vec_x[1] * vec_y[1];
  664. if (fabs(l) > 0.1f) {
  665. SERIAL_ECHOLNPGM("Invalid bed correction matrix. X/Y axes are far from being perpendicular.");
  666. reset = true;
  667. }
  668. }
  669. if (reset) {
  670. SERIAL_ECHOLNPGM("Invalid bed correction matrix. Resetting to identity.");
  671. reset_bed_offset_and_skew();
  672. world2machine_reset();
  673. } else {
  674. world2machine_update(vec_x, vec_y, cntr);
  675. /*
  676. SERIAL_ECHOPGM("world2machine_initialize() loaded: ");
  677. MYSERIAL.print(world2machine_rotation_and_skew[0][0], 5);
  678. SERIAL_ECHOPGM(", ");
  679. MYSERIAL.print(world2machine_rotation_and_skew[0][1], 5);
  680. SERIAL_ECHOPGM(", ");
  681. MYSERIAL.print(world2machine_rotation_and_skew[1][0], 5);
  682. SERIAL_ECHOPGM(", ");
  683. MYSERIAL.print(world2machine_rotation_and_skew[1][1], 5);
  684. SERIAL_ECHOPGM(", offset ");
  685. MYSERIAL.print(world2machine_shift[0], 5);
  686. SERIAL_ECHOPGM(", ");
  687. MYSERIAL.print(world2machine_shift[1], 5);
  688. SERIAL_ECHOLNPGM("");
  689. */
  690. }
  691. }
  692. // When switching from absolute to corrected coordinates,
  693. // this will get the absolute coordinates from the servos,
  694. // applies the inverse world2machine transformation
  695. // and stores the result into current_position[x,y].
  696. void world2machine_update_current()
  697. {
  698. float x = current_position[X_AXIS] - world2machine_shift[0];
  699. float y = current_position[Y_AXIS] - world2machine_shift[1];
  700. current_position[X_AXIS] = world2machine_rotation_and_skew_inv[0][0] * x + world2machine_rotation_and_skew_inv[0][1] * y;
  701. current_position[Y_AXIS] = world2machine_rotation_and_skew_inv[1][0] * x + world2machine_rotation_and_skew_inv[1][1] * y;
  702. }
  703. static inline void go_xyz(float x, float y, float z, float fr)
  704. {
  705. plan_buffer_line(x, y, z, current_position[E_AXIS], fr, active_extruder);
  706. st_synchronize();
  707. }
  708. static inline void go_xy(float x, float y, float fr)
  709. {
  710. plan_buffer_line(x, y, current_position[Z_AXIS], current_position[E_AXIS], fr, active_extruder);
  711. st_synchronize();
  712. }
  713. static inline void go_to_current(float fr)
  714. {
  715. plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], fr, active_extruder);
  716. st_synchronize();
  717. }
  718. static inline void update_current_position_xyz()
  719. {
  720. current_position[X_AXIS] = st_get_position_mm(X_AXIS);
  721. current_position[Y_AXIS] = st_get_position_mm(Y_AXIS);
  722. current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
  723. plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
  724. }
  725. static inline void update_current_position_z()
  726. {
  727. current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
  728. plan_set_z_position(current_position[Z_AXIS]);
  729. }
  730. // At the current position, find the Z stop.
  731. inline bool find_bed_induction_sensor_point_z(float minimum_z, uint8_t n_iter, int verbosity_level)
  732. {
  733. if(verbosity_level >= 10) SERIAL_ECHOLNPGM("find bed induction sensor point z");
  734. bool endstops_enabled = enable_endstops(true);
  735. bool endstop_z_enabled = enable_z_endstop(false);
  736. float z = 0.f;
  737. endstop_z_hit_on_purpose();
  738. // move down until you find the bed
  739. current_position[Z_AXIS] = minimum_z;
  740. go_to_current(homing_feedrate[Z_AXIS]/60);
  741. // we have to let the planner know where we are right now as it is not where we said to go.
  742. update_current_position_z();
  743. if (! endstop_z_hit_on_purpose())
  744. goto error;
  745. for (uint8_t i = 0; i < n_iter; ++ i) {
  746. // Move up the retract distance.
  747. current_position[Z_AXIS] += .5f;
  748. go_to_current(homing_feedrate[Z_AXIS]/60);
  749. // Move back down slowly to find bed.
  750. current_position[Z_AXIS] = minimum_z;
  751. go_to_current(homing_feedrate[Z_AXIS]/(4*60));
  752. // we have to let the planner know where we are right now as it is not where we said to go.
  753. update_current_position_z();
  754. if (! endstop_z_hit_on_purpose())
  755. goto error;
  756. // SERIAL_ECHOPGM("Bed find_bed_induction_sensor_point_z low, height: ");
  757. // MYSERIAL.print(current_position[Z_AXIS], 5);
  758. // SERIAL_ECHOLNPGM("");
  759. z += current_position[Z_AXIS];
  760. }
  761. current_position[Z_AXIS] = z;
  762. if (n_iter > 1)
  763. current_position[Z_AXIS] /= float(n_iter);
  764. enable_endstops(endstops_enabled);
  765. enable_z_endstop(endstop_z_enabled);
  766. // SERIAL_ECHOLNPGM("find_bed_induction_sensor_point_z 3");
  767. return true;
  768. error:
  769. // SERIAL_ECHOLNPGM("find_bed_induction_sensor_point_z 4");
  770. enable_endstops(endstops_enabled);
  771. enable_z_endstop(endstop_z_enabled);
  772. return false;
  773. }
  774. // Search around the current_position[X,Y],
  775. // look for the induction sensor response.
  776. // Adjust the current_position[X,Y,Z] to the center of the target dot and its response Z coordinate.
  777. #define FIND_BED_INDUCTION_SENSOR_POINT_X_RADIUS (8.f)
  778. #define FIND_BED_INDUCTION_SENSOR_POINT_Y_RADIUS (6.f)
  779. #define FIND_BED_INDUCTION_SENSOR_POINT_XY_STEP (1.f)
  780. #define FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP (0.2f)
  781. inline bool find_bed_induction_sensor_point_xy(int verbosity_level)
  782. {
  783. if(verbosity_level >= 10) MYSERIAL.println("find bed induction sensor point xy");
  784. float feedrate = homing_feedrate[X_AXIS] / 60.f;
  785. bool found = false;
  786. {
  787. float x0 = current_position[X_AXIS] - FIND_BED_INDUCTION_SENSOR_POINT_X_RADIUS;
  788. float x1 = current_position[X_AXIS] + FIND_BED_INDUCTION_SENSOR_POINT_X_RADIUS;
  789. float y0 = current_position[Y_AXIS] - FIND_BED_INDUCTION_SENSOR_POINT_Y_RADIUS;
  790. float y1 = current_position[Y_AXIS] + FIND_BED_INDUCTION_SENSOR_POINT_Y_RADIUS;
  791. uint8_t nsteps_y;
  792. uint8_t i;
  793. if (x0 < X_MIN_POS) {
  794. x0 = X_MIN_POS;
  795. if (verbosity_level >= 20) SERIAL_ECHOLNPGM("X searching radius lower than X_MIN. Clamping was done.");
  796. }
  797. if (x1 > X_MAX_POS) {
  798. x1 = X_MAX_POS;
  799. if (verbosity_level >= 20) SERIAL_ECHOLNPGM("X searching radius higher than X_MAX. Clamping was done.");
  800. }
  801. if (y0 < Y_MIN_POS_FOR_BED_CALIBRATION) {
  802. y0 = Y_MIN_POS_FOR_BED_CALIBRATION;
  803. if (verbosity_level >= 20) SERIAL_ECHOLNPGM("Y searching radius lower than Y_MIN. Clamping was done.");
  804. }
  805. if (y1 > Y_MAX_POS) {
  806. y1 = Y_MAX_POS;
  807. if (verbosity_level >= 20) SERIAL_ECHOLNPGM("Y searching radius higher than X_MAX. Clamping was done.");
  808. }
  809. nsteps_y = int(ceil((y1 - y0) / FIND_BED_INDUCTION_SENSOR_POINT_XY_STEP));
  810. enable_endstops(false);
  811. bool dir_positive = true;
  812. // go_xyz(current_position[X_AXIS], current_position[Y_AXIS], MESH_HOME_Z_SEARCH, homing_feedrate[Z_AXIS]/60);
  813. go_xyz(x0, y0, current_position[Z_AXIS], feedrate);
  814. // Continously lower the Z axis.
  815. endstops_hit_on_purpose();
  816. enable_z_endstop(true);
  817. while (current_position[Z_AXIS] > -10.f) {
  818. // Do nsteps_y zig-zag movements.
  819. current_position[Y_AXIS] = y0;
  820. for (i = 0; i < nsteps_y; current_position[Y_AXIS] += (y1 - y0) / float(nsteps_y - 1), ++ i) {
  821. // Run with a slightly decreasing Z axis, zig-zag movement. Stop at the Z end-stop.
  822. current_position[Z_AXIS] -= FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP / float(nsteps_y);
  823. go_xyz(dir_positive ? x1 : x0, current_position[Y_AXIS], current_position[Z_AXIS], feedrate);
  824. dir_positive = ! dir_positive;
  825. if (endstop_z_hit_on_purpose())
  826. goto endloop;
  827. }
  828. for (i = 0; i < nsteps_y; current_position[Y_AXIS] -= (y1 - y0) / float(nsteps_y - 1), ++ i) {
  829. // Run with a slightly decreasing Z axis, zig-zag movement. Stop at the Z end-stop.
  830. current_position[Z_AXIS] -= FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP / float(nsteps_y);
  831. go_xyz(dir_positive ? x1 : x0, current_position[Y_AXIS], current_position[Z_AXIS], feedrate);
  832. dir_positive = ! dir_positive;
  833. if (endstop_z_hit_on_purpose())
  834. goto endloop;
  835. }
  836. }
  837. endloop:
  838. // SERIAL_ECHOLN("First hit");
  839. // we have to let the planner know where we are right now as it is not where we said to go.
  840. update_current_position_xyz();
  841. // Search in this plane for the first hit. Zig-zag first in X, then in Y axis.
  842. for (int8_t iter = 0; iter < 3; ++ iter) {
  843. if (iter > 0) {
  844. // Slightly lower the Z axis to get a reliable trigger.
  845. current_position[Z_AXIS] -= 0.02f;
  846. go_xyz(current_position[X_AXIS], current_position[Y_AXIS], MESH_HOME_Z_SEARCH, homing_feedrate[Z_AXIS]/60);
  847. }
  848. // Do nsteps_y zig-zag movements.
  849. float a, b;
  850. enable_endstops(false);
  851. enable_z_endstop(false);
  852. current_position[Y_AXIS] = y0;
  853. go_xy(x0, current_position[Y_AXIS], feedrate);
  854. enable_z_endstop(true);
  855. found = false;
  856. for (i = 0, dir_positive = true; i < nsteps_y; current_position[Y_AXIS] += (y1 - y0) / float(nsteps_y - 1), ++ i, dir_positive = ! dir_positive) {
  857. go_xy(dir_positive ? x1 : x0, current_position[Y_AXIS], feedrate);
  858. if (endstop_z_hit_on_purpose()) {
  859. found = true;
  860. break;
  861. }
  862. }
  863. update_current_position_xyz();
  864. if (! found) {
  865. // SERIAL_ECHOLN("Search in Y - not found");
  866. continue;
  867. }
  868. // SERIAL_ECHOLN("Search in Y - found");
  869. a = current_position[Y_AXIS];
  870. enable_z_endstop(false);
  871. current_position[Y_AXIS] = y1;
  872. go_xy(x0, current_position[Y_AXIS], feedrate);
  873. enable_z_endstop(true);
  874. found = false;
  875. for (i = 0, dir_positive = true; i < nsteps_y; current_position[Y_AXIS] -= (y1 - y0) / float(nsteps_y - 1), ++ i, dir_positive = ! dir_positive) {
  876. go_xy(dir_positive ? x1 : x0, current_position[Y_AXIS], feedrate);
  877. if (endstop_z_hit_on_purpose()) {
  878. found = true;
  879. break;
  880. }
  881. }
  882. update_current_position_xyz();
  883. if (! found) {
  884. // SERIAL_ECHOLN("Search in Y2 - not found");
  885. continue;
  886. }
  887. // SERIAL_ECHOLN("Search in Y2 - found");
  888. b = current_position[Y_AXIS];
  889. current_position[Y_AXIS] = 0.5f * (a + b);
  890. // Search in the X direction along a cross.
  891. found = false;
  892. enable_z_endstop(false);
  893. go_xy(x0, current_position[Y_AXIS], feedrate);
  894. enable_z_endstop(true);
  895. go_xy(x1, current_position[Y_AXIS], feedrate);
  896. update_current_position_xyz();
  897. if (! endstop_z_hit_on_purpose()) {
  898. // SERIAL_ECHOLN("Search X span 0 - not found");
  899. continue;
  900. }
  901. // SERIAL_ECHOLN("Search X span 0 - found");
  902. a = current_position[X_AXIS];
  903. enable_z_endstop(false);
  904. go_xy(x1, current_position[Y_AXIS], feedrate);
  905. enable_z_endstop(true);
  906. go_xy(x0, current_position[Y_AXIS], feedrate);
  907. update_current_position_xyz();
  908. if (! endstop_z_hit_on_purpose()) {
  909. // SERIAL_ECHOLN("Search X span 1 - not found");
  910. continue;
  911. }
  912. // SERIAL_ECHOLN("Search X span 1 - found");
  913. b = current_position[X_AXIS];
  914. // Go to the center.
  915. enable_z_endstop(false);
  916. current_position[X_AXIS] = 0.5f * (a + b);
  917. go_xy(current_position[X_AXIS], current_position[Y_AXIS], feedrate);
  918. found = true;
  919. #if 1
  920. // Search in the Y direction along a cross.
  921. found = false;
  922. enable_z_endstop(false);
  923. go_xy(current_position[X_AXIS], y0, feedrate);
  924. enable_z_endstop(true);
  925. go_xy(current_position[X_AXIS], y1, feedrate);
  926. update_current_position_xyz();
  927. if (! endstop_z_hit_on_purpose()) {
  928. // SERIAL_ECHOLN("Search Y2 span 0 - not found");
  929. continue;
  930. }
  931. // SERIAL_ECHOLN("Search Y2 span 0 - found");
  932. a = current_position[Y_AXIS];
  933. enable_z_endstop(false);
  934. go_xy(current_position[X_AXIS], y1, feedrate);
  935. enable_z_endstop(true);
  936. go_xy(current_position[X_AXIS], y0, feedrate);
  937. update_current_position_xyz();
  938. if (! endstop_z_hit_on_purpose()) {
  939. // SERIAL_ECHOLN("Search Y2 span 1 - not found");
  940. continue;
  941. }
  942. // SERIAL_ECHOLN("Search Y2 span 1 - found");
  943. b = current_position[Y_AXIS];
  944. // Go to the center.
  945. enable_z_endstop(false);
  946. current_position[Y_AXIS] = 0.5f * (a + b);
  947. go_xy(current_position[X_AXIS], current_position[Y_AXIS], feedrate);
  948. found = true;
  949. #endif
  950. break;
  951. }
  952. }
  953. enable_z_endstop(false);
  954. return found;
  955. }
  956. // Search around the current_position[X,Y,Z].
  957. // It is expected, that the induction sensor is switched on at the current position.
  958. // Look around this center point by painting a star around the point.
  959. inline bool improve_bed_induction_sensor_point()
  960. {
  961. static const float search_radius = 8.f;
  962. bool endstops_enabled = enable_endstops(false);
  963. bool endstop_z_enabled = enable_z_endstop(false);
  964. bool found = false;
  965. float feedrate = homing_feedrate[X_AXIS] / 60.f;
  966. float center_old_x = current_position[X_AXIS];
  967. float center_old_y = current_position[Y_AXIS];
  968. float center_x = 0.f;
  969. float center_y = 0.f;
  970. for (uint8_t iter = 0; iter < 4; ++ iter) {
  971. switch (iter) {
  972. case 0:
  973. destination[X_AXIS] = center_old_x - search_radius * 0.707;
  974. destination[Y_AXIS] = center_old_y - search_radius * 0.707;
  975. break;
  976. case 1:
  977. destination[X_AXIS] = center_old_x + search_radius * 0.707;
  978. destination[Y_AXIS] = center_old_y + search_radius * 0.707;
  979. break;
  980. case 2:
  981. destination[X_AXIS] = center_old_x + search_radius * 0.707;
  982. destination[Y_AXIS] = center_old_y - search_radius * 0.707;
  983. break;
  984. case 3:
  985. default:
  986. destination[X_AXIS] = center_old_x - search_radius * 0.707;
  987. destination[Y_AXIS] = center_old_y + search_radius * 0.707;
  988. break;
  989. }
  990. // Trim the vector from center_old_[x,y] to destination[x,y] by the bed dimensions.
  991. float vx = destination[X_AXIS] - center_old_x;
  992. float vy = destination[Y_AXIS] - center_old_y;
  993. float l = sqrt(vx*vx+vy*vy);
  994. float t;
  995. if (destination[X_AXIS] < X_MIN_POS) {
  996. // Exiting the bed at xmin.
  997. t = (center_x - X_MIN_POS) / l;
  998. destination[X_AXIS] = X_MIN_POS;
  999. destination[Y_AXIS] = center_old_y + t * vy;
  1000. } else if (destination[X_AXIS] > X_MAX_POS) {
  1001. // Exiting the bed at xmax.
  1002. t = (X_MAX_POS - center_x) / l;
  1003. destination[X_AXIS] = X_MAX_POS;
  1004. destination[Y_AXIS] = center_old_y + t * vy;
  1005. }
  1006. if (destination[Y_AXIS] < Y_MIN_POS_FOR_BED_CALIBRATION) {
  1007. // Exiting the bed at ymin.
  1008. t = (center_y - Y_MIN_POS_FOR_BED_CALIBRATION) / l;
  1009. destination[X_AXIS] = center_old_x + t * vx;
  1010. destination[Y_AXIS] = Y_MIN_POS_FOR_BED_CALIBRATION;
  1011. } else if (destination[Y_AXIS] > Y_MAX_POS) {
  1012. // Exiting the bed at xmax.
  1013. t = (Y_MAX_POS - center_y) / l;
  1014. destination[X_AXIS] = center_old_x + t * vx;
  1015. destination[Y_AXIS] = Y_MAX_POS;
  1016. }
  1017. // Move away from the measurement point.
  1018. enable_endstops(false);
  1019. go_xy(destination[X_AXIS], destination[Y_AXIS], feedrate);
  1020. // Move towards the measurement point, until the induction sensor triggers.
  1021. enable_endstops(true);
  1022. go_xy(center_old_x, center_old_y, feedrate);
  1023. update_current_position_xyz();
  1024. // if (! endstop_z_hit_on_purpose()) return false;
  1025. center_x += current_position[X_AXIS];
  1026. center_y += current_position[Y_AXIS];
  1027. }
  1028. // Calculate the new center, move to the new center.
  1029. center_x /= 4.f;
  1030. center_y /= 4.f;
  1031. current_position[X_AXIS] = center_x;
  1032. current_position[Y_AXIS] = center_y;
  1033. enable_endstops(false);
  1034. go_xy(current_position[X_AXIS], current_position[Y_AXIS], feedrate);
  1035. enable_endstops(endstops_enabled);
  1036. enable_z_endstop(endstop_z_enabled);
  1037. return found;
  1038. }
  1039. static inline void debug_output_point(const char *type, const float &x, const float &y, const float &z)
  1040. {
  1041. SERIAL_ECHOPGM("Measured ");
  1042. SERIAL_ECHORPGM(type);
  1043. SERIAL_ECHOPGM(" ");
  1044. MYSERIAL.print(x, 5);
  1045. SERIAL_ECHOPGM(", ");
  1046. MYSERIAL.print(y, 5);
  1047. SERIAL_ECHOPGM(", ");
  1048. MYSERIAL.print(z, 5);
  1049. SERIAL_ECHOLNPGM("");
  1050. }
  1051. // Search around the current_position[X,Y,Z].
  1052. // It is expected, that the induction sensor is switched on at the current position.
  1053. // Look around this center point by painting a star around the point.
  1054. #define IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS (8.f)
  1055. inline bool improve_bed_induction_sensor_point2(bool lift_z_on_min_y, int8_t verbosity_level)
  1056. {
  1057. float center_old_x = current_position[X_AXIS];
  1058. float center_old_y = current_position[Y_AXIS];
  1059. float a, b;
  1060. bool point_small = false;
  1061. enable_endstops(false);
  1062. {
  1063. float x0 = center_old_x - IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS;
  1064. float x1 = center_old_x + IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS;
  1065. if (x0 < X_MIN_POS)
  1066. x0 = X_MIN_POS;
  1067. if (x1 > X_MAX_POS)
  1068. x1 = X_MAX_POS;
  1069. // Search in the X direction along a cross.
  1070. enable_z_endstop(false);
  1071. go_xy(x0, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
  1072. enable_z_endstop(true);
  1073. go_xy(x1, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
  1074. update_current_position_xyz();
  1075. if (! endstop_z_hit_on_purpose()) {
  1076. current_position[X_AXIS] = center_old_x;
  1077. goto canceled;
  1078. }
  1079. a = current_position[X_AXIS];
  1080. enable_z_endstop(false);
  1081. go_xy(x1, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
  1082. enable_z_endstop(true);
  1083. go_xy(x0, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
  1084. update_current_position_xyz();
  1085. if (! endstop_z_hit_on_purpose()) {
  1086. current_position[X_AXIS] = center_old_x;
  1087. goto canceled;
  1088. }
  1089. b = current_position[X_AXIS];
  1090. if (b - a < MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
  1091. if (verbosity_level >= 5) {
  1092. SERIAL_ECHOPGM("Point width too small: ");
  1093. SERIAL_ECHO(b - a);
  1094. SERIAL_ECHOLNPGM("");
  1095. }
  1096. // We force the calibration routine to move the Z axis slightly down to make the response more pronounced.
  1097. if (b - a < 0.5f * MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
  1098. // Don't use the new X value.
  1099. current_position[X_AXIS] = center_old_x;
  1100. goto canceled;
  1101. } else {
  1102. // Use the new value, but force the Z axis to go a bit lower.
  1103. point_small = true;
  1104. }
  1105. }
  1106. if (verbosity_level >= 5) {
  1107. debug_output_point(PSTR("left" ), a, current_position[Y_AXIS], current_position[Z_AXIS]);
  1108. debug_output_point(PSTR("right"), b, current_position[Y_AXIS], current_position[Z_AXIS]);
  1109. }
  1110. // Go to the center.
  1111. enable_z_endstop(false);
  1112. current_position[X_AXIS] = 0.5f * (a + b);
  1113. go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
  1114. }
  1115. {
  1116. float y0 = center_old_y - IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS;
  1117. float y1 = center_old_y + IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS;
  1118. if (y0 < Y_MIN_POS_FOR_BED_CALIBRATION)
  1119. y0 = Y_MIN_POS_FOR_BED_CALIBRATION;
  1120. if (y1 > Y_MAX_POS)
  1121. y1 = Y_MAX_POS;
  1122. // Search in the Y direction along a cross.
  1123. enable_z_endstop(false);
  1124. go_xy(current_position[X_AXIS], y0, homing_feedrate[X_AXIS] / 60.f);
  1125. if (lift_z_on_min_y) {
  1126. // The first row of points are very close to the end stop.
  1127. // Lift the sensor to disengage the trigger. This is necessary because of the sensor hysteresis.
  1128. go_xyz(current_position[X_AXIS], y0, current_position[Z_AXIS]+1.5f, homing_feedrate[Z_AXIS] / 60.f);
  1129. // and go back.
  1130. go_xyz(current_position[X_AXIS], y0, current_position[Z_AXIS], homing_feedrate[Z_AXIS] / 60.f);
  1131. }
  1132. if (lift_z_on_min_y && (READ(Z_MIN_PIN) ^ Z_MIN_ENDSTOP_INVERTING) == 1) {
  1133. // Already triggering before we started the move.
  1134. // Shift the trigger point slightly outwards.
  1135. // a = current_position[Y_AXIS] - 1.5f;
  1136. a = current_position[Y_AXIS];
  1137. } else {
  1138. enable_z_endstop(true);
  1139. go_xy(current_position[X_AXIS], y1, homing_feedrate[X_AXIS] / 60.f);
  1140. update_current_position_xyz();
  1141. if (! endstop_z_hit_on_purpose()) {
  1142. current_position[Y_AXIS] = center_old_y;
  1143. goto canceled;
  1144. }
  1145. a = current_position[Y_AXIS];
  1146. }
  1147. enable_z_endstop(false);
  1148. go_xy(current_position[X_AXIS], y1, homing_feedrate[X_AXIS] / 60.f);
  1149. enable_z_endstop(true);
  1150. go_xy(current_position[X_AXIS], y0, homing_feedrate[X_AXIS] / 60.f);
  1151. update_current_position_xyz();
  1152. if (! endstop_z_hit_on_purpose()) {
  1153. current_position[Y_AXIS] = center_old_y;
  1154. goto canceled;
  1155. }
  1156. b = current_position[Y_AXIS];
  1157. if (b - a < MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
  1158. // We force the calibration routine to move the Z axis slightly down to make the response more pronounced.
  1159. if (verbosity_level >= 5) {
  1160. SERIAL_ECHOPGM("Point height too small: ");
  1161. SERIAL_ECHO(b - a);
  1162. SERIAL_ECHOLNPGM("");
  1163. }
  1164. if (b - a < 0.5f * MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
  1165. // Don't use the new Y value.
  1166. current_position[Y_AXIS] = center_old_y;
  1167. goto canceled;
  1168. } else {
  1169. // Use the new value, but force the Z axis to go a bit lower.
  1170. point_small = true;
  1171. }
  1172. }
  1173. if (verbosity_level >= 5) {
  1174. debug_output_point(PSTR("top" ), current_position[X_AXIS], a, current_position[Z_AXIS]);
  1175. debug_output_point(PSTR("bottom"), current_position[X_AXIS], b, current_position[Z_AXIS]);
  1176. }
  1177. // Go to the center.
  1178. enable_z_endstop(false);
  1179. current_position[Y_AXIS] = 0.5f * (a + b);
  1180. go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
  1181. }
  1182. // If point is small but not too small, then force the Z axis to be lowered a bit,
  1183. // but use the new value. This is important when the initial position was off in one axis,
  1184. // for example if the initial calibration was shifted in the Y axis systematically.
  1185. // Then this first step will center.
  1186. return ! point_small;
  1187. canceled:
  1188. // Go back to the center.
  1189. enable_z_endstop(false);
  1190. go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
  1191. return false;
  1192. }
  1193. // Searching the front points, where one cannot move the sensor head in front of the sensor point.
  1194. // Searching in a zig-zag movement in a plane for the maximum width of the response.
  1195. // This function may set the current_position[Y_AXIS] below Y_MIN_POS, if the function succeeded.
  1196. // If this function failed, the Y coordinate will never be outside the working space.
  1197. #define IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS (4.f)
  1198. #define IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y (0.1f)
  1199. inline bool improve_bed_induction_sensor_point3(int verbosity_level)
  1200. {
  1201. float center_old_x = current_position[X_AXIS];
  1202. float center_old_y = current_position[Y_AXIS];
  1203. float a, b;
  1204. bool result = true;
  1205. if (verbosity_level >= 20) MYSERIAL.println("Improve bed induction sensor point3");
  1206. // Was the sensor point detected too far in the minus Y axis?
  1207. // If yes, the center of the induction point cannot be reached by the machine.
  1208. {
  1209. float x0 = center_old_x - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
  1210. float x1 = center_old_x + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
  1211. float y0 = center_old_y - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
  1212. float y1 = center_old_y + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
  1213. float y = y0;
  1214. if (x0 < X_MIN_POS)
  1215. x0 = X_MIN_POS;
  1216. if (x1 > X_MAX_POS)
  1217. x1 = X_MAX_POS;
  1218. if (y0 < Y_MIN_POS_FOR_BED_CALIBRATION)
  1219. y0 = Y_MIN_POS_FOR_BED_CALIBRATION;
  1220. if (y1 > Y_MAX_POS)
  1221. y1 = Y_MAX_POS;
  1222. if (verbosity_level >= 20) {
  1223. SERIAL_ECHOPGM("Initial position: ");
  1224. SERIAL_ECHO(center_old_x);
  1225. SERIAL_ECHOPGM(", ");
  1226. SERIAL_ECHO(center_old_y);
  1227. SERIAL_ECHOLNPGM("");
  1228. }
  1229. // Search in the positive Y direction, until a maximum diameter is found.
  1230. // (the next diameter is smaller than the current one.)
  1231. float dmax = 0.f;
  1232. float xmax1 = 0.f;
  1233. float xmax2 = 0.f;
  1234. for (y = y0; y < y1; y += IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
  1235. enable_z_endstop(false);
  1236. go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
  1237. enable_z_endstop(true);
  1238. go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
  1239. update_current_position_xyz();
  1240. if (! endstop_z_hit_on_purpose()) {
  1241. continue;
  1242. // SERIAL_PROTOCOLPGM("Failed 1\n");
  1243. // current_position[X_AXIS] = center_old_x;
  1244. // goto canceled;
  1245. }
  1246. a = current_position[X_AXIS];
  1247. enable_z_endstop(false);
  1248. go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
  1249. enable_z_endstop(true);
  1250. go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
  1251. update_current_position_xyz();
  1252. if (! endstop_z_hit_on_purpose()) {
  1253. continue;
  1254. // SERIAL_PROTOCOLPGM("Failed 2\n");
  1255. // current_position[X_AXIS] = center_old_x;
  1256. // goto canceled;
  1257. }
  1258. b = current_position[X_AXIS];
  1259. if (verbosity_level >= 5) {
  1260. debug_output_point(PSTR("left" ), a, current_position[Y_AXIS], current_position[Z_AXIS]);
  1261. debug_output_point(PSTR("right"), b, current_position[Y_AXIS], current_position[Z_AXIS]);
  1262. }
  1263. float d = b - a;
  1264. if (d > dmax) {
  1265. xmax1 = 0.5f * (a + b);
  1266. dmax = d;
  1267. } else if (dmax > 0.) {
  1268. y0 = y - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y;
  1269. break;
  1270. }
  1271. }
  1272. if (dmax == 0.) {
  1273. if (verbosity_level > 0)
  1274. SERIAL_PROTOCOLPGM("failed - not found\n");
  1275. current_position[X_AXIS] = center_old_x;
  1276. current_position[Y_AXIS] = center_old_y;
  1277. goto canceled;
  1278. }
  1279. {
  1280. // Find the positive Y hit. This gives the extreme Y value for the search of the maximum diameter in the -Y direction.
  1281. enable_z_endstop(false);
  1282. go_xy(xmax1, y0 + IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS, homing_feedrate[X_AXIS] / 60.f);
  1283. enable_z_endstop(true);
  1284. go_xy(xmax1, max(y0 - IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS, Y_MIN_POS_FOR_BED_CALIBRATION), homing_feedrate[X_AXIS] / 60.f);
  1285. update_current_position_xyz();
  1286. if (! endstop_z_hit_on_purpose()) {
  1287. current_position[Y_AXIS] = center_old_y;
  1288. goto canceled;
  1289. }
  1290. if (verbosity_level >= 5)
  1291. debug_output_point(PSTR("top" ), current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
  1292. y1 = current_position[Y_AXIS];
  1293. }
  1294. if (y1 <= y0) {
  1295. // Either the induction sensor is too high, or the induction sensor target is out of reach.
  1296. current_position[Y_AXIS] = center_old_y;
  1297. goto canceled;
  1298. }
  1299. // Search in the negative Y direction, until a maximum diameter is found.
  1300. dmax = 0.f;
  1301. // if (y0 + 1.f < y1)
  1302. // y1 = y0 + 1.f;
  1303. for (y = y1; y >= y0; y -= IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
  1304. enable_z_endstop(false);
  1305. go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
  1306. enable_z_endstop(true);
  1307. go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
  1308. update_current_position_xyz();
  1309. if (! endstop_z_hit_on_purpose()) {
  1310. continue;
  1311. /*
  1312. current_position[X_AXIS] = center_old_x;
  1313. SERIAL_PROTOCOLPGM("Failed 3\n");
  1314. goto canceled;
  1315. */
  1316. }
  1317. a = current_position[X_AXIS];
  1318. enable_z_endstop(false);
  1319. go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
  1320. enable_z_endstop(true);
  1321. go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
  1322. update_current_position_xyz();
  1323. if (! endstop_z_hit_on_purpose()) {
  1324. continue;
  1325. /*
  1326. current_position[X_AXIS] = center_old_x;
  1327. SERIAL_PROTOCOLPGM("Failed 4\n");
  1328. goto canceled;
  1329. */
  1330. }
  1331. b = current_position[X_AXIS];
  1332. if (verbosity_level >= 5) {
  1333. debug_output_point(PSTR("left" ), a, current_position[Y_AXIS], current_position[Z_AXIS]);
  1334. debug_output_point(PSTR("right"), b, current_position[Y_AXIS], current_position[Z_AXIS]);
  1335. }
  1336. float d = b - a;
  1337. if (d > dmax) {
  1338. xmax2 = 0.5f * (a + b);
  1339. dmax = d;
  1340. } else if (dmax > 0.) {
  1341. y1 = y + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y;
  1342. break;
  1343. }
  1344. }
  1345. float xmax, ymax;
  1346. if (dmax == 0.f) {
  1347. // Only the hit in the positive direction found.
  1348. xmax = xmax1;
  1349. ymax = y0;
  1350. } else {
  1351. // Both positive and negative directions found.
  1352. xmax = xmax2;
  1353. ymax = 0.5f * (y0 + y1);
  1354. for (; y >= y0; y -= IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
  1355. enable_z_endstop(false);
  1356. go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
  1357. enable_z_endstop(true);
  1358. go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
  1359. update_current_position_xyz();
  1360. if (! endstop_z_hit_on_purpose()) {
  1361. continue;
  1362. /*
  1363. current_position[X_AXIS] = center_old_x;
  1364. SERIAL_PROTOCOLPGM("Failed 3\n");
  1365. goto canceled;
  1366. */
  1367. }
  1368. a = current_position[X_AXIS];
  1369. enable_z_endstop(false);
  1370. go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
  1371. enable_z_endstop(true);
  1372. go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
  1373. update_current_position_xyz();
  1374. if (! endstop_z_hit_on_purpose()) {
  1375. continue;
  1376. /*
  1377. current_position[X_AXIS] = center_old_x;
  1378. SERIAL_PROTOCOLPGM("Failed 4\n");
  1379. goto canceled;
  1380. */
  1381. }
  1382. b = current_position[X_AXIS];
  1383. if (verbosity_level >= 5) {
  1384. debug_output_point(PSTR("left" ), a, current_position[Y_AXIS], current_position[Z_AXIS]);
  1385. debug_output_point(PSTR("right"), b, current_position[Y_AXIS], current_position[Z_AXIS]);
  1386. }
  1387. float d = b - a;
  1388. if (d > dmax) {
  1389. xmax = 0.5f * (a + b);
  1390. ymax = y;
  1391. dmax = d;
  1392. }
  1393. }
  1394. }
  1395. {
  1396. // Compare the distance in the Y+ direction with the diameter in the X direction.
  1397. // Find the positive Y hit once again, this time along the Y axis going through the X point with the highest diameter.
  1398. enable_z_endstop(false);
  1399. go_xy(xmax, ymax + IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS, homing_feedrate[X_AXIS] / 60.f);
  1400. enable_z_endstop(true);
  1401. go_xy(xmax, max(ymax - IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS, Y_MIN_POS_FOR_BED_CALIBRATION), homing_feedrate[X_AXIS] / 60.f);
  1402. update_current_position_xyz();
  1403. if (! endstop_z_hit_on_purpose()) {
  1404. current_position[Y_AXIS] = center_old_y;
  1405. goto canceled;
  1406. }
  1407. if (verbosity_level >= 5)
  1408. debug_output_point(PSTR("top" ), current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
  1409. if (current_position[Y_AXIS] - Y_MIN_POS_FOR_BED_CALIBRATION < 0.5f * dmax) {
  1410. // Probably not even a half circle was detected. The induction point is likely too far in the minus Y direction.
  1411. // First verify, if the measurement has been done at a sufficient height. If no, lower the Z axis a bit.
  1412. if (current_position[Y_AXIS] < ymax || dmax < 0.5f * MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
  1413. if (verbosity_level >= 5) {
  1414. SERIAL_ECHOPGM("Partial point diameter too small: ");
  1415. SERIAL_ECHO(dmax);
  1416. SERIAL_ECHOLNPGM("");
  1417. }
  1418. result = false;
  1419. } else {
  1420. // Estimate the circle radius from the maximum diameter and height:
  1421. float h = current_position[Y_AXIS] - ymax;
  1422. float r = dmax * dmax / (8.f * h) + 0.5f * h;
  1423. if (r < 0.8f * MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
  1424. if (verbosity_level >= 5) {
  1425. SERIAL_ECHOPGM("Partial point estimated radius too small: ");
  1426. SERIAL_ECHO(r);
  1427. SERIAL_ECHOPGM(", dmax:");
  1428. SERIAL_ECHO(dmax);
  1429. SERIAL_ECHOPGM(", h:");
  1430. SERIAL_ECHO(h);
  1431. SERIAL_ECHOLNPGM("");
  1432. }
  1433. result = false;
  1434. } else {
  1435. // The point may end up outside of the machine working space.
  1436. // That is all right as it helps to improve the accuracy of the measurement point
  1437. // due to averaging.
  1438. // For the y correction, use an average of dmax/2 and the estimated radius.
  1439. r = 0.5f * (0.5f * dmax + r);
  1440. ymax = current_position[Y_AXIS] - r;
  1441. }
  1442. }
  1443. } else {
  1444. // If the diameter of the detected spot was smaller than a minimum allowed,
  1445. // the induction sensor is probably too high. Returning false will force
  1446. // the sensor to be lowered a tiny bit.
  1447. result = xmax >= MIN_BED_SENSOR_POINT_RESPONSE_DMR;
  1448. if (y0 > Y_MIN_POS_FOR_BED_CALIBRATION + 0.2f)
  1449. // Only in case both left and right y tangents are known, use them.
  1450. // If y0 is close to the bed edge, it may not be symmetric to the right tangent.
  1451. ymax = 0.5f * ymax + 0.25f * (y0 + y1);
  1452. }
  1453. }
  1454. // Go to the center.
  1455. enable_z_endstop(false);
  1456. current_position[X_AXIS] = xmax;
  1457. current_position[Y_AXIS] = ymax;
  1458. if (verbosity_level >= 20) {
  1459. SERIAL_ECHOPGM("Adjusted position: ");
  1460. SERIAL_ECHO(current_position[X_AXIS]);
  1461. SERIAL_ECHOPGM(", ");
  1462. SERIAL_ECHO(current_position[Y_AXIS]);
  1463. SERIAL_ECHOLNPGM("");
  1464. }
  1465. // Don't clamp current_position[Y_AXIS], because the out-of-reach Y coordinate may actually be true.
  1466. // Only clamp the coordinate to go.
  1467. go_xy(current_position[X_AXIS], max(Y_MIN_POS, current_position[Y_AXIS]), homing_feedrate[X_AXIS] / 60.f);
  1468. // delay_keep_alive(3000);
  1469. }
  1470. if (result)
  1471. return true;
  1472. // otherwise clamp the Y coordinate
  1473. canceled:
  1474. // Go back to the center.
  1475. enable_z_endstop(false);
  1476. if (current_position[Y_AXIS] < Y_MIN_POS)
  1477. current_position[Y_AXIS] = Y_MIN_POS;
  1478. go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
  1479. return false;
  1480. }
  1481. // Scan the mesh bed induction points one by one by a left-right zig-zag movement,
  1482. // write the trigger coordinates to the serial line.
  1483. // Useful for visualizing the behavior of the bed induction detector.
  1484. inline void scan_bed_induction_sensor_point()
  1485. {
  1486. float center_old_x = current_position[X_AXIS];
  1487. float center_old_y = current_position[Y_AXIS];
  1488. float x0 = center_old_x - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
  1489. float x1 = center_old_x + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
  1490. float y0 = center_old_y - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
  1491. float y1 = center_old_y + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
  1492. float y = y0;
  1493. if (x0 < X_MIN_POS)
  1494. x0 = X_MIN_POS;
  1495. if (x1 > X_MAX_POS)
  1496. x1 = X_MAX_POS;
  1497. if (y0 < Y_MIN_POS_FOR_BED_CALIBRATION)
  1498. y0 = Y_MIN_POS_FOR_BED_CALIBRATION;
  1499. if (y1 > Y_MAX_POS)
  1500. y1 = Y_MAX_POS;
  1501. for (float y = y0; y < y1; y += IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
  1502. enable_z_endstop(false);
  1503. go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
  1504. enable_z_endstop(true);
  1505. go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
  1506. update_current_position_xyz();
  1507. if (endstop_z_hit_on_purpose())
  1508. debug_output_point(PSTR("left" ), current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
  1509. enable_z_endstop(false);
  1510. go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
  1511. enable_z_endstop(true);
  1512. go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
  1513. update_current_position_xyz();
  1514. if (endstop_z_hit_on_purpose())
  1515. debug_output_point(PSTR("right"), current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
  1516. }
  1517. enable_z_endstop(false);
  1518. current_position[X_AXIS] = center_old_x;
  1519. current_position[Y_AXIS] = center_old_y;
  1520. go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
  1521. }
  1522. #define MESH_BED_CALIBRATION_SHOW_LCD
  1523. BedSkewOffsetDetectionResultType find_bed_offset_and_skew(int8_t verbosity_level, uint8_t &too_far_mask)
  1524. {
  1525. // Don't let the manage_inactivity() function remove power from the motors.
  1526. refresh_cmd_timeout();
  1527. // Reusing the z_values memory for the measurement cache.
  1528. // 7x7=49 floats, good for 16 (x,y,z) vectors.
  1529. float *pts = &mbl.z_values[0][0];
  1530. float *vec_x = pts + 2 * 4;
  1531. float *vec_y = vec_x + 2;
  1532. float *cntr = vec_y + 2;
  1533. memset(pts, 0, sizeof(float) * 7 * 7);
  1534. uint8_t iteration = 0;
  1535. BedSkewOffsetDetectionResultType result;
  1536. // SERIAL_ECHOLNPGM("find_bed_offset_and_skew verbosity level: ");
  1537. // SERIAL_ECHO(int(verbosity_level));
  1538. // SERIAL_ECHOPGM("");
  1539. while (iteration < 3) {
  1540. SERIAL_ECHOPGM("Iteration: ");
  1541. MYSERIAL.println(int(iteration + 1));
  1542. if (verbosity_level >= 20) {
  1543. SERIAL_ECHOLNPGM("Vectors: ");
  1544. SERIAL_ECHOPGM("vec_x[0]:");
  1545. MYSERIAL.print(vec_x[0], 5);
  1546. SERIAL_ECHOLNPGM("");
  1547. SERIAL_ECHOPGM("vec_x[1]:");
  1548. MYSERIAL.print(vec_x[1], 5);
  1549. SERIAL_ECHOLNPGM("");
  1550. SERIAL_ECHOPGM("vec_y[0]:");
  1551. MYSERIAL.print(vec_y[0], 5);
  1552. SERIAL_ECHOLNPGM("");
  1553. SERIAL_ECHOPGM("vec_y[1]:");
  1554. MYSERIAL.print(vec_y[1], 5);
  1555. SERIAL_ECHOLNPGM("");
  1556. SERIAL_ECHOPGM("cntr[0]:");
  1557. MYSERIAL.print(cntr[0], 5);
  1558. SERIAL_ECHOLNPGM("");
  1559. SERIAL_ECHOPGM("cntr[1]:");
  1560. MYSERIAL.print(cntr[1], 5);
  1561. SERIAL_ECHOLNPGM("");
  1562. }
  1563. #ifdef MESH_BED_CALIBRATION_SHOW_LCD
  1564. uint8_t next_line;
  1565. lcd_display_message_fullscreen_P(MSG_FIND_BED_OFFSET_AND_SKEW_LINE1, next_line);
  1566. if (next_line > 3)
  1567. next_line = 3;
  1568. #endif /* MESH_BED_CALIBRATION_SHOW_LCD */
  1569. // Collect the rear 2x3 points.
  1570. current_position[Z_AXIS] = MESH_HOME_Z_SEARCH + FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP * iteration * 0.3;
  1571. for (int k = 0; k < 4; ++k) {
  1572. // Don't let the manage_inactivity() function remove power from the motors.
  1573. refresh_cmd_timeout();
  1574. #ifdef MESH_BED_CALIBRATION_SHOW_LCD
  1575. lcd_implementation_print_at(0, next_line, k + 1);
  1576. lcd_printPGM(MSG_FIND_BED_OFFSET_AND_SKEW_LINE2);
  1577. if (iteration > 0) {
  1578. lcd_print_at_PGM(0, next_line + 1, MSG_FIND_BED_OFFSET_AND_SKEW_ITERATION);
  1579. lcd_implementation_print(int(iteration + 1));
  1580. }
  1581. #endif /* MESH_BED_CALIBRATION_SHOW_LCD */
  1582. float *pt = pts + k * 2;
  1583. // Go up to z_initial.
  1584. go_to_current(homing_feedrate[Z_AXIS] / 60.f);
  1585. if (verbosity_level >= 20) {
  1586. // Go to Y0, wait, then go to Y-4.
  1587. current_position[Y_AXIS] = 0.f;
  1588. go_to_current(homing_feedrate[X_AXIS] / 60.f);
  1589. SERIAL_ECHOLNPGM("At Y0");
  1590. delay_keep_alive(5000);
  1591. current_position[Y_AXIS] = Y_MIN_POS;
  1592. go_to_current(homing_feedrate[X_AXIS] / 60.f);
  1593. SERIAL_ECHOLNPGM("At Y-4");
  1594. delay_keep_alive(5000);
  1595. }
  1596. // Go to the measurement point position.
  1597. //if (iteration == 0) {
  1598. current_position[X_AXIS] = pgm_read_float(bed_ref_points_4 + k * 2);
  1599. current_position[Y_AXIS] = pgm_read_float(bed_ref_points_4 + k * 2 + 1);
  1600. /*}
  1601. else {
  1602. // if first iteration failed, count corrected point coordinates as initial
  1603. // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
  1604. current_position[X_AXIS] = vec_x[0] * pgm_read_float(bed_ref_points_4 + k * 2) + vec_y[0] * pgm_read_float(bed_ref_points_4 + k * 2 + 1) + cntr[0];
  1605. current_position[Y_AXIS] = vec_x[1] * pgm_read_float(bed_ref_points_4 + k * 2) + vec_y[1] * pgm_read_float(bed_ref_points_4 + k * 2 + 1) + cntr[1];
  1606. // The calibration points are very close to the min Y.
  1607. if (current_position[Y_AXIS] < Y_MIN_POS_FOR_BED_CALIBRATION)
  1608. current_position[Y_AXIS] = Y_MIN_POS_FOR_BED_CALIBRATION;
  1609. }*/
  1610. if (verbosity_level >= 20) {
  1611. SERIAL_ECHOPGM("current_position[X_AXIS]:");
  1612. MYSERIAL.print(current_position[X_AXIS], 5);
  1613. SERIAL_ECHOLNPGM("");
  1614. SERIAL_ECHOPGM("current_position[Y_AXIS]:");
  1615. MYSERIAL.print(current_position[Y_AXIS], 5);
  1616. SERIAL_ECHOLNPGM("");
  1617. SERIAL_ECHOPGM("current_position[Z_AXIS]:");
  1618. MYSERIAL.print(current_position[Z_AXIS], 5);
  1619. SERIAL_ECHOLNPGM("");
  1620. }
  1621. go_to_current(homing_feedrate[X_AXIS] / 60.f);
  1622. if (verbosity_level >= 10)
  1623. delay_keep_alive(3000);
  1624. if (!find_bed_induction_sensor_point_xy(verbosity_level))
  1625. return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
  1626. #if 1
  1627. if (k == 0) {
  1628. // Improve the position of the 1st row sensor points by a zig-zag movement.
  1629. find_bed_induction_sensor_point_z();
  1630. int8_t i = 4;
  1631. for (;;) {
  1632. if (improve_bed_induction_sensor_point3(verbosity_level))
  1633. break;
  1634. if (--i == 0)
  1635. return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
  1636. // Try to move the Z axis down a bit to increase a chance of the sensor to trigger.
  1637. current_position[Z_AXIS] -= 0.025f;
  1638. enable_endstops(false);
  1639. enable_z_endstop(false);
  1640. go_to_current(homing_feedrate[Z_AXIS]);
  1641. }
  1642. if (i == 0)
  1643. // not found
  1644. return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
  1645. }
  1646. #endif
  1647. if (verbosity_level >= 10)
  1648. delay_keep_alive(3000);
  1649. // Save the detected point position and then clamp the Y coordinate, which may have been estimated
  1650. // to lie outside the machine working space.
  1651. if (verbosity_level >= 20) {
  1652. SERIAL_ECHOLNPGM("Measured:");
  1653. MYSERIAL.println(current_position[X_AXIS]);
  1654. MYSERIAL.println(current_position[Y_AXIS]);
  1655. }
  1656. pt[0] = (pt[0] * iteration) / (iteration + 1);
  1657. pt[0] += (current_position[X_AXIS]/(iteration + 1)); //count average
  1658. pt[1] = (pt[1] * iteration) / (iteration + 1);
  1659. pt[1] += (current_position[Y_AXIS] / (iteration + 1));
  1660. //pt[0] += current_position[X_AXIS];
  1661. //if(iteration > 0) pt[0] = pt[0] / 2;
  1662. //pt[1] += current_position[Y_AXIS];
  1663. //if (iteration > 0) pt[1] = pt[1] / 2;
  1664. if (verbosity_level >= 20) {
  1665. SERIAL_ECHOLNPGM("");
  1666. SERIAL_ECHOPGM("pt[0]:");
  1667. MYSERIAL.println(pt[0]);
  1668. SERIAL_ECHOPGM("pt[1]:");
  1669. MYSERIAL.println(pt[1]);
  1670. }
  1671. if (current_position[Y_AXIS] < Y_MIN_POS)
  1672. current_position[Y_AXIS] = Y_MIN_POS;
  1673. // Start searching for the other points at 3mm above the last point.
  1674. current_position[Z_AXIS] += 3.f + FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP * iteration * 0.3;
  1675. //cntr[0] += pt[0];
  1676. //cntr[1] += pt[1];
  1677. if (verbosity_level >= 10 && k == 0) {
  1678. // Show the zero. Test, whether the Y motor skipped steps.
  1679. current_position[Y_AXIS] = MANUAL_Y_HOME_POS;
  1680. go_to_current(homing_feedrate[X_AXIS] / 60.f);
  1681. delay_keep_alive(3000);
  1682. }
  1683. }
  1684. if (verbosity_level >= 20) {
  1685. // Test the positions. Are the positions reproducible? Now the calibration is active in the planner.
  1686. delay_keep_alive(3000);
  1687. for (int8_t mesh_point = 0; mesh_point < 4; ++mesh_point) {
  1688. // Don't let the manage_inactivity() function remove power from the motors.
  1689. refresh_cmd_timeout();
  1690. // Go to the measurement point.
  1691. // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
  1692. current_position[X_AXIS] = pts[mesh_point * 2];
  1693. current_position[Y_AXIS] = pts[mesh_point * 2 + 1];
  1694. go_to_current(homing_feedrate[X_AXIS] / 60);
  1695. delay_keep_alive(3000);
  1696. }
  1697. }
  1698. if (pts[1] < Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH) {
  1699. too_far_mask |= 1 << 1; //front center point is out of reach
  1700. SERIAL_ECHOLNPGM("");
  1701. SERIAL_ECHOPGM("WARNING: Front point not reachable. Y coordinate:");
  1702. MYSERIAL.print(pts[1]);
  1703. SERIAL_ECHOPGM(" < ");
  1704. MYSERIAL.println(Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH);
  1705. }
  1706. result = calculate_machine_skew_and_offset_LS(pts, 4, bed_ref_points_4, vec_x, vec_y, cntr, verbosity_level);
  1707. if (result >= 0) {
  1708. world2machine_update(vec_x, vec_y, cntr);
  1709. #if 1
  1710. // Fearlessly store the calibration values into the eeprom.
  1711. eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER + 0), cntr[0]);
  1712. eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER + 4), cntr[1]);
  1713. eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X + 0), vec_x[0]);
  1714. eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X + 4), vec_x[1]);
  1715. eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y + 0), vec_y[0]);
  1716. eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y + 4), vec_y[1]);
  1717. #endif
  1718. if (verbosity_level >= 10) {
  1719. // Length of the vec_x
  1720. float l = sqrt(vec_x[0] * vec_x[0] + vec_x[1] * vec_x[1]);
  1721. SERIAL_ECHOLNPGM("X vector length:");
  1722. MYSERIAL.println(l);
  1723. // Length of the vec_y
  1724. l = sqrt(vec_y[0] * vec_y[0] + vec_y[1] * vec_y[1]);
  1725. SERIAL_ECHOLNPGM("Y vector length:");
  1726. MYSERIAL.println(l);
  1727. // Zero point correction
  1728. l = sqrt(cntr[0] * cntr[0] + cntr[1] * cntr[1]);
  1729. SERIAL_ECHOLNPGM("Zero point correction:");
  1730. MYSERIAL.println(l);
  1731. // vec_x and vec_y shall be nearly perpendicular.
  1732. l = vec_x[0] * vec_y[0] + vec_x[1] * vec_y[1];
  1733. SERIAL_ECHOLNPGM("Perpendicularity");
  1734. MYSERIAL.println(fabs(l));
  1735. SERIAL_ECHOLNPGM("Saving bed calibration vectors to EEPROM");
  1736. }
  1737. // Correct the current_position to match the transformed coordinate system after world2machine_rotation_and_skew and world2machine_shift were set.
  1738. world2machine_update_current();
  1739. if (verbosity_level >= 20) {
  1740. // Test the positions. Are the positions reproducible? Now the calibration is active in the planner.
  1741. delay_keep_alive(3000);
  1742. for (int8_t mesh_point = 0; mesh_point < 9; ++mesh_point) {
  1743. // Don't let the manage_inactivity() function remove power from the motors.
  1744. refresh_cmd_timeout();
  1745. // Go to the measurement point.
  1746. // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
  1747. current_position[X_AXIS] = pgm_read_float(bed_ref_points + mesh_point * 2);
  1748. current_position[Y_AXIS] = pgm_read_float(bed_ref_points + mesh_point * 2 + 1);
  1749. go_to_current(homing_feedrate[X_AXIS] / 60);
  1750. delay_keep_alive(3000);
  1751. }
  1752. }
  1753. return result;
  1754. }
  1755. if (result == BED_SKEW_OFFSET_DETECTION_FITTING_FAILED && too_far_mask == 2) return result; //if fitting failed and front center point is out of reach, terminate calibration and inform user
  1756. iteration++;
  1757. }
  1758. return result;
  1759. }
  1760. BedSkewOffsetDetectionResultType improve_bed_offset_and_skew(int8_t method, int8_t verbosity_level, uint8_t &too_far_mask)
  1761. {
  1762. // Don't let the manage_inactivity() function remove power from the motors.
  1763. refresh_cmd_timeout();
  1764. // Mask of the first three points. Are they too far?
  1765. too_far_mask = 0;
  1766. // Reusing the z_values memory for the measurement cache.
  1767. // 7x7=49 floats, good for 16 (x,y,z) vectors.
  1768. float *pts = &mbl.z_values[0][0];
  1769. float *vec_x = pts + 2 * 9;
  1770. float *vec_y = vec_x + 2;
  1771. float *cntr = vec_y + 2;
  1772. memset(pts, 0, sizeof(float) * 7 * 7);
  1773. if (verbosity_level >= 10) SERIAL_ECHOLNPGM("Improving bed offset and skew");
  1774. // Cache the current correction matrix.
  1775. world2machine_initialize();
  1776. vec_x[0] = world2machine_rotation_and_skew[0][0];
  1777. vec_x[1] = world2machine_rotation_and_skew[1][0];
  1778. vec_y[0] = world2machine_rotation_and_skew[0][1];
  1779. vec_y[1] = world2machine_rotation_and_skew[1][1];
  1780. cntr[0] = world2machine_shift[0];
  1781. cntr[1] = world2machine_shift[1];
  1782. // and reset the correction matrix, so the planner will not do anything.
  1783. world2machine_reset();
  1784. bool endstops_enabled = enable_endstops(false);
  1785. bool endstop_z_enabled = enable_z_endstop(false);
  1786. #ifdef MESH_BED_CALIBRATION_SHOW_LCD
  1787. uint8_t next_line;
  1788. lcd_display_message_fullscreen_P(MSG_IMPROVE_BED_OFFSET_AND_SKEW_LINE1, next_line);
  1789. if (next_line > 3)
  1790. next_line = 3;
  1791. #endif /* MESH_BED_CALIBRATION_SHOW_LCD */
  1792. // Collect a matrix of 9x9 points.
  1793. BedSkewOffsetDetectionResultType result = BED_SKEW_OFFSET_DETECTION_PERFECT;
  1794. for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
  1795. // Don't let the manage_inactivity() function remove power from the motors.
  1796. refresh_cmd_timeout();
  1797. // Print the decrasing ID of the measurement point.
  1798. #ifdef MESH_BED_CALIBRATION_SHOW_LCD
  1799. lcd_implementation_print_at(0, next_line, mesh_point+1);
  1800. lcd_printPGM(MSG_IMPROVE_BED_OFFSET_AND_SKEW_LINE2);
  1801. #endif /* MESH_BED_CALIBRATION_SHOW_LCD */
  1802. // Move up.
  1803. current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
  1804. enable_endstops(false);
  1805. enable_z_endstop(false);
  1806. go_to_current(homing_feedrate[Z_AXIS]/60);
  1807. if (verbosity_level >= 20) {
  1808. // Go to Y0, wait, then go to Y-4.
  1809. current_position[Y_AXIS] = 0.f;
  1810. go_to_current(homing_feedrate[X_AXIS] / 60.f);
  1811. SERIAL_ECHOLNPGM("At Y0");
  1812. delay_keep_alive(5000);
  1813. current_position[Y_AXIS] = Y_MIN_POS;
  1814. go_to_current(homing_feedrate[X_AXIS] / 60.f);
  1815. SERIAL_ECHOLNPGM("At Y_MIN_POS");
  1816. delay_keep_alive(5000);
  1817. }
  1818. // Go to the measurement point.
  1819. // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
  1820. current_position[X_AXIS] = vec_x[0] * pgm_read_float(bed_ref_points+mesh_point*2) + vec_y[0] * pgm_read_float(bed_ref_points+mesh_point*2+1) + cntr[0];
  1821. current_position[Y_AXIS] = vec_x[1] * pgm_read_float(bed_ref_points+mesh_point*2) + vec_y[1] * pgm_read_float(bed_ref_points+mesh_point*2+1) + cntr[1];
  1822. // The calibration points are very close to the min Y.
  1823. if (current_position[Y_AXIS] < Y_MIN_POS_FOR_BED_CALIBRATION){
  1824. current_position[Y_AXIS] = Y_MIN_POS_FOR_BED_CALIBRATION;
  1825. if (verbosity_level >= 20) {
  1826. SERIAL_ECHOPGM("Calibration point ");
  1827. SERIAL_ECHO(mesh_point);
  1828. SERIAL_ECHOPGM("lower than Ymin. Y coordinate clamping was used.");
  1829. SERIAL_ECHOLNPGM("");
  1830. }
  1831. }
  1832. go_to_current(homing_feedrate[X_AXIS]/60);
  1833. // Find its Z position by running the normal vertical search.
  1834. if (verbosity_level >= 10)
  1835. delay_keep_alive(3000);
  1836. find_bed_induction_sensor_point_z();
  1837. if (verbosity_level >= 10)
  1838. delay_keep_alive(3000);
  1839. // Try to move the Z axis down a bit to increase a chance of the sensor to trigger.
  1840. current_position[Z_AXIS] -= 0.025f;
  1841. // Improve the point position by searching its center in a current plane.
  1842. int8_t n_errors = 3;
  1843. for (int8_t iter = 0; iter < 8; ) {
  1844. if (verbosity_level > 20) {
  1845. SERIAL_ECHOPGM("Improving bed point ");
  1846. SERIAL_ECHO(mesh_point);
  1847. SERIAL_ECHOPGM(", iteration ");
  1848. SERIAL_ECHO(iter);
  1849. SERIAL_ECHOPGM(", z");
  1850. MYSERIAL.print(current_position[Z_AXIS], 5);
  1851. SERIAL_ECHOLNPGM("");
  1852. }
  1853. bool found = false;
  1854. if (mesh_point < 3) {
  1855. // Because the sensor cannot move in front of the first row
  1856. // of the sensor points, the y position cannot be measured
  1857. // by a cross center method.
  1858. // Use a zig-zag search for the first row of the points.
  1859. found = improve_bed_induction_sensor_point3(verbosity_level);
  1860. } else {
  1861. switch (method) {
  1862. case 0: found = improve_bed_induction_sensor_point(); break;
  1863. case 1: found = improve_bed_induction_sensor_point2(mesh_point < 3, verbosity_level); break;
  1864. default: break;
  1865. }
  1866. }
  1867. if (found) {
  1868. if (iter > 3) {
  1869. // Average the last 4 measurements.
  1870. pts[mesh_point*2 ] += current_position[X_AXIS];
  1871. pts[mesh_point*2+1] += current_position[Y_AXIS];
  1872. }
  1873. if (current_position[Y_AXIS] < Y_MIN_POS)
  1874. current_position[Y_AXIS] = Y_MIN_POS;
  1875. ++ iter;
  1876. } else if (n_errors -- == 0) {
  1877. // Give up.
  1878. result = BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
  1879. goto canceled;
  1880. } else {
  1881. // Try to move the Z axis down a bit to increase a chance of the sensor to trigger.
  1882. current_position[Z_AXIS] -= 0.05f;
  1883. enable_endstops(false);
  1884. enable_z_endstop(false);
  1885. go_to_current(homing_feedrate[Z_AXIS]);
  1886. if (verbosity_level >= 5) {
  1887. SERIAL_ECHOPGM("Improving bed point ");
  1888. SERIAL_ECHO(mesh_point);
  1889. SERIAL_ECHOPGM(", iteration ");
  1890. SERIAL_ECHO(iter);
  1891. SERIAL_ECHOPGM(" failed. Lowering z to ");
  1892. MYSERIAL.print(current_position[Z_AXIS], 5);
  1893. SERIAL_ECHOLNPGM("");
  1894. }
  1895. }
  1896. }
  1897. if (verbosity_level >= 10)
  1898. delay_keep_alive(3000);
  1899. }
  1900. // Don't let the manage_inactivity() function remove power from the motors.
  1901. refresh_cmd_timeout();
  1902. // Average the last 4 measurements.
  1903. for (int8_t i = 0; i < 18; ++ i)
  1904. pts[i] *= (1.f/4.f);
  1905. enable_endstops(false);
  1906. enable_z_endstop(false);
  1907. if (verbosity_level >= 5) {
  1908. // Test the positions. Are the positions reproducible?
  1909. current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
  1910. for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
  1911. // Don't let the manage_inactivity() function remove power from the motors.
  1912. refresh_cmd_timeout();
  1913. // Go to the measurement point.
  1914. // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
  1915. current_position[X_AXIS] = pts[mesh_point*2];
  1916. current_position[Y_AXIS] = pts[mesh_point*2+1];
  1917. if (verbosity_level >= 10) {
  1918. go_to_current(homing_feedrate[X_AXIS]/60);
  1919. delay_keep_alive(3000);
  1920. }
  1921. SERIAL_ECHOPGM("Final measured bed point ");
  1922. SERIAL_ECHO(mesh_point);
  1923. SERIAL_ECHOPGM(": ");
  1924. MYSERIAL.print(current_position[X_AXIS], 5);
  1925. SERIAL_ECHOPGM(", ");
  1926. MYSERIAL.print(current_position[Y_AXIS], 5);
  1927. SERIAL_ECHOLNPGM("");
  1928. }
  1929. }
  1930. {
  1931. // First fill in the too_far_mask from the measured points.
  1932. for (uint8_t mesh_point = 0; mesh_point < 3; ++ mesh_point)
  1933. if (pts[mesh_point * 2 + 1] < Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH)
  1934. too_far_mask |= 1 << mesh_point;
  1935. result = calculate_machine_skew_and_offset_LS(pts, 9, bed_ref_points, vec_x, vec_y, cntr, verbosity_level);
  1936. if (result < 0) {
  1937. SERIAL_ECHOLNPGM("Calculation of the machine skew and offset failed.");
  1938. goto canceled;
  1939. }
  1940. // In case of success, update the too_far_mask from the calculated points.
  1941. for (uint8_t mesh_point = 0; mesh_point < 3; ++ mesh_point) {
  1942. float y = vec_x[1] * pgm_read_float(bed_ref_points+mesh_point*2) + vec_y[1] * pgm_read_float(bed_ref_points+mesh_point*2+1) + cntr[1];
  1943. distance_from_min[mesh_point] = (y - Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH);
  1944. if (verbosity_level >= 20) {
  1945. SERIAL_ECHOLNPGM("");
  1946. SERIAL_ECHOPGM("Distance from min:");
  1947. MYSERIAL.print(distance_from_min[mesh_point]);
  1948. SERIAL_ECHOLNPGM("");
  1949. SERIAL_ECHOPGM("y:");
  1950. MYSERIAL.print(y);
  1951. SERIAL_ECHOLNPGM("");
  1952. }
  1953. if (y < Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH)
  1954. too_far_mask |= 1 << mesh_point;
  1955. }
  1956. }
  1957. world2machine_update(vec_x, vec_y, cntr);
  1958. #if 1
  1959. // Fearlessly store the calibration values into the eeprom.
  1960. eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER+0), cntr [0]);
  1961. eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER+4), cntr [1]);
  1962. eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +0), vec_x[0]);
  1963. eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +4), vec_x[1]);
  1964. eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +0), vec_y[0]);
  1965. eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +4), vec_y[1]);
  1966. #endif
  1967. // Correct the current_position to match the transformed coordinate system after world2machine_rotation_and_skew and world2machine_shift were set.
  1968. world2machine_update_current();
  1969. enable_endstops(false);
  1970. enable_z_endstop(false);
  1971. if (verbosity_level >= 5) {
  1972. // Test the positions. Are the positions reproducible? Now the calibration is active in the planner.
  1973. delay_keep_alive(3000);
  1974. current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
  1975. for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
  1976. // Don't let the manage_inactivity() function remove power from the motors.
  1977. refresh_cmd_timeout();
  1978. // Go to the measurement point.
  1979. // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
  1980. current_position[X_AXIS] = pgm_read_float(bed_ref_points+mesh_point*2);
  1981. current_position[Y_AXIS] = pgm_read_float(bed_ref_points+mesh_point*2+1);
  1982. if (verbosity_level >= 10) {
  1983. go_to_current(homing_feedrate[X_AXIS]/60);
  1984. delay_keep_alive(3000);
  1985. }
  1986. {
  1987. float x, y;
  1988. world2machine(current_position[X_AXIS], current_position[Y_AXIS], x, y);
  1989. SERIAL_ECHOPGM("Final calculated bed point ");
  1990. SERIAL_ECHO(mesh_point);
  1991. SERIAL_ECHOPGM(": ");
  1992. MYSERIAL.print(x, 5);
  1993. SERIAL_ECHOPGM(", ");
  1994. MYSERIAL.print(y, 5);
  1995. SERIAL_ECHOLNPGM("");
  1996. }
  1997. }
  1998. }
  1999. // Sample Z heights for the mesh bed leveling.
  2000. // In addition, store the results into an eeprom, to be used later for verification of the bed leveling process.
  2001. if (! sample_mesh_and_store_reference())
  2002. goto canceled;
  2003. enable_endstops(endstops_enabled);
  2004. enable_z_endstop(endstop_z_enabled);
  2005. // Don't let the manage_inactivity() function remove power from the motors.
  2006. refresh_cmd_timeout();
  2007. return result;
  2008. canceled:
  2009. // Don't let the manage_inactivity() function remove power from the motors.
  2010. refresh_cmd_timeout();
  2011. // Print head up.
  2012. current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
  2013. go_to_current(homing_feedrate[Z_AXIS]/60);
  2014. // Store the identity matrix to EEPROM.
  2015. reset_bed_offset_and_skew();
  2016. enable_endstops(endstops_enabled);
  2017. enable_z_endstop(endstop_z_enabled);
  2018. return result;
  2019. }
  2020. void go_home_with_z_lift()
  2021. {
  2022. // Don't let the manage_inactivity() function remove power from the motors.
  2023. refresh_cmd_timeout();
  2024. // Go home.
  2025. // First move up to a safe height.
  2026. current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
  2027. go_to_current(homing_feedrate[Z_AXIS]/60);
  2028. // Second move to XY [0, 0].
  2029. current_position[X_AXIS] = X_MIN_POS+0.2;
  2030. current_position[Y_AXIS] = Y_MIN_POS+0.2;
  2031. // Clamp to the physical coordinates.
  2032. world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
  2033. go_to_current(homing_feedrate[X_AXIS]/60);
  2034. // Third move up to a safe height.
  2035. current_position[Z_AXIS] = Z_MIN_POS;
  2036. go_to_current(homing_feedrate[Z_AXIS]/60);
  2037. }
  2038. // Sample the 9 points of the bed and store them into the EEPROM as a reference.
  2039. // When calling this function, the X, Y, Z axes should be already homed,
  2040. // and the world2machine correction matrix should be active.
  2041. // Returns false if the reference values are more than 3mm far away.
  2042. bool sample_mesh_and_store_reference()
  2043. {
  2044. bool endstops_enabled = enable_endstops(false);
  2045. bool endstop_z_enabled = enable_z_endstop(false);
  2046. // Don't let the manage_inactivity() function remove power from the motors.
  2047. refresh_cmd_timeout();
  2048. #ifdef MESH_BED_CALIBRATION_SHOW_LCD
  2049. uint8_t next_line;
  2050. lcd_display_message_fullscreen_P(MSG_MEASURE_BED_REFERENCE_HEIGHT_LINE1, next_line);
  2051. if (next_line > 3)
  2052. next_line = 3;
  2053. // display "point xx of yy"
  2054. lcd_implementation_print_at(0, next_line, 1);
  2055. lcd_printPGM(MSG_MEASURE_BED_REFERENCE_HEIGHT_LINE2);
  2056. #endif /* MESH_BED_CALIBRATION_SHOW_LCD */
  2057. // Sample Z heights for the mesh bed leveling.
  2058. // In addition, store the results into an eeprom, to be used later for verification of the bed leveling process.
  2059. {
  2060. // The first point defines the reference.
  2061. current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
  2062. go_to_current(homing_feedrate[Z_AXIS]/60);
  2063. current_position[X_AXIS] = pgm_read_float(bed_ref_points);
  2064. current_position[Y_AXIS] = pgm_read_float(bed_ref_points+1);
  2065. world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
  2066. go_to_current(homing_feedrate[X_AXIS]/60);
  2067. memcpy(destination, current_position, sizeof(destination));
  2068. enable_endstops(true);
  2069. homeaxis(Z_AXIS);
  2070. enable_endstops(false);
  2071. find_bed_induction_sensor_point_z();
  2072. mbl.set_z(0, 0, current_position[Z_AXIS]);
  2073. }
  2074. for (int8_t mesh_point = 1; mesh_point != MESH_MEAS_NUM_X_POINTS * MESH_MEAS_NUM_Y_POINTS; ++ mesh_point) {
  2075. // Don't let the manage_inactivity() function remove power from the motors.
  2076. refresh_cmd_timeout();
  2077. // Print the decrasing ID of the measurement point.
  2078. current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
  2079. go_to_current(homing_feedrate[Z_AXIS]/60);
  2080. current_position[X_AXIS] = pgm_read_float(bed_ref_points+2*mesh_point);
  2081. current_position[Y_AXIS] = pgm_read_float(bed_ref_points+2*mesh_point+1);
  2082. world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
  2083. go_to_current(homing_feedrate[X_AXIS]/60);
  2084. #ifdef MESH_BED_CALIBRATION_SHOW_LCD
  2085. // display "point xx of yy"
  2086. lcd_implementation_print_at(0, next_line, mesh_point+1);
  2087. lcd_printPGM(MSG_MEASURE_BED_REFERENCE_HEIGHT_LINE2);
  2088. #endif /* MESH_BED_CALIBRATION_SHOW_LCD */
  2089. find_bed_induction_sensor_point_z();
  2090. // Get cords of measuring point
  2091. int8_t ix = mesh_point % MESH_MEAS_NUM_X_POINTS;
  2092. int8_t iy = mesh_point / MESH_MEAS_NUM_X_POINTS;
  2093. if (iy & 1) ix = (MESH_MEAS_NUM_X_POINTS - 1) - ix; // Zig zag
  2094. mbl.set_z(ix, iy, current_position[Z_AXIS]);
  2095. }
  2096. {
  2097. // Verify the span of the Z values.
  2098. float zmin = mbl.z_values[0][0];
  2099. float zmax = zmax;
  2100. for (int8_t j = 0; j < 3; ++ j)
  2101. for (int8_t i = 0; i < 3; ++ i) {
  2102. zmin = min(zmin, mbl.z_values[j][i]);
  2103. zmax = min(zmax, mbl.z_values[j][i]);
  2104. }
  2105. if (zmax - zmin > 3.f) {
  2106. // The span of the Z offsets is extreme. Give up.
  2107. // Homing failed on some of the points.
  2108. SERIAL_PROTOCOLLNPGM("Exreme span of the Z values!");
  2109. return false;
  2110. }
  2111. }
  2112. // Store the correction values to EEPROM.
  2113. // Offsets of the Z heiths of the calibration points from the first point.
  2114. // The offsets are saved as 16bit signed int, scaled to tenths of microns.
  2115. {
  2116. uint16_t addr = EEPROM_BED_CALIBRATION_Z_JITTER;
  2117. for (int8_t j = 0; j < 3; ++ j)
  2118. for (int8_t i = 0; i < 3; ++ i) {
  2119. if (i == 0 && j == 0)
  2120. continue;
  2121. float dif = mbl.z_values[j][i] - mbl.z_values[0][0];
  2122. int16_t dif_quantized = int16_t(floor(dif * 100.f + 0.5f));
  2123. eeprom_update_word((uint16_t*)addr, *reinterpret_cast<uint16_t*>(&dif_quantized));
  2124. #if 0
  2125. {
  2126. uint16_t z_offset_u = eeprom_read_word((uint16_t*)addr);
  2127. float dif2 = *reinterpret_cast<int16_t*>(&z_offset_u) * 0.01;
  2128. SERIAL_ECHOPGM("Bed point ");
  2129. SERIAL_ECHO(i);
  2130. SERIAL_ECHOPGM(",");
  2131. SERIAL_ECHO(j);
  2132. SERIAL_ECHOPGM(", differences: written ");
  2133. MYSERIAL.print(dif, 5);
  2134. SERIAL_ECHOPGM(", read: ");
  2135. MYSERIAL.print(dif2, 5);
  2136. SERIAL_ECHOLNPGM("");
  2137. }
  2138. #endif
  2139. addr += 2;
  2140. }
  2141. }
  2142. mbl.upsample_3x3();
  2143. mbl.active = true;
  2144. go_home_with_z_lift();
  2145. enable_endstops(endstops_enabled);
  2146. enable_z_endstop(endstop_z_enabled);
  2147. return true;
  2148. }
  2149. bool scan_bed_induction_points(int8_t verbosity_level)
  2150. {
  2151. // Don't let the manage_inactivity() function remove power from the motors.
  2152. refresh_cmd_timeout();
  2153. // Reusing the z_values memory for the measurement cache.
  2154. // 7x7=49 floats, good for 16 (x,y,z) vectors.
  2155. float *pts = &mbl.z_values[0][0];
  2156. float *vec_x = pts + 2 * 9;
  2157. float *vec_y = vec_x + 2;
  2158. float *cntr = vec_y + 2;
  2159. memset(pts, 0, sizeof(float) * 7 * 7);
  2160. // Cache the current correction matrix.
  2161. world2machine_initialize();
  2162. vec_x[0] = world2machine_rotation_and_skew[0][0];
  2163. vec_x[1] = world2machine_rotation_and_skew[1][0];
  2164. vec_y[0] = world2machine_rotation_and_skew[0][1];
  2165. vec_y[1] = world2machine_rotation_and_skew[1][1];
  2166. cntr[0] = world2machine_shift[0];
  2167. cntr[1] = world2machine_shift[1];
  2168. // and reset the correction matrix, so the planner will not do anything.
  2169. world2machine_reset();
  2170. bool endstops_enabled = enable_endstops(false);
  2171. bool endstop_z_enabled = enable_z_endstop(false);
  2172. // Collect a matrix of 9x9 points.
  2173. for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
  2174. // Don't let the manage_inactivity() function remove power from the motors.
  2175. refresh_cmd_timeout();
  2176. // Move up.
  2177. current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
  2178. enable_endstops(false);
  2179. enable_z_endstop(false);
  2180. go_to_current(homing_feedrate[Z_AXIS]/60);
  2181. // Go to the measurement point.
  2182. // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
  2183. current_position[X_AXIS] = vec_x[0] * pgm_read_float(bed_ref_points+mesh_point*2) + vec_y[0] * pgm_read_float(bed_ref_points+mesh_point*2+1) + cntr[0];
  2184. current_position[Y_AXIS] = vec_x[1] * pgm_read_float(bed_ref_points+mesh_point*2) + vec_y[1] * pgm_read_float(bed_ref_points+mesh_point*2+1) + cntr[1];
  2185. // The calibration points are very close to the min Y.
  2186. if (current_position[Y_AXIS] < Y_MIN_POS_FOR_BED_CALIBRATION)
  2187. current_position[Y_AXIS] = Y_MIN_POS_FOR_BED_CALIBRATION;
  2188. go_to_current(homing_feedrate[X_AXIS]/60);
  2189. find_bed_induction_sensor_point_z();
  2190. scan_bed_induction_sensor_point();
  2191. }
  2192. // Don't let the manage_inactivity() function remove power from the motors.
  2193. refresh_cmd_timeout();
  2194. enable_endstops(false);
  2195. enable_z_endstop(false);
  2196. // Don't let the manage_inactivity() function remove power from the motors.
  2197. refresh_cmd_timeout();
  2198. enable_endstops(endstops_enabled);
  2199. enable_z_endstop(endstop_z_enabled);
  2200. return true;
  2201. }
  2202. // Shift a Z axis by a given delta.
  2203. // To replace loading of the babystep correction.
  2204. static void shift_z(float delta)
  2205. {
  2206. plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] - delta, current_position[E_AXIS], homing_feedrate[Z_AXIS]/40, active_extruder);
  2207. st_synchronize();
  2208. plan_set_z_position(current_position[Z_AXIS]);
  2209. }
  2210. #define BABYSTEP_LOADZ_BY_PLANNER
  2211. // Number of baby steps applied
  2212. static int babystepLoadZ = 0;
  2213. void babystep_apply()
  2214. {
  2215. // Apply Z height correction aka baby stepping before mesh bed leveling gets activated.
  2216. if(calibration_status() < CALIBRATION_STATUS_LIVE_ADJUST)
  2217. {
  2218. check_babystep(); //checking if babystep is in allowed range, otherwise setting babystep to 0
  2219. // End of G80: Apply the baby stepping value.
  2220. EEPROM_read_B(EEPROM_BABYSTEP_Z,&babystepLoadZ);
  2221. #if 0
  2222. SERIAL_ECHO("Z baby step: ");
  2223. SERIAL_ECHO(babystepLoadZ);
  2224. SERIAL_ECHO(", current Z: ");
  2225. SERIAL_ECHO(current_position[Z_AXIS]);
  2226. SERIAL_ECHO("correction: ");
  2227. SERIAL_ECHO(float(babystepLoadZ) / float(axis_steps_per_unit[Z_AXIS]));
  2228. SERIAL_ECHOLN("");
  2229. #endif
  2230. #ifdef BABYSTEP_LOADZ_BY_PLANNER
  2231. shift_z(- float(babystepLoadZ) / float(axis_steps_per_unit[Z_AXIS]));
  2232. #else
  2233. babystepsTodoZadd(babystepLoadZ);
  2234. #endif /* BABYSTEP_LOADZ_BY_PLANNER */
  2235. }
  2236. }
  2237. void babystep_undo()
  2238. {
  2239. #ifdef BABYSTEP_LOADZ_BY_PLANNER
  2240. shift_z(float(babystepLoadZ) / float(axis_steps_per_unit[Z_AXIS]));
  2241. #else
  2242. babystepsTodoZsubtract(babystepLoadZ);
  2243. #endif /* BABYSTEP_LOADZ_BY_PLANNER */
  2244. babystepLoadZ = 0;
  2245. }
  2246. void babystep_reset()
  2247. {
  2248. babystepLoadZ = 0;
  2249. }
  2250. void count_xyz_details() {
  2251. float a1, a2;
  2252. float cntr[2] = {
  2253. eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_CENTER + 0)),
  2254. eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_CENTER + 4))
  2255. };
  2256. float vec_x[2] = {
  2257. eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_X + 0)),
  2258. eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_X + 4))
  2259. };
  2260. float vec_y[2] = {
  2261. eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y + 0)),
  2262. eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y + 4))
  2263. };
  2264. a2 = -1 * asin(vec_y[0] / MACHINE_AXIS_SCALE_Y);
  2265. a1 = asin(vec_x[1] / MACHINE_AXIS_SCALE_X);
  2266. angleDiff = fabs(a2 - a1);
  2267. for (uint8_t mesh_point = 0; mesh_point < 3; ++mesh_point) {
  2268. float y = vec_x[1] * pgm_read_float(bed_ref_points + mesh_point * 2) + vec_y[1] * pgm_read_float(bed_ref_points + mesh_point * 2 + 1) + cntr[1];
  2269. distance_from_min[mesh_point] = (y - Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH);
  2270. }
  2271. }
  2272. /*countDistanceFromMin() {
  2273. }*/