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

I2CPositionEncoder.h 12KB

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