mesh_bed_calibration.cpp 123 KB

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