mesh_bed_calibration.cpp 116 KB

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