My Marlin configs for Fabrikator Mini and CTC i3 Pro B
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685
  1. /**
  2. * Marlin 3D Printer Firmware
  3. * Copyright (c) 2019 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
  4. *
  5. * Based on Sprinter and grbl.
  6. * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
  7. *
  8. * This program is free software: you can redistribute it and/or modify
  9. * it under the terms of the GNU General Public License as published by
  10. * the Free Software Foundation, either version 3 of the License, or
  11. * (at your option) any later version.
  12. *
  13. * This program is distributed in the hope that it will be useful,
  14. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  15. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  16. * GNU General Public License for more details.
  17. *
  18. * You should have received a copy of the GNU General Public License
  19. * along with this program. If not, see <http://www.gnu.org/licenses/>.
  20. *
  21. */
  22. #include "../../inc/MarlinConfig.h"
  23. #if ENABLED(DELTA_AUTO_CALIBRATION)
  24. #include "../gcode.h"
  25. #include "../../module/delta.h"
  26. #include "../../module/motion.h"
  27. #include "../../module/stepper.h"
  28. #include "../../module/endstops.h"
  29. #include "../../lcd/ultralcd.h"
  30. #if HAS_BED_PROBE
  31. #include "../../module/probe.h"
  32. #endif
  33. #if HOTENDS > 1
  34. #include "../../module/tool_change.h"
  35. #endif
  36. #if HAS_LEVELING
  37. #include "../../feature/bedlevel/bedlevel.h"
  38. #endif
  39. constexpr uint8_t _7P_STEP = 1, // 7-point step - to change number of calibration points
  40. _4P_STEP = _7P_STEP * 2, // 4-point step
  41. NPP = _7P_STEP * 6; // number of calibration points on the radius
  42. enum CalEnum : char { // the 7 main calibration points - add definitions if needed
  43. CEN = 0,
  44. __A = 1,
  45. _AB = __A + _7P_STEP,
  46. __B = _AB + _7P_STEP,
  47. _BC = __B + _7P_STEP,
  48. __C = _BC + _7P_STEP,
  49. _CA = __C + _7P_STEP,
  50. };
  51. #define LOOP_CAL_PT(VAR, S, N) for (uint8_t VAR=S; VAR<=NPP; VAR+=N)
  52. #define F_LOOP_CAL_PT(VAR, S, N) for (float VAR=S; VAR<NPP+0.9999; VAR+=N)
  53. #define I_LOOP_CAL_PT(VAR, S, N) for (float VAR=S; VAR>CEN+0.9999; VAR-=N)
  54. #define LOOP_CAL_ALL(VAR) LOOP_CAL_PT(VAR, CEN, 1)
  55. #define LOOP_CAL_RAD(VAR) LOOP_CAL_PT(VAR, __A, _7P_STEP)
  56. #define LOOP_CAL_ACT(VAR, _4P, _OP) LOOP_CAL_PT(VAR, _OP ? _AB : __A, _4P ? _4P_STEP : _7P_STEP)
  57. #if HOTENDS > 1
  58. const uint8_t old_tool_index = active_extruder;
  59. #define AC_CLEANUP() ac_cleanup(old_tool_index)
  60. #else
  61. #define AC_CLEANUP() ac_cleanup()
  62. #endif
  63. float lcd_probe_pt(const float &rx, const float &ry);
  64. void ac_home() {
  65. endstops.enable(true);
  66. home_delta();
  67. endstops.not_homing();
  68. }
  69. void ac_setup(const bool reset_bed) {
  70. #if HOTENDS > 1
  71. tool_change(0, true);
  72. #endif
  73. planner.synchronize();
  74. setup_for_endstop_or_probe_move();
  75. #if HAS_LEVELING
  76. if (reset_bed) reset_bed_level(); // After full calibration bed-level data is no longer valid
  77. #endif
  78. }
  79. void ac_cleanup(
  80. #if HOTENDS > 1
  81. const uint8_t old_tool_index
  82. #endif
  83. ) {
  84. #if ENABLED(DELTA_HOME_TO_SAFE_ZONE)
  85. do_blocking_move_to_z(delta_clip_start_height);
  86. #endif
  87. #if HAS_BED_PROBE
  88. STOW_PROBE();
  89. #endif
  90. clean_up_after_endstop_or_probe_move();
  91. #if HOTENDS > 1
  92. tool_change(old_tool_index, true);
  93. #endif
  94. }
  95. void print_signed_float(PGM_P const prefix, const float &f) {
  96. SERIAL_ECHOPGM(" ");
  97. serialprintPGM(prefix);
  98. SERIAL_CHAR(':');
  99. if (f >= 0) SERIAL_CHAR('+');
  100. SERIAL_ECHO_F(f, 2);
  101. }
  102. /**
  103. * - Print the delta settings
  104. */
  105. static void print_calibration_settings(const bool end_stops, const bool tower_angles) {
  106. SERIAL_ECHOPAIR(".Height:", delta_height);
  107. if (end_stops) {
  108. print_signed_float(PSTR("Ex"), delta_endstop_adj[A_AXIS]);
  109. print_signed_float(PSTR("Ey"), delta_endstop_adj[B_AXIS]);
  110. print_signed_float(PSTR("Ez"), delta_endstop_adj[C_AXIS]);
  111. }
  112. if (end_stops && tower_angles) {
  113. SERIAL_ECHOPAIR(" Radius:", delta_radius);
  114. SERIAL_EOL();
  115. SERIAL_CHAR('.');
  116. SERIAL_ECHO_SP(13);
  117. }
  118. if (tower_angles) {
  119. print_signed_float(PSTR("Tx"), delta_tower_angle_trim[A_AXIS]);
  120. print_signed_float(PSTR("Ty"), delta_tower_angle_trim[B_AXIS]);
  121. print_signed_float(PSTR("Tz"), delta_tower_angle_trim[C_AXIS]);
  122. }
  123. if ((!end_stops && tower_angles) || (end_stops && !tower_angles)) { // XOR
  124. SERIAL_ECHOPAIR(" Radius:", delta_radius);
  125. }
  126. SERIAL_EOL();
  127. }
  128. /**
  129. * - Print the probe results
  130. */
  131. static void print_calibration_results(const float z_pt[NPP + 1], const bool tower_points, const bool opposite_points) {
  132. SERIAL_ECHOPGM(". ");
  133. print_signed_float(PSTR("c"), z_pt[CEN]);
  134. if (tower_points) {
  135. print_signed_float(PSTR(" x"), z_pt[__A]);
  136. print_signed_float(PSTR(" y"), z_pt[__B]);
  137. print_signed_float(PSTR(" z"), z_pt[__C]);
  138. }
  139. if (tower_points && opposite_points) {
  140. SERIAL_EOL();
  141. SERIAL_CHAR('.');
  142. SERIAL_ECHO_SP(13);
  143. }
  144. if (opposite_points) {
  145. print_signed_float(PSTR("yz"), z_pt[_BC]);
  146. print_signed_float(PSTR("zx"), z_pt[_CA]);
  147. print_signed_float(PSTR("xy"), z_pt[_AB]);
  148. }
  149. SERIAL_EOL();
  150. }
  151. /**
  152. * - Calculate the standard deviation from the zero plane
  153. */
  154. static float std_dev_points(float z_pt[NPP + 1], const bool _0p_cal, const bool _1p_cal, const bool _4p_cal, const bool _4p_opp) {
  155. if (!_0p_cal) {
  156. float S2 = sq(z_pt[CEN]);
  157. int16_t N = 1;
  158. if (!_1p_cal) { // std dev from zero plane
  159. LOOP_CAL_ACT(rad, _4p_cal, _4p_opp) {
  160. S2 += sq(z_pt[rad]);
  161. N++;
  162. }
  163. return LROUND(SQRT(S2 / N) * 1000.0) / 1000.0 + 0.00001;
  164. }
  165. }
  166. return 0.00001;
  167. }
  168. /**
  169. * - Probe a point
  170. */
  171. static float calibration_probe(const float &nx, const float &ny, const bool stow) {
  172. #if HAS_BED_PROBE
  173. return probe_pt(nx, ny, stow ? PROBE_PT_STOW : PROBE_PT_RAISE, 0, false);
  174. #else
  175. UNUSED(stow);
  176. return lcd_probe_pt(nx, ny);
  177. #endif
  178. }
  179. /**
  180. * - Probe a grid
  181. */
  182. static bool probe_calibration_points(float z_pt[NPP + 1], const int8_t probe_points, const bool towers_set, const bool stow_after_each) {
  183. const bool _0p_calibration = probe_points == 0,
  184. _1p_calibration = probe_points == 1 || probe_points == -1,
  185. _4p_calibration = probe_points == 2,
  186. _4p_opposite_points = _4p_calibration && !towers_set,
  187. _7p_calibration = probe_points >= 3,
  188. _7p_no_intermediates = probe_points == 3,
  189. _7p_1_intermediates = probe_points == 4,
  190. _7p_2_intermediates = probe_points == 5,
  191. _7p_4_intermediates = probe_points == 6,
  192. _7p_6_intermediates = probe_points == 7,
  193. _7p_8_intermediates = probe_points == 8,
  194. _7p_11_intermediates = probe_points == 9,
  195. _7p_14_intermediates = probe_points == 10,
  196. _7p_intermed_points = probe_points >= 4,
  197. _7p_6_center = probe_points >= 5 && probe_points <= 7,
  198. _7p_9_center = probe_points >= 8;
  199. LOOP_CAL_ALL(rad) z_pt[rad] = 0.0;
  200. if (!_0p_calibration) {
  201. if (!_7p_no_intermediates && !_7p_4_intermediates && !_7p_11_intermediates) { // probe the center
  202. z_pt[CEN] += calibration_probe(0, 0, stow_after_each);
  203. if (isnan(z_pt[CEN])) return false;
  204. }
  205. if (_7p_calibration) { // probe extra center points
  206. const float start = _7p_9_center ? float(_CA) + _7P_STEP / 3.0 : _7p_6_center ? float(_CA) : float(__C),
  207. steps = _7p_9_center ? _4P_STEP / 3.0 : _7p_6_center ? _7P_STEP : _4P_STEP;
  208. I_LOOP_CAL_PT(rad, start, steps) {
  209. const float a = RADIANS(210 + (360 / NPP) * (rad - 1)),
  210. r = delta_calibration_radius * 0.1;
  211. z_pt[CEN] += calibration_probe(cos(a) * r, sin(a) * r, stow_after_each);
  212. if (isnan(z_pt[CEN])) return false;
  213. }
  214. z_pt[CEN] /= float(_7p_2_intermediates ? 7 : probe_points);
  215. }
  216. if (!_1p_calibration) { // probe the radius
  217. const CalEnum start = _4p_opposite_points ? _AB : __A;
  218. const float steps = _7p_14_intermediates ? _7P_STEP / 15.0 : // 15r * 6 + 10c = 100
  219. _7p_11_intermediates ? _7P_STEP / 12.0 : // 12r * 6 + 9c = 81
  220. _7p_8_intermediates ? _7P_STEP / 9.0 : // 9r * 6 + 10c = 64
  221. _7p_6_intermediates ? _7P_STEP / 7.0 : // 7r * 6 + 7c = 49
  222. _7p_4_intermediates ? _7P_STEP / 5.0 : // 5r * 6 + 6c = 36
  223. _7p_2_intermediates ? _7P_STEP / 3.0 : // 3r * 6 + 7c = 25
  224. _7p_1_intermediates ? _7P_STEP / 2.0 : // 2r * 6 + 4c = 16
  225. _7p_no_intermediates ? _7P_STEP : // 1r * 6 + 3c = 9
  226. _4P_STEP; // .5r * 6 + 1c = 4
  227. bool zig_zag = true;
  228. F_LOOP_CAL_PT(rad, start, _7p_9_center ? steps * 3 : steps) {
  229. const int8_t offset = _7p_9_center ? 2 : 0;
  230. for (int8_t circle = 0; circle <= offset; circle++) {
  231. const float a = RADIANS(210 + (360 / NPP) * (rad - 1)),
  232. r = delta_calibration_radius * (1 - 0.1 * (zig_zag ? offset - circle : circle)),
  233. interpol = FMOD(rad, 1);
  234. const float z_temp = calibration_probe(cos(a) * r, sin(a) * r, stow_after_each);
  235. if (isnan(z_temp)) return false;
  236. // split probe point to neighbouring calibration points
  237. z_pt[uint8_t(LROUND(rad - interpol + NPP - 1)) % NPP + 1] += z_temp * sq(cos(RADIANS(interpol * 90)));
  238. z_pt[uint8_t(LROUND(rad - interpol)) % NPP + 1] += z_temp * sq(sin(RADIANS(interpol * 90)));
  239. }
  240. zig_zag = !zig_zag;
  241. }
  242. if (_7p_intermed_points)
  243. LOOP_CAL_RAD(rad)
  244. z_pt[rad] /= _7P_STEP / steps;
  245. do_blocking_move_to_xy(0.0, 0.0);
  246. }
  247. }
  248. return true;
  249. }
  250. /**
  251. * kinematics routines and auto tune matrix scaling parameters:
  252. * see https://github.com/LVD-AC/Marlin-AC/tree/1.1.x-AC/documentation for
  253. * - formulae for approximative forward kinematics in the end-stop displacement matrix
  254. * - definition of the matrix scaling parameters
  255. */
  256. static void reverse_kinematics_probe_points(float z_pt[NPP + 1], float mm_at_pt_axis[NPP + 1][ABC]) {
  257. float pos[XYZ] = { 0.0 };
  258. LOOP_CAL_ALL(rad) {
  259. const float a = RADIANS(210 + (360 / NPP) * (rad - 1)),
  260. r = (rad == CEN ? 0.0 : delta_calibration_radius);
  261. pos[X_AXIS] = cos(a) * r;
  262. pos[Y_AXIS] = sin(a) * r;
  263. pos[Z_AXIS] = z_pt[rad];
  264. inverse_kinematics(pos);
  265. LOOP_XYZ(axis) mm_at_pt_axis[rad][axis] = delta[axis];
  266. }
  267. }
  268. static void forward_kinematics_probe_points(float mm_at_pt_axis[NPP + 1][ABC], float z_pt[NPP + 1]) {
  269. const float r_quot = delta_calibration_radius / delta_radius;
  270. #define ZPP(N,I,A) ((1 / 3.0 + r_quot * (N) / 3.0 ) * mm_at_pt_axis[I][A])
  271. #define Z00(I, A) ZPP( 0, I, A)
  272. #define Zp1(I, A) ZPP(+1, I, A)
  273. #define Zm1(I, A) ZPP(-1, I, A)
  274. #define Zp2(I, A) ZPP(+2, I, A)
  275. #define Zm2(I, A) ZPP(-2, I, A)
  276. z_pt[CEN] = Z00(CEN, A_AXIS) + Z00(CEN, B_AXIS) + Z00(CEN, C_AXIS);
  277. z_pt[__A] = Zp2(__A, A_AXIS) + Zm1(__A, B_AXIS) + Zm1(__A, C_AXIS);
  278. z_pt[__B] = Zm1(__B, A_AXIS) + Zp2(__B, B_AXIS) + Zm1(__B, C_AXIS);
  279. z_pt[__C] = Zm1(__C, A_AXIS) + Zm1(__C, B_AXIS) + Zp2(__C, C_AXIS);
  280. z_pt[_BC] = Zm2(_BC, A_AXIS) + Zp1(_BC, B_AXIS) + Zp1(_BC, C_AXIS);
  281. z_pt[_CA] = Zp1(_CA, A_AXIS) + Zm2(_CA, B_AXIS) + Zp1(_CA, C_AXIS);
  282. z_pt[_AB] = Zp1(_AB, A_AXIS) + Zp1(_AB, B_AXIS) + Zm2(_AB, C_AXIS);
  283. }
  284. static void calc_kinematics_diff_probe_points(float z_pt[NPP + 1], float delta_e[ABC], float delta_r, float delta_t[ABC]) {
  285. const float z_center = z_pt[CEN];
  286. float diff_mm_at_pt_axis[NPP + 1][ABC],
  287. new_mm_at_pt_axis[NPP + 1][ABC];
  288. reverse_kinematics_probe_points(z_pt, diff_mm_at_pt_axis);
  289. delta_radius += delta_r;
  290. LOOP_XYZ(axis) delta_tower_angle_trim[axis] += delta_t[axis];
  291. recalc_delta_settings();
  292. reverse_kinematics_probe_points(z_pt, new_mm_at_pt_axis);
  293. LOOP_XYZ(axis) LOOP_CAL_ALL(rad) diff_mm_at_pt_axis[rad][axis] -= new_mm_at_pt_axis[rad][axis] + delta_e[axis];
  294. forward_kinematics_probe_points(diff_mm_at_pt_axis, z_pt);
  295. LOOP_CAL_RAD(rad) z_pt[rad] -= z_pt[CEN] - z_center;
  296. z_pt[CEN] = z_center;
  297. delta_radius -= delta_r;
  298. LOOP_XYZ(axis) delta_tower_angle_trim[axis] -= delta_t[axis];
  299. recalc_delta_settings();
  300. }
  301. static float auto_tune_h() {
  302. const float r_quot = delta_calibration_radius / delta_radius;
  303. float h_fac = 0.0;
  304. h_fac = r_quot / (2.0 / 3.0);
  305. h_fac = 1.0f / h_fac; // (2/3)/CR
  306. return h_fac;
  307. }
  308. static float auto_tune_r() {
  309. const float diff = 0.01;
  310. float r_fac = 0.0,
  311. z_pt[NPP + 1] = { 0.0 },
  312. delta_e[ABC] = {0.0},
  313. delta_r = {0.0},
  314. delta_t[ABC] = {0.0};
  315. delta_r = diff;
  316. calc_kinematics_diff_probe_points(z_pt, delta_e, delta_r, delta_t);
  317. r_fac = -(z_pt[__A] + z_pt[__B] + z_pt[__C] + z_pt[_BC] + z_pt[_CA] + z_pt[_AB]) / 6.0;
  318. r_fac = diff / r_fac / 3.0; // 1/(3*delta_Z)
  319. return r_fac;
  320. }
  321. static float auto_tune_a() {
  322. const float diff = 0.01;
  323. float a_fac = 0.0,
  324. z_pt[NPP + 1] = { 0.0 },
  325. delta_e[ABC] = {0.0},
  326. delta_r = {0.0},
  327. delta_t[ABC] = {0.0};
  328. LOOP_XYZ(axis) {
  329. LOOP_XYZ(axis_2) delta_t[axis_2] = 0.0;
  330. delta_t[axis] = diff;
  331. calc_kinematics_diff_probe_points(z_pt, delta_e, delta_r, delta_t);
  332. a_fac += z_pt[uint8_t((axis * _4P_STEP) - _7P_STEP + NPP) % NPP + 1] / 6.0;
  333. a_fac -= z_pt[uint8_t((axis * _4P_STEP) + 1 + _7P_STEP)] / 6.0;
  334. }
  335. a_fac = diff / a_fac / 3.0; // 1/(3*delta_Z)
  336. return a_fac;
  337. }
  338. /**
  339. * G33 - Delta '1-4-7-point' Auto-Calibration
  340. * Calibrate height, z_offset, endstops, delta radius, and tower angles.
  341. *
  342. * Parameters:
  343. *
  344. * Pn Number of probe points:
  345. * P0 Normalizes calibration.
  346. * P1 Calibrates height only with center probe.
  347. * P2 Probe center and towers. Calibrate height, endstops and delta radius.
  348. * P3 Probe all positions: center, towers and opposite towers. Calibrate all.
  349. * P4-P10 Probe all positions at different intermediate locations and average them.
  350. *
  351. * T Don't calibrate tower angle corrections
  352. *
  353. * Cn.nn Calibration precision; when omitted calibrates to maximum precision
  354. *
  355. * Fn Force to run at least n iterations and take the best result
  356. *
  357. * Vn Verbose level:
  358. * V0 Dry-run mode. Report settings and probe results. No calibration.
  359. * V1 Report start and end settings only
  360. * V2 Report settings at each iteration
  361. * V3 Report settings and probe results
  362. *
  363. * E Engage the probe for each point
  364. */
  365. void GcodeSuite::G33() {
  366. const int8_t probe_points = parser.intval('P', DELTA_CALIBRATION_DEFAULT_POINTS);
  367. if (!WITHIN(probe_points, 0, 10)) {
  368. SERIAL_ECHOLNPGM("?(P)oints implausible (0-10).");
  369. return;
  370. }
  371. const bool towers_set = !parser.seen('T');
  372. const float calibration_precision = parser.floatval('C', 0.0);
  373. if (calibration_precision < 0) {
  374. SERIAL_ECHOLNPGM("?(C)alibration precision implausible (>=0).");
  375. return;
  376. }
  377. const int8_t force_iterations = parser.intval('F', 0);
  378. if (!WITHIN(force_iterations, 0, 30)) {
  379. SERIAL_ECHOLNPGM("?(F)orce iteration implausible (0-30).");
  380. return;
  381. }
  382. const int8_t verbose_level = parser.byteval('V', 1);
  383. if (!WITHIN(verbose_level, 0, 3)) {
  384. SERIAL_ECHOLNPGM("?(V)erbose level implausible (0-3).");
  385. return;
  386. }
  387. const bool stow_after_each = parser.seen('E');
  388. const bool _0p_calibration = probe_points == 0,
  389. _1p_calibration = probe_points == 1 || probe_points == -1,
  390. _4p_calibration = probe_points == 2,
  391. _4p_opposite_points = _4p_calibration && !towers_set,
  392. _7p_9_center = probe_points >= 8,
  393. _tower_results = (_4p_calibration && towers_set) || probe_points >= 3,
  394. _opposite_results = (_4p_calibration && !towers_set) || probe_points >= 3,
  395. _endstop_results = probe_points != 1 && probe_points != -1 && probe_points != 0,
  396. _angle_results = probe_points >= 3 && towers_set;
  397. static const char save_message[] PROGMEM = "Save with M500 and/or copy to Configuration.h";
  398. int8_t iterations = 0;
  399. float test_precision,
  400. zero_std_dev = (verbose_level ? 999.0 : 0.0), // 0.0 in dry-run mode : forced end
  401. zero_std_dev_min = zero_std_dev,
  402. zero_std_dev_old = zero_std_dev,
  403. h_factor,
  404. r_factor,
  405. a_factor,
  406. e_old[ABC] = {
  407. delta_endstop_adj[A_AXIS],
  408. delta_endstop_adj[B_AXIS],
  409. delta_endstop_adj[C_AXIS]
  410. },
  411. r_old = delta_radius,
  412. h_old = delta_height,
  413. a_old[ABC] = {
  414. delta_tower_angle_trim[A_AXIS],
  415. delta_tower_angle_trim[B_AXIS],
  416. delta_tower_angle_trim[C_AXIS]
  417. };
  418. SERIAL_ECHOLNPGM("G33 Auto Calibrate");
  419. if (!_1p_calibration && !_0p_calibration) { // test if the outer radius is reachable
  420. LOOP_CAL_RAD(axis) {
  421. const float a = RADIANS(210 + (360 / NPP) * (axis - 1)),
  422. r = delta_calibration_radius;
  423. if (!position_is_reachable(cos(a) * r, sin(a) * r)) {
  424. SERIAL_ECHOLNPGM("?(M665 B)ed radius implausible.");
  425. return;
  426. }
  427. }
  428. }
  429. // Report settings
  430. PGM_P checkingac = PSTR("Checking... AC");
  431. serialprintPGM(checkingac);
  432. if (verbose_level == 0) SERIAL_ECHOPGM(" (DRY-RUN)");
  433. SERIAL_EOL();
  434. ui.set_status_P(checkingac);
  435. print_calibration_settings(_endstop_results, _angle_results);
  436. ac_setup(!_0p_calibration && !_1p_calibration);
  437. if (!_0p_calibration) ac_home();
  438. do { // start iterations
  439. float z_at_pt[NPP + 1] = { 0.0 };
  440. test_precision = zero_std_dev_old != 999.0 ? (zero_std_dev + zero_std_dev_old) / 2 : zero_std_dev;
  441. iterations++;
  442. // Probe the points
  443. zero_std_dev_old = zero_std_dev;
  444. if (!probe_calibration_points(z_at_pt, probe_points, towers_set, stow_after_each)) {
  445. SERIAL_ECHOLNPGM("Correct delta settings with M665 and M666");
  446. return AC_CLEANUP();
  447. }
  448. zero_std_dev = std_dev_points(z_at_pt, _0p_calibration, _1p_calibration, _4p_calibration, _4p_opposite_points);
  449. // Solve matrices
  450. if ((zero_std_dev < test_precision || iterations <= force_iterations) && zero_std_dev > calibration_precision) {
  451. #if !HAS_BED_PROBE
  452. test_precision = 0.00; // forced end
  453. #endif
  454. if (zero_std_dev < zero_std_dev_min) {
  455. // set roll-back point
  456. COPY(e_old, delta_endstop_adj);
  457. r_old = delta_radius;
  458. h_old = delta_height;
  459. COPY(a_old, delta_tower_angle_trim);
  460. }
  461. float e_delta[ABC] = { 0.0 },
  462. r_delta = 0.0,
  463. t_delta[ABC] = { 0.0 };
  464. /**
  465. * convergence matrices:
  466. * see https://github.com/LVD-AC/Marlin-AC/tree/1.1.x-AC/documentation for
  467. * - definition of the matrix scaling parameters
  468. * - matrices for 4 and 7 point calibration
  469. */
  470. #define ZP(N,I) ((N) * z_at_pt[I] / 4.0) // 4.0 = divider to normalize to integers
  471. #define Z12(I) ZP(12, I)
  472. #define Z4(I) ZP(4, I)
  473. #define Z2(I) ZP(2, I)
  474. #define Z1(I) ZP(1, I)
  475. #define Z0(I) ZP(0, I)
  476. // calculate factors
  477. const float cr_old = delta_calibration_radius;
  478. if (_7p_9_center) delta_calibration_radius *= 0.9;
  479. h_factor = auto_tune_h();
  480. r_factor = auto_tune_r();
  481. a_factor = auto_tune_a();
  482. delta_calibration_radius = cr_old;
  483. switch (probe_points) {
  484. case 0:
  485. test_precision = 0.00; // forced end
  486. break;
  487. case 1:
  488. test_precision = 0.00; // forced end
  489. LOOP_XYZ(axis) e_delta[axis] = +Z4(CEN);
  490. break;
  491. case 2:
  492. if (towers_set) { // see 4 point calibration (towers) matrix
  493. e_delta[A_AXIS] = (+Z4(__A) -Z2(__B) -Z2(__C)) * h_factor +Z4(CEN);
  494. e_delta[B_AXIS] = (-Z2(__A) +Z4(__B) -Z2(__C)) * h_factor +Z4(CEN);
  495. e_delta[C_AXIS] = (-Z2(__A) -Z2(__B) +Z4(__C)) * h_factor +Z4(CEN);
  496. r_delta = (+Z4(__A) +Z4(__B) +Z4(__C) -Z12(CEN)) * r_factor;
  497. }
  498. else { // see 4 point calibration (opposites) matrix
  499. e_delta[A_AXIS] = (-Z4(_BC) +Z2(_CA) +Z2(_AB)) * h_factor +Z4(CEN);
  500. e_delta[B_AXIS] = (+Z2(_BC) -Z4(_CA) +Z2(_AB)) * h_factor +Z4(CEN);
  501. e_delta[C_AXIS] = (+Z2(_BC) +Z2(_CA) -Z4(_AB)) * h_factor +Z4(CEN);
  502. r_delta = (+Z4(_BC) +Z4(_CA) +Z4(_AB) -Z12(CEN)) * r_factor;
  503. }
  504. break;
  505. default: // see 7 point calibration (towers & opposites) matrix
  506. e_delta[A_AXIS] = (+Z2(__A) -Z1(__B) -Z1(__C) -Z2(_BC) +Z1(_CA) +Z1(_AB)) * h_factor +Z4(CEN);
  507. e_delta[B_AXIS] = (-Z1(__A) +Z2(__B) -Z1(__C) +Z1(_BC) -Z2(_CA) +Z1(_AB)) * h_factor +Z4(CEN);
  508. e_delta[C_AXIS] = (-Z1(__A) -Z1(__B) +Z2(__C) +Z1(_BC) +Z1(_CA) -Z2(_AB)) * h_factor +Z4(CEN);
  509. r_delta = (+Z2(__A) +Z2(__B) +Z2(__C) +Z2(_BC) +Z2(_CA) +Z2(_AB) -Z12(CEN)) * r_factor;
  510. if (towers_set) { // see 7 point tower angle calibration (towers & opposites) matrix
  511. t_delta[A_AXIS] = (+Z0(__A) -Z4(__B) +Z4(__C) +Z0(_BC) -Z4(_CA) +Z4(_AB) +Z0(CEN)) * a_factor;
  512. t_delta[B_AXIS] = (+Z4(__A) +Z0(__B) -Z4(__C) +Z4(_BC) +Z0(_CA) -Z4(_AB) +Z0(CEN)) * a_factor;
  513. t_delta[C_AXIS] = (-Z4(__A) +Z4(__B) +Z0(__C) -Z4(_BC) +Z4(_CA) +Z0(_AB) +Z0(CEN)) * a_factor;
  514. }
  515. break;
  516. }
  517. LOOP_XYZ(axis) delta_endstop_adj[axis] += e_delta[axis];
  518. delta_radius += r_delta;
  519. LOOP_XYZ(axis) delta_tower_angle_trim[axis] += t_delta[axis];
  520. }
  521. else if (zero_std_dev >= test_precision) {
  522. // roll back
  523. COPY(delta_endstop_adj, e_old);
  524. delta_radius = r_old;
  525. delta_height = h_old;
  526. COPY(delta_tower_angle_trim, a_old);
  527. }
  528. if (verbose_level != 0) { // !dry run
  529. // Normalize angles to least-squares
  530. if (_angle_results) {
  531. float a_sum = 0.0;
  532. LOOP_XYZ(axis) a_sum += delta_tower_angle_trim[axis];
  533. LOOP_XYZ(axis) delta_tower_angle_trim[axis] -= a_sum / 3.0;
  534. }
  535. // adjust delta_height and endstops by the max amount
  536. const float z_temp = _MAX(delta_endstop_adj[A_AXIS], delta_endstop_adj[B_AXIS], delta_endstop_adj[C_AXIS]);
  537. delta_height -= z_temp;
  538. LOOP_XYZ(axis) delta_endstop_adj[axis] -= z_temp;
  539. }
  540. recalc_delta_settings();
  541. NOMORE(zero_std_dev_min, zero_std_dev);
  542. // print report
  543. if (verbose_level == 3)
  544. print_calibration_results(z_at_pt, _tower_results, _opposite_results);
  545. if (verbose_level != 0) { // !dry run
  546. if ((zero_std_dev >= test_precision && iterations > force_iterations) || zero_std_dev <= calibration_precision) { // end iterations
  547. SERIAL_ECHOPGM("Calibration OK");
  548. SERIAL_ECHO_SP(32);
  549. #if HAS_BED_PROBE
  550. if (zero_std_dev >= test_precision && !_1p_calibration && !_0p_calibration)
  551. SERIAL_ECHOPGM("rolling back.");
  552. else
  553. #endif
  554. {
  555. SERIAL_ECHOPAIR_F("std dev:", zero_std_dev_min, 3);
  556. }
  557. SERIAL_EOL();
  558. char mess[21];
  559. strcpy_P(mess, PSTR("Calibration sd:"));
  560. if (zero_std_dev_min < 1)
  561. sprintf_P(&mess[15], PSTR("0.%03i"), (int)LROUND(zero_std_dev_min * 1000.0));
  562. else
  563. sprintf_P(&mess[15], PSTR("%03i.x"), (int)LROUND(zero_std_dev_min));
  564. ui.set_status(mess);
  565. print_calibration_settings(_endstop_results, _angle_results);
  566. serialprintPGM(save_message);
  567. SERIAL_EOL();
  568. }
  569. else { // !end iterations
  570. char mess[15];
  571. if (iterations < 31)
  572. sprintf_P(mess, PSTR("Iteration : %02i"), (unsigned int)iterations);
  573. else
  574. strcpy_P(mess, PSTR("No convergence"));
  575. SERIAL_ECHO(mess);
  576. SERIAL_ECHO_SP(32);
  577. SERIAL_ECHOLNPAIR_F("std dev:", zero_std_dev, 3);
  578. ui.set_status(mess);
  579. if (verbose_level > 1)
  580. print_calibration_settings(_endstop_results, _angle_results);
  581. }
  582. }
  583. else { // dry run
  584. PGM_P enddryrun = PSTR("End DRY-RUN");
  585. serialprintPGM(enddryrun);
  586. SERIAL_ECHO_SP(35);
  587. SERIAL_ECHOLNPAIR_F("std dev:", zero_std_dev, 3);
  588. char mess[21];
  589. strcpy_P(mess, enddryrun);
  590. strcpy_P(&mess[11], PSTR(" sd:"));
  591. if (zero_std_dev < 1)
  592. sprintf_P(&mess[15], PSTR("0.%03i"), (int)LROUND(zero_std_dev * 1000.0));
  593. else
  594. sprintf_P(&mess[15], PSTR("%03i.x"), (int)LROUND(zero_std_dev));
  595. ui.set_status(mess);
  596. }
  597. ac_home();
  598. }
  599. while (((zero_std_dev < test_precision && iterations < 31) || iterations <= force_iterations) && zero_std_dev > calibration_precision);
  600. AC_CLEANUP();
  601. }
  602. #endif // DELTA_AUTO_CALIBRATION