My Marlin configs for Fabrikator Mini and CTC i3 Pro B
Ви не можете вибрати більше 25 тем Теми мають розпочинатися з літери або цифри, можуть містити дефіси (-) і не повинні перевищувати 35 символів.

I2CPositionEncoder.h 12KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359
  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() do{ if (!WITHIN(idx, 0, I2CPE_ENCODER_CNT - 1)) return; }while(0)
  79. extern const char axis_codes[XYZE];
  80. typedef union {
  81. volatile int32_t 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. errorCount = 0,
  94. errorPrev = 0;
  95. float ecThreshold = I2CPE_DEF_EC_THRESH;
  96. bool homed = false,
  97. trusted = false,
  98. initialised = false,
  99. active = false,
  100. invert = false,
  101. ec = true;
  102. float axisOffset = 0;
  103. int32_t axisOffsetTicks = 0,
  104. zeroOffset = 0,
  105. lastPosition = 0,
  106. position;
  107. millis_t lastPositionTime = 0,
  108. nextErrorCountTime = 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. //float positionMm; //calculate
  116. public:
  117. void init(const uint8_t address, const AxisEnum axis);
  118. void reset();
  119. void update();
  120. void set_homed();
  121. int32_t get_raw_count();
  122. FORCE_INLINE float mm_from_count(const int32_t count) {
  123. switch (type) {
  124. default: return -1;
  125. case I2CPE_ENC_TYPE_LINEAR:
  126. return count / encoderTicksPerUnit;
  127. case I2CPE_ENC_TYPE_ROTARY:
  128. return (count * stepperTicks) / (encoderTicksPerUnit * planner.axis_steps_per_mm[encoderAxis]);
  129. }
  130. }
  131. FORCE_INLINE float get_position_mm() { return mm_from_count(get_position()); }
  132. FORCE_INLINE int32_t get_position() { return get_raw_count() - zeroOffset - axisOffsetTicks; }
  133. int32_t get_axis_error_steps(const bool report);
  134. float get_axis_error_mm(const bool report);
  135. void calibrate_steps_mm(const uint8_t iter);
  136. bool passes_test(const bool report);
  137. bool test_axis(void);
  138. FORCE_INLINE int get_error_count(void) { return errorCount; }
  139. FORCE_INLINE void set_error_count(const int newCount) { errorCount = newCount; }
  140. FORCE_INLINE uint8_t get_address() { return i2cAddress; }
  141. FORCE_INLINE void set_address(const uint8_t addr) { i2cAddress = addr; }
  142. FORCE_INLINE bool get_active(void) { return active; }
  143. FORCE_INLINE void set_active(const bool a) { active = a; }
  144. FORCE_INLINE void set_inverted(const bool i) { invert = i; }
  145. FORCE_INLINE AxisEnum get_axis() { return encoderAxis; }
  146. FORCE_INLINE bool get_ec_enabled() { return ec; }
  147. FORCE_INLINE void set_ec_enabled(const bool enabled) { ec = enabled; }
  148. FORCE_INLINE uint8_t get_ec_method() { return ecMethod; }
  149. FORCE_INLINE void set_ec_method(const byte method) { ecMethod = method; }
  150. FORCE_INLINE float get_ec_threshold() { return ecThreshold; }
  151. FORCE_INLINE void set_ec_threshold(const float newThreshold) { ecThreshold = newThreshold; }
  152. FORCE_INLINE int get_encoder_ticks_mm() {
  153. switch (type) {
  154. default: return 0;
  155. case I2CPE_ENC_TYPE_LINEAR:
  156. return encoderTicksPerUnit;
  157. case I2CPE_ENC_TYPE_ROTARY:
  158. return (int)((encoderTicksPerUnit / stepperTicks) * planner.axis_steps_per_mm[encoderAxis]);
  159. }
  160. }
  161. FORCE_INLINE int get_ticks_unit() { return encoderTicksPerUnit; }
  162. FORCE_INLINE void set_ticks_unit(const int ticks) { encoderTicksPerUnit = ticks; }
  163. FORCE_INLINE uint8_t get_type() { return type; }
  164. FORCE_INLINE void set_type(const byte newType) { type = newType; }
  165. FORCE_INLINE int get_stepper_ticks() { return stepperTicks; }
  166. FORCE_INLINE void set_stepper_ticks(const int ticks) { stepperTicks = ticks; }
  167. FORCE_INLINE float get_axis_offset() { return axisOffset; }
  168. FORCE_INLINE void set_axis_offset(const float newOffset) {
  169. axisOffset = newOffset;
  170. axisOffsetTicks = int32_t(axisOffset * get_encoder_ticks_mm());
  171. }
  172. FORCE_INLINE void set_current_position(const float newPositionMm) {
  173. set_axis_offset(get_position_mm() - newPositionMm + axisOffset);
  174. }
  175. };
  176. class I2CPositionEncodersMgr {
  177. private:
  178. static bool I2CPE_anyaxis;
  179. static uint8_t I2CPE_addr, I2CPE_idx;
  180. public:
  181. static void init(void);
  182. // consider only updating one endoder per call / tick if encoders become too time intensive
  183. static void update(void) { LOOP_PE(i) encoders[i].update(); }
  184. static void homed(const AxisEnum axis) {
  185. LOOP_PE(i)
  186. if (encoders[i].get_axis() == axis) encoders[i].set_homed();
  187. }
  188. static void report_position(const int8_t idx, const bool units, const bool noOffset);
  189. static void report_status(const int8_t idx) {
  190. CHECK_IDX();
  191. SERIAL_ECHOPAIR("Encoder ",idx);
  192. SERIAL_ECHOPGM(": ");
  193. encoders[idx].get_raw_count();
  194. encoders[idx].passes_test(true);
  195. }
  196. static void report_error(const int8_t idx) {
  197. CHECK_IDX();
  198. encoders[idx].get_axis_error_steps(true);
  199. }
  200. static void test_axis(const int8_t idx) {
  201. CHECK_IDX();
  202. encoders[idx].test_axis();
  203. }
  204. static void calibrate_steps_mm(const int8_t idx, const int iterations) {
  205. CHECK_IDX();
  206. encoders[idx].calibrate_steps_mm(iterations);
  207. }
  208. static void change_module_address(const uint8_t oldaddr, const uint8_t newaddr);
  209. static void report_module_firmware(const uint8_t address);
  210. static void report_error_count(const int8_t idx, const AxisEnum axis) {
  211. CHECK_IDX();
  212. SERIAL_ECHOPAIR("Error count on ", axis_codes[axis]);
  213. SERIAL_ECHOLNPAIR(" axis is ", encoders[idx].get_error_count());
  214. }
  215. static void reset_error_count(const int8_t idx, const AxisEnum axis) {
  216. CHECK_IDX();
  217. encoders[idx].set_error_count(0);
  218. SERIAL_ECHOPAIR("Error count on ", axis_codes[axis]);
  219. SERIAL_ECHOLNPGM(" axis has been reset.");
  220. }
  221. static void enable_ec(const int8_t idx, const bool enabled, const AxisEnum axis) {
  222. CHECK_IDX();
  223. encoders[idx].set_ec_enabled(enabled);
  224. SERIAL_ECHOPAIR("Error correction on ", axis_codes[axis]);
  225. SERIAL_ECHOPGM(" axis is ");
  226. serialprintPGM(encoders[idx].get_ec_enabled() ? PSTR("en") : PSTR("dis"));
  227. SERIAL_ECHOLNPGM("abled.");
  228. }
  229. static void set_ec_threshold(const int8_t idx, const float newThreshold, const AxisEnum axis) {
  230. CHECK_IDX();
  231. encoders[idx].set_ec_threshold(newThreshold);
  232. SERIAL_ECHOPAIR("Error correct threshold for ", axis_codes[axis]);
  233. SERIAL_ECHOPAIR_F(" axis set to ", newThreshold);
  234. SERIAL_ECHOLNPGM("mm.");
  235. }
  236. static void get_ec_threshold(const int8_t idx, const AxisEnum axis) {
  237. CHECK_IDX();
  238. const float threshold = encoders[idx].get_ec_threshold();
  239. SERIAL_ECHOPAIR("Error correct threshold for ", axis_codes[axis]);
  240. SERIAL_ECHOPAIR_F(" axis is ", threshold);
  241. SERIAL_ECHOLNPGM("mm.");
  242. }
  243. static int8_t idx_from_axis(const AxisEnum axis) {
  244. LOOP_PE(i)
  245. if (encoders[i].get_axis() == axis) return i;
  246. return -1;
  247. }
  248. static int8_t idx_from_addr(const uint8_t addr) {
  249. LOOP_PE(i)
  250. if (encoders[i].get_address() == addr) return i;
  251. return -1;
  252. }
  253. static int8_t parse();
  254. static void M860();
  255. static void M861();
  256. static void M862();
  257. static void M863();
  258. static void M864();
  259. static void M865();
  260. static void M866();
  261. static void M867();
  262. static void M868();
  263. static void M869();
  264. static I2CPositionEncoder encoders[I2CPE_ENCODER_CNT];
  265. };
  266. extern I2CPositionEncodersMgr I2CPEM;
  267. FORCE_INLINE static void gcode_M860() { I2CPEM.M860(); }
  268. FORCE_INLINE static void gcode_M861() { I2CPEM.M861(); }
  269. FORCE_INLINE static void gcode_M862() { I2CPEM.M862(); }
  270. FORCE_INLINE static void gcode_M863() { I2CPEM.M863(); }
  271. FORCE_INLINE static void gcode_M864() { I2CPEM.M864(); }
  272. FORCE_INLINE static void gcode_M865() { I2CPEM.M865(); }
  273. FORCE_INLINE static void gcode_M866() { I2CPEM.M866(); }
  274. FORCE_INLINE static void gcode_M867() { I2CPEM.M867(); }
  275. FORCE_INLINE static void gcode_M868() { I2CPEM.M868(); }
  276. FORCE_INLINE static void gcode_M869() { I2CPEM.M869(); }
  277. #endif //I2C_POSITION_ENCODERS
  278. #endif //I2CPOSENC_H