My Marlin configs for Fabrikator Mini and CTC i3 Pro B
您最多选择25个主题 主题必须以字母或数字开头,可以包含连字符 (-),并且长度不得超过35个字符

I2CPositionEncoder.h 12KB

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