mesh_bed_calibration.cpp 117 KB

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