mesh_bed_calibration.cpp 123 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453145414551456145714581459146014611462146314641465146614671468146914701471147214731474147514761477147814791480148114821483148414851486148714881489149014911492149314941495149614971498149915001501150215031504150515061507150815091510151115121513151415151516151715181519152015211522152315241525152615271528152915301531153215331534153515361537153815391540154115421543154415451546154715481549155015511552155315541555155615571558155915601561156215631564156515661567156815691570157115721573157415751576157715781579158015811582158315841585158615871588158915901591159215931594159515961597159815991600160116021603160416051606160716081609161016111612161316141615161616171618161916201621162216231624162516261627162816291630163116321633163416351636163716381639164016411642164316441645164616471648164916501651165216531654165516561657165816591660166116621663166416651666166716681669167016711672167316741675167616771678167916801681168216831684168516861687168816891690169116921693169416951696169716981699170017011702170317041705170617071708170917101711171217131714171517161717171817191720172117221723172417251726172717281729173017311732173317341735173617371738173917401741174217431744174517461747174817491750175117521753175417551756175717581759176017611762176317641765176617671768176917701771177217731774177517761777177817791780178117821783178417851786178717881789179017911792179317941795179617971798179918001801180218031804180518061807180818091810181118121813181418151816181718181819182018211822182318241825182618271828182918301831183218331834183518361837183818391840184118421843184418451846184718481849185018511852185318541855185618571858185918601861186218631864186518661867186818691870187118721873187418751876187718781879188018811882188318841885188618871888188918901891189218931894189518961897189818991900190119021903190419051906190719081909191019111912191319141915191619171918191919201921192219231924192519261927192819291930193119321933193419351936193719381939194019411942194319441945194619471948194919501951195219531954195519561957195819591960196119621963196419651966196719681969197019711972197319741975197619771978197919801981198219831984198519861987198819891990199119921993199419951996199719981999200020012002200320042005200620072008200920102011201220132014201520162017201820192020202120222023202420252026202720282029203020312032203320342035203620372038203920402041204220432044204520462047204820492050205120522053205420552056205720582059206020612062206320642065206620672068206920702071207220732074207520762077207820792080208120822083208420852086208720882089209020912092209320942095209620972098209921002101210221032104210521062107210821092110211121122113211421152116211721182119212021212122212321242125212621272128212921302131213221332134213521362137213821392140214121422143214421452146214721482149215021512152215321542155215621572158215921602161216221632164216521662167216821692170217121722173217421752176217721782179218021812182218321842185218621872188218921902191219221932194219521962197219821992200220122022203220422052206220722082209221022112212221322142215221622172218221922202221222222232224222522262227222822292230223122322233223422352236223722382239224022412242224322442245224622472248224922502251225222532254225522562257225822592260226122622263226422652266226722682269227022712272227322742275227622772278227922802281228222832284228522862287228822892290229122922293229422952296229722982299230023012302230323042305230623072308230923102311231223132314231523162317231823192320232123222323232423252326232723282329233023312332233323342335233623372338233923402341234223432344234523462347234823492350235123522353235423552356235723582359236023612362236323642365236623672368236923702371237223732374237523762377237823792380238123822383238423852386238723882389239023912392239323942395239623972398239924002401240224032404240524062407240824092410241124122413241424152416241724182419242024212422242324242425242624272428242924302431243224332434243524362437243824392440244124422443244424452446244724482449245024512452245324542455245624572458245924602461246224632464246524662467246824692470247124722473247424752476247724782479248024812482248324842485248624872488248924902491249224932494249524962497249824992500250125022503250425052506250725082509251025112512251325142515251625172518251925202521252225232524252525262527252825292530253125322533253425352536253725382539254025412542254325442545254625472548254925502551255225532554255525562557255825592560256125622563256425652566256725682569257025712572257325742575257625772578257925802581258225832584258525862587258825892590259125922593259425952596259725982599260026012602260326042605260626072608260926102611261226132614261526162617261826192620262126222623262426252626262726282629263026312632263326342635263626372638263926402641264226432644264526462647264826492650265126522653265426552656265726582659266026612662266326642665266626672668266926702671267226732674267526762677267826792680268126822683268426852686268726882689269026912692269326942695269626972698269927002701270227032704270527062707270827092710271127122713271427152716271727182719272027212722272327242725272627272728272927302731273227332734273527362737273827392740274127422743274427452746274727482749275027512752275327542755275627572758275927602761276227632764276527662767276827692770277127722773277427752776277727782779278027812782278327842785278627872788278927902791279227932794279527962797279827992800280128022803280428052806280728082809281028112812281328142815281628172818281928202821282228232824282528262827282828292830283128322833283428352836283728382839284028412842284328442845284628472848284928502851285228532854285528562857285828592860286128622863286428652866286728682869287028712872287328742875287628772878287928802881288228832884288528862887288828892890289128922893289428952896289728982899290029012902290329042905290629072908290929102911291229132914291529162917291829192920292129222923292429252926292729282929293029312932293329342935293629372938293929402941294229432944294529462947294829492950295129522953295429552956295729582959296029612962296329642965296629672968296929702971297229732974297529762977297829792980298129822983298429852986298729882989299029912992299329942995299629972998299930003001300230033004300530063007300830093010301130123013301430153016301730183019302030213022302330243025302630273028302930303031303230333034303530363037303830393040304130423043304430453046304730483049305030513052305330543055305630573058305930603061306230633064306530663067306830693070307130723073307430753076307730783079308030813082308330843085308630873088308930903091309230933094309530963097309830993100310131023103310431053106310731083109311031113112311331143115311631173118311931203121312231233124312531263127312831293130313131323133313431353136313731383139314031413142314331443145314631473148314931503151315231533154315531563157315831593160316131623163316431653166316731683169317031713172317331743175317631773178317931803181318231833184318531863187318831893190319131923193319431953196319731983199
  1. #include "Configuration.h"
  2. #include "ConfigurationStore.h"
  3. #include "language.h"
  4. #include "mesh_bed_calibration.h"
  5. #include "mesh_bed_leveling.h"
  6. #include "stepper.h"
  7. #include "ultralcd.h"
  8. #include "temperature.h"
  9. #ifdef TMC2130
  10. #include "tmc2130.h"
  11. #endif //TMC2130
  12. #define DBG(args...) printf_P(args)
  13. uint8_t world2machine_correction_mode;
  14. float world2machine_rotation_and_skew[2][2];
  15. float world2machine_rotation_and_skew_inv[2][2];
  16. float world2machine_shift[2];
  17. // Weight of the Y coordinate for the least squares fitting of the bed induction sensor targets.
  18. // Only used for the first row of the points, which may not befully in reach of the sensor.
  19. #define WEIGHT_FIRST_ROW_X_HIGH (1.f)
  20. #define WEIGHT_FIRST_ROW_X_LOW (0.35f)
  21. #define WEIGHT_FIRST_ROW_Y_HIGH (0.3f)
  22. #define WEIGHT_FIRST_ROW_Y_LOW (0.0f)
  23. // Scaling of the real machine axes against the programmed dimensions in the firmware.
  24. // The correction is tiny, here around 0.5mm on 250mm length.
  25. //#define MACHINE_AXIS_SCALE_X ((250.f - 0.5f) / 250.f)
  26. //#define MACHINE_AXIS_SCALE_Y ((250.f - 0.5f) / 250.f)
  27. #define MACHINE_AXIS_SCALE_X 1.f
  28. #define MACHINE_AXIS_SCALE_Y 1.f
  29. #define BED_CALIBRATION_POINT_OFFSET_MAX_EUCLIDIAN (1.5f)
  30. #define BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_X (1.5f)
  31. #define BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_Y (1.5f)
  32. #define MIN_BED_SENSOR_POINT_RESPONSE_DMR (2.0f)
  33. //#define Y_MIN_POS_FOR_BED_CALIBRATION (MANUAL_Y_HOME_POS-0.2f)
  34. #define Y_MIN_POS_FOR_BED_CALIBRATION (Y_MIN_POS)
  35. // Distances toward the print bed edge may not be accurate.
  36. #define Y_MIN_POS_CALIBRATION_POINT_ACCURATE (Y_MIN_POS + 3.f)
  37. // When the measured point center is out of reach of the sensor, Y coordinate will be ignored
  38. // by the Least Squares fitting and the X coordinate will be weighted low.
  39. #define Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH (Y_MIN_POS - 0.5f)
  40. // 0.12 degrees equals to an offset of 0.5mm on 250mm length.
  41. const float bed_skew_angle_mild = (0.12f * M_PI / 180.f);
  42. // 0.25 degrees equals to an offset of 1.1mm on 250mm length.
  43. const float bed_skew_angle_extreme = (0.25f * M_PI / 180.f);
  44. // Positions of the bed reference points in the machine coordinates, referenced to the P.I.N.D.A sensor.
  45. // The points are ordered in a zig-zag fashion to speed up the calibration.
  46. #ifdef HEATBED_V2
  47. /**
  48. * [0,0] bed print area point X coordinate in bed coordinates ver. 05d/24V
  49. */
  50. #define BED_PRINT_ZERO_REF_X 2.f
  51. /**
  52. * [0,0] bed print area point Y coordinate in bed coordinates ver. 05d/24V
  53. */
  54. #define BED_PRINT_ZERO_REF_Y 9.4f
  55. /**
  56. * @brief Positions of the bed reference points in print area coordinates. ver. 05d/24V
  57. *
  58. * Numeral constants are in bed coordinates, subtracting macro defined values converts it to print area coordinates.
  59. *
  60. * The points are the following:
  61. * MK2: center front, center right, center rear, center left.
  62. * MK25 and MK3: front left, front right, rear right, rear left
  63. */
  64. const float bed_ref_points_4[] PROGMEM = {
  65. 37.f - BED_PRINT_ZERO_REF_X - X_PROBE_OFFSET_FROM_EXTRUDER - SHEET_PRINT_ZERO_REF_X,
  66. 18.4f - BED_PRINT_ZERO_REF_Y - Y_PROBE_OFFSET_FROM_EXTRUDER - SHEET_PRINT_ZERO_REF_Y,
  67. 245.f - BED_PRINT_ZERO_REF_X - X_PROBE_OFFSET_FROM_EXTRUDER - SHEET_PRINT_ZERO_REF_X,
  68. 18.4f - BED_PRINT_ZERO_REF_Y - Y_PROBE_OFFSET_FROM_EXTRUDER - SHEET_PRINT_ZERO_REF_Y,
  69. 245.f - BED_PRINT_ZERO_REF_X - X_PROBE_OFFSET_FROM_EXTRUDER - SHEET_PRINT_ZERO_REF_X,
  70. 210.4f - BED_PRINT_ZERO_REF_Y - Y_PROBE_OFFSET_FROM_EXTRUDER - SHEET_PRINT_ZERO_REF_Y,
  71. 37.f - BED_PRINT_ZERO_REF_X - X_PROBE_OFFSET_FROM_EXTRUDER - SHEET_PRINT_ZERO_REF_X,
  72. 210.4f - BED_PRINT_ZERO_REF_Y - Y_PROBE_OFFSET_FROM_EXTRUDER - SHEET_PRINT_ZERO_REF_Y
  73. };
  74. #else
  75. // Positions of the bed reference points in the machine coordinates, referenced to the P.I.N.D.A sensor.
  76. // The points are the following: center front, center right, center rear, center left.
  77. const float bed_ref_points_4[] PROGMEM = {
  78. 115.f - BED_ZERO_REF_X, 8.4f - BED_ZERO_REF_Y,
  79. 216.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y,
  80. 115.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y,
  81. 13.f - BED_ZERO_REF_X, 104.4f - BED_ZERO_REF_Y
  82. };
  83. #endif //not HEATBED_V2
  84. static inline float sqr(float x) { return x * x; }
  85. #ifdef HEATBED_V2
  86. static inline bool point_on_1st_row(const uint8_t /*i*/)
  87. {
  88. return false;
  89. }
  90. #else //HEATBED_V2
  91. static inline bool point_on_1st_row(const uint8_t i)
  92. {
  93. return (i < 3);
  94. }
  95. #endif //HEATBED_V2
  96. // Weight of a point coordinate in a least squares optimization.
  97. // The first row of points may not be fully reachable
  98. // and the y values may be shortened a bit by the bed carriage
  99. // pulling the belt up.
  100. static inline float point_weight_x(const uint8_t i, const float &y)
  101. {
  102. float w = 1.f;
  103. if (point_on_1st_row(i)) {
  104. if (y >= Y_MIN_POS_CALIBRATION_POINT_ACCURATE) {
  105. w = WEIGHT_FIRST_ROW_X_HIGH;
  106. } else if (y < Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH) {
  107. // If the point is fully outside, give it some weight.
  108. w = WEIGHT_FIRST_ROW_X_LOW;
  109. } else {
  110. // Linearly interpolate the weight from 1 to WEIGHT_FIRST_ROW_X.
  111. 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);
  112. w = (1.f - t) * WEIGHT_FIRST_ROW_X_LOW + t * WEIGHT_FIRST_ROW_X_HIGH;
  113. }
  114. }
  115. return w;
  116. }
  117. // Weight of a point coordinate in a least squares optimization.
  118. // The first row of points may not be fully reachable
  119. // and the y values may be shortened a bit by the bed carriage
  120. // pulling the belt up.
  121. static inline float point_weight_y(const uint8_t i, const float &y)
  122. {
  123. float w = 1.f;
  124. if (point_on_1st_row(i)) {
  125. if (y >= Y_MIN_POS_CALIBRATION_POINT_ACCURATE) {
  126. w = WEIGHT_FIRST_ROW_Y_HIGH;
  127. } else if (y < Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH) {
  128. // If the point is fully outside, give it some weight.
  129. w = WEIGHT_FIRST_ROW_Y_LOW;
  130. } else {
  131. // Linearly interpolate the weight from 1 to WEIGHT_FIRST_ROW_X.
  132. 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);
  133. w = (1.f - t) * WEIGHT_FIRST_ROW_Y_LOW + t * WEIGHT_FIRST_ROW_Y_HIGH;
  134. }
  135. }
  136. return w;
  137. }
  138. /**
  139. * @brief Calculate machine skew and offset
  140. *
  141. * Non-Linear Least Squares fitting of the bed to the measured induction points
  142. * using the Gauss-Newton method.
  143. * This method will maintain a unity length of the machine axes,
  144. * which is the correct approach if the sensor points are not measured precisely.
  145. * @param measured_pts Matrix of 2D points (maximum 18 floats)
  146. * @param npts Number of points (maximum 9)
  147. * @param true_pts
  148. * @param [out] vec_x Resulting correction matrix. X axis vector
  149. * @param [out] vec_y Resulting correction matrix. Y axis vector
  150. * @param [out] cntr Resulting correction matrix. [0;0] pont offset
  151. * @param verbosity_level
  152. * @return BedSkewOffsetDetectionResultType
  153. */
  154. BedSkewOffsetDetectionResultType calculate_machine_skew_and_offset_LS(
  155. const float *measured_pts,
  156. uint8_t npts,
  157. const float *true_pts,
  158. float *vec_x,
  159. float *vec_y,
  160. float *cntr,
  161. int8_t
  162. #ifdef SUPPORT_VERBOSITY
  163. verbosity_level
  164. #endif //SUPPORT_VERBOSITY
  165. )
  166. {
  167. float angleDiff;
  168. #ifdef SUPPORT_VERBOSITY
  169. if (verbosity_level >= 10) {
  170. SERIAL_ECHOLNPGM("calculate machine skew and offset LS");
  171. // Show the initial state, before the fitting.
  172. SERIAL_ECHOPGM("X vector, initial: ");
  173. MYSERIAL.print(vec_x[0], 5);
  174. SERIAL_ECHOPGM(", ");
  175. MYSERIAL.print(vec_x[1], 5);
  176. SERIAL_ECHOLNPGM("");
  177. SERIAL_ECHOPGM("Y vector, initial: ");
  178. MYSERIAL.print(vec_y[0], 5);
  179. SERIAL_ECHOPGM(", ");
  180. MYSERIAL.print(vec_y[1], 5);
  181. SERIAL_ECHOLNPGM("");
  182. SERIAL_ECHOPGM("center, initial: ");
  183. MYSERIAL.print(cntr[0], 5);
  184. SERIAL_ECHOPGM(", ");
  185. MYSERIAL.print(cntr[1], 5);
  186. SERIAL_ECHOLNPGM("");
  187. for (uint8_t i = 0; i < npts; ++i) {
  188. SERIAL_ECHOPGM("point #");
  189. MYSERIAL.print(int(i));
  190. SERIAL_ECHOPGM(" measured: (");
  191. MYSERIAL.print(measured_pts[i * 2], 5);
  192. SERIAL_ECHOPGM(", ");
  193. MYSERIAL.print(measured_pts[i * 2 + 1], 5);
  194. SERIAL_ECHOPGM("); target: (");
  195. MYSERIAL.print(pgm_read_float(true_pts + i * 2), 5);
  196. SERIAL_ECHOPGM(", ");
  197. MYSERIAL.print(pgm_read_float(true_pts + i * 2 + 1), 5);
  198. SERIAL_ECHOPGM("), error: ");
  199. MYSERIAL.print(sqrt(
  200. sqr(pgm_read_float(true_pts + i * 2) - measured_pts[i * 2]) +
  201. sqr(pgm_read_float(true_pts + i * 2 + 1) - measured_pts[i * 2 + 1])), 5);
  202. SERIAL_ECHOLNPGM("");
  203. }
  204. delay_keep_alive(100);
  205. }
  206. #endif // SUPPORT_VERBOSITY
  207. // Run some iterations of the Gauss-Newton method of non-linear least squares.
  208. // Initial set of parameters:
  209. // X,Y offset
  210. cntr[0] = 0.f;
  211. cntr[1] = 0.f;
  212. // Rotation of the machine X axis from the bed X axis.
  213. float a1 = 0;
  214. // Rotation of the machine Y axis from the bed Y axis.
  215. float a2 = 0;
  216. for (int8_t iter = 0; iter < 100; ++iter) {
  217. float c1 = cos(a1) * MACHINE_AXIS_SCALE_X;
  218. float s1 = sin(a1) * MACHINE_AXIS_SCALE_X;
  219. float c2 = cos(a2) * MACHINE_AXIS_SCALE_Y;
  220. float s2 = sin(a2) * MACHINE_AXIS_SCALE_Y;
  221. // Prepare the Normal equation for the Gauss-Newton method.
  222. float A[4][4] = { 0.f };
  223. float b[4] = { 0.f };
  224. float acc;
  225. delay_keep_alive(0); //manage heater, reset watchdog, manage inactivity
  226. for (uint8_t r = 0; r < 4; ++r) {
  227. for (uint8_t c = 0; c < 4; ++c) {
  228. acc = 0;
  229. // J^T times J
  230. for (uint8_t i = 0; i < npts; ++i) {
  231. // First for the residuum in the x axis:
  232. if (r != 1 && c != 1) {
  233. float a =
  234. (r == 0) ? 1.f :
  235. ((r == 2) ? (-s1 * measured_pts[2 * i]) :
  236. (-c2 * measured_pts[2 * i + 1]));
  237. float b =
  238. (c == 0) ? 1.f :
  239. ((c == 2) ? (-s1 * measured_pts[2 * i]) :
  240. (-c2 * measured_pts[2 * i + 1]));
  241. float w = point_weight_x(i, measured_pts[2 * i + 1]);
  242. acc += a * b * w;
  243. }
  244. // Second for the residuum in the y axis.
  245. // The first row of the points have a low weight, because their position may not be known
  246. // with a sufficient accuracy.
  247. if (r != 0 && c != 0) {
  248. float a =
  249. (r == 1) ? 1.f :
  250. ((r == 2) ? ( c1 * measured_pts[2 * i]) :
  251. (-s2 * measured_pts[2 * i + 1]));
  252. float b =
  253. (c == 1) ? 1.f :
  254. ((c == 2) ? ( c1 * measured_pts[2 * i]) :
  255. (-s2 * measured_pts[2 * i + 1]));
  256. float w = point_weight_y(i, measured_pts[2 * i + 1]);
  257. acc += a * b * w;
  258. }
  259. }
  260. A[r][c] = acc;
  261. }
  262. // J^T times f(x)
  263. acc = 0.f;
  264. for (uint8_t i = 0; i < npts; ++i) {
  265. {
  266. float j =
  267. (r == 0) ? 1.f :
  268. ((r == 1) ? 0.f :
  269. ((r == 2) ? (-s1 * measured_pts[2 * i]) :
  270. (-c2 * measured_pts[2 * i + 1])));
  271. float fx = c1 * measured_pts[2 * i] - s2 * measured_pts[2 * i + 1] + cntr[0] - pgm_read_float(true_pts + i * 2);
  272. float w = point_weight_x(i, measured_pts[2 * i + 1]);
  273. acc += j * fx * w;
  274. }
  275. {
  276. float j =
  277. (r == 0) ? 0.f :
  278. ((r == 1) ? 1.f :
  279. ((r == 2) ? ( c1 * measured_pts[2 * i]) :
  280. (-s2 * measured_pts[2 * i + 1])));
  281. float fy = s1 * measured_pts[2 * i] + c2 * measured_pts[2 * i + 1] + cntr[1] - pgm_read_float(true_pts + i * 2 + 1);
  282. float w = point_weight_y(i, measured_pts[2 * i + 1]);
  283. acc += j * fy * w;
  284. }
  285. }
  286. b[r] = -acc;
  287. }
  288. // Solve for h by a Gauss iteration method.
  289. float h[4] = { 0.f };
  290. for (uint8_t gauss_iter = 0; gauss_iter < 100; ++gauss_iter) {
  291. h[0] = (b[0] - A[0][1] * h[1] - A[0][2] * h[2] - A[0][3] * h[3]) / A[0][0];
  292. h[1] = (b[1] - A[1][0] * h[0] - A[1][2] * h[2] - A[1][3] * h[3]) / A[1][1];
  293. h[2] = (b[2] - A[2][0] * h[0] - A[2][1] * h[1] - A[2][3] * h[3]) / A[2][2];
  294. h[3] = (b[3] - A[3][0] * h[0] - A[3][1] * h[1] - A[3][2] * h[2]) / A[3][3];
  295. }
  296. // and update the current position with h.
  297. // It may be better to use the Levenberg-Marquart method here,
  298. // but because we are very close to the solution alread,
  299. // the simple Gauss-Newton non-linear Least Squares method works well enough.
  300. cntr[0] += h[0];
  301. cntr[1] += h[1];
  302. a1 += h[2];
  303. a2 += h[3];
  304. #ifdef SUPPORT_VERBOSITY
  305. if (verbosity_level >= 20) {
  306. SERIAL_ECHOPGM("iteration: ");
  307. MYSERIAL.print(int(iter));
  308. SERIAL_ECHOPGM("; correction vector: ");
  309. MYSERIAL.print(h[0], 5);
  310. SERIAL_ECHOPGM(", ");
  311. MYSERIAL.print(h[1], 5);
  312. SERIAL_ECHOPGM(", ");
  313. MYSERIAL.print(h[2], 5);
  314. SERIAL_ECHOPGM(", ");
  315. MYSERIAL.print(h[3], 5);
  316. SERIAL_ECHOLNPGM("");
  317. SERIAL_ECHOPGM("corrected x/y: ");
  318. MYSERIAL.print(cntr[0], 5);
  319. SERIAL_ECHOPGM(", ");
  320. MYSERIAL.print(cntr[0], 5);
  321. SERIAL_ECHOLNPGM("");
  322. SERIAL_ECHOPGM("corrected angles: ");
  323. MYSERIAL.print(180.f * a1 / M_PI, 5);
  324. SERIAL_ECHOPGM(", ");
  325. MYSERIAL.print(180.f * a2 / M_PI, 5);
  326. SERIAL_ECHOLNPGM("");
  327. }
  328. #endif // SUPPORT_VERBOSITY
  329. }
  330. vec_x[0] = cos(a1) * MACHINE_AXIS_SCALE_X;
  331. vec_x[1] = sin(a1) * MACHINE_AXIS_SCALE_X;
  332. vec_y[0] = -sin(a2) * MACHINE_AXIS_SCALE_Y;
  333. vec_y[1] = cos(a2) * MACHINE_AXIS_SCALE_Y;
  334. BedSkewOffsetDetectionResultType result = BED_SKEW_OFFSET_DETECTION_PERFECT;
  335. {
  336. angleDiff = fabs(a2 - a1);
  337. /// XY skew and Y-bed skew
  338. DBG(_n("Measured skews: %f %f\n"), degrees(a2 - a1), degrees(a2));
  339. eeprom_update_float((float *)(EEPROM_XYZ_CAL_SKEW), angleDiff); //storing xyz cal. skew to be able to show in support menu later
  340. if (angleDiff > bed_skew_angle_mild)
  341. result = (angleDiff > bed_skew_angle_extreme) ?
  342. BED_SKEW_OFFSET_DETECTION_SKEW_EXTREME :
  343. BED_SKEW_OFFSET_DETECTION_SKEW_MILD;
  344. if (fabs(a1) > bed_skew_angle_extreme ||
  345. fabs(a2) > bed_skew_angle_extreme)
  346. result = BED_SKEW_OFFSET_DETECTION_SKEW_EXTREME;
  347. }
  348. #ifdef SUPPORT_VERBOSITY
  349. if (verbosity_level >= 1) {
  350. SERIAL_ECHOPGM("correction angles: ");
  351. MYSERIAL.print(180.f * a1 / M_PI, 5);
  352. SERIAL_ECHOPGM(", ");
  353. MYSERIAL.print(180.f * a2 / M_PI, 5);
  354. SERIAL_ECHOLNPGM("");
  355. }
  356. if (verbosity_level >= 10) {
  357. // Show the adjusted state, before the fitting.
  358. SERIAL_ECHOPGM("X vector new, inverted: ");
  359. MYSERIAL.print(vec_x[0], 5);
  360. SERIAL_ECHOPGM(", ");
  361. MYSERIAL.print(vec_x[1], 5);
  362. SERIAL_ECHOLNPGM("");
  363. SERIAL_ECHOPGM("Y vector new, inverted: ");
  364. MYSERIAL.print(vec_y[0], 5);
  365. SERIAL_ECHOPGM(", ");
  366. MYSERIAL.print(vec_y[1], 5);
  367. SERIAL_ECHOLNPGM("");
  368. SERIAL_ECHOPGM("center new, inverted: ");
  369. MYSERIAL.print(cntr[0], 5);
  370. SERIAL_ECHOPGM(", ");
  371. MYSERIAL.print(cntr[1], 5);
  372. SERIAL_ECHOLNPGM("");
  373. delay_keep_alive(100);
  374. SERIAL_ECHOLNPGM("Error after correction: ");
  375. }
  376. #endif // SUPPORT_VERBOSITY
  377. // Measure the error after correction.
  378. for (uint8_t i = 0; i < npts; ++i) {
  379. float x = vec_x[0] * measured_pts[i * 2] + vec_y[0] * measured_pts[i * 2 + 1] + cntr[0];
  380. float y = vec_x[1] * measured_pts[i * 2] + vec_y[1] * measured_pts[i * 2 + 1] + cntr[1];
  381. float errX = sqr(pgm_read_float(true_pts + i * 2) - x);
  382. float errY = sqr(pgm_read_float(true_pts + i * 2 + 1) - y);
  383. float err = sqrt(errX + errY);
  384. #ifdef SUPPORT_VERBOSITY
  385. if (verbosity_level >= 10) {
  386. SERIAL_ECHOPGM("point #");
  387. MYSERIAL.print(int(i));
  388. SERIAL_ECHOLNPGM(":");
  389. }
  390. #endif // SUPPORT_VERBOSITY
  391. if (point_on_1st_row(i)) {
  392. #ifdef SUPPORT_VERBOSITY
  393. if(verbosity_level >= 20) SERIAL_ECHOPGM("Point on first row");
  394. #endif // SUPPORT_VERBOSITY
  395. float w = point_weight_y(i, measured_pts[2 * i + 1]);
  396. if (sqrt(errX) > BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_X ||
  397. (w != 0.f && sqrt(errY) > BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_Y)) {
  398. result = BED_SKEW_OFFSET_DETECTION_FITTING_FAILED;
  399. #ifdef SUPPORT_VERBOSITY
  400. if (verbosity_level >= 20) {
  401. SERIAL_ECHOPGM(", weigth Y: ");
  402. MYSERIAL.print(w);
  403. if (sqrt(errX) > BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_X) SERIAL_ECHOPGM(", error X > max. error X");
  404. if (w != 0.f && sqrt(errY) > BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_Y) SERIAL_ECHOPGM(", error Y > max. error Y");
  405. }
  406. #endif // SUPPORT_VERBOSITY
  407. }
  408. }
  409. else {
  410. #ifdef SUPPORT_VERBOSITY
  411. if(verbosity_level >=20 ) SERIAL_ECHOPGM("Point not on first row");
  412. #endif // SUPPORT_VERBOSITY
  413. if (err > BED_CALIBRATION_POINT_OFFSET_MAX_EUCLIDIAN) {
  414. result = BED_SKEW_OFFSET_DETECTION_FITTING_FAILED;
  415. #ifdef SUPPORT_VERBOSITY
  416. if(verbosity_level >= 20) SERIAL_ECHOPGM(", error > max. error euclidian");
  417. #endif // SUPPORT_VERBOSITY
  418. }
  419. }
  420. #ifdef SUPPORT_VERBOSITY
  421. if (verbosity_level >= 10) {
  422. SERIAL_ECHOLNPGM("");
  423. SERIAL_ECHOPGM("measured: (");
  424. MYSERIAL.print(measured_pts[i * 2], 5);
  425. SERIAL_ECHOPGM(", ");
  426. MYSERIAL.print(measured_pts[i * 2 + 1], 5);
  427. SERIAL_ECHOPGM("); corrected: (");
  428. MYSERIAL.print(x, 5);
  429. SERIAL_ECHOPGM(", ");
  430. MYSERIAL.print(y, 5);
  431. SERIAL_ECHOPGM("); target: (");
  432. MYSERIAL.print(pgm_read_float(true_pts + i * 2), 5);
  433. SERIAL_ECHOPGM(", ");
  434. MYSERIAL.print(pgm_read_float(true_pts + i * 2 + 1), 5);
  435. SERIAL_ECHOLNPGM(")");
  436. SERIAL_ECHOPGM("error: ");
  437. MYSERIAL.print(err);
  438. SERIAL_ECHOPGM(", error X: ");
  439. MYSERIAL.print(sqrt(errX));
  440. SERIAL_ECHOPGM(", error Y: ");
  441. MYSERIAL.print(sqrt(errY));
  442. SERIAL_ECHOLNPGM("");
  443. SERIAL_ECHOLNPGM("");
  444. }
  445. #endif // SUPPORT_VERBOSITY
  446. }
  447. #ifdef SUPPORT_VERBOSITY
  448. if (verbosity_level >= 20) {
  449. SERIAL_ECHOLNPGM("Max. errors:");
  450. SERIAL_ECHOPGM("Max. error X:");
  451. MYSERIAL.println(BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_X);
  452. SERIAL_ECHOPGM("Max. error Y:");
  453. MYSERIAL.println(BED_CALIBRATION_POINT_OFFSET_MAX_1ST_ROW_Y);
  454. SERIAL_ECHOPGM("Max. error euclidian:");
  455. MYSERIAL.println(BED_CALIBRATION_POINT_OFFSET_MAX_EUCLIDIAN);
  456. SERIAL_ECHOLNPGM("");
  457. }
  458. #endif // SUPPORT_VERBOSITY
  459. #if 0
  460. if (result == BED_SKEW_OFFSET_DETECTION_PERFECT && fabs(a1) < bed_skew_angle_mild && fabs(a2) < bed_skew_angle_mild) {
  461. #ifdef SUPPORT_VERBOSITY
  462. if (verbosity_level > 0)
  463. SERIAL_ECHOLNPGM("Very little skew detected. Disabling skew correction.");
  464. #endif // SUPPORT_VERBOSITY
  465. // Just disable the skew correction.
  466. vec_x[0] = MACHINE_AXIS_SCALE_X;
  467. vec_x[1] = 0.f;
  468. vec_y[0] = 0.f;
  469. vec_y[1] = MACHINE_AXIS_SCALE_Y;
  470. }
  471. #else
  472. if (result == BED_SKEW_OFFSET_DETECTION_PERFECT) {
  473. #ifdef SUPPORT_VERBOSITY
  474. if (verbosity_level > 0)
  475. SERIAL_ECHOLNPGM("Very little skew detected. Orthogonalizing the axes.");
  476. #endif // SUPPORT_VERBOSITY
  477. // Orthogonalize the axes.
  478. a1 = 0.5f * (a1 + a2);
  479. vec_x[0] = cos(a1) * MACHINE_AXIS_SCALE_X;
  480. vec_x[1] = sin(a1) * MACHINE_AXIS_SCALE_X;
  481. vec_y[0] = -sin(a1) * MACHINE_AXIS_SCALE_Y;
  482. vec_y[1] = cos(a1) * MACHINE_AXIS_SCALE_Y;
  483. // Refresh the offset.
  484. cntr[0] = 0.f;
  485. cntr[1] = 0.f;
  486. float wx = 0.f;
  487. float wy = 0.f;
  488. for (int8_t i = 0; i < npts; ++ i) {
  489. float x = vec_x[0] * measured_pts[i * 2] + vec_y[0] * measured_pts[i * 2 + 1];
  490. float y = vec_x[1] * measured_pts[i * 2] + vec_y[1] * measured_pts[i * 2 + 1];
  491. float w = point_weight_x(i, y);
  492. cntr[0] += w * (pgm_read_float(true_pts + i * 2) - x);
  493. wx += w;
  494. #ifdef SUPPORT_VERBOSITY
  495. if (verbosity_level >= 20) {
  496. MYSERIAL.print(i);
  497. SERIAL_ECHOLNPGM("");
  498. SERIAL_ECHOLNPGM("Weight_x:");
  499. MYSERIAL.print(w);
  500. SERIAL_ECHOLNPGM("");
  501. SERIAL_ECHOLNPGM("cntr[0]:");
  502. MYSERIAL.print(cntr[0]);
  503. SERIAL_ECHOLNPGM("");
  504. SERIAL_ECHOLNPGM("wx:");
  505. MYSERIAL.print(wx);
  506. }
  507. #endif // SUPPORT_VERBOSITY
  508. w = point_weight_y(i, y);
  509. cntr[1] += w * (pgm_read_float(true_pts + i * 2 + 1) - y);
  510. wy += w;
  511. #ifdef SUPPORT_VERBOSITY
  512. if (verbosity_level >= 20) {
  513. SERIAL_ECHOLNPGM("");
  514. SERIAL_ECHOLNPGM("Weight_y:");
  515. MYSERIAL.print(w);
  516. SERIAL_ECHOLNPGM("");
  517. SERIAL_ECHOLNPGM("cntr[1]:");
  518. MYSERIAL.print(cntr[1]);
  519. SERIAL_ECHOLNPGM("");
  520. SERIAL_ECHOLNPGM("wy:");
  521. MYSERIAL.print(wy);
  522. SERIAL_ECHOLNPGM("");
  523. SERIAL_ECHOLNPGM("");
  524. }
  525. #endif // SUPPORT_VERBOSITY
  526. }
  527. cntr[0] /= wx;
  528. cntr[1] /= wy;
  529. #ifdef SUPPORT_VERBOSITY
  530. if (verbosity_level >= 20) {
  531. SERIAL_ECHOLNPGM("");
  532. SERIAL_ECHOLNPGM("Final cntr values:");
  533. SERIAL_ECHOLNPGM("cntr[0]:");
  534. MYSERIAL.print(cntr[0]);
  535. SERIAL_ECHOLNPGM("");
  536. SERIAL_ECHOLNPGM("cntr[1]:");
  537. MYSERIAL.print(cntr[1]);
  538. SERIAL_ECHOLNPGM("");
  539. }
  540. #endif // SUPPORT_VERBOSITY
  541. }
  542. #endif
  543. // Invert the transformation matrix made of vec_x, vec_y and cntr.
  544. {
  545. float d = vec_x[0] * vec_y[1] - vec_x[1] * vec_y[0];
  546. float Ainv[2][2] = {
  547. { vec_y[1] / d, -vec_y[0] / d },
  548. { -vec_x[1] / d, vec_x[0] / d }
  549. };
  550. float cntrInv[2] = {
  551. -Ainv[0][0] * cntr[0] - Ainv[0][1] * cntr[1],
  552. -Ainv[1][0] * cntr[0] - Ainv[1][1] * cntr[1]
  553. };
  554. vec_x[0] = Ainv[0][0];
  555. vec_x[1] = Ainv[1][0];
  556. vec_y[0] = Ainv[0][1];
  557. vec_y[1] = Ainv[1][1];
  558. cntr[0] = cntrInv[0];
  559. cntr[1] = cntrInv[1];
  560. }
  561. #ifdef SUPPORT_VERBOSITY
  562. if (verbosity_level >= 1) {
  563. // Show the adjusted state, before the fitting.
  564. SERIAL_ECHOPGM("X vector, adjusted: ");
  565. MYSERIAL.print(vec_x[0], 5);
  566. SERIAL_ECHOPGM(", ");
  567. MYSERIAL.print(vec_x[1], 5);
  568. SERIAL_ECHOLNPGM("");
  569. SERIAL_ECHOPGM("Y vector, adjusted: ");
  570. MYSERIAL.print(vec_y[0], 5);
  571. SERIAL_ECHOPGM(", ");
  572. MYSERIAL.print(vec_y[1], 5);
  573. SERIAL_ECHOLNPGM("");
  574. SERIAL_ECHOPGM("center, adjusted: ");
  575. MYSERIAL.print(cntr[0], 5);
  576. SERIAL_ECHOPGM(", ");
  577. MYSERIAL.print(cntr[1], 5);
  578. SERIAL_ECHOLNPGM("");
  579. delay_keep_alive(100);
  580. }
  581. if (verbosity_level >= 2) {
  582. SERIAL_ECHOLNPGM("Difference after correction: ");
  583. for (uint8_t i = 0; i < npts; ++i) {
  584. 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];
  585. 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];
  586. SERIAL_ECHOPGM("point #");
  587. MYSERIAL.print(int(i));
  588. SERIAL_ECHOPGM("measured: (");
  589. MYSERIAL.print(measured_pts[i * 2], 5);
  590. SERIAL_ECHOPGM(", ");
  591. MYSERIAL.print(measured_pts[i * 2 + 1], 5);
  592. SERIAL_ECHOPGM("); measured-corrected: (");
  593. MYSERIAL.print(x, 5);
  594. SERIAL_ECHOPGM(", ");
  595. MYSERIAL.print(y, 5);
  596. SERIAL_ECHOPGM("); target: (");
  597. MYSERIAL.print(pgm_read_float(true_pts + i * 2), 5);
  598. SERIAL_ECHOPGM(", ");
  599. MYSERIAL.print(pgm_read_float(true_pts + i * 2 + 1), 5);
  600. SERIAL_ECHOPGM("), error: ");
  601. MYSERIAL.print(sqrt(sqr(measured_pts[i * 2] - x) + sqr(measured_pts[i * 2 + 1] - y)));
  602. SERIAL_ECHOLNPGM("");
  603. }
  604. if (verbosity_level >= 20) {
  605. SERIAL_ECHOLNPGM("");
  606. SERIAL_ECHOLNPGM("Calculate offset and skew returning result:");
  607. MYSERIAL.print(int(result));
  608. SERIAL_ECHOLNPGM("");
  609. SERIAL_ECHOLNPGM("");
  610. }
  611. delay_keep_alive(100);
  612. }
  613. #endif // SUPPORT_VERBOSITY
  614. return result;
  615. }
  616. /**
  617. * @brief Erase calibration data stored in EEPROM
  618. */
  619. void reset_bed_offset_and_skew()
  620. {
  621. eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_CENTER+0), 0x0FFFFFFFF);
  622. eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_CENTER+4), 0x0FFFFFFFF);
  623. eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_X +0), 0x0FFFFFFFF);
  624. eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_X +4), 0x0FFFFFFFF);
  625. eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_Y +0), 0x0FFFFFFFF);
  626. eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_VEC_Y +4), 0x0FFFFFFFF);
  627. // Reset the 8 16bit offsets.
  628. for (int8_t i = 0; i < 4; ++ i)
  629. eeprom_update_dword((uint32_t*)(EEPROM_BED_CALIBRATION_Z_JITTER+i*4), 0x0FFFFFFFF);
  630. }
  631. bool is_bed_z_jitter_data_valid()
  632. // offsets of the Z heiths of the calibration points from the first point are saved as 16bit signed int, scaled to tenths of microns
  633. // if at least one 16bit integer has different value then -1 (0x0FFFF), data are considered valid and function returns true, otherwise it returns false
  634. {
  635. bool data_valid = false;
  636. for (int8_t i = 0; i < 8; ++i) {
  637. if (eeprom_read_word((uint16_t*)(EEPROM_BED_CALIBRATION_Z_JITTER + i * 2)) != 0x0FFFF) data_valid = true;
  638. }
  639. return data_valid;
  640. }
  641. static void world2machine_update(const float vec_x[2], const float vec_y[2], const float cntr[2])
  642. {
  643. world2machine_rotation_and_skew[0][0] = vec_x[0];
  644. world2machine_rotation_and_skew[1][0] = vec_x[1];
  645. world2machine_rotation_and_skew[0][1] = vec_y[0];
  646. world2machine_rotation_and_skew[1][1] = vec_y[1];
  647. world2machine_shift[0] = cntr[0];
  648. world2machine_shift[1] = cntr[1];
  649. // No correction.
  650. world2machine_correction_mode = WORLD2MACHINE_CORRECTION_NONE;
  651. if (world2machine_shift[0] != 0.f || world2machine_shift[1] != 0.f)
  652. // Shift correction.
  653. world2machine_correction_mode |= WORLD2MACHINE_CORRECTION_SHIFT;
  654. if (world2machine_rotation_and_skew[0][0] != 1.f || world2machine_rotation_and_skew[0][1] != 0.f ||
  655. world2machine_rotation_and_skew[1][0] != 0.f || world2machine_rotation_and_skew[1][1] != 1.f) {
  656. // Rotation & skew correction.
  657. world2machine_correction_mode |= WORLD2MACHINE_CORRECTION_SKEW;
  658. // Invert the world2machine matrix.
  659. 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];
  660. world2machine_rotation_and_skew_inv[0][0] = world2machine_rotation_and_skew[1][1] / d;
  661. world2machine_rotation_and_skew_inv[0][1] = -world2machine_rotation_and_skew[0][1] / d;
  662. world2machine_rotation_and_skew_inv[1][0] = -world2machine_rotation_and_skew[1][0] / d;
  663. world2machine_rotation_and_skew_inv[1][1] = world2machine_rotation_and_skew[0][0] / d;
  664. } else {
  665. world2machine_rotation_and_skew_inv[0][0] = 1.f;
  666. world2machine_rotation_and_skew_inv[0][1] = 0.f;
  667. world2machine_rotation_and_skew_inv[1][0] = 0.f;
  668. world2machine_rotation_and_skew_inv[1][1] = 1.f;
  669. }
  670. }
  671. /**
  672. * @brief Set calibration matrix to identity
  673. *
  674. * In contrast with world2machine_revert_to_uncorrected(), it doesn't wait for finishing moves
  675. * nor updates the current position with the absolute values.
  676. */
  677. void world2machine_reset()
  678. {
  679. const float vx[] = { 1.f, 0.f };
  680. const float vy[] = { 0.f, 1.f };
  681. const float cntr[] = { 0.f, 0.f };
  682. world2machine_update(vx, vy, cntr);
  683. }
  684. /**
  685. * @brief Get calibration matrix default value
  686. *
  687. * This is used if no valid calibration data can be read from EEPROM.
  688. * @param [out] vec_x axis x vector
  689. * @param [out] vec_y axis y vector
  690. * @param [out] cntr offset vector
  691. */
  692. static void world2machine_default(float vec_x[2], float vec_y[2], float cntr[2])
  693. {
  694. vec_x[0] = 1.f;
  695. vec_x[1] = 0.f;
  696. vec_y[0] = 0.f;
  697. vec_y[1] = 1.f;
  698. cntr[0] = 0.f;
  699. #ifdef DEFAULT_Y_OFFSET
  700. cntr[1] = DEFAULT_Y_OFFSET;
  701. #else
  702. cntr[1] = 0.f;
  703. #endif
  704. }
  705. /**
  706. * @brief Set calibration matrix to identity and update current position with absolute position
  707. *
  708. * Wait for the motors to stop and then update the current position with the absolute values.
  709. */
  710. void world2machine_revert_to_uncorrected()
  711. {
  712. if (world2machine_correction_mode != WORLD2MACHINE_CORRECTION_NONE) {
  713. world2machine_reset();
  714. st_synchronize();
  715. current_position[X_AXIS] = st_get_position_mm(X_AXIS);
  716. current_position[Y_AXIS] = st_get_position_mm(Y_AXIS);
  717. }
  718. }
  719. static inline bool vec_undef(const float v[2])
  720. {
  721. const uint32_t *vx = (const uint32_t*)v;
  722. return vx[0] == 0x0FFFFFFFF || vx[1] == 0x0FFFFFFFF;
  723. }
  724. /**
  725. * @brief Read calibration data from EEPROM
  726. *
  727. * If no calibration data has been stored in EEPROM or invalid,
  728. * world2machine_default() is used.
  729. *
  730. * If stored calibration data is invalid, EEPROM storage is cleared.
  731. * @param [out] vec_x axis x vector
  732. * @param [out] vec_y axis y vector
  733. * @param [out] cntr offset vector
  734. */
  735. void world2machine_read_valid(float vec_x[2], float vec_y[2], float cntr[2])
  736. {
  737. vec_x[0] = eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +0));
  738. vec_x[1] = eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +4));
  739. vec_y[0] = eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +0));
  740. vec_y[1] = eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +4));
  741. cntr[0] = eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_CENTER+0));
  742. cntr[1] = eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_CENTER+4));
  743. bool reset = false;
  744. if (vec_undef(cntr) || vec_undef(vec_x) || vec_undef(vec_y))
  745. {
  746. #if 0
  747. SERIAL_ECHOLNPGM("Undefined bed correction matrix.");
  748. #endif
  749. reset = true;
  750. }
  751. else
  752. {
  753. // Length of the vec_x shall be close to unity.
  754. float l = sqrt(vec_x[0] * vec_x[0] + vec_x[1] * vec_x[1]);
  755. if (l < 0.9 || l > 1.1)
  756. {
  757. #if 0
  758. SERIAL_ECHOLNPGM("X vector length:");
  759. MYSERIAL.println(l);
  760. SERIAL_ECHOLNPGM("Invalid bed correction matrix. Length of the X vector out of range.");
  761. #endif
  762. reset = true;
  763. }
  764. // Length of the vec_y shall be close to unity.
  765. l = sqrt(vec_y[0] * vec_y[0] + vec_y[1] * vec_y[1]);
  766. if (l < 0.9 || l > 1.1)
  767. {
  768. #if 0
  769. SERIAL_ECHOLNPGM("Y vector length:");
  770. MYSERIAL.println(l);
  771. SERIAL_ECHOLNPGM("Invalid bed correction matrix. Length of the Y vector out of range.");
  772. #endif
  773. reset = true;
  774. }
  775. // Correction of the zero point shall be reasonably small.
  776. l = sqrt(cntr[0] * cntr[0] + cntr[1] * cntr[1]);
  777. if (l > 15.f)
  778. {
  779. #if 0
  780. SERIAL_ECHOLNPGM("Zero point correction:");
  781. MYSERIAL.println(l);
  782. SERIAL_ECHOLNPGM("Invalid bed correction matrix. Shift out of range.");
  783. #endif
  784. reset = true;
  785. }
  786. // vec_x and vec_y shall be nearly perpendicular.
  787. l = vec_x[0] * vec_y[0] + vec_x[1] * vec_y[1];
  788. if (fabs(l) > 0.1f)
  789. {
  790. #if 0
  791. SERIAL_ECHOLNPGM("Invalid bed correction matrix. X/Y axes are far from being perpendicular.");
  792. #endif
  793. reset = true;
  794. }
  795. }
  796. if (reset)
  797. {
  798. #if 0
  799. SERIAL_ECHOLNPGM("Invalid bed correction matrix. Resetting to identity.");
  800. #endif
  801. reset_bed_offset_and_skew();
  802. world2machine_default(vec_x, vec_y, cntr);
  803. }
  804. }
  805. /**
  806. * @brief Read and apply validated calibration data from EEPROM
  807. */
  808. void world2machine_initialize()
  809. {
  810. #if 0
  811. SERIAL_ECHOLNPGM("world2machine_initialize");
  812. #endif
  813. float vec_x[2];
  814. float vec_y[2];
  815. float cntr[2];
  816. world2machine_read_valid(vec_x, vec_y, cntr);
  817. world2machine_update(vec_x, vec_y, cntr);
  818. #if 0
  819. SERIAL_ECHOPGM("world2machine_initialize() loaded: ");
  820. MYSERIAL.print(world2machine_rotation_and_skew[0][0], 5);
  821. SERIAL_ECHOPGM(", ");
  822. MYSERIAL.print(world2machine_rotation_and_skew[0][1], 5);
  823. SERIAL_ECHOPGM(", ");
  824. MYSERIAL.print(world2machine_rotation_and_skew[1][0], 5);
  825. SERIAL_ECHOPGM(", ");
  826. MYSERIAL.print(world2machine_rotation_and_skew[1][1], 5);
  827. SERIAL_ECHOPGM(", offset ");
  828. MYSERIAL.print(world2machine_shift[0], 5);
  829. SERIAL_ECHOPGM(", ");
  830. MYSERIAL.print(world2machine_shift[1], 5);
  831. SERIAL_ECHOLNPGM("");
  832. #endif
  833. }
  834. /**
  835. * @brief Update current position after switching to corrected coordinates
  836. *
  837. * When switching from absolute to corrected coordinates,
  838. * this will get the absolute coordinates from the servos,
  839. * applies the inverse world2machine transformation
  840. * and stores the result into current_position[x,y].
  841. */
  842. void world2machine_update_current()
  843. {
  844. float x = current_position[X_AXIS] - world2machine_shift[0];
  845. float y = current_position[Y_AXIS] - world2machine_shift[1];
  846. current_position[X_AXIS] = world2machine_rotation_and_skew_inv[0][0] * x + world2machine_rotation_and_skew_inv[0][1] * y;
  847. current_position[Y_AXIS] = world2machine_rotation_and_skew_inv[1][0] * x + world2machine_rotation_and_skew_inv[1][1] * y;
  848. }
  849. static inline void go_xyz(float x, float y, float z, float fr)
  850. {
  851. plan_buffer_line(x, y, z, current_position[E_AXIS], fr, active_extruder);
  852. st_synchronize();
  853. }
  854. static inline void go_xy(float x, float y, float fr)
  855. {
  856. plan_buffer_line(x, y, current_position[Z_AXIS], current_position[E_AXIS], fr, active_extruder);
  857. st_synchronize();
  858. }
  859. static inline void go_to_current(float fr)
  860. {
  861. plan_buffer_line_curposXYZE(fr);
  862. st_synchronize();
  863. }
  864. static inline void update_current_position_xyz()
  865. {
  866. current_position[X_AXIS] = st_get_position_mm(X_AXIS);
  867. current_position[Y_AXIS] = st_get_position_mm(Y_AXIS);
  868. current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
  869. plan_set_position_curposXYZE();
  870. }
  871. static inline void update_current_position_z()
  872. {
  873. current_position[Z_AXIS] = st_get_position_mm(Z_AXIS);
  874. plan_set_z_position(current_position[Z_AXIS]);
  875. }
  876. // At the current position, find the Z stop.
  877. bool find_bed_induction_sensor_point_z(float minimum_z, uint8_t n_iter, int
  878. #ifdef SUPPORT_VERBOSITY
  879. verbosity_level
  880. #endif //SUPPORT_VERBOSITY
  881. )
  882. {
  883. bool high_deviation_occured = false;
  884. bedPWMDisabled = 1;
  885. #ifdef TMC2130
  886. FORCE_HIGH_POWER_START;
  887. #endif
  888. //printf_P(PSTR("Min. Z: %f\n"), minimum_z);
  889. #ifdef SUPPORT_VERBOSITY
  890. if(verbosity_level >= 10) SERIAL_ECHOLNPGM("find bed induction sensor point z");
  891. #endif // SUPPORT_VERBOSITY
  892. bool endstops_enabled = enable_endstops(true);
  893. bool endstop_z_enabled = enable_z_endstop(false);
  894. float z = 0.f;
  895. endstop_z_hit_on_purpose();
  896. // move down until you find the bed
  897. current_position[Z_AXIS] = minimum_z;
  898. go_to_current(homing_feedrate[Z_AXIS]/60);
  899. // we have to let the planner know where we are right now as it is not where we said to go.
  900. update_current_position_z();
  901. if (! endstop_z_hit_on_purpose())
  902. {
  903. //printf_P(PSTR("endstop not hit 1, current_pos[Z]: %f \n"), current_position[Z_AXIS]);
  904. goto error;
  905. }
  906. #ifdef TMC2130
  907. if (READ(Z_TMC2130_DIAG) != 0)
  908. {
  909. //printf_P(PSTR("crash detected 1, current_pos[Z]: %f \n"), current_position[Z_AXIS]);
  910. goto error; //crash Z detected
  911. }
  912. #endif //TMC2130
  913. for (uint8_t i = 0; i < n_iter; ++ i)
  914. {
  915. current_position[Z_AXIS] += high_deviation_occured ? 0.5 : 0.2;
  916. float z_bckp = current_position[Z_AXIS];
  917. go_to_current(homing_feedrate[Z_AXIS]/60);
  918. // Move back down slowly to find bed.
  919. current_position[Z_AXIS] = minimum_z;
  920. //printf_P(PSTR("init Z = %f, min_z = %f, i = %d\n"), z_bckp, minimum_z, i);
  921. go_to_current(homing_feedrate[Z_AXIS]/(4*60));
  922. // we have to let the planner know where we are right now as it is not where we said to go.
  923. update_current_position_z();
  924. //printf_P(PSTR("Zs: %f, Z: %f, delta Z: %f"), z_bckp, current_position[Z_AXIS], (z_bckp - current_position[Z_AXIS]));
  925. if (abs(current_position[Z_AXIS] - z_bckp) < 0.025) {
  926. //printf_P(PSTR("PINDA triggered immediately, move Z higher and repeat measurement\n"));
  927. current_position[Z_AXIS] += 0.5;
  928. go_to_current(homing_feedrate[Z_AXIS]/60);
  929. current_position[Z_AXIS] = minimum_z;
  930. go_to_current(homing_feedrate[Z_AXIS]/(4*60));
  931. // we have to let the planner know where we are right now as it is not where we said to go.
  932. update_current_position_z();
  933. }
  934. if (!endstop_z_hit_on_purpose())
  935. {
  936. //printf_P(PSTR("i = %d, endstop not hit 2, current_pos[Z]: %f \n"), i, current_position[Z_AXIS]);
  937. goto error;
  938. }
  939. #ifdef TMC2130
  940. if (READ(Z_TMC2130_DIAG) != 0) {
  941. //printf_P(PSTR("crash detected 2, current_pos[Z]: %f \n"), current_position[Z_AXIS]);
  942. goto error; //crash Z detected
  943. }
  944. #endif //TMC2130
  945. // SERIAL_ECHOPGM("Bed find_bed_induction_sensor_point_z low, height: ");
  946. // MYSERIAL.print(current_position[Z_AXIS], 5);
  947. // SERIAL_ECHOLNPGM("");
  948. float dz = i?abs(current_position[Z_AXIS] - (z / i)):0;
  949. z += current_position[Z_AXIS];
  950. //printf_P(PSTR("Z[%d] = %d, dz=%d\n"), i, (int)(current_position[Z_AXIS] * 1000), (int)(dz * 1000));
  951. //printf_P(PSTR("Z- measurement deviation from avg value %f um\n"), dz);
  952. if (dz > 0.05) { //deviation > 50um
  953. if (high_deviation_occured == false) { //first occurence may be caused in some cases by mechanic resonance probably especially if printer is placed on unstable surface
  954. //printf_P(PSTR("high dev. first occurence\n"));
  955. delay_keep_alive(500); //damping
  956. //start measurement from the begining, but this time with higher movements in Z axis which should help to reduce mechanical resonance
  957. high_deviation_occured = true;
  958. i = -1;
  959. z = 0;
  960. }
  961. else {
  962. goto error;
  963. }
  964. }
  965. //printf_P(PSTR("PINDA triggered at %f\n"), current_position[Z_AXIS]);
  966. }
  967. current_position[Z_AXIS] = z;
  968. if (n_iter > 1)
  969. current_position[Z_AXIS] /= float(n_iter);
  970. enable_endstops(endstops_enabled);
  971. enable_z_endstop(endstop_z_enabled);
  972. // SERIAL_ECHOLNPGM("find_bed_induction_sensor_point_z 3");
  973. #ifdef TMC2130
  974. FORCE_HIGH_POWER_END;
  975. #endif
  976. bedPWMDisabled = 0;
  977. return true;
  978. error:
  979. // SERIAL_ECHOLNPGM("find_bed_induction_sensor_point_z 4");
  980. enable_endstops(endstops_enabled);
  981. enable_z_endstop(endstop_z_enabled);
  982. #ifdef TMC2130
  983. FORCE_HIGH_POWER_END;
  984. #endif
  985. bedPWMDisabled = 0;
  986. return false;
  987. }
  988. #ifdef NEW_XYZCAL
  989. BedSkewOffsetDetectionResultType xyzcal_find_bed_induction_sensor_point_xy();
  990. #endif //NEW_XYZCAL
  991. // Search around the current_position[X,Y],
  992. // look for the induction sensor response.
  993. // Adjust the current_position[X,Y,Z] to the center of the target dot and its response Z coordinate.
  994. #define FIND_BED_INDUCTION_SENSOR_POINT_X_RADIUS (8.f)
  995. #define FIND_BED_INDUCTION_SENSOR_POINT_Y_RADIUS (4.f)
  996. #define FIND_BED_INDUCTION_SENSOR_POINT_XY_STEP (1.f)
  997. #ifdef HEATBED_V2
  998. #define FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP (2.f)
  999. #define FIND_BED_INDUCTION_SENSOR_POINT_MAX_Z_ERROR (0.03f)
  1000. #else //HEATBED_V2
  1001. #define FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP (0.2f)
  1002. #endif //HEATBED_V2
  1003. #ifdef HEATBED_V2
  1004. BedSkewOffsetDetectionResultType find_bed_induction_sensor_point_xy(int
  1005. #if !defined (NEW_XYZCAL) && defined (SUPPORT_VERBOSITY)
  1006. verbosity_level
  1007. #endif
  1008. )
  1009. {
  1010. #ifdef NEW_XYZCAL
  1011. return xyzcal_find_bed_induction_sensor_point_xy();
  1012. #else //NEW_XYZCAL
  1013. #ifdef SUPPORT_VERBOSITY
  1014. if (verbosity_level >= 10) MYSERIAL.println("find bed induction sensor point xy");
  1015. #endif // SUPPORT_VERBOSITY
  1016. float feedrate = homing_feedrate[X_AXIS] / 60.f;
  1017. bool found = false;
  1018. {
  1019. float x0 = current_position[X_AXIS] - FIND_BED_INDUCTION_SENSOR_POINT_X_RADIUS;
  1020. float x1 = current_position[X_AXIS] + FIND_BED_INDUCTION_SENSOR_POINT_X_RADIUS;
  1021. float y0 = current_position[Y_AXIS] - FIND_BED_INDUCTION_SENSOR_POINT_Y_RADIUS;
  1022. float y1 = current_position[Y_AXIS] + FIND_BED_INDUCTION_SENSOR_POINT_Y_RADIUS;
  1023. uint8_t nsteps_y;
  1024. uint8_t i;
  1025. if (x0 < X_MIN_POS) {
  1026. x0 = X_MIN_POS;
  1027. #ifdef SUPPORT_VERBOSITY
  1028. if (verbosity_level >= 20) SERIAL_ECHOLNPGM("X searching radius lower than X_MIN. Clamping was done.");
  1029. #endif // SUPPORT_VERBOSITY
  1030. }
  1031. if (x1 > X_MAX_POS) {
  1032. x1 = X_MAX_POS;
  1033. #ifdef SUPPORT_VERBOSITY
  1034. if (verbosity_level >= 20) SERIAL_ECHOLNPGM("X searching radius higher than X_MAX. Clamping was done.");
  1035. #endif // SUPPORT_VERBOSITY
  1036. }
  1037. if (y0 < Y_MIN_POS_FOR_BED_CALIBRATION) {
  1038. y0 = Y_MIN_POS_FOR_BED_CALIBRATION;
  1039. #ifdef SUPPORT_VERBOSITY
  1040. if (verbosity_level >= 20) SERIAL_ECHOLNPGM("Y searching radius lower than Y_MIN. Clamping was done.");
  1041. #endif // SUPPORT_VERBOSITY
  1042. }
  1043. if (y1 > Y_MAX_POS) {
  1044. y1 = Y_MAX_POS;
  1045. #ifdef SUPPORT_VERBOSITY
  1046. if (verbosity_level >= 20) SERIAL_ECHOLNPGM("Y searching radius higher than X_MAX. Clamping was done.");
  1047. #endif // SUPPORT_VERBOSITY
  1048. }
  1049. nsteps_y = int(ceil((y1 - y0) / FIND_BED_INDUCTION_SENSOR_POINT_XY_STEP));
  1050. enable_endstops(false);
  1051. bool dir_positive = true;
  1052. float z_error = 2 * FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP;
  1053. float find_bed_induction_sensor_point_z_step = FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP;
  1054. float initial_z_position = current_position[Z_AXIS];
  1055. // go_xyz(current_position[X_AXIS], current_position[Y_AXIS], MESH_HOME_Z_SEARCH, homing_feedrate[Z_AXIS]/60);
  1056. go_xyz(x0, y0, current_position[Z_AXIS], feedrate);
  1057. // Continuously lower the Z axis.
  1058. endstops_hit_on_purpose();
  1059. enable_z_endstop(true);
  1060. bool direction = false;
  1061. while (current_position[Z_AXIS] > -10.f && z_error > FIND_BED_INDUCTION_SENSOR_POINT_MAX_Z_ERROR) {
  1062. // Do nsteps_y zig-zag movements.
  1063. SERIAL_ECHOPGM("z_error: ");
  1064. MYSERIAL.println(z_error);
  1065. current_position[Y_AXIS] = direction ? y1 : y0;
  1066. initial_z_position = current_position[Z_AXIS];
  1067. for (i = 0; i < (nsteps_y - 1); (direction == false) ? (current_position[Y_AXIS] += (y1 - y0) / float(nsteps_y - 1)) : (current_position[Y_AXIS] -= (y1 - y0) / float(nsteps_y - 1)), ++i) {
  1068. // Run with a slightly decreasing Z axis, zig-zag movement. Stop at the Z end-stop.
  1069. current_position[Z_AXIS] -= find_bed_induction_sensor_point_z_step / float(nsteps_y - 1);
  1070. go_xyz(dir_positive ? x1 : x0, current_position[Y_AXIS], current_position[Z_AXIS], feedrate);
  1071. dir_positive = !dir_positive;
  1072. if (endstop_z_hit_on_purpose()) {
  1073. update_current_position_xyz();
  1074. z_error = initial_z_position - current_position[Z_AXIS] + find_bed_induction_sensor_point_z_step;
  1075. if (z_error > FIND_BED_INDUCTION_SENSOR_POINT_MAX_Z_ERROR) {
  1076. find_bed_induction_sensor_point_z_step = z_error / 2;
  1077. current_position[Z_AXIS] += z_error;
  1078. enable_z_endstop(false);
  1079. (direction == false) ? go_xyz(x0, y0, current_position[Z_AXIS], feedrate) : go_xyz(x0, y1, current_position[Z_AXIS], feedrate);
  1080. enable_z_endstop(true);
  1081. }
  1082. goto endloop;
  1083. }
  1084. }
  1085. for (i = 0; i < (nsteps_y - 1); (direction == false) ? (current_position[Y_AXIS] -= (y1 - y0) / float(nsteps_y - 1)) : (current_position[Y_AXIS] += (y1 - y0) / float(nsteps_y - 1)), ++i) {
  1086. // Run with a slightly decreasing Z axis, zig-zag movement. Stop at the Z end-stop.
  1087. current_position[Z_AXIS] -= find_bed_induction_sensor_point_z_step / float(nsteps_y - 1);
  1088. go_xyz(dir_positive ? x1 : x0, current_position[Y_AXIS], current_position[Z_AXIS], feedrate);
  1089. dir_positive = !dir_positive;
  1090. if (endstop_z_hit_on_purpose()) {
  1091. update_current_position_xyz();
  1092. z_error = initial_z_position - current_position[Z_AXIS];
  1093. if (z_error > FIND_BED_INDUCTION_SENSOR_POINT_MAX_Z_ERROR) {
  1094. find_bed_induction_sensor_point_z_step = z_error / 2;
  1095. current_position[Z_AXIS] += z_error;
  1096. enable_z_endstop(false);
  1097. direction = !direction;
  1098. (direction == false) ? go_xyz(x0, y0, current_position[Z_AXIS], feedrate) : go_xyz(x0, y1, current_position[Z_AXIS], feedrate);
  1099. enable_z_endstop(true);
  1100. }
  1101. goto endloop;
  1102. }
  1103. }
  1104. endloop:;
  1105. }
  1106. #ifdef SUPPORT_VERBOSITY
  1107. if (verbosity_level >= 20) {
  1108. SERIAL_ECHO("First hit");
  1109. SERIAL_ECHO("- X: ");
  1110. MYSERIAL.print(current_position[X_AXIS]);
  1111. SERIAL_ECHO("; Y: ");
  1112. MYSERIAL.print(current_position[Y_AXIS]);
  1113. SERIAL_ECHO("; Z: ");
  1114. MYSERIAL.println(current_position[Z_AXIS]);
  1115. }
  1116. #endif //SUPPORT_VERBOSITY
  1117. //lcd_show_fullscreen_message_and_wait_P(PSTR("First hit"));
  1118. //lcd_update_enable(true);
  1119. float init_x_position = current_position[X_AXIS];
  1120. float init_y_position = current_position[Y_AXIS];
  1121. // we have to let the planner know where we are right now as it is not where we said to go.
  1122. update_current_position_xyz();
  1123. enable_z_endstop(false);
  1124. for (int8_t iter = 0; iter < 2; ++iter) {
  1125. /*SERIAL_ECHOPGM("iter: ");
  1126. MYSERIAL.println(iter);
  1127. SERIAL_ECHOPGM("1 - current_position[Z_AXIS]: ");
  1128. MYSERIAL.println(current_position[Z_AXIS]);*/
  1129. // Slightly lower the Z axis to get a reliable trigger.
  1130. current_position[Z_AXIS] -= 0.1f;
  1131. go_xyz(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], homing_feedrate[Z_AXIS] / (60 * 10));
  1132. SERIAL_ECHOPGM("2 - current_position[Z_AXIS]: ");
  1133. MYSERIAL.println(current_position[Z_AXIS]);
  1134. // Do nsteps_y zig-zag movements.
  1135. float a, b;
  1136. float avg[2] = { 0,0 };
  1137. invert_z_endstop(true);
  1138. for (int iteration = 0; iteration < 8; iteration++) {
  1139. found = false;
  1140. enable_z_endstop(true);
  1141. go_xy(init_x_position + 16.0f, current_position[Y_AXIS], feedrate / 5);
  1142. update_current_position_xyz();
  1143. if (!endstop_z_hit_on_purpose()) {
  1144. // SERIAL_ECHOLN("Search X span 0 - not found");
  1145. continue;
  1146. }
  1147. // SERIAL_ECHOLN("Search X span 0 - found");
  1148. a = current_position[X_AXIS];
  1149. enable_z_endstop(false);
  1150. go_xy(init_x_position, current_position[Y_AXIS], feedrate / 5);
  1151. enable_z_endstop(true);
  1152. go_xy(init_x_position - 16.0f, current_position[Y_AXIS], feedrate / 5);
  1153. update_current_position_xyz();
  1154. if (!endstop_z_hit_on_purpose()) {
  1155. // SERIAL_ECHOLN("Search X span 1 - not found");
  1156. continue;
  1157. }
  1158. // SERIAL_ECHOLN("Search X span 1 - found");
  1159. b = current_position[X_AXIS];
  1160. // Go to the center.
  1161. enable_z_endstop(false);
  1162. current_position[X_AXIS] = 0.5f * (a + b);
  1163. go_xy(current_position[X_AXIS], init_y_position, feedrate / 5);
  1164. found = true;
  1165. // Search in the Y direction along a cross.
  1166. found = false;
  1167. enable_z_endstop(true);
  1168. go_xy(current_position[X_AXIS], init_y_position + 16.0f, feedrate / 5);
  1169. update_current_position_xyz();
  1170. if (!endstop_z_hit_on_purpose()) {
  1171. // SERIAL_ECHOLN("Search Y2 span 0 - not found");
  1172. continue;
  1173. }
  1174. // SERIAL_ECHOLN("Search Y2 span 0 - found");
  1175. a = current_position[Y_AXIS];
  1176. enable_z_endstop(false);
  1177. go_xy(current_position[X_AXIS], init_y_position, feedrate / 5);
  1178. enable_z_endstop(true);
  1179. go_xy(current_position[X_AXIS], init_y_position - 16.0f, feedrate / 5);
  1180. update_current_position_xyz();
  1181. if (!endstop_z_hit_on_purpose()) {
  1182. // SERIAL_ECHOLN("Search Y2 span 1 - not found");
  1183. continue;
  1184. }
  1185. // SERIAL_ECHOLN("Search Y2 span 1 - found");
  1186. b = current_position[Y_AXIS];
  1187. // Go to the center.
  1188. enable_z_endstop(false);
  1189. current_position[Y_AXIS] = 0.5f * (a + b);
  1190. go_xy(current_position[X_AXIS], current_position[Y_AXIS], feedrate / 5);
  1191. #ifdef SUPPORT_VERBOSITY
  1192. if (verbosity_level >= 20) {
  1193. SERIAL_ECHOPGM("ITERATION: ");
  1194. MYSERIAL.println(iteration);
  1195. SERIAL_ECHOPGM("CURRENT POSITION X: ");
  1196. MYSERIAL.println(current_position[X_AXIS]);
  1197. SERIAL_ECHOPGM("CURRENT POSITION Y: ");
  1198. MYSERIAL.println(current_position[Y_AXIS]);
  1199. }
  1200. #endif //SUPPORT_VERBOSITY
  1201. if (iteration > 0) {
  1202. // Average the last 7 measurements.
  1203. avg[X_AXIS] += current_position[X_AXIS];
  1204. avg[Y_AXIS] += current_position[Y_AXIS];
  1205. }
  1206. init_x_position = current_position[X_AXIS];
  1207. init_y_position = current_position[Y_AXIS];
  1208. found = true;
  1209. }
  1210. invert_z_endstop(false);
  1211. avg[X_AXIS] *= (1.f / 7.f);
  1212. avg[Y_AXIS] *= (1.f / 7.f);
  1213. current_position[X_AXIS] = avg[X_AXIS];
  1214. current_position[Y_AXIS] = avg[Y_AXIS];
  1215. #ifdef SUPPORT_VERBOSITY
  1216. if (verbosity_level >= 20) {
  1217. SERIAL_ECHOPGM("AVG CURRENT POSITION X: ");
  1218. MYSERIAL.println(current_position[X_AXIS]);
  1219. SERIAL_ECHOPGM("AVG CURRENT POSITION Y: ");
  1220. MYSERIAL.println(current_position[Y_AXIS]);
  1221. }
  1222. #endif // SUPPORT_VERBOSITY
  1223. go_xy(current_position[X_AXIS], current_position[Y_AXIS], feedrate);
  1224. #ifdef SUPPORT_VERBOSITY
  1225. if (verbosity_level >= 20) {
  1226. lcd_show_fullscreen_message_and_wait_P(PSTR("Final position"));
  1227. lcd_update_enable(true);
  1228. }
  1229. #endif //SUPPORT_VERBOSITY
  1230. break;
  1231. }
  1232. }
  1233. enable_z_endstop(false);
  1234. invert_z_endstop(false);
  1235. return found;
  1236. #endif //NEW_XYZCAL
  1237. }
  1238. #else //HEATBED_V2
  1239. BedSkewOffsetDetectionResultType find_bed_induction_sensor_point_xy(int verbosity_level)
  1240. {
  1241. #ifdef NEW_XYZCAL
  1242. return xyzcal_find_bed_induction_sensor_point_xy();
  1243. #else //NEW_XYZCAL
  1244. #ifdef SUPPORT_VERBOSITY
  1245. if (verbosity_level >= 10) MYSERIAL.println("find bed induction sensor point xy");
  1246. #endif // SUPPORT_VERBOSITY
  1247. float feedrate = homing_feedrate[X_AXIS] / 60.f;
  1248. bool found = false;
  1249. {
  1250. float x0 = current_position[X_AXIS] - FIND_BED_INDUCTION_SENSOR_POINT_X_RADIUS;
  1251. float x1 = current_position[X_AXIS] + FIND_BED_INDUCTION_SENSOR_POINT_X_RADIUS;
  1252. float y0 = current_position[Y_AXIS] - FIND_BED_INDUCTION_SENSOR_POINT_Y_RADIUS;
  1253. float y1 = current_position[Y_AXIS] + FIND_BED_INDUCTION_SENSOR_POINT_Y_RADIUS;
  1254. uint8_t nsteps_y;
  1255. uint8_t i;
  1256. if (x0 < X_MIN_POS) {
  1257. x0 = X_MIN_POS;
  1258. #ifdef SUPPORT_VERBOSITY
  1259. if (verbosity_level >= 20) SERIAL_ECHOLNPGM("X searching radius lower than X_MIN. Clamping was done.");
  1260. #endif // SUPPORT_VERBOSITY
  1261. }
  1262. if (x1 > X_MAX_POS) {
  1263. x1 = X_MAX_POS;
  1264. #ifdef SUPPORT_VERBOSITY
  1265. if (verbosity_level >= 20) SERIAL_ECHOLNPGM("X searching radius higher than X_MAX. Clamping was done.");
  1266. #endif // SUPPORT_VERBOSITY
  1267. }
  1268. if (y0 < Y_MIN_POS_FOR_BED_CALIBRATION) {
  1269. y0 = Y_MIN_POS_FOR_BED_CALIBRATION;
  1270. #ifdef SUPPORT_VERBOSITY
  1271. if (verbosity_level >= 20) SERIAL_ECHOLNPGM("Y searching radius lower than Y_MIN. Clamping was done.");
  1272. #endif // SUPPORT_VERBOSITY
  1273. }
  1274. if (y1 > Y_MAX_POS) {
  1275. y1 = Y_MAX_POS;
  1276. #ifdef SUPPORT_VERBOSITY
  1277. if (verbosity_level >= 20) SERIAL_ECHOLNPGM("Y searching radius higher than X_MAX. Clamping was done.");
  1278. #endif // SUPPORT_VERBOSITY
  1279. }
  1280. nsteps_y = int(ceil((y1 - y0) / FIND_BED_INDUCTION_SENSOR_POINT_XY_STEP));
  1281. enable_endstops(false);
  1282. bool dir_positive = true;
  1283. // go_xyz(current_position[X_AXIS], current_position[Y_AXIS], MESH_HOME_Z_SEARCH, homing_feedrate[Z_AXIS]/60);
  1284. go_xyz(x0, y0, current_position[Z_AXIS], feedrate);
  1285. // Continuously lower the Z axis.
  1286. endstops_hit_on_purpose();
  1287. enable_z_endstop(true);
  1288. while (current_position[Z_AXIS] > -10.f) {
  1289. // Do nsteps_y zig-zag movements.
  1290. current_position[Y_AXIS] = y0;
  1291. for (i = 0; i < nsteps_y; current_position[Y_AXIS] += (y1 - y0) / float(nsteps_y - 1), ++i) {
  1292. // Run with a slightly decreasing Z axis, zig-zag movement. Stop at the Z end-stop.
  1293. current_position[Z_AXIS] -= FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP / float(nsteps_y);
  1294. go_xyz(dir_positive ? x1 : x0, current_position[Y_AXIS], current_position[Z_AXIS], feedrate);
  1295. dir_positive = !dir_positive;
  1296. if (endstop_z_hit_on_purpose())
  1297. goto endloop;
  1298. }
  1299. for (i = 0; i < nsteps_y; current_position[Y_AXIS] -= (y1 - y0) / float(nsteps_y - 1), ++i) {
  1300. // Run with a slightly decreasing Z axis, zig-zag movement. Stop at the Z end-stop.
  1301. current_position[Z_AXIS] -= FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP / float(nsteps_y);
  1302. go_xyz(dir_positive ? x1 : x0, current_position[Y_AXIS], current_position[Z_AXIS], feedrate);
  1303. dir_positive = !dir_positive;
  1304. if (endstop_z_hit_on_purpose())
  1305. goto endloop;
  1306. }
  1307. }
  1308. endloop:
  1309. // SERIAL_ECHOLN("First hit");
  1310. // we have to let the planner know where we are right now as it is not where we said to go.
  1311. update_current_position_xyz();
  1312. // Search in this plane for the first hit. Zig-zag first in X, then in Y axis.
  1313. for (int8_t iter = 0; iter < 3; ++iter) {
  1314. if (iter > 0) {
  1315. // Slightly lower the Z axis to get a reliable trigger.
  1316. current_position[Z_AXIS] -= 0.02f;
  1317. go_xyz(current_position[X_AXIS], current_position[Y_AXIS], MESH_HOME_Z_SEARCH, homing_feedrate[Z_AXIS] / 60);
  1318. }
  1319. // Do nsteps_y zig-zag movements.
  1320. float a, b;
  1321. enable_endstops(false);
  1322. enable_z_endstop(false);
  1323. current_position[Y_AXIS] = y0;
  1324. go_xy(x0, current_position[Y_AXIS], feedrate);
  1325. enable_z_endstop(true);
  1326. found = false;
  1327. for (i = 0, dir_positive = true; i < nsteps_y; current_position[Y_AXIS] += (y1 - y0) / float(nsteps_y - 1), ++i, dir_positive = !dir_positive) {
  1328. go_xy(dir_positive ? x1 : x0, current_position[Y_AXIS], feedrate);
  1329. if (endstop_z_hit_on_purpose()) {
  1330. found = true;
  1331. break;
  1332. }
  1333. }
  1334. update_current_position_xyz();
  1335. if (!found) {
  1336. // SERIAL_ECHOLN("Search in Y - not found");
  1337. continue;
  1338. }
  1339. // SERIAL_ECHOLN("Search in Y - found");
  1340. a = current_position[Y_AXIS];
  1341. enable_z_endstop(false);
  1342. current_position[Y_AXIS] = y1;
  1343. go_xy(x0, current_position[Y_AXIS], feedrate);
  1344. enable_z_endstop(true);
  1345. found = false;
  1346. for (i = 0, dir_positive = true; i < nsteps_y; current_position[Y_AXIS] -= (y1 - y0) / float(nsteps_y - 1), ++i, dir_positive = !dir_positive) {
  1347. go_xy(dir_positive ? x1 : x0, current_position[Y_AXIS], feedrate);
  1348. if (endstop_z_hit_on_purpose()) {
  1349. found = true;
  1350. break;
  1351. }
  1352. }
  1353. update_current_position_xyz();
  1354. if (!found) {
  1355. // SERIAL_ECHOLN("Search in Y2 - not found");
  1356. continue;
  1357. }
  1358. // SERIAL_ECHOLN("Search in Y2 - found");
  1359. b = current_position[Y_AXIS];
  1360. current_position[Y_AXIS] = 0.5f * (a + b);
  1361. // Search in the X direction along a cross.
  1362. found = false;
  1363. enable_z_endstop(false);
  1364. go_xy(x0, current_position[Y_AXIS], feedrate);
  1365. enable_z_endstop(true);
  1366. go_xy(x1, current_position[Y_AXIS], feedrate);
  1367. update_current_position_xyz();
  1368. if (!endstop_z_hit_on_purpose()) {
  1369. // SERIAL_ECHOLN("Search X span 0 - not found");
  1370. continue;
  1371. }
  1372. // SERIAL_ECHOLN("Search X span 0 - found");
  1373. a = current_position[X_AXIS];
  1374. enable_z_endstop(false);
  1375. go_xy(x1, current_position[Y_AXIS], feedrate);
  1376. enable_z_endstop(true);
  1377. go_xy(x0, current_position[Y_AXIS], feedrate);
  1378. update_current_position_xyz();
  1379. if (!endstop_z_hit_on_purpose()) {
  1380. // SERIAL_ECHOLN("Search X span 1 - not found");
  1381. continue;
  1382. }
  1383. // SERIAL_ECHOLN("Search X span 1 - found");
  1384. b = current_position[X_AXIS];
  1385. // Go to the center.
  1386. enable_z_endstop(false);
  1387. current_position[X_AXIS] = 0.5f * (a + b);
  1388. go_xy(current_position[X_AXIS], current_position[Y_AXIS], feedrate);
  1389. found = true;
  1390. #if 1
  1391. // Search in the Y direction along a cross.
  1392. found = false;
  1393. enable_z_endstop(false);
  1394. go_xy(current_position[X_AXIS], y0, feedrate);
  1395. enable_z_endstop(true);
  1396. go_xy(current_position[X_AXIS], y1, feedrate);
  1397. update_current_position_xyz();
  1398. if (!endstop_z_hit_on_purpose()) {
  1399. // SERIAL_ECHOLN("Search Y2 span 0 - not found");
  1400. continue;
  1401. }
  1402. // SERIAL_ECHOLN("Search Y2 span 0 - found");
  1403. a = current_position[Y_AXIS];
  1404. enable_z_endstop(false);
  1405. go_xy(current_position[X_AXIS], y1, feedrate);
  1406. enable_z_endstop(true);
  1407. go_xy(current_position[X_AXIS], y0, feedrate);
  1408. update_current_position_xyz();
  1409. if (!endstop_z_hit_on_purpose()) {
  1410. // SERIAL_ECHOLN("Search Y2 span 1 - not found");
  1411. continue;
  1412. }
  1413. // SERIAL_ECHOLN("Search Y2 span 1 - found");
  1414. b = current_position[Y_AXIS];
  1415. // Go to the center.
  1416. enable_z_endstop(false);
  1417. current_position[Y_AXIS] = 0.5f * (a + b);
  1418. go_xy(current_position[X_AXIS], current_position[Y_AXIS], feedrate);
  1419. found = true;
  1420. #endif
  1421. break;
  1422. }
  1423. }
  1424. enable_z_endstop(false);
  1425. if (found)
  1426. return BED_SKEW_OFFSET_DETECTION_POINT_FOUND;
  1427. return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
  1428. #endif //NEW_XYZCAL
  1429. }
  1430. #endif //HEATBED_V2
  1431. #ifndef NEW_XYZCAL
  1432. // Search around the current_position[X,Y,Z].
  1433. // It is expected, that the induction sensor is switched on at the current position.
  1434. // Look around this center point by painting a star around the point.
  1435. inline bool improve_bed_induction_sensor_point()
  1436. {
  1437. static const float search_radius = 8.f;
  1438. bool endstops_enabled = enable_endstops(false);
  1439. bool endstop_z_enabled = enable_z_endstop(false);
  1440. bool found = false;
  1441. float feedrate = homing_feedrate[X_AXIS] / 60.f;
  1442. float center_old_x = current_position[X_AXIS];
  1443. float center_old_y = current_position[Y_AXIS];
  1444. float center_x = 0.f;
  1445. float center_y = 0.f;
  1446. for (uint8_t iter = 0; iter < 4; ++ iter) {
  1447. switch (iter) {
  1448. case 0:
  1449. destination[X_AXIS] = center_old_x - search_radius * 0.707;
  1450. destination[Y_AXIS] = center_old_y - search_radius * 0.707;
  1451. break;
  1452. case 1:
  1453. destination[X_AXIS] = center_old_x + search_radius * 0.707;
  1454. destination[Y_AXIS] = center_old_y + search_radius * 0.707;
  1455. break;
  1456. case 2:
  1457. destination[X_AXIS] = center_old_x + search_radius * 0.707;
  1458. destination[Y_AXIS] = center_old_y - search_radius * 0.707;
  1459. break;
  1460. case 3:
  1461. default:
  1462. destination[X_AXIS] = center_old_x - search_radius * 0.707;
  1463. destination[Y_AXIS] = center_old_y + search_radius * 0.707;
  1464. break;
  1465. }
  1466. // Trim the vector from center_old_[x,y] to destination[x,y] by the bed dimensions.
  1467. float vx = destination[X_AXIS] - center_old_x;
  1468. float vy = destination[Y_AXIS] - center_old_y;
  1469. float l = sqrt(vx*vx+vy*vy);
  1470. float t;
  1471. if (destination[X_AXIS] < X_MIN_POS) {
  1472. // Exiting the bed at xmin.
  1473. t = (center_x - X_MIN_POS) / l;
  1474. destination[X_AXIS] = X_MIN_POS;
  1475. destination[Y_AXIS] = center_old_y + t * vy;
  1476. } else if (destination[X_AXIS] > X_MAX_POS) {
  1477. // Exiting the bed at xmax.
  1478. t = (X_MAX_POS - center_x) / l;
  1479. destination[X_AXIS] = X_MAX_POS;
  1480. destination[Y_AXIS] = center_old_y + t * vy;
  1481. }
  1482. if (destination[Y_AXIS] < Y_MIN_POS_FOR_BED_CALIBRATION) {
  1483. // Exiting the bed at ymin.
  1484. t = (center_y - Y_MIN_POS_FOR_BED_CALIBRATION) / l;
  1485. destination[X_AXIS] = center_old_x + t * vx;
  1486. destination[Y_AXIS] = Y_MIN_POS_FOR_BED_CALIBRATION;
  1487. } else if (destination[Y_AXIS] > Y_MAX_POS) {
  1488. // Exiting the bed at xmax.
  1489. t = (Y_MAX_POS - center_y) / l;
  1490. destination[X_AXIS] = center_old_x + t * vx;
  1491. destination[Y_AXIS] = Y_MAX_POS;
  1492. }
  1493. // Move away from the measurement point.
  1494. enable_endstops(false);
  1495. go_xy(destination[X_AXIS], destination[Y_AXIS], feedrate);
  1496. // Move towards the measurement point, until the induction sensor triggers.
  1497. enable_endstops(true);
  1498. go_xy(center_old_x, center_old_y, feedrate);
  1499. update_current_position_xyz();
  1500. // if (! endstop_z_hit_on_purpose()) return false;
  1501. center_x += current_position[X_AXIS];
  1502. center_y += current_position[Y_AXIS];
  1503. }
  1504. // Calculate the new center, move to the new center.
  1505. center_x /= 4.f;
  1506. center_y /= 4.f;
  1507. current_position[X_AXIS] = center_x;
  1508. current_position[Y_AXIS] = center_y;
  1509. enable_endstops(false);
  1510. go_xy(current_position[X_AXIS], current_position[Y_AXIS], feedrate);
  1511. enable_endstops(endstops_enabled);
  1512. enable_z_endstop(endstop_z_enabled);
  1513. return found;
  1514. }
  1515. #endif //NEW_XYZCAL
  1516. #ifndef NEW_XYZCAL
  1517. static inline void debug_output_point(const char *type, const float &x, const float &y, const float &z)
  1518. {
  1519. SERIAL_ECHOPGM("Measured ");
  1520. SERIAL_ECHORPGM(type);
  1521. SERIAL_ECHOPGM(" ");
  1522. MYSERIAL.print(x, 5);
  1523. SERIAL_ECHOPGM(", ");
  1524. MYSERIAL.print(y, 5);
  1525. SERIAL_ECHOPGM(", ");
  1526. MYSERIAL.print(z, 5);
  1527. SERIAL_ECHOLNPGM("");
  1528. }
  1529. #endif //NEW_XYZCAL
  1530. #ifndef NEW_XYZCAL
  1531. // Search around the current_position[X,Y,Z].
  1532. // It is expected, that the induction sensor is switched on at the current position.
  1533. // Look around this center point by painting a star around the point.
  1534. #define IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS (8.f)
  1535. inline bool improve_bed_induction_sensor_point2(bool lift_z_on_min_y, int8_t verbosity_level)
  1536. {
  1537. float center_old_x = current_position[X_AXIS];
  1538. float center_old_y = current_position[Y_AXIS];
  1539. float a, b;
  1540. bool point_small = false;
  1541. enable_endstops(false);
  1542. {
  1543. float x0 = center_old_x - IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS;
  1544. float x1 = center_old_x + IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS;
  1545. if (x0 < X_MIN_POS)
  1546. x0 = X_MIN_POS;
  1547. if (x1 > X_MAX_POS)
  1548. x1 = X_MAX_POS;
  1549. // Search in the X direction along a cross.
  1550. enable_z_endstop(false);
  1551. go_xy(x0, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
  1552. enable_z_endstop(true);
  1553. go_xy(x1, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
  1554. update_current_position_xyz();
  1555. if (! endstop_z_hit_on_purpose()) {
  1556. current_position[X_AXIS] = center_old_x;
  1557. goto canceled;
  1558. }
  1559. a = current_position[X_AXIS];
  1560. enable_z_endstop(false);
  1561. go_xy(x1, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
  1562. enable_z_endstop(true);
  1563. go_xy(x0, current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
  1564. update_current_position_xyz();
  1565. if (! endstop_z_hit_on_purpose()) {
  1566. current_position[X_AXIS] = center_old_x;
  1567. goto canceled;
  1568. }
  1569. b = current_position[X_AXIS];
  1570. if (b - a < MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
  1571. #ifdef SUPPORT_VERBOSITY
  1572. if (verbosity_level >= 5) {
  1573. SERIAL_ECHOPGM("Point width too small: ");
  1574. SERIAL_ECHO(b - a);
  1575. SERIAL_ECHOLNPGM("");
  1576. }
  1577. #endif // SUPPORT_VERBOSITY
  1578. // We force the calibration routine to move the Z axis slightly down to make the response more pronounced.
  1579. if (b - a < 0.5f * MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
  1580. // Don't use the new X value.
  1581. current_position[X_AXIS] = center_old_x;
  1582. goto canceled;
  1583. } else {
  1584. // Use the new value, but force the Z axis to go a bit lower.
  1585. point_small = true;
  1586. }
  1587. }
  1588. #ifdef SUPPORT_VERBOSITY
  1589. if (verbosity_level >= 5) {
  1590. debug_output_point(PSTR("left" ), a, current_position[Y_AXIS], current_position[Z_AXIS]);
  1591. debug_output_point(PSTR("right"), b, current_position[Y_AXIS], current_position[Z_AXIS]);
  1592. }
  1593. #endif // SUPPORT_VERBOSITY
  1594. // Go to the center.
  1595. enable_z_endstop(false);
  1596. current_position[X_AXIS] = 0.5f * (a + b);
  1597. go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
  1598. }
  1599. {
  1600. float y0 = center_old_y - IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS;
  1601. float y1 = center_old_y + IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS;
  1602. if (y0 < Y_MIN_POS_FOR_BED_CALIBRATION)
  1603. y0 = Y_MIN_POS_FOR_BED_CALIBRATION;
  1604. if (y1 > Y_MAX_POS)
  1605. y1 = Y_MAX_POS;
  1606. // Search in the Y direction along a cross.
  1607. enable_z_endstop(false);
  1608. go_xy(current_position[X_AXIS], y0, homing_feedrate[X_AXIS] / 60.f);
  1609. if (lift_z_on_min_y) {
  1610. // The first row of points are very close to the end stop.
  1611. // Lift the sensor to disengage the trigger. This is necessary because of the sensor hysteresis.
  1612. go_xyz(current_position[X_AXIS], y0, current_position[Z_AXIS]+1.5f, homing_feedrate[Z_AXIS] / 60.f);
  1613. // and go back.
  1614. go_xyz(current_position[X_AXIS], y0, current_position[Z_AXIS], homing_feedrate[Z_AXIS] / 60.f);
  1615. }
  1616. if (lift_z_on_min_y && (READ(Z_MIN_PIN) ^ Z_MIN_ENDSTOP_INVERTING) == 1) {
  1617. // Already triggering before we started the move.
  1618. // Shift the trigger point slightly outwards.
  1619. // a = current_position[Y_AXIS] - 1.5f;
  1620. a = current_position[Y_AXIS];
  1621. } else {
  1622. enable_z_endstop(true);
  1623. go_xy(current_position[X_AXIS], y1, homing_feedrate[X_AXIS] / 60.f);
  1624. update_current_position_xyz();
  1625. if (! endstop_z_hit_on_purpose()) {
  1626. current_position[Y_AXIS] = center_old_y;
  1627. goto canceled;
  1628. }
  1629. a = current_position[Y_AXIS];
  1630. }
  1631. enable_z_endstop(false);
  1632. go_xy(current_position[X_AXIS], y1, homing_feedrate[X_AXIS] / 60.f);
  1633. enable_z_endstop(true);
  1634. go_xy(current_position[X_AXIS], y0, homing_feedrate[X_AXIS] / 60.f);
  1635. update_current_position_xyz();
  1636. if (! endstop_z_hit_on_purpose()) {
  1637. current_position[Y_AXIS] = center_old_y;
  1638. goto canceled;
  1639. }
  1640. b = current_position[Y_AXIS];
  1641. if (b - a < MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
  1642. // We force the calibration routine to move the Z axis slightly down to make the response more pronounced.
  1643. #ifdef SUPPORT_VERBOSITY
  1644. if (verbosity_level >= 5) {
  1645. SERIAL_ECHOPGM("Point height too small: ");
  1646. SERIAL_ECHO(b - a);
  1647. SERIAL_ECHOLNPGM("");
  1648. }
  1649. #endif // SUPPORT_VERBOSITY
  1650. if (b - a < 0.5f * MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
  1651. // Don't use the new Y value.
  1652. current_position[Y_AXIS] = center_old_y;
  1653. goto canceled;
  1654. } else {
  1655. // Use the new value, but force the Z axis to go a bit lower.
  1656. point_small = true;
  1657. }
  1658. }
  1659. #ifdef SUPPORT_VERBOSITY
  1660. if (verbosity_level >= 5) {
  1661. debug_output_point(PSTR("top" ), current_position[X_AXIS], a, current_position[Z_AXIS]);
  1662. debug_output_point(PSTR("bottom"), current_position[X_AXIS], b, current_position[Z_AXIS]);
  1663. }
  1664. #endif // SUPPORT_VERBOSITY
  1665. // Go to the center.
  1666. enable_z_endstop(false);
  1667. current_position[Y_AXIS] = 0.5f * (a + b);
  1668. go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
  1669. }
  1670. // If point is small but not too small, then force the Z axis to be lowered a bit,
  1671. // but use the new value. This is important when the initial position was off in one axis,
  1672. // for example if the initial calibration was shifted in the Y axis systematically.
  1673. // Then this first step will center.
  1674. return ! point_small;
  1675. canceled:
  1676. // Go back to the center.
  1677. enable_z_endstop(false);
  1678. go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
  1679. return false;
  1680. }
  1681. #endif //NEW_XYZCAL
  1682. #ifndef NEW_XYZCAL
  1683. // Searching the front points, where one cannot move the sensor head in front of the sensor point.
  1684. // Searching in a zig-zag movement in a plane for the maximum width of the response.
  1685. // This function may set the current_position[Y_AXIS] below Y_MIN_POS, if the function succeeded.
  1686. // If this function failed, the Y coordinate will never be outside the working space.
  1687. #define IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS (8.f)
  1688. #define IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y (0.1f)
  1689. inline bool improve_bed_induction_sensor_point3(int verbosity_level)
  1690. {
  1691. float center_old_x = current_position[X_AXIS];
  1692. float center_old_y = current_position[Y_AXIS];
  1693. float a, b;
  1694. bool result = true;
  1695. #ifdef SUPPORT_VERBOSITY
  1696. if (verbosity_level >= 20) MYSERIAL.println("Improve bed induction sensor point3");
  1697. #endif // SUPPORT_VERBOSITY
  1698. // Was the sensor point detected too far in the minus Y axis?
  1699. // If yes, the center of the induction point cannot be reached by the machine.
  1700. {
  1701. float x0 = center_old_x - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
  1702. float x1 = center_old_x + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
  1703. float y0 = center_old_y - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
  1704. float y1 = center_old_y + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
  1705. float y = y0;
  1706. if (x0 < X_MIN_POS)
  1707. x0 = X_MIN_POS;
  1708. if (x1 > X_MAX_POS)
  1709. x1 = X_MAX_POS;
  1710. if (y0 < Y_MIN_POS_FOR_BED_CALIBRATION)
  1711. y0 = Y_MIN_POS_FOR_BED_CALIBRATION;
  1712. if (y1 > Y_MAX_POS)
  1713. y1 = Y_MAX_POS;
  1714. #ifdef SUPPORT_VERBOSITY
  1715. if (verbosity_level >= 20) {
  1716. SERIAL_ECHOPGM("Initial position: ");
  1717. SERIAL_ECHO(center_old_x);
  1718. SERIAL_ECHOPGM(", ");
  1719. SERIAL_ECHO(center_old_y);
  1720. SERIAL_ECHOLNPGM("");
  1721. }
  1722. #endif // SUPPORT_VERBOSITY
  1723. // Search in the positive Y direction, until a maximum diameter is found.
  1724. // (the next diameter is smaller than the current one.)
  1725. float dmax = 0.f;
  1726. float xmax1 = 0.f;
  1727. float xmax2 = 0.f;
  1728. for (y = y0; y < y1; y += IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
  1729. enable_z_endstop(false);
  1730. go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
  1731. enable_z_endstop(true);
  1732. go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
  1733. update_current_position_xyz();
  1734. if (! endstop_z_hit_on_purpose()) {
  1735. continue;
  1736. // SERIAL_PROTOCOLPGM("Failed 1\n");
  1737. // current_position[X_AXIS] = center_old_x;
  1738. // goto canceled;
  1739. }
  1740. a = current_position[X_AXIS];
  1741. enable_z_endstop(false);
  1742. go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
  1743. enable_z_endstop(true);
  1744. go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
  1745. update_current_position_xyz();
  1746. if (! endstop_z_hit_on_purpose()) {
  1747. continue;
  1748. // SERIAL_PROTOCOLPGM("Failed 2\n");
  1749. // current_position[X_AXIS] = center_old_x;
  1750. // goto canceled;
  1751. }
  1752. b = current_position[X_AXIS];
  1753. #ifdef SUPPORT_VERBOSITY
  1754. if (verbosity_level >= 5) {
  1755. debug_output_point(PSTR("left" ), a, current_position[Y_AXIS], current_position[Z_AXIS]);
  1756. debug_output_point(PSTR("right"), b, current_position[Y_AXIS], current_position[Z_AXIS]);
  1757. }
  1758. #endif // SUPPORT_VERBOSITY
  1759. float d = b - a;
  1760. if (d > dmax) {
  1761. xmax1 = 0.5f * (a + b);
  1762. dmax = d;
  1763. } else if (dmax > 0.) {
  1764. y0 = y - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y;
  1765. break;
  1766. }
  1767. }
  1768. if (dmax == 0.) {
  1769. #ifdef SUPPORT_VERBOSITY
  1770. if (verbosity_level > 0)
  1771. SERIAL_PROTOCOLPGM("failed - not found\n");
  1772. #endif // SUPPORT_VERBOSITY
  1773. current_position[X_AXIS] = center_old_x;
  1774. current_position[Y_AXIS] = center_old_y;
  1775. goto canceled;
  1776. }
  1777. {
  1778. // Find the positive Y hit. This gives the extreme Y value for the search of the maximum diameter in the -Y direction.
  1779. enable_z_endstop(false);
  1780. go_xy(xmax1, y0 + IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS, homing_feedrate[X_AXIS] / 60.f);
  1781. enable_z_endstop(true);
  1782. go_xy(xmax1, max(y0 - IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS, Y_MIN_POS_FOR_BED_CALIBRATION), homing_feedrate[X_AXIS] / 60.f);
  1783. update_current_position_xyz();
  1784. if (! endstop_z_hit_on_purpose()) {
  1785. current_position[Y_AXIS] = center_old_y;
  1786. goto canceled;
  1787. }
  1788. #ifdef SUPPORT_VERBOSITY
  1789. if (verbosity_level >= 5)
  1790. debug_output_point(PSTR("top" ), current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
  1791. #endif // SUPPORT_VERBOSITY
  1792. y1 = current_position[Y_AXIS];
  1793. }
  1794. if (y1 <= y0) {
  1795. // Either the induction sensor is too high, or the induction sensor target is out of reach.
  1796. current_position[Y_AXIS] = center_old_y;
  1797. goto canceled;
  1798. }
  1799. // Search in the negative Y direction, until a maximum diameter is found.
  1800. dmax = 0.f;
  1801. // if (y0 + 1.f < y1)
  1802. // y1 = y0 + 1.f;
  1803. for (y = y1; y >= y0; y -= IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
  1804. enable_z_endstop(false);
  1805. go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
  1806. enable_z_endstop(true);
  1807. go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
  1808. update_current_position_xyz();
  1809. if (! endstop_z_hit_on_purpose()) {
  1810. continue;
  1811. /*
  1812. current_position[X_AXIS] = center_old_x;
  1813. SERIAL_PROTOCOLPGM("Failed 3\n");
  1814. goto canceled;
  1815. */
  1816. }
  1817. a = current_position[X_AXIS];
  1818. enable_z_endstop(false);
  1819. go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
  1820. enable_z_endstop(true);
  1821. go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
  1822. update_current_position_xyz();
  1823. if (! endstop_z_hit_on_purpose()) {
  1824. continue;
  1825. /*
  1826. current_position[X_AXIS] = center_old_x;
  1827. SERIAL_PROTOCOLPGM("Failed 4\n");
  1828. goto canceled;
  1829. */
  1830. }
  1831. b = current_position[X_AXIS];
  1832. #ifdef SUPPORT_VERBOSITY
  1833. if (verbosity_level >= 5) {
  1834. debug_output_point(PSTR("left" ), a, current_position[Y_AXIS], current_position[Z_AXIS]);
  1835. debug_output_point(PSTR("right"), b, current_position[Y_AXIS], current_position[Z_AXIS]);
  1836. }
  1837. #endif // SUPPORT_VERBOSITY
  1838. float d = b - a;
  1839. if (d > dmax) {
  1840. xmax2 = 0.5f * (a + b);
  1841. dmax = d;
  1842. } else if (dmax > 0.) {
  1843. y1 = y + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y;
  1844. break;
  1845. }
  1846. }
  1847. float xmax, ymax;
  1848. if (dmax == 0.f) {
  1849. // Only the hit in the positive direction found.
  1850. xmax = xmax1;
  1851. ymax = y0;
  1852. } else {
  1853. // Both positive and negative directions found.
  1854. xmax = xmax2;
  1855. ymax = 0.5f * (y0 + y1);
  1856. for (; y >= y0; y -= IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
  1857. enable_z_endstop(false);
  1858. go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
  1859. enable_z_endstop(true);
  1860. go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
  1861. update_current_position_xyz();
  1862. if (! endstop_z_hit_on_purpose()) {
  1863. continue;
  1864. /*
  1865. current_position[X_AXIS] = center_old_x;
  1866. SERIAL_PROTOCOLPGM("Failed 3\n");
  1867. goto canceled;
  1868. */
  1869. }
  1870. a = current_position[X_AXIS];
  1871. enable_z_endstop(false);
  1872. go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
  1873. enable_z_endstop(true);
  1874. go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
  1875. update_current_position_xyz();
  1876. if (! endstop_z_hit_on_purpose()) {
  1877. continue;
  1878. /*
  1879. current_position[X_AXIS] = center_old_x;
  1880. SERIAL_PROTOCOLPGM("Failed 4\n");
  1881. goto canceled;
  1882. */
  1883. }
  1884. b = current_position[X_AXIS];
  1885. #ifdef SUPPORT_VERBOSITY
  1886. if (verbosity_level >= 5) {
  1887. debug_output_point(PSTR("left" ), a, current_position[Y_AXIS], current_position[Z_AXIS]);
  1888. debug_output_point(PSTR("right"), b, current_position[Y_AXIS], current_position[Z_AXIS]);
  1889. }
  1890. #endif // SUPPORT_VERBOSITY
  1891. float d = b - a;
  1892. if (d > dmax) {
  1893. xmax = 0.5f * (a + b);
  1894. ymax = y;
  1895. dmax = d;
  1896. }
  1897. }
  1898. }
  1899. {
  1900. // Compare the distance in the Y+ direction with the diameter in the X direction.
  1901. // Find the positive Y hit once again, this time along the Y axis going through the X point with the highest diameter.
  1902. enable_z_endstop(false);
  1903. go_xy(xmax, ymax + IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS, homing_feedrate[X_AXIS] / 60.f);
  1904. enable_z_endstop(true);
  1905. go_xy(xmax, max(ymax - IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS, Y_MIN_POS_FOR_BED_CALIBRATION), homing_feedrate[X_AXIS] / 60.f);
  1906. update_current_position_xyz();
  1907. if (! endstop_z_hit_on_purpose()) {
  1908. current_position[Y_AXIS] = center_old_y;
  1909. goto canceled;
  1910. }
  1911. #ifdef SUPPORT_VERBOSITY
  1912. if (verbosity_level >= 5)
  1913. debug_output_point(PSTR("top" ), current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
  1914. #endif // SUPPORT_VERBOSITY
  1915. if (current_position[Y_AXIS] - Y_MIN_POS_FOR_BED_CALIBRATION < 0.5f * dmax) {
  1916. // Probably not even a half circle was detected. The induction point is likely too far in the minus Y direction.
  1917. // First verify, if the measurement has been done at a sufficient height. If no, lower the Z axis a bit.
  1918. if (current_position[Y_AXIS] < ymax || dmax < 0.5f * MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
  1919. #ifdef SUPPORT_VERBOSITY
  1920. if (verbosity_level >= 5) {
  1921. SERIAL_ECHOPGM("Partial point diameter too small: ");
  1922. SERIAL_ECHO(dmax);
  1923. SERIAL_ECHOLNPGM("");
  1924. }
  1925. #endif // SUPPORT_VERBOSITY
  1926. result = false;
  1927. } else {
  1928. // Estimate the circle radius from the maximum diameter and height:
  1929. float h = current_position[Y_AXIS] - ymax;
  1930. float r = dmax * dmax / (8.f * h) + 0.5f * h;
  1931. if (r < 0.8f * MIN_BED_SENSOR_POINT_RESPONSE_DMR) {
  1932. #ifdef SUPPORT_VERBOSITY
  1933. if (verbosity_level >= 5) {
  1934. SERIAL_ECHOPGM("Partial point estimated radius too small: ");
  1935. SERIAL_ECHO(r);
  1936. SERIAL_ECHOPGM(", dmax:");
  1937. SERIAL_ECHO(dmax);
  1938. SERIAL_ECHOPGM(", h:");
  1939. SERIAL_ECHO(h);
  1940. SERIAL_ECHOLNPGM("");
  1941. }
  1942. #endif // SUPPORT_VERBOSITY
  1943. result = false;
  1944. } else {
  1945. // The point may end up outside of the machine working space.
  1946. // That is all right as it helps to improve the accuracy of the measurement point
  1947. // due to averaging.
  1948. // For the y correction, use an average of dmax/2 and the estimated radius.
  1949. r = 0.5f * (0.5f * dmax + r);
  1950. ymax = current_position[Y_AXIS] - r;
  1951. }
  1952. }
  1953. } else {
  1954. // If the diameter of the detected spot was smaller than a minimum allowed,
  1955. // the induction sensor is probably too high. Returning false will force
  1956. // the sensor to be lowered a tiny bit.
  1957. result = xmax >= MIN_BED_SENSOR_POINT_RESPONSE_DMR;
  1958. if (y0 > Y_MIN_POS_FOR_BED_CALIBRATION + 0.2f)
  1959. // Only in case both left and right y tangents are known, use them.
  1960. // If y0 is close to the bed edge, it may not be symmetric to the right tangent.
  1961. ymax = 0.5f * ymax + 0.25f * (y0 + y1);
  1962. }
  1963. }
  1964. // Go to the center.
  1965. enable_z_endstop(false);
  1966. current_position[X_AXIS] = xmax;
  1967. current_position[Y_AXIS] = ymax;
  1968. #ifdef SUPPORT_VERBOSITY
  1969. if (verbosity_level >= 20) {
  1970. SERIAL_ECHOPGM("Adjusted position: ");
  1971. SERIAL_ECHO(current_position[X_AXIS]);
  1972. SERIAL_ECHOPGM(", ");
  1973. SERIAL_ECHO(current_position[Y_AXIS]);
  1974. SERIAL_ECHOLNPGM("");
  1975. }
  1976. #endif // SUPPORT_VERBOSITY
  1977. // Don't clamp current_position[Y_AXIS], because the out-of-reach Y coordinate may actually be true.
  1978. // Only clamp the coordinate to go.
  1979. go_xy(current_position[X_AXIS], max(Y_MIN_POS, current_position[Y_AXIS]), homing_feedrate[X_AXIS] / 60.f);
  1980. // delay_keep_alive(3000);
  1981. }
  1982. if (result)
  1983. return true;
  1984. // otherwise clamp the Y coordinate
  1985. canceled:
  1986. // Go back to the center.
  1987. enable_z_endstop(false);
  1988. if (current_position[Y_AXIS] < Y_MIN_POS)
  1989. current_position[Y_AXIS] = Y_MIN_POS;
  1990. go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
  1991. return false;
  1992. }
  1993. #endif //NEW_XYZCAL
  1994. #ifndef NEW_XYZCAL
  1995. // Scan the mesh bed induction points one by one by a left-right zig-zag movement,
  1996. // write the trigger coordinates to the serial line.
  1997. // Useful for visualizing the behavior of the bed induction detector.
  1998. inline void scan_bed_induction_sensor_point()
  1999. {
  2000. float center_old_x = current_position[X_AXIS];
  2001. float center_old_y = current_position[Y_AXIS];
  2002. float x0 = center_old_x - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
  2003. float x1 = center_old_x + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
  2004. float y0 = center_old_y - IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
  2005. float y1 = center_old_y + IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS;
  2006. float y = y0;
  2007. if (x0 < X_MIN_POS)
  2008. x0 = X_MIN_POS;
  2009. if (x1 > X_MAX_POS)
  2010. x1 = X_MAX_POS;
  2011. if (y0 < Y_MIN_POS_FOR_BED_CALIBRATION)
  2012. y0 = Y_MIN_POS_FOR_BED_CALIBRATION;
  2013. if (y1 > Y_MAX_POS)
  2014. y1 = Y_MAX_POS;
  2015. for (float y = y0; y < y1; y += IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y) {
  2016. enable_z_endstop(false);
  2017. go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
  2018. enable_z_endstop(true);
  2019. go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
  2020. update_current_position_xyz();
  2021. if (endstop_z_hit_on_purpose())
  2022. debug_output_point(PSTR("left" ), current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
  2023. enable_z_endstop(false);
  2024. go_xy(x1, y, homing_feedrate[X_AXIS] / 60.f);
  2025. enable_z_endstop(true);
  2026. go_xy(x0, y, homing_feedrate[X_AXIS] / 60.f);
  2027. update_current_position_xyz();
  2028. if (endstop_z_hit_on_purpose())
  2029. debug_output_point(PSTR("right"), current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
  2030. }
  2031. enable_z_endstop(false);
  2032. current_position[X_AXIS] = center_old_x;
  2033. current_position[Y_AXIS] = center_old_y;
  2034. go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
  2035. }
  2036. #endif //NEW_XYZCAL
  2037. #define MESH_BED_CALIBRATION_SHOW_LCD
  2038. BedSkewOffsetDetectionResultType find_bed_offset_and_skew(int8_t verbosity_level, uint8_t &too_far_mask)
  2039. {
  2040. // Don't let the manage_inactivity() function remove power from the motors.
  2041. refresh_cmd_timeout();
  2042. // Reusing the z_values memory for the measurement cache.
  2043. // 7x7=49 floats, good for 16 (x,y,z) vectors.
  2044. float *pts = &mbl.z_values[0][0];
  2045. float *vec_x = pts + 2 * 4;
  2046. float *vec_y = vec_x + 2;
  2047. float *cntr = vec_y + 2;
  2048. memset(pts, 0, sizeof(float) * 7 * 7);
  2049. uint8_t iteration = 0;
  2050. BedSkewOffsetDetectionResultType result;
  2051. // SERIAL_ECHOLNPGM("find_bed_offset_and_skew verbosity level: ");
  2052. // SERIAL_ECHO(int(verbosity_level));
  2053. // SERIAL_ECHOPGM("");
  2054. #ifdef NEW_XYZCAL
  2055. {
  2056. #else //NEW_XYZCAL
  2057. while (iteration < 3) {
  2058. #endif //NEW_XYZCAL
  2059. SERIAL_ECHOPGM("Iteration: ");
  2060. MYSERIAL.println(int(iteration + 1));
  2061. #ifdef SUPPORT_VERBOSITY
  2062. if (verbosity_level >= 20) {
  2063. SERIAL_ECHOLNPGM("Vectors: ");
  2064. SERIAL_ECHOPGM("vec_x[0]:");
  2065. MYSERIAL.print(vec_x[0], 5);
  2066. SERIAL_ECHOLNPGM("");
  2067. SERIAL_ECHOPGM("vec_x[1]:");
  2068. MYSERIAL.print(vec_x[1], 5);
  2069. SERIAL_ECHOLNPGM("");
  2070. SERIAL_ECHOPGM("vec_y[0]:");
  2071. MYSERIAL.print(vec_y[0], 5);
  2072. SERIAL_ECHOLNPGM("");
  2073. SERIAL_ECHOPGM("vec_y[1]:");
  2074. MYSERIAL.print(vec_y[1], 5);
  2075. SERIAL_ECHOLNPGM("");
  2076. SERIAL_ECHOPGM("cntr[0]:");
  2077. MYSERIAL.print(cntr[0], 5);
  2078. SERIAL_ECHOLNPGM("");
  2079. SERIAL_ECHOPGM("cntr[1]:");
  2080. MYSERIAL.print(cntr[1], 5);
  2081. SERIAL_ECHOLNPGM("");
  2082. }
  2083. #endif // SUPPORT_VERBOSITY
  2084. #ifdef MESH_BED_CALIBRATION_SHOW_LCD
  2085. lcd_display_message_fullscreen_P(_T(MSG_FIND_BED_OFFSET_AND_SKEW_LINE1));
  2086. #endif /* MESH_BED_CALIBRATION_SHOW_LCD */
  2087. // Collect the rear 2x3 points.
  2088. current_position[Z_AXIS] = MESH_HOME_Z_SEARCH + FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP * iteration * 0.3;
  2089. /// Retry point scanning if a point with bad data appears.
  2090. /// Bad data could be cause by "cold" sensor.
  2091. /// This behavior vanishes after few point scans so retry will help.
  2092. for (int retries = 0; retries <= 1; ++retries) {
  2093. bool retry = false;
  2094. for (int k = 0; k < 4; ++k) {
  2095. // Don't let the manage_inactivity() function remove power from the motors.
  2096. refresh_cmd_timeout();
  2097. #ifdef MESH_BED_CALIBRATION_SHOW_LCD
  2098. lcd_set_cursor(0, 3);
  2099. lcd_printf_P(PSTR("%d/4"),(k+1));
  2100. if (iteration > 0) {
  2101. lcd_printf_P(PSTR(" %S %d/1"),_T(MSG_ITERATION),int(iteration + 1));
  2102. }
  2103. #endif /* MESH_BED_CALIBRATION_SHOW_LCD */
  2104. float *pt = pts + k * 2;
  2105. // Go up to z_initial.
  2106. go_to_current(homing_feedrate[Z_AXIS] / 60.f);
  2107. #ifdef SUPPORT_VERBOSITY
  2108. if (verbosity_level >= 20) {
  2109. // Go to Y0, wait, then go to Y-4.
  2110. current_position[Y_AXIS] = 0.f;
  2111. go_to_current(homing_feedrate[X_AXIS] / 60.f);
  2112. SERIAL_ECHOLNPGM("At Y0");
  2113. delay_keep_alive(5000);
  2114. current_position[Y_AXIS] = Y_MIN_POS;
  2115. go_to_current(homing_feedrate[X_AXIS] / 60.f);
  2116. SERIAL_ECHOLNPGM("At Y_MIN_POS");
  2117. delay_keep_alive(5000);
  2118. }
  2119. #endif // SUPPORT_VERBOSITY
  2120. // Go to the measurement point position.
  2121. //if (iteration == 0) {
  2122. current_position[X_AXIS] = pgm_read_float(bed_ref_points_4 + k * 2);
  2123. current_position[Y_AXIS] = pgm_read_float(bed_ref_points_4 + k * 2 + 1);
  2124. /*}
  2125. else {
  2126. // if first iteration failed, count corrected point coordinates as initial
  2127. // Use the corrected coordinate, which is a result of find_bed_offset_and_skew().
  2128. 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];
  2129. 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];
  2130. // The calibration points are very close to the min Y.
  2131. if (current_position[Y_AXIS] < Y_MIN_POS_FOR_BED_CALIBRATION)
  2132. current_position[Y_AXIS] = Y_MIN_POS_FOR_BED_CALIBRATION;
  2133. }*/
  2134. #ifdef SUPPORT_VERBOSITY
  2135. if (verbosity_level >= 20) {
  2136. SERIAL_ECHOPGM("current_position[X_AXIS]:");
  2137. MYSERIAL.print(current_position[X_AXIS], 5);
  2138. SERIAL_ECHOLNPGM("");
  2139. SERIAL_ECHOPGM("current_position[Y_AXIS]:");
  2140. MYSERIAL.print(current_position[Y_AXIS], 5);
  2141. SERIAL_ECHOLNPGM("");
  2142. SERIAL_ECHOPGM("current_position[Z_AXIS]:");
  2143. MYSERIAL.print(current_position[Z_AXIS], 5);
  2144. SERIAL_ECHOLNPGM("");
  2145. }
  2146. #endif // SUPPORT_VERBOSITY
  2147. go_to_current(homing_feedrate[X_AXIS] / 60.f);
  2148. #ifdef SUPPORT_VERBOSITY
  2149. if (verbosity_level >= 10)
  2150. delay_keep_alive(3000);
  2151. #endif // SUPPORT_VERBOSITY
  2152. BedSkewOffsetDetectionResultType result;
  2153. result = find_bed_induction_sensor_point_xy(verbosity_level);
  2154. switch(result){
  2155. case BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND:
  2156. return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
  2157. case BED_SKEW_OFFSET_DETECTION_POINT_SCAN_FAILED:
  2158. retry = true;
  2159. break;
  2160. default:
  2161. break;
  2162. }
  2163. #ifndef NEW_XYZCAL
  2164. #ifndef HEATBED_V2
  2165. if (k == 0 || k == 1) {
  2166. // Improve the position of the 1st row sensor points by a zig-zag movement.
  2167. find_bed_induction_sensor_point_z();
  2168. int8_t i = 4;
  2169. for (;;) {
  2170. if (improve_bed_induction_sensor_point3(verbosity_level))
  2171. break;
  2172. if (--i == 0)
  2173. return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
  2174. // Try to move the Z axis down a bit to increase a chance of the sensor to trigger.
  2175. current_position[Z_AXIS] -= 0.025f;
  2176. enable_endstops(false);
  2177. enable_z_endstop(false);
  2178. go_to_current(homing_feedrate[Z_AXIS]);
  2179. }
  2180. if (i == 0)
  2181. // not found
  2182. return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
  2183. }
  2184. #endif //HEATBED_V2
  2185. #endif
  2186. #ifdef SUPPORT_VERBOSITY
  2187. if (verbosity_level >= 10)
  2188. delay_keep_alive(3000);
  2189. #endif // SUPPORT_VERBOSITY
  2190. // Save the detected point position and then clamp the Y coordinate, which may have been estimated
  2191. // to lie outside the machine working space.
  2192. #ifdef SUPPORT_VERBOSITY
  2193. if (verbosity_level >= 20) {
  2194. SERIAL_ECHOLNPGM("Measured:");
  2195. MYSERIAL.println(current_position[X_AXIS]);
  2196. MYSERIAL.println(current_position[Y_AXIS]);
  2197. }
  2198. #endif // SUPPORT_VERBOSITY
  2199. pt[0] = (pt[0] * iteration) / (iteration + 1);
  2200. pt[0] += (current_position[X_AXIS]/(iteration + 1)); //count average
  2201. pt[1] = (pt[1] * iteration) / (iteration + 1);
  2202. pt[1] += (current_position[Y_AXIS] / (iteration + 1));
  2203. //pt[0] += current_position[X_AXIS];
  2204. //if(iteration > 0) pt[0] = pt[0] / 2;
  2205. //pt[1] += current_position[Y_AXIS];
  2206. //if (iteration > 0) pt[1] = pt[1] / 2;
  2207. #ifdef SUPPORT_VERBOSITY
  2208. if (verbosity_level >= 20) {
  2209. SERIAL_ECHOLNPGM("");
  2210. SERIAL_ECHOPGM("pt[0]:");
  2211. MYSERIAL.println(pt[0]);
  2212. SERIAL_ECHOPGM("pt[1]:");
  2213. MYSERIAL.println(pt[1]);
  2214. }
  2215. #endif // SUPPORT_VERBOSITY
  2216. if (current_position[Y_AXIS] < Y_MIN_POS)
  2217. current_position[Y_AXIS] = Y_MIN_POS;
  2218. // Start searching for the other points at 3mm above the last point.
  2219. current_position[Z_AXIS] += 3.f + FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP * iteration * 0.3;
  2220. //cntr[0] += pt[0];
  2221. //cntr[1] += pt[1];
  2222. #ifdef SUPPORT_VERBOSITY
  2223. if (verbosity_level >= 10 && k == 0) {
  2224. // Show the zero. Test, whether the Y motor skipped steps.
  2225. current_position[Y_AXIS] = MANUAL_Y_HOME_POS;
  2226. go_to_current(homing_feedrate[X_AXIS] / 60.f);
  2227. delay_keep_alive(3000);
  2228. }
  2229. #endif // SUPPORT_VERBOSITY
  2230. }
  2231. if (!retry)
  2232. break;
  2233. }
  2234. DBG(_n("All 4 calibration points found.\n"));
  2235. delay_keep_alive(0); //manage_heater, reset watchdog, manage inactivity
  2236. #ifdef SUPPORT_VERBOSITY
  2237. if (verbosity_level >= 20) {
  2238. // Test the positions. Are the positions reproducible? Now the calibration is active in the planner.
  2239. delay_keep_alive(3000);
  2240. for (int8_t mesh_point = 0; mesh_point < 4; ++mesh_point) {
  2241. // Don't let the manage_inactivity() function remove power from the motors.
  2242. refresh_cmd_timeout();
  2243. // Go to the measurement point.
  2244. // Use the corrected coordinate, which is a result of find_bed_offset_and_skew().
  2245. current_position[X_AXIS] = pts[mesh_point * 2];
  2246. current_position[Y_AXIS] = pts[mesh_point * 2 + 1];
  2247. go_to_current(homing_feedrate[X_AXIS] / 60);
  2248. delay_keep_alive(3000);
  2249. }
  2250. }
  2251. #endif // SUPPORT_VERBOSITY
  2252. if (pts[1] < Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH) {
  2253. too_far_mask |= 1 << 1; //front center point is out of reach
  2254. SERIAL_ECHOLNPGM("");
  2255. SERIAL_ECHOPGM("WARNING: Front point not reachable. Y coordinate:");
  2256. MYSERIAL.print(pts[1]);
  2257. SERIAL_ECHOPGM(" < ");
  2258. MYSERIAL.println(Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH);
  2259. }
  2260. result = calculate_machine_skew_and_offset_LS(pts, 4, bed_ref_points_4, vec_x, vec_y, cntr, verbosity_level);
  2261. delay_keep_alive(0); //manage_heater, reset watchdog, manage inactivity
  2262. if (result >= 0) {
  2263. DBG(_n("Calibration success.\n"));
  2264. world2machine_update(vec_x, vec_y, cntr);
  2265. #if 1
  2266. // Fearlessly store the calibration values into the eeprom.
  2267. eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER + 0), cntr[0]);
  2268. eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER + 4), cntr[1]);
  2269. eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X + 0), vec_x[0]);
  2270. eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X + 4), vec_x[1]);
  2271. eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y + 0), vec_y[0]);
  2272. eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y + 4), vec_y[1]);
  2273. #endif
  2274. #ifdef SUPPORT_VERBOSITY
  2275. if (verbosity_level >= 10) {
  2276. // Length of the vec_x
  2277. float l = sqrt(vec_x[0] * vec_x[0] + vec_x[1] * vec_x[1]);
  2278. SERIAL_ECHOLNPGM("X vector length:");
  2279. MYSERIAL.println(l);
  2280. // Length of the vec_y
  2281. l = sqrt(vec_y[0] * vec_y[0] + vec_y[1] * vec_y[1]);
  2282. SERIAL_ECHOLNPGM("Y vector length:");
  2283. MYSERIAL.println(l);
  2284. // Zero point correction
  2285. l = sqrt(cntr[0] * cntr[0] + cntr[1] * cntr[1]);
  2286. SERIAL_ECHOLNPGM("Zero point correction:");
  2287. MYSERIAL.println(l);
  2288. // vec_x and vec_y shall be nearly perpendicular.
  2289. l = vec_x[0] * vec_y[0] + vec_x[1] * vec_y[1];
  2290. SERIAL_ECHOLNPGM("Perpendicularity");
  2291. MYSERIAL.println(fabs(l));
  2292. SERIAL_ECHOLNPGM("Saving bed calibration vectors to EEPROM");
  2293. }
  2294. #endif // SUPPORT_VERBOSITY
  2295. // Correct the current_position to match the transformed coordinate system after world2machine_rotation_and_skew and world2machine_shift were set.
  2296. world2machine_update_current();
  2297. #ifdef SUPPORT_VERBOSITY
  2298. if (verbosity_level >= 20) {
  2299. // Test the positions. Are the positions reproducible? Now the calibration is active in the planner.
  2300. delay_keep_alive(3000);
  2301. for (int8_t mesh_point = 0; mesh_point < 9; ++mesh_point) {
  2302. // Don't let the manage_inactivity() function remove power from the motors.
  2303. refresh_cmd_timeout();
  2304. // Go to the measurement point.
  2305. // Use the corrected coordinate, which is a result of find_bed_offset_and_skew().
  2306. uint8_t ix = mesh_point % MESH_MEAS_NUM_X_POINTS; // from 0 to MESH_NUM_X_POINTS - 1
  2307. uint8_t iy = mesh_point / MESH_MEAS_NUM_X_POINTS;
  2308. if (iy & 1) ix = (MESH_MEAS_NUM_X_POINTS - 1) - ix;
  2309. current_position[X_AXIS] = BED_X(ix, MESH_MEAS_NUM_X_POINTS);
  2310. current_position[Y_AXIS] = BED_Y(iy, MESH_MEAS_NUM_Y_POINTS);
  2311. go_to_current(homing_feedrate[X_AXIS] / 60);
  2312. delay_keep_alive(3000);
  2313. }
  2314. }
  2315. #endif // SUPPORT_VERBOSITY
  2316. return result;
  2317. }
  2318. if (result == BED_SKEW_OFFSET_DETECTION_FITTING_FAILED && too_far_mask == 2){
  2319. DBG(_n("Fitting failed => calibration failed.\n"));
  2320. return result; //if fitting failed and front center point is out of reach, terminate calibration and inform user
  2321. }
  2322. iteration++;
  2323. }
  2324. return result;
  2325. }
  2326. #ifndef NEW_XYZCAL
  2327. BedSkewOffsetDetectionResultType improve_bed_offset_and_skew(int8_t method, int8_t verbosity_level, uint8_t &too_far_mask)
  2328. {
  2329. // Don't let the manage_inactivity() function remove power from the motors.
  2330. refresh_cmd_timeout();
  2331. // Mask of the first three points. Are they too far?
  2332. too_far_mask = 0;
  2333. // Reusing the z_values memory for the measurement cache.
  2334. // 7x7=49 floats, good for 16 (x,y,z) vectors.
  2335. float *pts = &mbl.z_values[0][0];
  2336. float *vec_x = pts + 2 * 9;
  2337. float *vec_y = vec_x + 2;
  2338. float *cntr = vec_y + 2;
  2339. memset(pts, 0, sizeof(float) * 7 * 7);
  2340. #ifdef SUPPORT_VERBOSITY
  2341. if (verbosity_level >= 10) SERIAL_ECHOLNPGM("Improving bed offset and skew");
  2342. #endif // SUPPORT_VERBOSITY
  2343. // Cache the current correction matrix.
  2344. world2machine_initialize();
  2345. vec_x[0] = world2machine_rotation_and_skew[0][0];
  2346. vec_x[1] = world2machine_rotation_and_skew[1][0];
  2347. vec_y[0] = world2machine_rotation_and_skew[0][1];
  2348. vec_y[1] = world2machine_rotation_and_skew[1][1];
  2349. cntr[0] = world2machine_shift[0];
  2350. cntr[1] = world2machine_shift[1];
  2351. // and reset the correction matrix, so the planner will not do anything.
  2352. world2machine_reset();
  2353. bool endstops_enabled = enable_endstops(false);
  2354. bool endstop_z_enabled = enable_z_endstop(false);
  2355. #ifdef MESH_BED_CALIBRATION_SHOW_LCD
  2356. lcd_display_message_fullscreen_P(_i("Improving bed calibration point"));////MSG_IMPROVE_BED_OFFSET_AND_SKEW_LINE1 c=60
  2357. #endif /* MESH_BED_CALIBRATION_SHOW_LCD */
  2358. // Collect a matrix of 9x9 points.
  2359. BedSkewOffsetDetectionResultType result = BED_SKEW_OFFSET_DETECTION_PERFECT;
  2360. for (int8_t mesh_point = 0; mesh_point < 4; ++ mesh_point) {
  2361. // Don't let the manage_inactivity() function remove power from the motors.
  2362. refresh_cmd_timeout();
  2363. // Print the decrasing ID of the measurement point.
  2364. #ifdef MESH_BED_CALIBRATION_SHOW_LCD
  2365. lcd_set_cursor(0, 3);
  2366. lcd_printf_P(PSTR("%d/4"),mesh_point+1);
  2367. #endif /* MESH_BED_CALIBRATION_SHOW_LCD */
  2368. // Move up.
  2369. current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
  2370. enable_endstops(false);
  2371. enable_z_endstop(false);
  2372. go_to_current(homing_feedrate[Z_AXIS]/60);
  2373. #ifdef SUPPORT_VERBOSITY
  2374. if (verbosity_level >= 20) {
  2375. // Go to Y0, wait, then go to Y-4.
  2376. current_position[Y_AXIS] = 0.f;
  2377. go_to_current(homing_feedrate[X_AXIS] / 60.f);
  2378. SERIAL_ECHOLNPGM("At Y0");
  2379. delay_keep_alive(5000);
  2380. current_position[Y_AXIS] = Y_MIN_POS;
  2381. go_to_current(homing_feedrate[X_AXIS] / 60.f);
  2382. SERIAL_ECHOLNPGM("At Y_MIN_POS");
  2383. delay_keep_alive(5000);
  2384. }
  2385. #endif // SUPPORT_VERBOSITY
  2386. // Go to the measurement point.
  2387. // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
  2388. current_position[X_AXIS] = vec_x[0] * pgm_read_float(bed_ref_points_4+mesh_point*2) + vec_y[0] * pgm_read_float(bed_ref_points_4+mesh_point*2+1) + cntr[0];
  2389. current_position[Y_AXIS] = vec_x[1] * pgm_read_float(bed_ref_points_4+mesh_point*2) + vec_y[1] * pgm_read_float(bed_ref_points_4+mesh_point*2+1) + cntr[1];
  2390. // The calibration points are very close to the min Y.
  2391. if (current_position[Y_AXIS] < Y_MIN_POS_FOR_BED_CALIBRATION){
  2392. current_position[Y_AXIS] = Y_MIN_POS_FOR_BED_CALIBRATION;
  2393. #ifdef SUPPORT_VERBOSITY
  2394. if (verbosity_level >= 20) {
  2395. SERIAL_ECHOPGM("Calibration point ");
  2396. SERIAL_ECHO(mesh_point);
  2397. SERIAL_ECHOPGM("lower than Ymin. Y coordinate clamping was used.");
  2398. SERIAL_ECHOLNPGM("");
  2399. }
  2400. #endif // SUPPORT_VERBOSITY
  2401. }
  2402. go_to_current(homing_feedrate[X_AXIS]/60);
  2403. // Find its Z position by running the normal vertical search.
  2404. #ifdef SUPPORT_VERBOSITY
  2405. if (verbosity_level >= 10)
  2406. delay_keep_alive(3000);
  2407. #endif // SUPPORT_VERBOSITY
  2408. find_bed_induction_sensor_point_z();
  2409. #ifdef SUPPORT_VERBOSITY
  2410. if (verbosity_level >= 10)
  2411. delay_keep_alive(3000);
  2412. #endif // SUPPORT_VERBOSITY
  2413. // Try to move the Z axis down a bit to increase a chance of the sensor to trigger.
  2414. current_position[Z_AXIS] -= 0.025f;
  2415. // Improve the point position by searching its center in a current plane.
  2416. int8_t n_errors = 3;
  2417. for (int8_t iter = 0; iter < 8; ) {
  2418. #ifdef SUPPORT_VERBOSITY
  2419. if (verbosity_level > 20) {
  2420. SERIAL_ECHOPGM("Improving bed point ");
  2421. SERIAL_ECHO(mesh_point);
  2422. SERIAL_ECHOPGM(", iteration ");
  2423. SERIAL_ECHO(iter);
  2424. SERIAL_ECHOPGM(", z");
  2425. MYSERIAL.print(current_position[Z_AXIS], 5);
  2426. SERIAL_ECHOLNPGM("");
  2427. }
  2428. #endif // SUPPORT_VERBOSITY
  2429. bool found = false;
  2430. if (mesh_point < 2) {
  2431. // Because the sensor cannot move in front of the first row
  2432. // of the sensor points, the y position cannot be measured
  2433. // by a cross center method.
  2434. // Use a zig-zag search for the first row of the points.
  2435. found = improve_bed_induction_sensor_point3(verbosity_level);
  2436. } else {
  2437. switch (method) {
  2438. case 0: found = improve_bed_induction_sensor_point(); break;
  2439. case 1: found = improve_bed_induction_sensor_point2(mesh_point < 2, verbosity_level); break;
  2440. default: break;
  2441. }
  2442. }
  2443. if (found) {
  2444. if (iter > 3) {
  2445. // Average the last 4 measurements.
  2446. pts[mesh_point*2 ] += current_position[X_AXIS];
  2447. pts[mesh_point*2+1] += current_position[Y_AXIS];
  2448. }
  2449. if (current_position[Y_AXIS] < Y_MIN_POS)
  2450. current_position[Y_AXIS] = Y_MIN_POS;
  2451. ++ iter;
  2452. } else if (n_errors -- == 0) {
  2453. // Give up.
  2454. result = BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
  2455. goto canceled;
  2456. } else {
  2457. // Try to move the Z axis down a bit to increase a chance of the sensor to trigger.
  2458. current_position[Z_AXIS] -= 0.05f;
  2459. enable_endstops(false);
  2460. enable_z_endstop(false);
  2461. go_to_current(homing_feedrate[Z_AXIS]);
  2462. #ifdef SUPPORT_VERBOSITY
  2463. if (verbosity_level >= 5) {
  2464. SERIAL_ECHOPGM("Improving bed point ");
  2465. SERIAL_ECHO(mesh_point);
  2466. SERIAL_ECHOPGM(", iteration ");
  2467. SERIAL_ECHO(iter);
  2468. SERIAL_ECHOPGM(" failed. Lowering z to ");
  2469. MYSERIAL.print(current_position[Z_AXIS], 5);
  2470. SERIAL_ECHOLNPGM("");
  2471. }
  2472. #endif // SUPPORT_VERBOSITY
  2473. }
  2474. }
  2475. #ifdef SUPPORT_VERBOSITY
  2476. if (verbosity_level >= 10)
  2477. delay_keep_alive(3000);
  2478. #endif // SUPPORT_VERBOSITY
  2479. }
  2480. // Don't let the manage_inactivity() function remove power from the motors.
  2481. refresh_cmd_timeout();
  2482. // Average the last 4 measurements.
  2483. for (int8_t i = 0; i < 8; ++ i)
  2484. pts[i] *= (1.f/4.f);
  2485. enable_endstops(false);
  2486. enable_z_endstop(false);
  2487. #ifdef SUPPORT_VERBOSITY
  2488. if (verbosity_level >= 5) {
  2489. // Test the positions. Are the positions reproducible?
  2490. current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
  2491. for (int8_t mesh_point = 0; mesh_point < 4; ++ mesh_point) {
  2492. // Don't let the manage_inactivity() function remove power from the motors.
  2493. refresh_cmd_timeout();
  2494. // Go to the measurement point.
  2495. // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
  2496. current_position[X_AXIS] = pts[mesh_point*2];
  2497. current_position[Y_AXIS] = pts[mesh_point*2+1];
  2498. if (verbosity_level >= 10) {
  2499. go_to_current(homing_feedrate[X_AXIS]/60);
  2500. delay_keep_alive(3000);
  2501. }
  2502. SERIAL_ECHOPGM("Final measured bed point ");
  2503. SERIAL_ECHO(mesh_point);
  2504. SERIAL_ECHOPGM(": ");
  2505. MYSERIAL.print(current_position[X_AXIS], 5);
  2506. SERIAL_ECHOPGM(", ");
  2507. MYSERIAL.print(current_position[Y_AXIS], 5);
  2508. SERIAL_ECHOLNPGM("");
  2509. }
  2510. }
  2511. #endif // SUPPORT_VERBOSITY
  2512. {
  2513. // First fill in the too_far_mask from the measured points.
  2514. for (uint8_t mesh_point = 0; mesh_point < 2; ++ mesh_point)
  2515. if (pts[mesh_point * 2 + 1] < Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH)
  2516. too_far_mask |= 1 << mesh_point;
  2517. result = calculate_machine_skew_and_offset_LS(pts, 4, bed_ref_points_4, vec_x, vec_y, cntr, verbosity_level);
  2518. if (result < 0) {
  2519. SERIAL_ECHOLNPGM("Calculation of the machine skew and offset failed.");
  2520. goto canceled;
  2521. }
  2522. // In case of success, update the too_far_mask from the calculated points.
  2523. for (uint8_t mesh_point = 0; mesh_point < 2; ++ mesh_point) {
  2524. float y = vec_x[1] * pgm_read_float(bed_ref_points_4+mesh_point*2) + vec_y[1] * pgm_read_float(bed_ref_points_4+mesh_point*2+1) + cntr[1];
  2525. #ifdef SUPPORT_VERBOSITY
  2526. if (verbosity_level >= 20) {
  2527. SERIAL_ECHOLNPGM("");
  2528. SERIAL_ECHOPGM("Distance from min:");
  2529. MYSERIAL.print(y - Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH);
  2530. SERIAL_ECHOLNPGM("");
  2531. SERIAL_ECHOPGM("y:");
  2532. MYSERIAL.print(y);
  2533. SERIAL_ECHOLNPGM("");
  2534. }
  2535. #endif // SUPPORT_VERBOSITY
  2536. if (y < Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH)
  2537. too_far_mask |= 1 << mesh_point;
  2538. }
  2539. }
  2540. world2machine_update(vec_x, vec_y, cntr);
  2541. #if 1
  2542. // Fearlessly store the calibration values into the eeprom.
  2543. eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER+0), cntr [0]);
  2544. eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_CENTER+4), cntr [1]);
  2545. eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +0), vec_x[0]);
  2546. eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_X +4), vec_x[1]);
  2547. eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +0), vec_y[0]);
  2548. eeprom_update_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y +4), vec_y[1]);
  2549. #endif
  2550. // Correct the current_position to match the transformed coordinate system after world2machine_rotation_and_skew and world2machine_shift were set.
  2551. world2machine_update_current();
  2552. enable_endstops(false);
  2553. enable_z_endstop(false);
  2554. #ifdef SUPPORT_VERBOSITY
  2555. if (verbosity_level >= 5) {
  2556. // Test the positions. Are the positions reproducible? Now the calibration is active in the planner.
  2557. delay_keep_alive(3000);
  2558. current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
  2559. for (int8_t mesh_point = 0; mesh_point < 4; ++ mesh_point) {
  2560. // Don't let the manage_inactivity() function remove power from the motors.
  2561. refresh_cmd_timeout();
  2562. // Go to the measurement point.
  2563. // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
  2564. current_position[X_AXIS] = pgm_read_float(bed_ref_points_4+mesh_point*2);
  2565. current_position[Y_AXIS] = pgm_read_float(bed_ref_points_4+mesh_point*2+1);
  2566. if (verbosity_level >= 10) {
  2567. go_to_current(homing_feedrate[X_AXIS]/60);
  2568. delay_keep_alive(3000);
  2569. }
  2570. {
  2571. float x, y;
  2572. world2machine(current_position[X_AXIS], current_position[Y_AXIS], x, y);
  2573. SERIAL_ECHOPGM("Final calculated bed point ");
  2574. SERIAL_ECHO(mesh_point);
  2575. SERIAL_ECHOPGM(": ");
  2576. MYSERIAL.print(x, 5);
  2577. SERIAL_ECHOPGM(", ");
  2578. MYSERIAL.print(y, 5);
  2579. SERIAL_ECHOLNPGM("");
  2580. }
  2581. }
  2582. }
  2583. #endif // SUPPORT_VERBOSITY
  2584. if(!sample_z())
  2585. goto canceled;
  2586. enable_endstops(endstops_enabled);
  2587. enable_z_endstop(endstop_z_enabled);
  2588. // Don't let the manage_inactivity() function remove power from the motors.
  2589. refresh_cmd_timeout();
  2590. return result;
  2591. canceled:
  2592. // Don't let the manage_inactivity() function remove power from the motors.
  2593. refresh_cmd_timeout();
  2594. // Print head up.
  2595. current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
  2596. go_to_current(homing_feedrate[Z_AXIS]/60);
  2597. // Store the identity matrix to EEPROM.
  2598. reset_bed_offset_and_skew();
  2599. enable_endstops(endstops_enabled);
  2600. enable_z_endstop(endstop_z_enabled);
  2601. return result;
  2602. }
  2603. #endif //NEW_XYZCAL
  2604. bool sample_z() {
  2605. bool sampled = true;
  2606. //make space
  2607. current_position[Z_AXIS] += 150;
  2608. go_to_current(homing_feedrate[Z_AXIS] / 60);
  2609. //plan_buffer_line_curposXYZE(feedrate, active_extruder););
  2610. lcd_show_fullscreen_message_and_wait_P(_T(MSG_PLACE_STEEL_SHEET));
  2611. // Sample Z heights for the mesh bed leveling.
  2612. // In addition, store the results into an eeprom, to be used later for verification of the bed leveling process.
  2613. if (!sample_mesh_and_store_reference()) sampled = false;
  2614. return sampled;
  2615. }
  2616. void go_home_with_z_lift()
  2617. {
  2618. // Don't let the manage_inactivity() function remove power from the motors.
  2619. refresh_cmd_timeout();
  2620. // Go home.
  2621. // First move up to a safe height.
  2622. current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
  2623. go_to_current(homing_feedrate[Z_AXIS]/60);
  2624. // Second move to XY [0, 0].
  2625. current_position[X_AXIS] = X_MIN_POS+0.2;
  2626. current_position[Y_AXIS] = Y_MIN_POS+0.2;
  2627. // Clamp to the physical coordinates.
  2628. world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
  2629. go_to_current(homing_feedrate[X_AXIS]/20);
  2630. // Third move up to a safe height.
  2631. current_position[Z_AXIS] = Z_MIN_POS;
  2632. go_to_current(homing_feedrate[Z_AXIS]/60);
  2633. }
  2634. // Sample the 9 points of the bed and store them into the EEPROM as a reference.
  2635. // When calling this function, the X, Y, Z axes should be already homed,
  2636. // and the world2machine correction matrix should be active.
  2637. // Returns false if the reference values are more than 3mm far away.
  2638. bool sample_mesh_and_store_reference()
  2639. {
  2640. bool endstops_enabled = enable_endstops(false);
  2641. bool endstop_z_enabled = enable_z_endstop(false);
  2642. // Don't let the manage_inactivity() function remove power from the motors.
  2643. refresh_cmd_timeout();
  2644. #ifdef MESH_BED_CALIBRATION_SHOW_LCD
  2645. lcd_display_message_fullscreen_P(_T(MSG_MEASURE_BED_REFERENCE_HEIGHT_LINE1));
  2646. // display "point xx of yy"
  2647. lcd_puts_at_P(0,3,_n("1/9"));
  2648. #endif /* MESH_BED_CALIBRATION_SHOW_LCD */
  2649. // Sample Z heights for the mesh bed leveling.
  2650. // In addition, store the results into an eeprom, to be used later for verification of the bed leveling process.
  2651. {
  2652. // The first point defines the reference.
  2653. current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
  2654. go_to_current(homing_feedrate[Z_AXIS]/60);
  2655. current_position[X_AXIS] = BED_X0;
  2656. current_position[Y_AXIS] = BED_Y0;
  2657. world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
  2658. go_to_current(homing_feedrate[X_AXIS]/60);
  2659. memcpy(destination, current_position, sizeof(destination));
  2660. enable_endstops(true);
  2661. homeaxis(Z_AXIS);
  2662. #ifdef TMC2130
  2663. if (!axis_known_position[Z_AXIS] && (READ(Z_TMC2130_DIAG) != 0)) //Z crash
  2664. {
  2665. kill(_T(MSG_BED_LEVELING_FAILED_POINT_LOW));
  2666. return false;
  2667. }
  2668. #endif //TMC2130
  2669. enable_endstops(false);
  2670. if (!find_bed_induction_sensor_point_z()) //Z crash or deviation > 50um
  2671. {
  2672. kill(_T(MSG_BED_LEVELING_FAILED_POINT_LOW));
  2673. return false;
  2674. }
  2675. mbl.set_z(0, 0, current_position[Z_AXIS]);
  2676. }
  2677. for (int8_t mesh_point = 1; mesh_point != MESH_MEAS_NUM_X_POINTS * MESH_MEAS_NUM_Y_POINTS; ++ mesh_point) {
  2678. // Don't let the manage_inactivity() function remove power from the motors.
  2679. refresh_cmd_timeout();
  2680. // Print the decrasing ID of the measurement point.
  2681. current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
  2682. go_to_current(homing_feedrate[Z_AXIS]/60);
  2683. int8_t ix = mesh_point % MESH_MEAS_NUM_X_POINTS;
  2684. int8_t iy = mesh_point / MESH_MEAS_NUM_X_POINTS;
  2685. if (iy & 1) ix = (MESH_MEAS_NUM_X_POINTS - 1) - ix; // Zig zag
  2686. current_position[X_AXIS] = BED_X(ix, MESH_MEAS_NUM_X_POINTS);
  2687. current_position[Y_AXIS] = BED_Y(iy, MESH_MEAS_NUM_Y_POINTS);
  2688. world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
  2689. go_to_current(homing_feedrate[X_AXIS]/60);
  2690. #ifdef MESH_BED_CALIBRATION_SHOW_LCD
  2691. // display "point xx of yy"
  2692. lcd_set_cursor(0, 3);
  2693. lcd_printf_P(PSTR("%d/9"),mesh_point+1);
  2694. #endif /* MESH_BED_CALIBRATION_SHOW_LCD */
  2695. if (!find_bed_induction_sensor_point_z()) //Z crash or deviation > 50um
  2696. {
  2697. kill(_T(MSG_BED_LEVELING_FAILED_POINT_LOW));
  2698. return false;
  2699. }
  2700. // Get cords of measuring point
  2701. mbl.set_z(ix, iy, current_position[Z_AXIS]);
  2702. }
  2703. {
  2704. // Verify the span of the Z values.
  2705. float zmin = mbl.z_values[0][0];
  2706. float zmax = zmin;
  2707. for (int8_t j = 0; j < 3; ++ j)
  2708. for (int8_t i = 0; i < 3; ++ i) {
  2709. zmin = min(zmin, mbl.z_values[j][i]);
  2710. zmax = max(zmax, mbl.z_values[j][i]);
  2711. }
  2712. if (zmax - zmin > 3.f) {
  2713. // The span of the Z offsets is extreme. Give up.
  2714. // Homing failed on some of the points.
  2715. SERIAL_PROTOCOLLNPGM("Exreme span of the Z values!");
  2716. return false;
  2717. }
  2718. }
  2719. // Store the correction values to EEPROM.
  2720. // Offsets of the Z heiths of the calibration points from the first point.
  2721. // The offsets are saved as 16bit signed int, scaled to tenths of microns.
  2722. {
  2723. uint16_t addr = EEPROM_BED_CALIBRATION_Z_JITTER;
  2724. for (int8_t j = 0; j < 3; ++ j)
  2725. for (int8_t i = 0; i < 3; ++ i) {
  2726. if (i == 0 && j == 0)
  2727. continue;
  2728. float dif = mbl.z_values[j][i] - mbl.z_values[0][0];
  2729. int16_t dif_quantized = int16_t(floor(dif * 100.f + 0.5f));
  2730. eeprom_update_word((uint16_t*)addr, *reinterpret_cast<uint16_t*>(&dif_quantized));
  2731. #if 0
  2732. {
  2733. uint16_t z_offset_u = eeprom_read_word((uint16_t*)addr);
  2734. float dif2 = *reinterpret_cast<int16_t*>(&z_offset_u) * 0.01;
  2735. SERIAL_ECHOPGM("Bed point ");
  2736. SERIAL_ECHO(i);
  2737. SERIAL_ECHOPGM(",");
  2738. SERIAL_ECHO(j);
  2739. SERIAL_ECHOPGM(", differences: written ");
  2740. MYSERIAL.print(dif, 5);
  2741. SERIAL_ECHOPGM(", read: ");
  2742. MYSERIAL.print(dif2, 5);
  2743. SERIAL_ECHOLNPGM("");
  2744. }
  2745. #endif
  2746. addr += 2;
  2747. }
  2748. }
  2749. mbl.upsample_3x3();
  2750. mbl.active = true;
  2751. go_home_with_z_lift();
  2752. enable_endstops(endstops_enabled);
  2753. enable_z_endstop(endstop_z_enabled);
  2754. return true;
  2755. }
  2756. #ifndef NEW_XYZCAL
  2757. bool scan_bed_induction_points(int8_t verbosity_level)
  2758. {
  2759. // Don't let the manage_inactivity() function remove power from the motors.
  2760. refresh_cmd_timeout();
  2761. // Reusing the z_values memory for the measurement cache.
  2762. // 7x7=49 floats, good for 16 (x,y,z) vectors.
  2763. float *pts = &mbl.z_values[0][0];
  2764. float *vec_x = pts + 2 * 9;
  2765. float *vec_y = vec_x + 2;
  2766. float *cntr = vec_y + 2;
  2767. memset(pts, 0, sizeof(float) * 7 * 7);
  2768. // Cache the current correction matrix.
  2769. world2machine_initialize();
  2770. vec_x[0] = world2machine_rotation_and_skew[0][0];
  2771. vec_x[1] = world2machine_rotation_and_skew[1][0];
  2772. vec_y[0] = world2machine_rotation_and_skew[0][1];
  2773. vec_y[1] = world2machine_rotation_and_skew[1][1];
  2774. cntr[0] = world2machine_shift[0];
  2775. cntr[1] = world2machine_shift[1];
  2776. // and reset the correction matrix, so the planner will not do anything.
  2777. world2machine_reset();
  2778. bool endstops_enabled = enable_endstops(false);
  2779. bool endstop_z_enabled = enable_z_endstop(false);
  2780. // Collect a matrix of 9x9 points.
  2781. for (int8_t mesh_point = 0; mesh_point < 9; ++ mesh_point) {
  2782. // Don't let the manage_inactivity() function remove power from the motors.
  2783. refresh_cmd_timeout();
  2784. // Move up.
  2785. current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
  2786. enable_endstops(false);
  2787. enable_z_endstop(false);
  2788. go_to_current(homing_feedrate[Z_AXIS]/60);
  2789. // Go to the measurement point.
  2790. // Use the coorrected coordinate, which is a result of find_bed_offset_and_skew().
  2791. uint8_t ix = mesh_point % MESH_MEAS_NUM_X_POINTS; // from 0 to MESH_NUM_X_POINTS - 1
  2792. uint8_t iy = mesh_point / MESH_MEAS_NUM_X_POINTS;
  2793. if (iy & 1) ix = (MESH_MEAS_NUM_X_POINTS - 1) - ix;
  2794. float bedX = BED_X(ix, MESH_MEAS_NUM_X_POINTS);
  2795. float bedY = BED_Y(iy, MESH_MEAS_NUM_Y_POINTS);
  2796. current_position[X_AXIS] = vec_x[0] * bedX + vec_y[0] * bedY + cntr[0];
  2797. current_position[Y_AXIS] = vec_x[1] * bedX + vec_y[1] * bedY + cntr[1];
  2798. // The calibration points are very close to the min Y.
  2799. if (current_position[Y_AXIS] < Y_MIN_POS_FOR_BED_CALIBRATION)
  2800. current_position[Y_AXIS] = Y_MIN_POS_FOR_BED_CALIBRATION;
  2801. go_to_current(homing_feedrate[X_AXIS]/60);
  2802. find_bed_induction_sensor_point_z();
  2803. scan_bed_induction_sensor_point();
  2804. }
  2805. // Don't let the manage_inactivity() function remove power from the motors.
  2806. refresh_cmd_timeout();
  2807. enable_endstops(false);
  2808. enable_z_endstop(false);
  2809. // Don't let the manage_inactivity() function remove power from the motors.
  2810. refresh_cmd_timeout();
  2811. enable_endstops(endstops_enabled);
  2812. enable_z_endstop(endstop_z_enabled);
  2813. return true;
  2814. }
  2815. #endif //NEW_XYZCAL
  2816. // Shift a Z axis by a given delta.
  2817. // To replace loading of the babystep correction.
  2818. static void shift_z(float delta)
  2819. {
  2820. 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);
  2821. st_synchronize();
  2822. plan_set_z_position(current_position[Z_AXIS]);
  2823. }
  2824. // Number of baby steps applied
  2825. static int babystepLoadZ = 0;
  2826. void babystep_load()
  2827. {
  2828. babystepLoadZ = 0;
  2829. // Apply Z height correction aka baby stepping before mesh bed leveling gets activated.
  2830. if (calibration_status() < CALIBRATION_STATUS_LIVE_ADJUST)
  2831. {
  2832. check_babystep(); //checking if babystep is in allowed range, otherwise setting babystep to 0
  2833. // End of G80: Apply the baby stepping value.
  2834. babystepLoadZ = eeprom_read_word(reinterpret_cast<uint16_t *>(&(EEPROM_Sheets_base->
  2835. s[(eeprom_read_byte(&(EEPROM_Sheets_base->active_sheet)))].z_offset)));
  2836. #if 0
  2837. SERIAL_ECHO("Z baby step: ");
  2838. SERIAL_ECHO(babystepLoadZ);
  2839. SERIAL_ECHO(", current Z: ");
  2840. SERIAL_ECHO(current_position[Z_AXIS]);
  2841. SERIAL_ECHO("correction: ");
  2842. SERIAL_ECHO(float(babystepLoadZ) / float(axis_steps_per_unit[Z_AXIS]));
  2843. SERIAL_ECHOLN("");
  2844. #endif
  2845. }
  2846. }
  2847. void babystep_apply()
  2848. {
  2849. babystep_load();
  2850. shift_z(- float(babystepLoadZ) / float(cs.axis_steps_per_unit[Z_AXIS]));
  2851. }
  2852. void babystep_undo()
  2853. {
  2854. shift_z(float(babystepLoadZ) / float(cs.axis_steps_per_unit[Z_AXIS]));
  2855. babystepLoadZ = 0;
  2856. }
  2857. void babystep_reset()
  2858. {
  2859. babystepLoadZ = 0;
  2860. }
  2861. void count_xyz_details(float (&distanceMin)[2]) {
  2862. float cntr[2] = {
  2863. eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_CENTER + 0)),
  2864. eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_CENTER + 4))
  2865. };
  2866. float vec_x[2] = {
  2867. eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_X + 0)),
  2868. eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_X + 4))
  2869. };
  2870. float vec_y[2] = {
  2871. eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y + 0)),
  2872. eeprom_read_float((float*)(EEPROM_BED_CALIBRATION_VEC_Y + 4))
  2873. };
  2874. #if 0
  2875. a2 = -1 * asin(vec_y[0] / MACHINE_AXIS_SCALE_Y);
  2876. a1 = asin(vec_x[1] / MACHINE_AXIS_SCALE_X);
  2877. angleDiff = fabs(a2 - a1);
  2878. #endif
  2879. for (uint8_t mesh_point = 0; mesh_point < 2; ++mesh_point) {
  2880. float y = vec_x[1] * pgm_read_float(bed_ref_points_4 + mesh_point * 2) + vec_y[1] * pgm_read_float(bed_ref_points_4 + mesh_point * 2 + 1) + cntr[1];
  2881. distanceMin[mesh_point] = (y - Y_MIN_POS_CALIBRATION_POINT_OUT_OF_REACH);
  2882. }
  2883. }
  2884. /*
  2885. e_MBL_TYPE e_mbl_type = e_MBL_OPTIMAL;
  2886. void mbl_mode_set() {
  2887. switch (e_mbl_type) {
  2888. case e_MBL_OPTIMAL: e_mbl_type = e_MBL_PREC; break;
  2889. case e_MBL_PREC: e_mbl_type = e_MBL_FAST; break;
  2890. case e_MBL_FAST: e_mbl_type = e_MBL_OPTIMAL; break;
  2891. default: e_mbl_type = e_MBL_OPTIMAL; break;
  2892. }
  2893. eeprom_update_byte((uint8_t*)EEPROM_MBL_TYPE,(uint8_t)e_mbl_type);
  2894. }
  2895. void mbl_mode_init() {
  2896. uint8_t mbl_type = eeprom_read_byte((uint8_t*)EEPROM_MBL_TYPE);
  2897. if (mbl_type == 0xFF) e_mbl_type = e_MBL_OPTIMAL;
  2898. else e_mbl_type = mbl_type;
  2899. }
  2900. */
  2901. void mbl_settings_init() {
  2902. //3x3 mesh; 3 Z-probes on each point, magnet elimination on
  2903. //magnet elimination: use aaproximate Z-coordinate instead of measured values for points which are near magnets
  2904. if (eeprom_read_byte((uint8_t*)EEPROM_MBL_MAGNET_ELIMINATION) == 0xFF) {
  2905. eeprom_update_byte((uint8_t*)EEPROM_MBL_MAGNET_ELIMINATION, 1);
  2906. }
  2907. if (eeprom_read_byte((uint8_t*)EEPROM_MBL_POINTS_NR) == 0xFF) {
  2908. eeprom_update_byte((uint8_t*)EEPROM_MBL_POINTS_NR, 3);
  2909. }
  2910. mbl_z_probe_nr = eeprom_read_byte((uint8_t*)EEPROM_MBL_PROBE_NR);
  2911. if (mbl_z_probe_nr == 0xFF) {
  2912. mbl_z_probe_nr = 3;
  2913. eeprom_update_byte((uint8_t*)EEPROM_MBL_PROBE_NR, mbl_z_probe_nr);
  2914. }
  2915. }
  2916. //parameter ix: index of mesh bed leveling point in X-axis (for meas_points == 7 is valid range from 0 to 6; for meas_points == 3 is valid range from 0 to 2 )
  2917. //parameter iy: index of mesh bed leveling point in Y-axis (for meas_points == 7 is valid range from 0 to 6; for meas_points == 3 is valid range from 0 to 2 )
  2918. //parameter meas_points: number of mesh bed leveling points in one axis; currently designed and tested for values 3 and 7
  2919. //parameter zigzag: false if ix is considered 0 on left side of bed and ix rises with rising X coordinate; true if ix is considered 0 on the right side of heatbed for odd iy values (zig zag mesh bed leveling movements)
  2920. //function returns true if point is considered valid (typicaly in safe distance from magnet or another object which inflences PINDA measurements)
  2921. bool mbl_point_measurement_valid(uint8_t ix, uint8_t iy, uint8_t meas_points, bool zigzag) {
  2922. //"human readable" heatbed plan
  2923. //magnet proximity influence Z coordinate measurements significantly (40 - 100 um)
  2924. //0 - measurement point is above magnet and Z coordinate can be influenced negatively
  2925. //1 - we should be in safe distance from magnets, measurement should be accurate
  2926. if ((ix >= meas_points) || (iy >= meas_points)) return false;
  2927. uint8_t valid_points_mask[7] = {
  2928. //[X_MAX,Y_MAX]
  2929. //0123456
  2930. 0b1111111,//6
  2931. 0b1111111,//5
  2932. 0b1110111,//4
  2933. 0b1111011,//3
  2934. 0b1110111,//2
  2935. 0b1111111,//1
  2936. 0b1111111,//0
  2937. //[0,0]
  2938. };
  2939. if (meas_points == 3) {
  2940. ix *= 3;
  2941. iy *= 3;
  2942. }
  2943. if (zigzag) {
  2944. if ((iy % 2) == 0) return (valid_points_mask[6 - iy] & (1 << (6 - ix)));
  2945. else return (valid_points_mask[6 - iy] & (1 << ix));
  2946. }
  2947. else {
  2948. return (valid_points_mask[6 - iy] & (1 << (6 - ix)));
  2949. }
  2950. }
  2951. void mbl_single_point_interpolation(uint8_t x, uint8_t y, uint8_t meas_points) {
  2952. //printf_P(PSTR("x = %d; y = %d \n"), x, y);
  2953. uint8_t count = 0;
  2954. float z = 0;
  2955. if (mbl_point_measurement_valid(x, y + 1, meas_points, false)) { z += mbl.z_values[y + 1][x]; /*printf_P(PSTR("x; y+1: Z = %f \n"), mbl.z_values[y + 1][x]);*/ count++; }
  2956. if (mbl_point_measurement_valid(x, y - 1, meas_points, false)) { z += mbl.z_values[y - 1][x]; /*printf_P(PSTR("x; y-1: Z = %f \n"), mbl.z_values[y - 1][x]);*/ count++; }
  2957. if (mbl_point_measurement_valid(x + 1, y, meas_points, false)) { z += mbl.z_values[y][x + 1]; /*printf_P(PSTR("x+1; y: Z = %f \n"), mbl.z_values[y][x + 1]);*/ count++; }
  2958. if (mbl_point_measurement_valid(x - 1, y, meas_points, false)) { z += mbl.z_values[y][x - 1]; /*printf_P(PSTR("x-1; y: Z = %f \n"), mbl.z_values[y][x - 1]);*/ count++; }
  2959. if(count != 0) mbl.z_values[y][x] = z / count; //if we have at least one valid point in surrounding area use average value, otherwise use inaccurately measured Z-coordinate
  2960. //printf_P(PSTR("result: Z = %f \n\n"), mbl.z_values[y][x]);
  2961. }
  2962. void mbl_interpolation(uint8_t meas_points) {
  2963. for (uint8_t x = 0; x < meas_points; x++) {
  2964. for (uint8_t y = 0; y < meas_points; y++) {
  2965. if (!mbl_point_measurement_valid(x, y, meas_points, false)) {
  2966. mbl_single_point_interpolation(x, y, meas_points);
  2967. }
  2968. }
  2969. }
  2970. }