mesh_bed_calibration.cpp 87 KB

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