My Marlin configs for Fabrikator Mini and CTC i3 Pro B
Du kan inte välja fler än 25 ämnen Ämnen måste starta med en bokstav eller siffra, kan innehålla bindestreck ('-') och vara max 35 tecken långa.

I2CPositionEncoder.cpp 35KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101
  1. /**
  2. * Marlin 3D Printer Firmware
  3. * Copyright (C) 2016, 2017 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. //todo: add support for multiple encoders on a single axis
  23. //todo: add z axis auto-leveling
  24. //todo: consolidate some of the related M codes?
  25. //todo: add endstop-replacement mode?
  26. //todo: try faster I2C speed; tweak TWI_FREQ (400000L, or faster?); or just TWBR = ((CPU_FREQ / 400000L) - 16) / 2;
  27. //todo: consider Marlin-optimized Wire library; i.e. MarlinWire, like MarlinSerial
  28. #include "MarlinConfig.h"
  29. #if ENABLED(I2C_POSITION_ENCODERS)
  30. #include "Marlin.h"
  31. #include "temperature.h"
  32. #include "stepper.h"
  33. #include "I2CPositionEncoder.h"
  34. #include "gcode.h"
  35. #include <Wire.h>
  36. void I2CPositionEncoder::init(uint8_t address, AxisEnum axis) {
  37. encoderAxis = axis;
  38. i2cAddress = address;
  39. initialised++;
  40. SERIAL_ECHOPAIR("Seetting up encoder on ", axis_codes[encoderAxis]);
  41. SERIAL_ECHOLNPAIR(" axis, addr = ", address);
  42. position = get_position();
  43. }
  44. void I2CPositionEncoder::update() {
  45. if (!initialised || !homed || !active) return; //check encoder is set up and active
  46. position = get_position();
  47. //we don't want to stop things just because the encoder missed a message,
  48. //so we only care about responses that indicate bad magnetic strength
  49. if (!passes_test(false)) { //check encoder data is good
  50. lastErrorTime = millis();
  51. /*
  52. if (trusted) { //commented out as part of the note below
  53. trusted = false;
  54. SERIAL_ECHOPGM("Fault detected on ");
  55. SERIAL_ECHO(axis_codes[encoderAxis]);
  56. SERIAL_ECHOLNPGM(" axis encoder. Disengaging error correction until module is trusted again.");
  57. }
  58. */
  59. return;
  60. }
  61. if (!trusted) {
  62. /**
  63. * This is commented out because it introduces error and can cause bad print quality.
  64. *
  65. * This code is intended to manage situations where the encoder has reported bad magnetic strength.
  66. * This indicates that the magnetic strip was too far away from the sensor to reliably track position.
  67. * When this happens, this code resets the offset based on where the printer thinks it is. This has been
  68. * shown to introduce errors in actual position which result in drifting prints and poor print quality.
  69. * Perhaps a better method would be to disable correction on the axis with a problem, report it to the
  70. * user via the status leds on the encoder module and prompt the user to re-home the axis at which point
  71. * the encoder would be re-enabled.
  72. */
  73. /*
  74. // If the magnetic strength has been good for a certain time, start trusting the module again
  75. if (millis() - lastErrorTime > I2CPE_TIME_TRUSTED) {
  76. trusted = true;
  77. SERIAL_ECHOPGM("Untrusted encoder module on ");
  78. SERIAL_ECHO(axis_codes[encoderAxis]);
  79. SERIAL_ECHOLNPGM(" axis has been fault-free for set duration, reinstating error correction.");
  80. //the encoder likely lost its place when the error occured, so we'll reset and use the printer's
  81. //idea of where it the axis is to re-initialise
  82. double position = stepper.get_axis_position_mm(encoderAxis);
  83. long positionInTicks = position * get_ticks_unit();
  84. //shift position from previous to current position
  85. zeroOffset -= (positionInTicks - get_position());
  86. #if defined(I2CPE_DEBUG)
  87. SERIAL_ECHOPGM("Current position is ");
  88. SERIAL_ECHOLN(position);
  89. SERIAL_ECHOPGM("Position in encoder ticks is ");
  90. SERIAL_ECHOLN(positionInTicks);
  91. SERIAL_ECHOPGM("New zero-offset of ");
  92. SERIAL_ECHOLN(zeroOffset);
  93. SERIAL_ECHOPGM("New position reads as ");
  94. SERIAL_ECHO(get_position());
  95. SERIAL_ECHOPGM("(");
  96. SERIAL_ECHO(mm_from_count(get_position()));
  97. SERIAL_ECHOLNPGM(")");
  98. #endif
  99. }
  100. */
  101. return;
  102. }
  103. lastPosition = position;
  104. unsigned long positionTime = millis();
  105. //only do error correction if setup and enabled
  106. if (ec && ecMethod != I2CPE_ECM_NONE) {
  107. #if defined(I2CPE_EC_THRESH_PROPORTIONAL)
  108. unsigned long distance = abs(position - lastPosition);
  109. unsigned long deltaTime = positionTime - lastPositionTime;
  110. unsigned long speed = distance / deltaTime;
  111. float threshold = constrain((speed / 50), 1, 50) * ecThreshold;
  112. #else
  113. float threshold = get_error_correct_threshold();
  114. #endif
  115. //check error
  116. #if ENABLED(I2CPE_ERR_ROLLING_AVERAGE)
  117. double sum = 0, diffSum = 0;
  118. errIdx = (errIdx >= I2CPE_ERR_ARRAY_SIZE - 1) ? 0 : errIdx + 1;
  119. err[errIdx] = get_axis_error_steps(false);
  120. LOOP_L_N(i, I2CPE_ERR_ARRAY_SIZE) {
  121. sum += err[i];
  122. if (i) diffSum += abs(err[i-1] - err[i]);
  123. }
  124. long error = (long)(sum/(I2CPE_ERR_ARRAY_SIZE + 1)); //calculate average for error
  125. #else
  126. long error = get_axis_error_steps(false);
  127. #endif
  128. //SERIAL_ECHOPGM("Axis err*r steps: ");
  129. //SERIAL_ECHOLN(error);
  130. #if defined(I2CPE_ERR_THRESH_ABORT)
  131. if (abs(error) > I2CPE_ERR_THRESH_ABORT * planner.axis_steps_per_mm[encoderAxis]) {
  132. //kill("Significant Error");
  133. SERIAL_ECHOPGM("Axis error greater than set threshold, aborting!");
  134. SERIAL_ECHOLN(error);
  135. safe_delay(5000);
  136. }
  137. #endif
  138. #if ENABLED(I2CPE_ERR_ROLLING_AVERAGE)
  139. if (errIdx == 0) {
  140. // in order to correct for "error" but avoid correcting for noise and non skips
  141. // it must be > threshold and have a difference average of < 10 and be < 2000 steps
  142. if (abs(error) > threshold * planner.axis_steps_per_mm[encoderAxis] &&
  143. diffSum < 10*(I2CPE_ERR_ARRAY_SIZE-1) && abs(error) < 2000) { //Check for persistent error (skip)
  144. SERIAL_ECHO(axis_codes[encoderAxis]);
  145. SERIAL_ECHOPAIR(" diffSum: ", diffSum/(I2CPE_ERR_ARRAY_SIZE-1));
  146. SERIAL_ECHOPAIR(" - err detected: ", error / planner.axis_steps_per_mm[encoderAxis]);
  147. SERIAL_ECHOLNPGM("mm; correcting!");
  148. thermalManager.babystepsTodo[encoderAxis] = -lround(error);
  149. }
  150. }
  151. #else
  152. if (abs(error) > threshold * planner.axis_steps_per_mm[encoderAxis]) {
  153. //SERIAL_ECHOLN(error);
  154. //SERIAL_ECHOLN(position);
  155. thermalManager.babystepsTodo[encoderAxis] = -lround(error/2);
  156. }
  157. #endif
  158. if (abs(error) > (I2CPE_ERR_CNT_THRESH * planner.axis_steps_per_mm[encoderAxis]) && millis() - lastErrorCountTime > I2CPE_ERR_CNT_DEBOUNCE_MS) {
  159. SERIAL_ECHOPAIR("Large error on ", axis_codes[encoderAxis]);
  160. SERIAL_ECHOPAIR(" axis. error: ", (int)error);
  161. SERIAL_ECHOLNPAIR("; diffSum: ", diffSum);
  162. errorCount++;
  163. lastErrorCountTime = millis();
  164. }
  165. }
  166. lastPositionTime = positionTime;
  167. }
  168. void I2CPositionEncoder::set_homed() {
  169. if (active) {
  170. reset(); // Reset module's offset to zero (so current position is homed / zero)
  171. delay(10);
  172. zeroOffset = get_raw_count();
  173. homed++;
  174. trusted++;
  175. #if defined(I2CPE_DEBUG)
  176. SERIAL_ECHO(axis_codes[encoderAxis]);
  177. SERIAL_ECHOPAIR(" axis encoder homed, offset of ", zeroOffset);
  178. SERIAL_ECHOLNPGM(" ticks.");
  179. #endif
  180. }
  181. }
  182. bool I2CPositionEncoder::passes_test(bool report) {
  183. if (H == I2CPE_MAG_SIG_GOOD) {
  184. if (report) {
  185. SERIAL_ECHO(axis_codes[encoderAxis]);
  186. SERIAL_ECHOLNPGM(" axis encoder passes test; field strength good.");
  187. }
  188. return true;
  189. } else if (H == I2CPE_MAG_SIG_MID) {
  190. if (report) {
  191. SERIAL_ECHOPAIR("Warning, ", axis_codes[encoderAxis]);
  192. SERIAL_ECHOLNPGM(" axis encoder passes test; field strength fair.");
  193. }
  194. return true;
  195. } else if (H == I2CPE_MAG_SIG_BAD) {
  196. if (report) {
  197. SERIAL_ECHOPAIR("Warning, ", axis_codes[encoderAxis]);
  198. SERIAL_ECHOLNPGM(" axis magnetic strip not detected!");
  199. }
  200. return false;
  201. }
  202. if (report) {
  203. SERIAL_ECHOPAIR("Warning, ", axis_codes[encoderAxis]);
  204. SERIAL_ECHOLNPGM(" axis encoder not detected!");
  205. }
  206. return false;
  207. }
  208. double I2CPositionEncoder::get_axis_error_mm(bool report) {
  209. double target, actual, error;
  210. target = stepper.get_axis_position_mm(encoderAxis);
  211. actual = mm_from_count(position);
  212. error = actual - target;
  213. if (abs(error) > 10000) error = 0; // ?
  214. if (report) {
  215. SERIAL_ECHO(axis_codes[encoderAxis]);
  216. SERIAL_ECHOPAIR(" axis target: ", target);
  217. SERIAL_ECHOPAIR(", actual: ", actual);
  218. SERIAL_ECHOLNPAIR(", error : ",error);
  219. }
  220. return error;
  221. }
  222. long I2CPositionEncoder::get_axis_error_steps(bool report) {
  223. if (!active) {
  224. if (report) {
  225. SERIAL_ECHO(axis_codes[encoderAxis]);
  226. SERIAL_ECHOLNPGM(" axis encoder not active!");
  227. }
  228. return 0;
  229. }
  230. float stepperTicksPerUnit;
  231. long encoderTicks = position, encoderCountInStepperTicksScaled;
  232. //long stepperTicks = stepper.position(encoderAxis);
  233. // With a rotary encoder we're concerned with ticks/rev; whereas with a linear we're concerned with ticks/mm
  234. stepperTicksPerUnit = (type == I2CPE_ENC_TYPE_ROTARY) ? stepperTicks : planner.axis_steps_per_mm[encoderAxis];
  235. //convert both 'ticks' into same units / base
  236. encoderCountInStepperTicksScaled = lround((stepperTicksPerUnit * encoderTicks) / encoderTicksPerUnit);
  237. long target = stepper.position(encoderAxis),
  238. error = (encoderCountInStepperTicksScaled - target);
  239. //suppress discontinuities (might be caused by bad I2C readings...?)
  240. bool suppressOutput = (abs(error - errorPrev) > 100);
  241. if (report) {
  242. SERIAL_ECHO(axis_codes[encoderAxis]);
  243. SERIAL_ECHOPAIR(" axis target: ", target);
  244. SERIAL_ECHOPAIR(", actual: ", encoderCountInStepperTicksScaled);
  245. SERIAL_ECHOLNPAIR(", error : ", error);
  246. if (suppressOutput) SERIAL_ECHOLNPGM("Discontinuity detected, suppressing error.");
  247. }
  248. errorPrev = error;
  249. return (suppressOutput ? 0 : error);
  250. }
  251. long I2CPositionEncoder::get_raw_count() {
  252. uint8_t index = 0;
  253. i2cLong encoderCount;
  254. encoderCount.val = 0x00;
  255. if (Wire.requestFrom((int)i2cAddress, 3) != 3) {
  256. //houston, we have a problem...
  257. H = I2CPE_MAG_SIG_NF;
  258. return 0;
  259. }
  260. while (Wire.available())
  261. encoderCount.bval[index++] = (uint8_t)Wire.read();
  262. //extract the magnetic strength
  263. H = (B00000011 & (encoderCount.bval[2] >> 6));
  264. //extract sign bit; sign = (encoderCount.bval[2] & B00100000);
  265. //set all upper bits to the sign value to overwrite H
  266. encoderCount.val = (encoderCount.bval[2] & B00100000) ? (encoderCount.val | 0xFFC00000) : (encoderCount.val & 0x003FFFFF);
  267. if (invert) encoderCount.val *= -1;
  268. return encoderCount.val;
  269. }
  270. bool I2CPositionEncoder::test_axis() {
  271. //only works on XYZ cartesian machines for the time being
  272. if (!(encoderAxis == X_AXIS || encoderAxis == Y_AXIS || encoderAxis == Z_AXIS)) return false;
  273. int feedrate;
  274. float startPosition, endPosition;
  275. float startCoord[NUM_AXIS] = {0}, endCoord[NUM_AXIS] = {0};
  276. startPosition = soft_endstop_min[encoderAxis] + 10;
  277. endPosition = soft_endstop_max[encoderAxis] - 10;
  278. feedrate = (int)MMM_TO_MMS((encoderAxis == Z_AXIS) ? HOMING_FEEDRATE_Z : HOMING_FEEDRATE_XY);
  279. ec = false;
  280. LOOP_NA(i) {
  281. startCoord[i] = stepper.get_axis_position_mm((AxisEnum)i);
  282. endCoord[i] = stepper.get_axis_position_mm((AxisEnum)i);
  283. }
  284. startCoord[encoderAxis] = startPosition;
  285. endCoord[encoderAxis] = endPosition;
  286. stepper.synchronize();
  287. planner.buffer_line(startCoord[X_AXIS],startCoord[Y_AXIS],startCoord[Z_AXIS],
  288. stepper.get_axis_position_mm(E_AXIS), feedrate, 0);
  289. stepper.synchronize();
  290. // if the module isn't currently trusted, wait until it is (or until it should be if things are working)
  291. if (!trusted) {
  292. long startWaitingTime = millis();
  293. while (!trusted && millis() - startWaitingTime < I2CPE_TIME_TRUSTED)
  294. safe_delay(500);
  295. }
  296. if (trusted) { // if trusted, commence test
  297. planner.buffer_line(endCoord[X_AXIS], endCoord[Y_AXIS], endCoord[Z_AXIS],
  298. stepper.get_axis_position_mm(E_AXIS), feedrate, 0);
  299. stepper.synchronize();
  300. }
  301. return trusted;
  302. }
  303. void I2CPositionEncoder::calibrate_steps_mm(int iter) {
  304. if (type != I2CPE_ENC_TYPE_LINEAR) {
  305. SERIAL_ECHOLNPGM("Steps per mm calibration is only available using linear encoders.");
  306. return;
  307. }
  308. if (!(encoderAxis == X_AXIS || encoderAxis == Y_AXIS || encoderAxis == Z_AXIS)) {
  309. SERIAL_ECHOLNPGM("Automatic steps / mm calibration not supported for this axis.");
  310. return;
  311. }
  312. float oldStepsMm, newStepsMm,
  313. startDistance, endDistance,
  314. travelDistance, travelledDistance, total = 0,
  315. startCoord[NUM_AXIS] = {0}, endCoord[NUM_AXIS] = {0};
  316. double feedrate;
  317. long startCount, stopCount;
  318. feedrate = MMM_TO_MMS((encoderAxis == Z_AXIS) ? HOMING_FEEDRATE_Z : HOMING_FEEDRATE_XY);
  319. bool oldec = ec;
  320. ec = false;
  321. startDistance = 20;
  322. endDistance = soft_endstop_max[encoderAxis] - 20;
  323. travelDistance = endDistance - startDistance;
  324. LOOP_NA(i) {
  325. startCoord[i] = stepper.get_axis_position_mm((AxisEnum)i);
  326. endCoord[i] = stepper.get_axis_position_mm((AxisEnum)i);
  327. }
  328. startCoord[encoderAxis] = startDistance;
  329. endCoord[encoderAxis] = endDistance;
  330. LOOP_L_N(i, iter) {
  331. stepper.synchronize();
  332. planner.buffer_line(startCoord[X_AXIS],startCoord[Y_AXIS],startCoord[Z_AXIS],
  333. stepper.get_axis_position_mm(E_AXIS), feedrate, 0);
  334. stepper.synchronize();
  335. delay(250);
  336. startCount = get_position();
  337. //do_blocking_move_to(endCoord[X_AXIS],endCoord[Y_AXIS],endCoord[Z_AXIS]);
  338. planner.buffer_line(endCoord[X_AXIS],endCoord[Y_AXIS],endCoord[Z_AXIS],
  339. stepper.get_axis_position_mm(E_AXIS), feedrate, 0);
  340. stepper.synchronize();
  341. //Read encoder distance
  342. delay(250);
  343. stopCount = get_position();
  344. travelledDistance = mm_from_count(abs(stopCount - startCount));
  345. SERIAL_ECHOPAIR("Attempted to travel: ", travelDistance);
  346. SERIAL_ECHOLNPGM("mm.");
  347. SERIAL_ECHOPAIR("Actually travelled: ", travelledDistance);
  348. SERIAL_ECHOLNPGM("mm.");
  349. //Calculate new axis steps per unit
  350. oldStepsMm = planner.axis_steps_per_mm[encoderAxis];
  351. newStepsMm = (oldStepsMm * travelDistance) / travelledDistance;
  352. SERIAL_ECHOLNPAIR("Old steps per mm: ", oldStepsMm);
  353. SERIAL_ECHOLNPAIR("New steps per mm: ", newStepsMm);
  354. //Save new value
  355. planner.axis_steps_per_mm[encoderAxis] = newStepsMm;
  356. if (iter > 1) {
  357. total += newStepsMm;
  358. // swap start and end points so next loop runs from current position
  359. float tempCoord = startCoord[encoderAxis];
  360. startCoord[encoderAxis] = endCoord[encoderAxis];
  361. endCoord[encoderAxis] = tempCoord;
  362. }
  363. }
  364. if (iter > 1) {
  365. total /= (float)iter;
  366. SERIAL_ECHOLNPAIR("Average steps per mm: ", total);
  367. }
  368. ec = oldec;
  369. SERIAL_ECHOLNPGM("Calculated steps per mm has been set. Please save to EEPROM (M500) if you wish to keep these values.");
  370. }
  371. void I2CPositionEncoder::reset() {
  372. Wire.beginTransmission(i2cAddress);
  373. Wire.write(I2CPE_RESET_COUNT);
  374. Wire.endTransmission();
  375. #if ENABLED(I2CPE_ERR_ROLLING_AVERAGE)
  376. ZERO(err);
  377. #endif
  378. }
  379. void I2CPositionEncodersMgr::init() {
  380. Wire.begin();
  381. #if I2CPE_ENCODER_CNT > 0
  382. uint8_t i = 0;
  383. encoders[i].init(I2CPE_ENC_1_ADDR, I2CPE_ENC_1_AXIS);
  384. #if defined(I2CPE_ENC_1_TYPE)
  385. encoders[i].set_type(I2CPE_ENC_1_TYPE);
  386. #endif
  387. #if defined(I2CPE_ENC_1_TICKS_UNIT)
  388. encoders[i].set_ticks_unit(I2CPE_ENC_1_TICKS_UNIT);
  389. #endif
  390. #if defined(I2CPE_ENC_1_TICKS_REV)
  391. encoders[i].set_stepper_ticks(I2CPE_ENC_1_TICKS_REV);
  392. #endif
  393. #if defined(I2CPE_ENC_1_INVERT)
  394. encoders[i].set_inverted(I2CPE_ENC_1_INVERT);
  395. #endif
  396. #if defined(I2CPE_ENC_1_EC_METHOD)
  397. encoders[i].set_ec_method(I2CPE_ENC_1_EC_METHOD);
  398. #endif
  399. #if defined(I2CPE_ENC_1_EC_THRESH)
  400. encoders[i].set_ec_threshold(I2CPE_ENC_1_EC_THRESH);
  401. #endif
  402. encoders[i].set_active(encoders[i].passes_test(true));
  403. #if (I2CPE_ENC_1_AXIS == E_AXIS)
  404. encoders[i].set_homed();
  405. #endif
  406. #endif
  407. #if I2CPE_ENCODER_CNT > 1
  408. i++;
  409. encoders[i].init(I2CPE_ENC_2_ADDR, I2CPE_ENC_2_AXIS);
  410. #if defined(I2CPE_ENC_2_TYPE)
  411. encoders[i].set_type(I2CPE_ENC_2_TYPE);
  412. #endif
  413. #if defined(I2CPE_ENC_2_TICKS_UNIT)
  414. encoders[i].set_ticks_unit(I2CPE_ENC_2_TICKS_UNIT);
  415. #endif
  416. #if defined(I2CPE_ENC_2_TICKS_REV)
  417. encoders[i].set_stepper_ticks(I2CPE_ENC_2_TICKS_REV);
  418. #endif
  419. #if defined(I2CPE_ENC_2_INVERT)
  420. encoders[i].set_inverted(I2CPE_ENC_2_INVERT);
  421. #endif
  422. #if defined(I2CPE_ENC_2_EC_METHOD)
  423. encoders[i].set_ec_method(I2CPE_ENC_2_EC_METHOD);
  424. #endif
  425. #if defined(I2CPE_ENC_2_EC_THRESH)
  426. encoders[i].set_ec_threshold(I2CPE_ENC_2_EC_THRESH);
  427. #endif
  428. encoders[i].set_active(encoders[i].passes_test(true));
  429. #if (I2CPE_ENC_2_AXIS == E_AXIS)
  430. encoders[i].set_homed();
  431. #endif
  432. #endif
  433. #if I2CPE_ENCODER_CNT > 2
  434. i++;
  435. encoders[i].init(I2CPE_ENC_3_ADDR, I2CPE_ENC_3_AXIS);
  436. #if defined(I2CPE_ENC_3_TYPE)
  437. encoders[i].set_type(I2CPE_ENC_3_TYPE);
  438. #endif
  439. #if defined(I2CPE_ENC_3_TICKS_UNIT)
  440. encoders[i].set_ticks_unit(I2CPE_ENC_3_TICKS_UNIT);
  441. #endif
  442. #if defined(I2CPE_ENC_3_TICKS_REV)
  443. encoders[i].set_stepper_ticks(I2CPE_ENC_3_TICKS_REV);
  444. #endif
  445. #if defined(I2CPE_ENC_3_INVERT)
  446. encoders[i].set_inverted(I2CPE_ENC_3_INVERT);
  447. #endif
  448. #if defined(I2CPE_ENC_3_EC_METHOD)
  449. encoders[i].set_ec_method(I2CPE_ENC_3_EC_METHOD);
  450. #endif
  451. #if defined(I2CPE_ENC_3_EC_THRESH)
  452. encoders[i].set_ec_threshold(I2CPE_ENC_3_EC_THRESH);
  453. #endif
  454. encoders[i].set_active(encoders[i].passes_test(true));
  455. #if (I2CPE_ENC_3_AXIS == E_AXIS)
  456. encoders[i].set_homed();
  457. #endif
  458. #endif
  459. #if I2CPE_ENCODER_CNT > 3
  460. i++;
  461. encoders[i].init(I2CPE_ENC_4_ADDR, I2CPE_ENC_4_AXIS);
  462. #if defined(I2CPE_ENC_4_TYPE)
  463. encoders[i].set_type(I2CPE_ENC_4_TYPE);
  464. #endif
  465. #if defined(I2CPE_ENC_4_TICKS_UNIT)
  466. encoders[i].set_ticks_unit(I2CPE_ENC_4_TICKS_UNIT);
  467. #endif
  468. #if defined(I2CPE_ENC_4_TICKS_REV)
  469. encoders[i].set_stepper_ticks(I2CPE_ENC_4_TICKS_REV);
  470. #endif
  471. #if defined(I2CPE_ENC_4_INVERT)
  472. encoders[i].set_inverted(I2CPE_ENC_4_INVERT);
  473. #endif
  474. #if defined(I2CPE_ENC_4_EC_METHOD)
  475. encoders[i].set_ec_method(I2CPE_ENC_4_EC_METHOD);
  476. #endif
  477. #if defined(I2CPE_ENC_4_EC_THRESH)
  478. encoders[i].set_ec_threshold(I2CPE_ENC_4_EC_THRESH);
  479. #endif
  480. encoders[i].set_active(encoders[i].passes_test(true));
  481. #if (I2CPE_ENC_4_AXIS == E_AXIS)
  482. encoders[i].set_homed();
  483. #endif
  484. #endif
  485. #if I2CPE_ENCODER_CNT > 4
  486. i++;
  487. encoders[i].init(I2CPE_ENC_5_ADDR, I2CPE_ENC_5_AXIS);
  488. #if defined(I2CPE_ENC_5_TYPE)
  489. encoders[i].set_type(I2CPE_ENC_5_TYPE);
  490. #endif
  491. #if defined(I2CPE_ENC_5_TICKS_UNIT)
  492. encoders[i].set_ticks_unit(I2CPE_ENC_5_TICKS_UNIT);
  493. #endif
  494. #if defined(I2CPE_ENC_5_TICKS_REV)
  495. encoders[i].set_stepper_ticks(I2CPE_ENC_5_TICKS_REV);
  496. #endif
  497. #if defined(I2CPE_ENC_5_INVERT)
  498. encoders[i].set_inverted(I2CPE_ENC_5_INVERT);
  499. #endif
  500. #if defined(I2CPE_ENC_5_EC_METHOD)
  501. encoders[i].set_ec_method(I2CPE_ENC_5_EC_METHOD);
  502. #endif
  503. #if defined(I2CPE_ENC_5_EC_THRESH)
  504. encoders[i].set_ec_threshold(I2CPE_ENC_5_EC_THRESH);
  505. #endif
  506. encoders[i].set_active(encoders[i].passes_test(true));
  507. #if (I2CPE_ENC_5_AXIS == E_AXIS)
  508. encoders[i].set_homed();
  509. #endif
  510. #endif
  511. }
  512. void I2CPositionEncodersMgr::report_position(uint8_t idx, bool units, bool noOffset) {
  513. CHECK_IDX
  514. if (units) {
  515. SERIAL_ECHOLN(noOffset ? encoders[idx].mm_from_count(encoders[idx].get_raw_count()) : encoders[idx].get_position_mm());
  516. } else {
  517. if (noOffset) {
  518. long raw_count = encoders[idx].get_raw_count();
  519. SERIAL_ECHO(axis_codes[encoders[idx].get_axis()]);
  520. SERIAL_ECHOPGM(" ");
  521. for (uint8_t j = 31; j > 0; j--)
  522. SERIAL_ECHO((bool)(0x00000001 & (raw_count >> j)));
  523. SERIAL_ECHO((bool)(0x00000001 & (raw_count)));
  524. SERIAL_ECHOLNPAIR(" ", raw_count);
  525. } else
  526. SERIAL_ECHOLN(encoders[idx].get_position());
  527. }
  528. }
  529. void I2CPositionEncodersMgr::change_module_address(uint8_t oldaddr, uint8_t newaddr) {
  530. // First check 'new' address is not in use
  531. Wire.beginTransmission(newaddr);
  532. if (!Wire.endTransmission()) {
  533. SERIAL_ECHOPAIR("?There is already a device with that address on the I2C bus! (", newaddr);
  534. SERIAL_ECHOLNPGM(")");
  535. return;
  536. }
  537. // Now check that we can find the module on the oldaddr address
  538. Wire.beginTransmission(oldaddr);
  539. if (Wire.endTransmission()) {
  540. SERIAL_ECHOPAIR("?No module detected at this address! (", oldaddr);
  541. SERIAL_ECHOLNPGM(")");
  542. return;
  543. }
  544. SERIAL_ECHOPAIR("Module found at ", oldaddr);
  545. SERIAL_ECHOLNPAIR(", changing address to ", newaddr);
  546. // Change the modules address
  547. Wire.beginTransmission(oldaddr);
  548. Wire.write(I2CPE_SET_ADDR);
  549. Wire.write(newaddr);
  550. Wire.endTransmission();
  551. SERIAL_ECHOLNPGM("Address changed, resetting and waiting for confirmation..");
  552. // Wait for the module to reset (can probably be improved by polling address with a timeout).
  553. safe_delay(I2CPE_REBOOT_TIME);
  554. // Look for the module at the new address.
  555. Wire.beginTransmission(newaddr);
  556. if (Wire.endTransmission()) {
  557. SERIAL_ECHOLNPGM("Address change failed! Check encoder module.");
  558. return;
  559. }
  560. SERIAL_ECHOLNPGM("Address change successful!");
  561. // Now, if this module is configured, find which encoder instance it's supposed to correspond to
  562. // and enable it (it will likely have failed initialisation on power-up, before the address change).
  563. int8_t idx = idx_from_addr(newaddr);
  564. if (idx >= 0 && !encoders[idx].get_active()) {
  565. SERIAL_ECHO(axis_codes[encoders[idx].get_axis()]);
  566. SERIAL_ECHOLNPGM(" axis encoder was not detected on printer startup. Trying again.");
  567. encoders[idx].set_active(encoders[idx].passes_test(true));
  568. }
  569. }
  570. void I2CPositionEncodersMgr::report_module_firmware(uint8_t address) {
  571. // First check there is a module
  572. Wire.beginTransmission(address);
  573. if (Wire.endTransmission()) {
  574. SERIAL_ECHOPAIR("?No module detected at this address! (", address);
  575. SERIAL_ECHOLNPGM(")");
  576. return;
  577. }
  578. SERIAL_ECHOPAIR("Requesting version info from module at address ", address);
  579. SERIAL_ECHOPGM(":\n");
  580. Wire.beginTransmission(address);
  581. Wire.write(I2CPE_SET_REPORT_MODE);
  582. Wire.write(I2CPE_REPORT_VERSION);
  583. Wire.endTransmission();
  584. // Read value
  585. if (Wire.requestFrom((int)address, 32)) {
  586. char c;
  587. while (Wire.available() > 0 && (c = (char)Wire.read()) > 0)
  588. SERIAL_ECHO(c);
  589. SERIAL_EOL();
  590. }
  591. // Set module back to normal (distance) mode
  592. Wire.beginTransmission((int)address);
  593. Wire.write(I2CPE_SET_REPORT_MODE);
  594. Wire.write(I2CPE_REPORT_DISTANCE);
  595. Wire.endTransmission();
  596. }
  597. int8_t I2CPositionEncodersMgr::parse() {
  598. I2CPE_addr = 0;
  599. if (parser.seen('A')) {
  600. if (!parser.has_value()) {
  601. SERIAL_PROTOCOLLNPGM("?A seen, but no address specified! [30-200]");
  602. return I2CPE_PARSE_ERR;
  603. };
  604. I2CPE_addr = parser.value_byte();
  605. if (!WITHIN(I2CPE_addr, 30, 200)) { // reserve the first 30 and last 55
  606. SERIAL_PROTOCOLLNPGM("?Address out of range. [30-200]");
  607. return I2CPE_PARSE_ERR;
  608. }
  609. I2CPE_idx = idx_from_addr(I2CPE_addr);
  610. if (!WITHIN(I2CPE_idx, 0, I2CPE_ENCODER_CNT - 1)) {
  611. SERIAL_PROTOCOLLNPGM("?No device with this address!");
  612. return I2CPE_PARSE_ERR;
  613. }
  614. } else if (parser.seenval('I')) {
  615. if (!parser.has_value()) {
  616. SERIAL_PROTOCOLLNPAIR("?I seen, but no index specified! [0-", I2CPE_ENCODER_CNT - 1);
  617. SERIAL_ECHOLNPGM("]");
  618. return I2CPE_PARSE_ERR;
  619. };
  620. I2CPE_idx = parser.value_byte();
  621. if (!WITHIN(I2CPE_idx, 0, I2CPE_ENCODER_CNT - 1)) {
  622. SERIAL_PROTOCOLLNPAIR("?Index out of range. [0-", I2CPE_ENCODER_CNT - 1);
  623. SERIAL_ECHOLNPGM("]");
  624. return I2CPE_PARSE_ERR;
  625. }
  626. I2CPE_addr = encoders[I2CPE_idx].get_address();
  627. } else {
  628. I2CPE_idx = -1;
  629. }
  630. I2CPE_anyaxis = parser.seen_axis();
  631. return I2CPE_PARSE_OK;
  632. };
  633. /**
  634. * M860: Report the position(s) of position encoder module(s).
  635. *
  636. * A<addr> Module I2C address. [30, 200].
  637. * I<index> Module index. [0, I2CPE_ENCODER_CNT - 1]
  638. * O Include homed zero-offset in returned position.
  639. * U Units in mm or raw step count.
  640. *
  641. * If A or I not specified:
  642. * X Report on X axis encoder, if present.
  643. * Y Report on Y axis encoder, if present.
  644. * Z Report on Z axis encoder, if present.
  645. * E Report on E axis encoder, if present.
  646. *
  647. */
  648. void I2CPositionEncodersMgr::M860() {
  649. if (parse()) return;
  650. bool hasU = parser.seen('U'), hasO = parser.seen('O');
  651. if (I2CPE_idx < 0) {
  652. int8_t idx;
  653. LOOP_XYZE(i) {
  654. if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0))
  655. report_position((uint8_t)idx, hasU, hasO);
  656. }
  657. } else report_position((uint8_t)I2CPE_idx, hasU, hasO);
  658. }
  659. /**
  660. * M861: Report the status of position encoder modules.
  661. *
  662. * A<addr> Module I2C address. [30, 200].
  663. * I<index> Module index. [0, I2CPE_ENCODER_CNT - 1]
  664. *
  665. * If A or I not specified:
  666. * X Report on X axis encoder, if present.
  667. * Y Report on Y axis encoder, if present.
  668. * Z Report on Z axis encoder, if present.
  669. * E Report on E axis encoder, if present.
  670. *
  671. */
  672. void I2CPositionEncodersMgr::M861() {
  673. if (parse()) return;
  674. if (I2CPE_idx < 0) {
  675. int8_t idx;
  676. LOOP_XYZE(i) {
  677. if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0))
  678. report_status((uint8_t)idx);
  679. }
  680. } else report_status((uint8_t)I2CPE_idx);
  681. }
  682. /**
  683. * M862: Perform an axis continuity test for position encoder
  684. * modules.
  685. *
  686. * A<addr> Module I2C address. [30, 200].
  687. * I<index> Module index. [0, I2CPE_ENCODER_CNT - 1]
  688. *
  689. * If A or I not specified:
  690. * X Report on X axis encoder, if present.
  691. * Y Report on Y axis encoder, if present.
  692. * Z Report on Z axis encoder, if present.
  693. * E Report on E axis encoder, if present.
  694. *
  695. */
  696. void I2CPositionEncodersMgr::M862() {
  697. if (parse()) return;
  698. if (I2CPE_idx < 0) {
  699. int8_t idx;
  700. LOOP_XYZE(i) {
  701. if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0))
  702. test_axis((uint8_t)idx);
  703. }
  704. } else test_axis((uint8_t)I2CPE_idx);
  705. }
  706. /**
  707. * M863: Perform steps-per-mm calibration for
  708. * position encoder modules.
  709. *
  710. * A<addr> Module I2C address. [30, 200].
  711. * I<index> Module index. [0, I2CPE_ENCODER_CNT - 1]
  712. * P Number of rePeats/iterations.
  713. *
  714. * If A or I not specified:
  715. * X Report on X axis encoder, if present.
  716. * Y Report on Y axis encoder, if present.
  717. * Z Report on Z axis encoder, if present.
  718. * E Report on E axis encoder, if present.
  719. *
  720. */
  721. void I2CPositionEncodersMgr::M863() {
  722. if (parse()) return;
  723. int iterations = parser.seenval('P') ? constrain(parser.value_byte(), 1, 10) : 1;
  724. if (I2CPE_idx < 0) {
  725. int8_t idx;
  726. LOOP_XYZE(i) {
  727. if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0))
  728. calibrate_steps_mm((uint8_t)idx, iterations);
  729. }
  730. } else calibrate_steps_mm((uint8_t)I2CPE_idx, iterations);
  731. }
  732. /**
  733. * M864: Change position encoder module I2C address.
  734. *
  735. * A<addr> Module current/old I2C address. If not present,
  736. * assumes default address (030). [30, 200].
  737. * N<addr> Module new I2C address. [30, 200].
  738. *
  739. * If N not specified:
  740. * X Use I2CPE_PRESET_ADDR_X (030).
  741. * Y Use I2CPE_PRESET_ADDR_Y (031).
  742. * Z Use I2CPE_PRESET_ADDR_Z (032).
  743. * E Use I2CPE_PRESET_ADDR_E (033).
  744. */
  745. void I2CPositionEncodersMgr::M864() {
  746. uint8_t newAddress;
  747. if (parse()) return;
  748. if (!I2CPE_addr) I2CPE_addr = I2CPE_PRESET_ADDR_X;
  749. if (parser.seen('N')) {
  750. if (!parser.has_value()) {
  751. SERIAL_PROTOCOLLNPGM("?N seen, but no address specified! [30-200]");
  752. return;
  753. };
  754. newAddress = parser.value_byte();
  755. if (!WITHIN(newAddress, 30, 200)) {
  756. SERIAL_PROTOCOLLNPGM("?New address out of range. [30-200]");
  757. return;
  758. }
  759. } else if (!I2CPE_anyaxis) {
  760. SERIAL_PROTOCOLLNPGM("?You must specify N or [XYZE].");
  761. return;
  762. } else {
  763. if (parser.seen('X')) newAddress = I2CPE_PRESET_ADDR_X;
  764. else if (parser.seen('Y')) newAddress = I2CPE_PRESET_ADDR_Y;
  765. else if (parser.seen('Z')) newAddress = I2CPE_PRESET_ADDR_Z;
  766. else if (parser.seen('E')) newAddress = I2CPE_PRESET_ADDR_E;
  767. else return;
  768. }
  769. SERIAL_ECHOPAIR("Changing module at address ", I2CPE_addr);
  770. SERIAL_ECHOLNPAIR(" to address ", newAddress);
  771. change_module_address(I2CPE_addr, newAddress);
  772. }
  773. /**
  774. * M865: Check position encoder module firmware version.
  775. *
  776. * A<addr> Module I2C address. [30, 200].
  777. * I<index> Module index. [0, I2CPE_ENCODER_CNT - 1].
  778. *
  779. * If A or I not specified:
  780. * X Check X axis encoder, if present.
  781. * Y Check Y axis encoder, if present.
  782. * Z Check Z axis encoder, if present.
  783. * E Check E axis encoder, if present.
  784. */
  785. void I2CPositionEncodersMgr::M865() {
  786. if (parse()) return;
  787. if (!I2CPE_addr) {
  788. int8_t idx;
  789. LOOP_XYZE(i) {
  790. if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0))
  791. report_module_firmware(encoders[idx].get_address());
  792. }
  793. } else report_module_firmware(I2CPE_addr);
  794. }
  795. /**
  796. * M866: Report or reset position encoder module error
  797. * count.
  798. *
  799. * A<addr> Module I2C address. [30, 200].
  800. * I<index> Module index. [0, I2CPE_ENCODER_CNT - 1].
  801. * R Reset error counter.
  802. *
  803. * If A or I not specified:
  804. * X Act on X axis encoder, if present.
  805. * Y Act on Y axis encoder, if present.
  806. * Z Act on Z axis encoder, if present.
  807. * E Act on E axis encoder, if present.
  808. */
  809. void I2CPositionEncodersMgr::M866() {
  810. if (parse()) return;
  811. bool hasR = parser.seen('R');
  812. if (I2CPE_idx < 0) {
  813. int8_t idx;
  814. LOOP_XYZE(i) {
  815. if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0)) {
  816. if (hasR) reset_error_count((uint8_t)idx, AxisEnum(i));
  817. else report_error_count((uint8_t)idx, AxisEnum(i));
  818. }
  819. }
  820. } else {
  821. if (hasR) reset_error_count((uint8_t)I2CPE_idx, encoders[I2CPE_idx].get_axis());
  822. else report_error_count((uint8_t)I2CPE_idx, encoders[I2CPE_idx].get_axis());
  823. }
  824. }
  825. /**
  826. * M867: Enable/disable or toggle error correction for position encoder modules.
  827. *
  828. * A<addr> Module I2C address. [30, 200].
  829. * I<index> Module index. [0, I2CPE_ENCODER_CNT - 1].
  830. * S<1|0> Enable/disable error correction. 1 enables, 0 disables. If not
  831. * supplied, toggle.
  832. *
  833. * If A or I not specified:
  834. * X Act on X axis encoder, if present.
  835. * Y Act on Y axis encoder, if present.
  836. * Z Act on Z axis encoder, if present.
  837. * E Act on E axis encoder, if present.
  838. */
  839. void I2CPositionEncodersMgr::M867() {
  840. if (parse()) return;
  841. int8_t onoff = parser.seenval('S') ? parser.value_int() : -1;
  842. if (I2CPE_idx < 0) {
  843. int8_t idx;
  844. LOOP_XYZE(i) {
  845. if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0)) {
  846. if (onoff == -1) enable_ec((uint8_t)idx, !encoders[idx].get_ec_enabled(), AxisEnum(i));
  847. else enable_ec((uint8_t)idx, (bool)onoff, AxisEnum(i));
  848. }
  849. }
  850. } else {
  851. if (onoff == -1) enable_ec((uint8_t)I2CPE_idx, !encoders[I2CPE_idx].get_ec_enabled(), encoders[I2CPE_idx].get_axis());
  852. else enable_ec((uint8_t)I2CPE_idx, (bool)onoff, encoders[I2CPE_idx].get_axis());
  853. }
  854. }
  855. /**
  856. * M868: Report or set position encoder module error correction
  857. * threshold.
  858. *
  859. * A<addr> Module I2C address. [30, 200].
  860. * I<index> Module index. [0, I2CPE_ENCODER_CNT - 1].
  861. * T New error correction threshold.
  862. *
  863. * If A not specified:
  864. * X Act on X axis encoder, if present.
  865. * Y Act on Y axis encoder, if present.
  866. * Z Act on Z axis encoder, if present.
  867. * E Act on E axis encoder, if present.
  868. */
  869. void I2CPositionEncodersMgr::M868() {
  870. if (parse()) return;
  871. float newThreshold = parser.seenval('T') ? parser.value_float() : -9999;
  872. if (I2CPE_idx < 0) {
  873. int8_t idx;
  874. LOOP_XYZE(i) {
  875. if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0)) {
  876. if (newThreshold != -9999) set_ec_threshold((uint8_t)idx, newThreshold, encoders[idx].get_axis());
  877. else get_ec_threshold((uint8_t)idx, encoders[idx].get_axis());
  878. }
  879. }
  880. } else {
  881. if (newThreshold != -9999) set_ec_threshold((uint8_t)I2CPE_idx, newThreshold, encoders[I2CPE_idx].get_axis());
  882. else get_ec_threshold((uint8_t)I2CPE_idx, encoders[I2CPE_idx].get_axis());
  883. }
  884. }
  885. /**
  886. * M869: Report position encoder module error.
  887. *
  888. * A<addr> Module I2C address. [30, 200].
  889. * I<index> Module index. [0, I2CPE_ENCODER_CNT - 1].
  890. *
  891. * If A not specified:
  892. * X Act on X axis encoder, if present.
  893. * Y Act on Y axis encoder, if present.
  894. * Z Act on Z axis encoder, if present.
  895. * E Act on E axis encoder, if present.
  896. */
  897. void I2CPositionEncodersMgr::M869() {
  898. if (parse()) return;
  899. if (I2CPE_idx < 0) {
  900. int8_t idx;
  901. LOOP_XYZE(i) {
  902. if ((!I2CPE_anyaxis || parser.seen(axis_codes[i])) && ((idx = idx_from_axis(AxisEnum(i))) >= 0))
  903. report_error((uint8_t)idx);
  904. }
  905. } else report_error((uint8_t)I2CPE_idx);
  906. }
  907. #endif