mesh_bed_calibration.cpp 114 KB

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