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.

encoder_i2c.h 11KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320
  1. /**
  2. * Marlin 3D Printer Firmware
  3. * Copyright (c) 2020 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 <https://www.gnu.org/licenses/>.
  20. *
  21. */
  22. #pragma once
  23. #include "../inc/MarlinConfig.h"
  24. #include "../module/planner.h"
  25. #include <Wire.h>
  26. //=========== Advanced / Less-Common Encoder Configuration Settings ==========
  27. #define I2CPE_EC_THRESH_PROPORTIONAL // if enabled adjusts the error correction threshold
  28. // proportional to the current speed of the axis allows
  29. // for very small error margin at low speeds without
  30. // stuttering due to reading latency at high speeds
  31. #define I2CPE_DEBUG // enable encoder-related debug serial echos
  32. #define I2CPE_REBOOT_TIME 5000 // time we wait for an encoder module to reboot
  33. // after changing address.
  34. #define I2CPE_MAG_SIG_GOOD 0
  35. #define I2CPE_MAG_SIG_MID 1
  36. #define I2CPE_MAG_SIG_BAD 2
  37. #define I2CPE_MAG_SIG_NF 255
  38. #define I2CPE_REQ_REPORT 0
  39. #define I2CPE_RESET_COUNT 1
  40. #define I2CPE_SET_ADDR 2
  41. #define I2CPE_SET_REPORT_MODE 3
  42. #define I2CPE_CLEAR_EEPROM 4
  43. #define I2CPE_LED_PAR_MODE 10
  44. #define I2CPE_LED_PAR_BRT 11
  45. #define I2CPE_LED_PAR_RATE 14
  46. #define I2CPE_REPORT_DISTANCE 0
  47. #define I2CPE_REPORT_STRENGTH 1
  48. #define I2CPE_REPORT_VERSION 2
  49. // Default I2C addresses
  50. #define I2CPE_PRESET_ADDR_X 30
  51. #define I2CPE_PRESET_ADDR_Y 31
  52. #define I2CPE_PRESET_ADDR_Z 32
  53. #define I2CPE_PRESET_ADDR_E 33
  54. #define I2CPE_DEF_AXIS X_AXIS
  55. #define I2CPE_DEF_ADDR I2CPE_PRESET_ADDR_X
  56. // Error event counter; tracks how many times there is an error exceeding a certain threshold
  57. #define I2CPE_ERR_CNT_THRESH 3.00
  58. #define I2CPE_ERR_CNT_DEBOUNCE_MS 2000
  59. #if ENABLED(I2CPE_ERR_ROLLING_AVERAGE)
  60. #define I2CPE_ERR_ARRAY_SIZE 32
  61. #define I2CPE_ERR_PRST_ARRAY_SIZE 10
  62. #endif
  63. // Error Correction Methods
  64. #define I2CPE_ECM_NONE 0
  65. #define I2CPE_ECM_MICROSTEP 1
  66. #define I2CPE_ECM_PLANNER 2
  67. #define I2CPE_ECM_STALLDETECT 3
  68. // Encoder types
  69. #define I2CPE_ENC_TYPE_ROTARY 0
  70. #define I2CPE_ENC_TYPE_LINEAR 1
  71. // Parser
  72. #define I2CPE_PARSE_ERR 1
  73. #define I2CPE_PARSE_OK 0
  74. #define LOOP_PE(VAR) LOOP_L_N(VAR, I2CPE_ENCODER_CNT)
  75. #define CHECK_IDX() do{ if (!WITHIN(idx, 0, I2CPE_ENCODER_CNT - 1)) return; }while(0)
  76. typedef union {
  77. volatile int32_t val = 0;
  78. uint8_t bval[4];
  79. } i2cLong;
  80. class I2CPositionEncoder {
  81. private:
  82. AxisEnum encoderAxis = I2CPE_DEF_AXIS;
  83. uint8_t i2cAddress = I2CPE_DEF_ADDR,
  84. ecMethod = I2CPE_DEF_EC_METHOD,
  85. type = I2CPE_DEF_TYPE,
  86. H = I2CPE_MAG_SIG_NF; // Magnetic field strength
  87. int encoderTicksPerUnit = I2CPE_DEF_ENC_TICKS_UNIT,
  88. stepperTicks = I2CPE_DEF_TICKS_REV,
  89. errorCount = 0,
  90. errorPrev = 0;
  91. float ecThreshold = I2CPE_DEF_EC_THRESH;
  92. bool homed = false,
  93. trusted = false,
  94. initialized = false,
  95. active = false,
  96. invert = false,
  97. ec = true;
  98. int32_t zeroOffset = 0,
  99. lastPosition = 0,
  100. position;
  101. millis_t lastPositionTime = 0,
  102. nextErrorCountTime = 0,
  103. lastErrorTime;
  104. #if ENABLED(I2CPE_ERR_ROLLING_AVERAGE)
  105. uint8_t errIdx = 0, errPrstIdx = 0;
  106. int err[I2CPE_ERR_ARRAY_SIZE] = { 0 },
  107. errPrst[I2CPE_ERR_PRST_ARRAY_SIZE] = { 0 };
  108. #endif
  109. public:
  110. void init(const uint8_t address, const AxisEnum axis);
  111. void reset();
  112. void update();
  113. void set_homed();
  114. void set_unhomed();
  115. int32_t get_raw_count();
  116. FORCE_INLINE float mm_from_count(const int32_t count) {
  117. switch (type) {
  118. default: return -1;
  119. case I2CPE_ENC_TYPE_LINEAR:
  120. return count / encoderTicksPerUnit;
  121. case I2CPE_ENC_TYPE_ROTARY:
  122. return (count * stepperTicks) / (encoderTicksPerUnit * planner.settings.axis_steps_per_mm[encoderAxis]);
  123. }
  124. }
  125. FORCE_INLINE float get_position_mm() { return mm_from_count(get_position()); }
  126. FORCE_INLINE int32_t get_position() { return get_raw_count() - zeroOffset; }
  127. int32_t get_axis_error_steps(const bool report);
  128. float get_axis_error_mm(const bool report);
  129. void calibrate_steps_mm(const uint8_t iter);
  130. bool passes_test(const bool report);
  131. bool test_axis();
  132. FORCE_INLINE int get_error_count() { return errorCount; }
  133. FORCE_INLINE void set_error_count(const int newCount) { errorCount = newCount; }
  134. FORCE_INLINE uint8_t get_address() { return i2cAddress; }
  135. FORCE_INLINE void set_address(const uint8_t addr) { i2cAddress = addr; }
  136. FORCE_INLINE bool get_active() { return active; }
  137. FORCE_INLINE void set_active(const bool a) { active = a; }
  138. FORCE_INLINE void set_inverted(const bool i) { invert = i; }
  139. FORCE_INLINE AxisEnum get_axis() { return encoderAxis; }
  140. FORCE_INLINE bool get_ec_enabled() { return ec; }
  141. FORCE_INLINE void set_ec_enabled(const bool enabled) { ec = enabled; }
  142. FORCE_INLINE uint8_t get_ec_method() { return ecMethod; }
  143. FORCE_INLINE void set_ec_method(const byte method) { ecMethod = method; }
  144. FORCE_INLINE float get_ec_threshold() { return ecThreshold; }
  145. FORCE_INLINE void set_ec_threshold(const float newThreshold) { ecThreshold = newThreshold; }
  146. FORCE_INLINE int get_encoder_ticks_mm() {
  147. switch (type) {
  148. default: return 0;
  149. case I2CPE_ENC_TYPE_LINEAR:
  150. return encoderTicksPerUnit;
  151. case I2CPE_ENC_TYPE_ROTARY:
  152. return (int)((encoderTicksPerUnit / stepperTicks) * planner.settings.axis_steps_per_mm[encoderAxis]);
  153. }
  154. }
  155. FORCE_INLINE int get_ticks_unit() { return encoderTicksPerUnit; }
  156. FORCE_INLINE void set_ticks_unit(const int ticks) { encoderTicksPerUnit = ticks; }
  157. FORCE_INLINE uint8_t get_type() { return type; }
  158. FORCE_INLINE void set_type(const byte newType) { type = newType; }
  159. FORCE_INLINE int get_stepper_ticks() { return stepperTicks; }
  160. FORCE_INLINE void set_stepper_ticks(const int ticks) { stepperTicks = ticks; }
  161. };
  162. class I2CPositionEncodersMgr {
  163. private:
  164. static bool I2CPE_anyaxis;
  165. static uint8_t I2CPE_addr, I2CPE_idx;
  166. public:
  167. static void init();
  168. // consider only updating one endoder per call / tick if encoders become too time intensive
  169. static void update() { LOOP_PE(i) encoders[i].update(); }
  170. static void homed(const AxisEnum axis) {
  171. LOOP_PE(i)
  172. if (encoders[i].get_axis() == axis) encoders[i].set_homed();
  173. }
  174. static void unhomed(const AxisEnum axis) {
  175. LOOP_PE(i)
  176. if (encoders[i].get_axis() == axis) encoders[i].set_unhomed();
  177. }
  178. static void report_position(const int8_t idx, const bool units, const bool noOffset);
  179. static void report_status(const int8_t idx) {
  180. CHECK_IDX();
  181. SERIAL_ECHOLNPAIR("Encoder ", idx, ": ");
  182. encoders[idx].get_raw_count();
  183. encoders[idx].passes_test(true);
  184. }
  185. static void report_error(const int8_t idx) {
  186. CHECK_IDX();
  187. encoders[idx].get_axis_error_steps(true);
  188. }
  189. static void test_axis(const int8_t idx) {
  190. CHECK_IDX();
  191. encoders[idx].test_axis();
  192. }
  193. static void calibrate_steps_mm(const int8_t idx, const int iterations) {
  194. CHECK_IDX();
  195. encoders[idx].calibrate_steps_mm(iterations);
  196. }
  197. static void change_module_address(const uint8_t oldaddr, const uint8_t newaddr);
  198. static void report_module_firmware(const uint8_t address);
  199. static void report_error_count(const int8_t idx, const AxisEnum axis) {
  200. CHECK_IDX();
  201. SERIAL_ECHOLNPAIR("Error count on ", axis_codes[axis], " axis is ", encoders[idx].get_error_count());
  202. }
  203. static void reset_error_count(const int8_t idx, const AxisEnum axis) {
  204. CHECK_IDX();
  205. encoders[idx].set_error_count(0);
  206. SERIAL_ECHOLNPAIR("Error count on ", axis_codes[axis], " axis has been reset.");
  207. }
  208. static void enable_ec(const int8_t idx, const bool enabled, const AxisEnum axis) {
  209. CHECK_IDX();
  210. encoders[idx].set_ec_enabled(enabled);
  211. SERIAL_ECHOPAIR("Error correction on ", axis_codes[axis]);
  212. SERIAL_ECHO_TERNARY(encoders[idx].get_ec_enabled(), " axis is ", "en", "dis", "abled.\n");
  213. }
  214. static void set_ec_threshold(const int8_t idx, const float newThreshold, const AxisEnum axis) {
  215. CHECK_IDX();
  216. encoders[idx].set_ec_threshold(newThreshold);
  217. SERIAL_ECHOLNPAIR("Error correct threshold for ", axis_codes[axis], " axis set to ", newThreshold, "mm.");
  218. }
  219. static void get_ec_threshold(const int8_t idx, const AxisEnum axis) {
  220. CHECK_IDX();
  221. const float threshold = encoders[idx].get_ec_threshold();
  222. SERIAL_ECHOLNPAIR("Error correct threshold for ", axis_codes[axis], " axis is ", threshold, "mm.");
  223. }
  224. static int8_t idx_from_axis(const AxisEnum axis) {
  225. LOOP_PE(i)
  226. if (encoders[i].get_axis() == axis) return i;
  227. return -1;
  228. }
  229. static int8_t idx_from_addr(const uint8_t addr) {
  230. LOOP_PE(i)
  231. if (encoders[i].get_address() == addr) return i;
  232. return -1;
  233. }
  234. static int8_t parse();
  235. static void M860();
  236. static void M861();
  237. static void M862();
  238. static void M863();
  239. static void M864();
  240. static void M865();
  241. static void M866();
  242. static void M867();
  243. static void M868();
  244. static void M869();
  245. static I2CPositionEncoder encoders[I2CPE_ENCODER_CNT];
  246. };
  247. extern I2CPositionEncodersMgr I2CPEM;