mesh_bed_calibration.cpp 118 KB

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