tmc2130.cpp 32 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006
  1. #include "Marlin.h"
  2. #ifdef TMC2130
  3. #include "tmc2130.h"
  4. #include "ultralcd.h"
  5. #include "language.h"
  6. #include "spi.h"
  7. #define TMC2130_GCONF_NORMAL 0x00000000 // spreadCycle
  8. #define TMC2130_GCONF_SGSENS 0x00003180 // spreadCycle with stallguard (stall activates DIAG0 and DIAG1 [pushpull])
  9. #define TMC2130_GCONF_SILENT 0x00000004 // stealthChop
  10. //externals for debuging
  11. extern float current_position[4];
  12. extern void st_get_position_xy(long &x, long &y);
  13. extern long st_get_position(uint8_t axis);
  14. extern void crashdet_stop_and_save_print();
  15. extern void crashdet_stop_and_save_print2();
  16. //mode
  17. uint8_t tmc2130_mode = TMC2130_MODE_NORMAL;
  18. //holding currents
  19. uint8_t tmc2130_current_h[4] = TMC2130_CURRENTS_H;
  20. //running currents
  21. uint8_t tmc2130_current_r[4] = TMC2130_CURRENTS_R;
  22. //running currents for homing
  23. uint8_t tmc2130_current_r_home[4] = {8, 10, 20, 18};
  24. //pwm_ampl
  25. uint8_t tmc2130_pwm_ampl[4] = {TMC2130_PWM_AMPL_X, TMC2130_PWM_AMPL_Y, TMC2130_PWM_AMPL_Z, TMC2130_PWM_AMPL_E};
  26. //pwm_grad
  27. uint8_t tmc2130_pwm_grad[4] = {TMC2130_PWM_GRAD_X, TMC2130_PWM_GRAD_Y, TMC2130_PWM_GRAD_Z, TMC2130_PWM_GRAD_E};
  28. //pwm_auto
  29. uint8_t tmc2130_pwm_auto[4] = {TMC2130_PWM_AUTO_X, TMC2130_PWM_AUTO_Y, TMC2130_PWM_AUTO_Z, TMC2130_PWM_AUTO_E};
  30. //pwm_freq
  31. uint8_t tmc2130_pwm_freq[4] = {TMC2130_PWM_FREQ_X, TMC2130_PWM_FREQ_Y, TMC2130_PWM_FREQ_Z, TMC2130_PWM_FREQ_E};
  32. uint8_t tmc2130_mres[4] = {0, 0, 0, 0}; //will be filed at begin of init
  33. uint8_t tmc2130_sg_thr[4] = {TMC2130_SG_THRS_X, TMC2130_SG_THRS_Y, TMC2130_SG_THRS_Z, TMC2130_SG_THRS_E};
  34. uint8_t tmc2130_sg_thr_home[4] = {3, 3, TMC2130_SG_THRS_Z, TMC2130_SG_THRS_E};
  35. uint8_t tmc2130_sg_homing_axes_mask = 0x00;
  36. uint8_t tmc2130_sg_meassure = 0xff;
  37. uint32_t tmc2130_sg_meassure_cnt = 0;
  38. uint32_t tmc2130_sg_meassure_val = 0;
  39. uint8_t tmc2130_home_enabled = 0;
  40. uint8_t tmc2130_home_origin[2] = {0, 0};
  41. uint8_t tmc2130_home_bsteps[2] = {48, 48};
  42. uint8_t tmc2130_home_fsteps[2] = {48, 48};
  43. uint8_t tmc2130_wave_fac[4] = {0, 0, 0, 0};
  44. tmc2130_chopper_config_t tmc2130_chopper_config[4] = {
  45. {TMC2130_TOFF_XYZ, 5, 1, 2, 0},
  46. {TMC2130_TOFF_XYZ, 5, 1, 2, 0},
  47. {TMC2130_TOFF_XYZ, 5, 1, 2, 0},
  48. {TMC2130_TOFF_E, 5, 1, 2, 0}
  49. };
  50. bool tmc2130_sg_stop_on_crash = true;
  51. uint8_t tmc2130_sg_diag_mask = 0x00;
  52. uint8_t tmc2130_sg_crash = 0;
  53. uint16_t tmc2130_sg_err[4] = {0, 0, 0, 0};
  54. uint16_t tmc2130_sg_cnt[4] = {0, 0, 0, 0};
  55. bool tmc2130_sg_change = false;
  56. bool skip_debug_msg = false;
  57. #define DBG(args...)
  58. //printf_P(args)
  59. #ifndef _n
  60. #define _n PSTR
  61. #endif //_n
  62. #ifndef _i
  63. #define _i PSTR
  64. #endif //_i
  65. //TMC2130 registers
  66. #define TMC2130_REG_GCONF 0x00 // 17 bits
  67. #define TMC2130_REG_GSTAT 0x01 // 3 bits
  68. #define TMC2130_REG_IOIN 0x04 // 8+8 bits
  69. #define TMC2130_REG_IHOLD_IRUN 0x10 // 5+5+4 bits
  70. #define TMC2130_REG_TPOWERDOWN 0x11 // 8 bits
  71. #define TMC2130_REG_TSTEP 0x12 // 20 bits
  72. #define TMC2130_REG_TPWMTHRS 0x13 // 20 bits
  73. #define TMC2130_REG_TCOOLTHRS 0x14 // 20 bits
  74. #define TMC2130_REG_THIGH 0x15 // 20 bits
  75. #define TMC2130_REG_XDIRECT 0x2d // 32 bits
  76. #define TMC2130_REG_VDCMIN 0x33 // 23 bits
  77. #define TMC2130_REG_MSLUT0 0x60 // 32 bits
  78. #define TMC2130_REG_MSLUT1 0x61 // 32 bits
  79. #define TMC2130_REG_MSLUT2 0x62 // 32 bits
  80. #define TMC2130_REG_MSLUT3 0x63 // 32 bits
  81. #define TMC2130_REG_MSLUT4 0x64 // 32 bits
  82. #define TMC2130_REG_MSLUT5 0x65 // 32 bits
  83. #define TMC2130_REG_MSLUT6 0x66 // 32 bits
  84. #define TMC2130_REG_MSLUT7 0x67 // 32 bits
  85. #define TMC2130_REG_MSLUTSEL 0x68 // 32 bits
  86. #define TMC2130_REG_MSLUTSTART 0x69 // 8+8 bits
  87. #define TMC2130_REG_MSCNT 0x6a // 10 bits
  88. #define TMC2130_REG_MSCURACT 0x6b // 9+9 bits
  89. #define TMC2130_REG_CHOPCONF 0x6c // 32 bits
  90. #define TMC2130_REG_COOLCONF 0x6d // 25 bits
  91. #define TMC2130_REG_DCCTRL 0x6e // 24 bits
  92. #define TMC2130_REG_DRV_STATUS 0x6f // 32 bits
  93. #define TMC2130_REG_PWMCONF 0x70 // 22 bits
  94. #define TMC2130_REG_PWM_SCALE 0x71 // 8 bits
  95. #define TMC2130_REG_ENCM_CTRL 0x72 // 2 bits
  96. #define TMC2130_REG_LOST_STEPS 0x73 // 20 bits
  97. uint16_t tmc2130_rd_TSTEP(uint8_t axis);
  98. uint16_t tmc2130_rd_MSCNT(uint8_t axis);
  99. uint32_t tmc2130_rd_MSCURACT(uint8_t axis);
  100. void tmc2130_wr_CHOPCONF(uint8_t axis, uint8_t toff = 3, uint8_t hstrt = 4, uint8_t hend = 1, uint8_t fd3 = 0, uint8_t disfdcc = 0, uint8_t rndtf = 0, uint8_t chm = 0, uint8_t tbl = 2, uint8_t vsense = 0, uint8_t vhighfs = 0, uint8_t vhighchm = 0, uint8_t sync = 0, uint8_t mres = 0b0100, uint8_t intpol = 1, uint8_t dedge = 0, uint8_t diss2g = 0);
  101. void tmc2130_wr_PWMCONF(uint8_t axis, uint8_t pwm_ampl, uint8_t pwm_grad, uint8_t pwm_freq, uint8_t pwm_auto, uint8_t pwm_symm, uint8_t freewheel);
  102. void tmc2130_wr_TPWMTHRS(uint8_t axis, uint32_t val32);
  103. void tmc2130_wr_THIGH(uint8_t axis, uint32_t val32);
  104. #define tmc2130_rd(axis, addr, rval) tmc2130_rx(axis, addr, rval)
  105. #define tmc2130_wr(axis, addr, wval) tmc2130_tx(axis, (addr) | 0x80, wval)
  106. static void tmc2130_tx(uint8_t axis, uint8_t addr, uint32_t wval);
  107. static uint8_t tmc2130_rx(uint8_t axis, uint8_t addr, uint32_t* rval);
  108. void tmc2130_setup_chopper(uint8_t axis, uint8_t mres, uint8_t current_h, uint8_t current_r);
  109. uint16_t __tcoolthrs(uint8_t axis)
  110. {
  111. switch (axis)
  112. {
  113. case X_AXIS: return TMC2130_TCOOLTHRS_X;
  114. case Y_AXIS: return TMC2130_TCOOLTHRS_Y;
  115. case Z_AXIS: return TMC2130_TCOOLTHRS_Z;
  116. }
  117. return 0;
  118. }
  119. void tmc2130_init()
  120. {
  121. // DBG(_n("tmc2130_init(), mode=%S\n"), tmc2130_mode?_n("STEALTH"):_n("NORMAL"));
  122. WRITE(X_TMC2130_CS, HIGH);
  123. WRITE(Y_TMC2130_CS, HIGH);
  124. WRITE(Z_TMC2130_CS, HIGH);
  125. WRITE(E0_TMC2130_CS, HIGH);
  126. SET_OUTPUT(X_TMC2130_CS);
  127. SET_OUTPUT(Y_TMC2130_CS);
  128. SET_OUTPUT(Z_TMC2130_CS);
  129. SET_OUTPUT(E0_TMC2130_CS);
  130. SET_INPUT(X_TMC2130_DIAG);
  131. SET_INPUT(Y_TMC2130_DIAG);
  132. SET_INPUT(Z_TMC2130_DIAG);
  133. SET_INPUT(E0_TMC2130_DIAG);
  134. for (int axis = 0; axis < 2; axis++) // X Y axes
  135. {
  136. tmc2130_setup_chopper(axis, tmc2130_mres[axis], tmc2130_current_h[axis], tmc2130_current_r[axis]);
  137. tmc2130_wr(axis, TMC2130_REG_TPOWERDOWN, 0x00000000);
  138. tmc2130_wr(axis, TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr[axis]) << 16));
  139. tmc2130_wr(axis, TMC2130_REG_TCOOLTHRS, (tmc2130_mode == TMC2130_MODE_SILENT)?0:__tcoolthrs(axis));
  140. tmc2130_wr(axis, TMC2130_REG_GCONF, (tmc2130_mode == TMC2130_MODE_SILENT)?TMC2130_GCONF_SILENT:TMC2130_GCONF_SGSENS);
  141. tmc2130_wr_PWMCONF(axis, tmc2130_pwm_ampl[axis], tmc2130_pwm_grad[axis], tmc2130_pwm_freq[axis], tmc2130_pwm_auto[axis], 0, 0);
  142. tmc2130_wr_TPWMTHRS(axis, TMC2130_TPWMTHRS);
  143. //tmc2130_wr_THIGH(axis, TMC2130_THIGH);
  144. }
  145. for (int axis = 2; axis < 3; axis++) // Z axis
  146. {
  147. tmc2130_setup_chopper(axis, tmc2130_mres[axis], tmc2130_current_h[axis], tmc2130_current_r[axis]);
  148. tmc2130_wr(axis, TMC2130_REG_TPOWERDOWN, 0x00000000);
  149. #ifndef TMC2130_STEALTH_Z
  150. tmc2130_wr(axis, TMC2130_REG_GCONF, TMC2130_GCONF_SGSENS);
  151. #else //TMC2130_STEALTH_Z
  152. tmc2130_wr(axis, TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr[axis]) << 16));
  153. tmc2130_wr(axis, TMC2130_REG_TCOOLTHRS, (tmc2130_mode == TMC2130_MODE_SILENT)?0:__tcoolthrs(axis));
  154. tmc2130_wr(axis, TMC2130_REG_GCONF, (tmc2130_mode == TMC2130_MODE_SILENT)?TMC2130_GCONF_SILENT:TMC2130_GCONF_SGSENS);
  155. tmc2130_wr_PWMCONF(axis, tmc2130_pwm_ampl[axis], tmc2130_pwm_grad[axis], tmc2130_pwm_freq[axis], tmc2130_pwm_auto[axis], 0, 0);
  156. tmc2130_wr_TPWMTHRS(axis, TMC2130_TPWMTHRS);
  157. #endif //TMC2130_STEALTH_Z
  158. }
  159. for (int axis = 3; axis < 4; axis++) // E axis
  160. {
  161. tmc2130_setup_chopper(axis, tmc2130_mres[axis], tmc2130_current_h[axis], tmc2130_current_r[axis]);
  162. tmc2130_wr(axis, TMC2130_REG_TPOWERDOWN, 0x00000000);
  163. #ifndef TMC2130_STEALTH_E
  164. tmc2130_wr(axis, TMC2130_REG_GCONF, TMC2130_GCONF_SGSENS);
  165. #else //TMC2130_STEALTH_E
  166. tmc2130_wr(axis, TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr[axis]) << 16));
  167. tmc2130_wr(axis, TMC2130_REG_TCOOLTHRS, 0);
  168. tmc2130_wr(axis, TMC2130_REG_GCONF, TMC2130_GCONF_SILENT);
  169. tmc2130_wr_PWMCONF(axis, tmc2130_pwm_ampl[axis], tmc2130_pwm_grad[axis], tmc2130_pwm_freq[axis], tmc2130_pwm_auto[axis], 0, 0);
  170. tmc2130_wr_TPWMTHRS(axis, TMC2130_TPWMTHRS);
  171. #endif //TMC2130_STEALTH_E
  172. }
  173. tmc2130_sg_err[0] = 0;
  174. tmc2130_sg_err[1] = 0;
  175. tmc2130_sg_err[2] = 0;
  176. tmc2130_sg_err[3] = 0;
  177. tmc2130_sg_cnt[0] = 0;
  178. tmc2130_sg_cnt[1] = 0;
  179. tmc2130_sg_cnt[2] = 0;
  180. tmc2130_sg_cnt[3] = 0;
  181. #ifdef TMC2130_LINEARITY_CORRECTION
  182. #ifdef TMC2130_LINEARITY_CORRECTION_XYZ
  183. tmc2130_set_wave(X_AXIS, 247, tmc2130_wave_fac[X_AXIS]);
  184. tmc2130_set_wave(Y_AXIS, 247, tmc2130_wave_fac[Y_AXIS]);
  185. tmc2130_set_wave(Z_AXIS, 247, tmc2130_wave_fac[Z_AXIS]);
  186. #endif //TMC2130_LINEARITY_CORRECTION_XYZ
  187. tmc2130_set_wave(E_AXIS, 247, tmc2130_wave_fac[E_AXIS]);
  188. #endif //TMC2130_LINEARITY_CORRECTION
  189. }
  190. uint8_t tmc2130_sample_diag()
  191. {
  192. uint8_t mask = 0;
  193. if (READ(X_TMC2130_DIAG)) mask |= X_AXIS_MASK;
  194. if (READ(Y_TMC2130_DIAG)) mask |= Y_AXIS_MASK;
  195. // if (READ(Z_TMC2130_DIAG)) mask |= Z_AXIS_MASK;
  196. // if (READ(E0_TMC2130_DIAG)) mask |= E_AXIS_MASK;
  197. return mask;
  198. }
  199. extern bool is_usb_printing;
  200. void tmc2130_st_isr(uint8_t last_step_mask)
  201. {
  202. if (tmc2130_mode == TMC2130_MODE_SILENT || tmc2130_sg_stop_on_crash == false) return;
  203. uint8_t crash = 0;
  204. uint8_t diag_mask = tmc2130_sample_diag();
  205. // for (uint8_t axis = X_AXIS; axis <= E_AXIS; axis++)
  206. for (uint8_t axis = X_AXIS; axis <= Z_AXIS; axis++)
  207. {
  208. uint8_t mask = (X_AXIS_MASK << axis);
  209. if (diag_mask & mask) tmc2130_sg_err[axis]++;
  210. else
  211. if (tmc2130_sg_err[axis] > 0) tmc2130_sg_err[axis]--;
  212. if (tmc2130_sg_cnt[axis] < tmc2130_sg_err[axis])
  213. {
  214. tmc2130_sg_cnt[axis] = tmc2130_sg_err[axis];
  215. tmc2130_sg_change = true;
  216. uint8_t sg_thr = 64;
  217. // if (axis == Y_AXIS) sg_thr = 64;
  218. if (tmc2130_sg_err[axis] >= sg_thr)
  219. {
  220. tmc2130_sg_err[axis] = 0;
  221. crash |= mask;
  222. }
  223. }
  224. }
  225. if (tmc2130_sg_homing_axes_mask == 0)
  226. {
  227. if (tmc2130_sg_stop_on_crash && crash)
  228. {
  229. tmc2130_sg_crash = crash;
  230. tmc2130_sg_stop_on_crash = false;
  231. crashdet_stop_and_save_print();
  232. }
  233. }
  234. }
  235. bool tmc2130_update_sg()
  236. {
  237. if (tmc2130_sg_meassure <= E_AXIS)
  238. {
  239. uint32_t val32 = 0;
  240. tmc2130_rd(tmc2130_sg_meassure, TMC2130_REG_DRV_STATUS, &val32);
  241. tmc2130_sg_meassure_val += (val32 & 0x3ff);
  242. tmc2130_sg_meassure_cnt++;
  243. return true;
  244. }
  245. return false;
  246. }
  247. void tmc2130_home_enter(uint8_t axes_mask)
  248. {
  249. printf_P(PSTR("tmc2130_home_enter(axes_mask=0x%02x)\n"), axes_mask);
  250. #ifdef TMC2130_SG_HOMING
  251. if (axes_mask & 0x03) //X or Y
  252. tmc2130_wait_standstill_xy(1000);
  253. for (uint8_t axis = X_AXIS; axis <= Z_AXIS; axis++) //X Y and Z axes
  254. {
  255. uint8_t mask = (X_AXIS_MASK << axis);
  256. if (axes_mask & mask)
  257. {
  258. tmc2130_sg_homing_axes_mask |= mask;
  259. //Configuration to spreadCycle
  260. tmc2130_wr(axis, TMC2130_REG_GCONF, TMC2130_GCONF_NORMAL);
  261. tmc2130_wr(axis, TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr_home[axis]) << 16));
  262. // tmc2130_wr(axis, TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr[axis]) << 16) | ((uint32_t)1 << 24));
  263. tmc2130_wr(axis, TMC2130_REG_TCOOLTHRS, __tcoolthrs(axis));
  264. tmc2130_setup_chopper(axis, tmc2130_mres[axis], tmc2130_current_h[axis], tmc2130_current_r_home[axis]);
  265. if (mask & (X_AXIS_MASK | Y_AXIS_MASK | Z_AXIS_MASK))
  266. tmc2130_wr(axis, TMC2130_REG_GCONF, TMC2130_GCONF_SGSENS); //stallguard output DIAG1, DIAG1 = pushpull
  267. }
  268. }
  269. #endif //TMC2130_SG_HOMING
  270. }
  271. void tmc2130_home_exit()
  272. {
  273. printf_P(PSTR("tmc2130_home_exit tmc2130_sg_homing_axes_mask=0x%02x\n"), tmc2130_sg_homing_axes_mask);
  274. #ifdef TMC2130_SG_HOMING
  275. if (tmc2130_sg_homing_axes_mask & 0x03) //X or Y
  276. tmc2130_wait_standstill_xy(1000);
  277. if (tmc2130_sg_homing_axes_mask)
  278. {
  279. for (uint8_t axis = X_AXIS; axis <= Z_AXIS; axis++) //X Y and Z axes
  280. {
  281. uint8_t mask = (X_AXIS_MASK << axis);
  282. if (tmc2130_sg_homing_axes_mask & mask & (X_AXIS_MASK | Y_AXIS_MASK | Z_AXIS_MASK))
  283. {
  284. #ifndef TMC2130_STEALTH_Z
  285. if ((tmc2130_mode == TMC2130_MODE_SILENT) && (axis != Z_AXIS))
  286. #else //TMC2130_STEALTH_Z
  287. if (tmc2130_mode == TMC2130_MODE_SILENT)
  288. #endif //TMC2130_STEALTH_Z
  289. {
  290. tmc2130_wr(axis, TMC2130_REG_GCONF, TMC2130_GCONF_SILENT); // Configuration back to stealthChop
  291. tmc2130_wr(axis, TMC2130_REG_TCOOLTHRS, 0);
  292. // tmc2130_wr_PWMCONF(i, tmc2130_pwm_ampl[i], tmc2130_pwm_grad[i], tmc2130_pwm_freq[i], tmc2130_pwm_auto[i], 0, 0);
  293. }
  294. else
  295. {
  296. // tmc2130_wr(axis, TMC2130_REG_GCONF, TMC2130_GCONF_NORMAL);
  297. tmc2130_setup_chopper(axis, tmc2130_mres[axis], tmc2130_current_h[axis], tmc2130_current_r[axis]);
  298. // tmc2130_wr(axis, TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr[axis]) << 16) | ((uint32_t)1 << 24));
  299. tmc2130_wr(axis, TMC2130_REG_COOLCONF, (((uint32_t)tmc2130_sg_thr[axis]) << 16));
  300. tmc2130_wr(axis, TMC2130_REG_TCOOLTHRS, __tcoolthrs(axis));
  301. tmc2130_wr(axis, TMC2130_REG_GCONF, TMC2130_GCONF_SGSENS);
  302. }
  303. }
  304. }
  305. tmc2130_sg_homing_axes_mask = 0x00;
  306. }
  307. tmc2130_sg_crash = false;
  308. #endif
  309. }
  310. void tmc2130_sg_meassure_start(uint8_t axis)
  311. {
  312. tmc2130_sg_meassure = axis;
  313. tmc2130_sg_meassure_cnt = 0;
  314. tmc2130_sg_meassure_val = 0;
  315. }
  316. uint16_t tmc2130_sg_meassure_stop()
  317. {
  318. tmc2130_sg_meassure = 0xff;
  319. return tmc2130_sg_meassure_val / tmc2130_sg_meassure_cnt;
  320. }
  321. bool tmc2130_wait_standstill_xy(int timeout)
  322. {
  323. // DBG(_n("tmc2130_wait_standstill_xy(timeout=%d)\n"), timeout);
  324. bool standstill = false;
  325. while (!standstill && (timeout > 0))
  326. {
  327. uint32_t drv_status_x = 0;
  328. uint32_t drv_status_y = 0;
  329. tmc2130_rd(X_AXIS, TMC2130_REG_DRV_STATUS, &drv_status_x);
  330. tmc2130_rd(Y_AXIS, TMC2130_REG_DRV_STATUS, &drv_status_y);
  331. // DBG(_n("\tdrv_status_x=0x%08x drv_status_x=0x%08x\n"), drv_status_x, drv_status_y);
  332. standstill = (drv_status_x & 0x80000000) && (drv_status_y & 0x80000000);
  333. tmc2130_check_overtemp();
  334. timeout--;
  335. }
  336. return standstill;
  337. }
  338. void tmc2130_check_overtemp()
  339. {
  340. static uint32_t checktime = 0;
  341. if (millis() - checktime > 1000 )
  342. {
  343. for (int i = 0; i < 4; i++)
  344. {
  345. uint32_t drv_status = 0;
  346. skip_debug_msg = true;
  347. tmc2130_rd(i, TMC2130_REG_DRV_STATUS, &drv_status);
  348. if (drv_status & ((uint32_t)1 << 26))
  349. { // BIT 26 - over temp prewarning ~120C (+-20C)
  350. SERIAL_ERRORRPGM(MSG_TMC_OVERTEMP);
  351. SERIAL_ECHOLN(i);
  352. for (int j = 0; j < 4; j++)
  353. tmc2130_wr(j, TMC2130_REG_CHOPCONF, 0x00010000);
  354. kill(MSG_TMC_OVERTEMP);
  355. }
  356. }
  357. checktime = millis();
  358. tmc2130_sg_change = true;
  359. }
  360. #ifdef DEBUG_CRASHDET_COUNTERS
  361. if (tmc2130_sg_change)
  362. {
  363. for (int i = 0; i < 4; i++)
  364. {
  365. tmc2130_sg_change = false;
  366. lcd_set_cursor(0 + i*4, 3);
  367. lcd_print(itostr3(tmc2130_sg_cnt[i]));
  368. lcd_print(' ');
  369. }
  370. }
  371. #endif //DEBUG_CRASHDET_COUNTERS
  372. }
  373. void tmc2130_setup_chopper(uint8_t axis, uint8_t mres, uint8_t current_h, uint8_t current_r)
  374. {
  375. uint8_t intpol = 1;
  376. uint8_t toff = tmc2130_chopper_config[axis].toff; // toff = 3 (fchop = 27.778kHz)
  377. uint8_t hstrt = tmc2130_chopper_config[axis].hstr; //initial 4, modified to 5
  378. uint8_t hend = tmc2130_chopper_config[axis].hend; //original value = 1
  379. uint8_t fd3 = 0;
  380. uint8_t rndtf = 0; //random off time
  381. uint8_t chm = 0; //spreadCycle
  382. uint8_t tbl = tmc2130_chopper_config[axis].tbl; //blanking time, original value = 2
  383. if (axis == E_AXIS)
  384. {
  385. #ifdef TMC2130_CNSTOFF_E
  386. // fd = 0 (slow decay only)
  387. hstrt = 0; //fd0..2
  388. fd3 = 0; //fd3
  389. hend = 0; //sine wave offset
  390. chm = 1; // constant off time mod
  391. #endif //TMC2130_CNSTOFF_E
  392. // toff = TMC2130_TOFF_E; // toff = 3-5
  393. // rndtf = 1;
  394. }
  395. // DBG(_n("tmc2130_setup_chopper(axis=%hhd, mres=%hhd, curh=%hhd, curr=%hhd\n"), axis, mres, current_h, current_r);
  396. // DBG(_n(" toff=%hhd, hstr=%hhd, hend=%hhd, tbl=%hhd\n"), toff, hstrt, hend, tbl);
  397. if (current_r <= 31)
  398. {
  399. tmc2130_wr_CHOPCONF(axis, toff, hstrt, hend, fd3, 0, rndtf, chm, tbl, 1, 0, 0, 0, mres, intpol, 0, 0);
  400. tmc2130_wr(axis, TMC2130_REG_IHOLD_IRUN, 0x000f0000 | ((current_r & 0x1f) << 8) | (current_h & 0x1f));
  401. }
  402. else
  403. {
  404. tmc2130_wr_CHOPCONF(axis, toff, hstrt, hend, fd3, 0, rndtf, chm, tbl, 0, 0, 0, 0, mres, intpol, 0, 0);
  405. tmc2130_wr(axis, TMC2130_REG_IHOLD_IRUN, 0x000f0000 | (((current_r >> 1) & 0x1f) << 8) | ((current_h >> 1) & 0x1f));
  406. }
  407. }
  408. void tmc2130_set_current_h(uint8_t axis, uint8_t current)
  409. {
  410. // DBG(_n("tmc2130_set_current_h(axis=%d, current=%d\n"), axis, current);
  411. tmc2130_current_h[axis] = current;
  412. tmc2130_setup_chopper(axis, tmc2130_mres[axis], tmc2130_current_h[axis], tmc2130_current_r[axis]);
  413. }
  414. void tmc2130_set_current_r(uint8_t axis, uint8_t current)
  415. {
  416. // DBG(_n("tmc2130_set_current_r(axis=%d, current=%d\n"), axis, current);
  417. tmc2130_current_r[axis] = current;
  418. tmc2130_setup_chopper(axis, tmc2130_mres[axis], tmc2130_current_h[axis], tmc2130_current_r[axis]);
  419. }
  420. void tmc2130_print_currents()
  421. {
  422. // DBG(_n("tmc2130_print_currents()\n\tH\tR\nX\t%d\t%d\nY\t%d\t%d\nZ\t%d\t%d\nE\t%d\t%d\n"),
  423. // tmc2130_current_h[0], tmc2130_current_r[0],
  424. // tmc2130_current_h[1], tmc2130_current_r[1],
  425. // tmc2130_current_h[2], tmc2130_current_r[2],
  426. // tmc2130_current_h[3], tmc2130_current_r[3]
  427. // );
  428. }
  429. void tmc2130_set_pwm_ampl(uint8_t axis, uint8_t pwm_ampl)
  430. {
  431. // DBG(_n("tmc2130_set_pwm_ampl(axis=%hhd, pwm_ampl=%hhd\n"), axis, pwm_ampl);
  432. tmc2130_pwm_ampl[axis] = pwm_ampl;
  433. if (((axis == 0) || (axis == 1)) && (tmc2130_mode == TMC2130_MODE_SILENT))
  434. tmc2130_wr_PWMCONF(axis, tmc2130_pwm_ampl[axis], tmc2130_pwm_grad[axis], tmc2130_pwm_freq[axis], tmc2130_pwm_auto[axis], 0, 0);
  435. }
  436. void tmc2130_set_pwm_grad(uint8_t axis, uint8_t pwm_grad)
  437. {
  438. // DBG(_n("tmc2130_set_pwm_grad(axis=%hhd, pwm_grad=%hhd\n"), axis, pwm_grad);
  439. tmc2130_pwm_grad[axis] = pwm_grad;
  440. if (((axis == 0) || (axis == 1)) && (tmc2130_mode == TMC2130_MODE_SILENT))
  441. tmc2130_wr_PWMCONF(axis, tmc2130_pwm_ampl[axis], tmc2130_pwm_grad[axis], tmc2130_pwm_freq[axis], tmc2130_pwm_auto[axis], 0, 0);
  442. }
  443. uint16_t tmc2130_rd_TSTEP(uint8_t axis)
  444. {
  445. uint32_t val32 = 0;
  446. tmc2130_rd(axis, TMC2130_REG_TSTEP, &val32);
  447. if (val32 & 0x000f0000) return 0xffff;
  448. return val32 & 0xffff;
  449. }
  450. uint16_t tmc2130_rd_MSCNT(uint8_t axis)
  451. {
  452. uint32_t val32 = 0;
  453. tmc2130_rd(axis, TMC2130_REG_MSCNT, &val32);
  454. return val32 & 0x3ff;
  455. }
  456. uint32_t tmc2130_rd_MSCURACT(uint8_t axis)
  457. {
  458. uint32_t val32 = 0;
  459. tmc2130_rd(axis, TMC2130_REG_MSCURACT, &val32);
  460. return val32;
  461. }
  462. void tmc2130_wr_MSLUTSTART(uint8_t axis, uint8_t start_sin, uint8_t start_sin90)
  463. {
  464. uint32_t val = 0;
  465. val |= (uint32_t)start_sin;
  466. val |= ((uint32_t)start_sin90) << 16;
  467. tmc2130_wr(axis, TMC2130_REG_MSLUTSTART, val);
  468. //printf_P(PSTR("MSLUTSTART=%08lx (start_sin=%d start_sin90=%d)\n"), val, start_sin, start_sin90);
  469. }
  470. void tmc2130_wr_MSLUTSEL(uint8_t axis, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t w0, uint8_t w1, uint8_t w2, uint8_t w3)
  471. {
  472. uint32_t val = 0;
  473. val |= ((uint32_t)w0);
  474. val |= ((uint32_t)w1) << 2;
  475. val |= ((uint32_t)w2) << 4;
  476. val |= ((uint32_t)w3) << 6;
  477. val |= ((uint32_t)x1) << 8;
  478. val |= ((uint32_t)x2) << 16;
  479. val |= ((uint32_t)x3) << 24;
  480. tmc2130_wr(axis, TMC2130_REG_MSLUTSEL, val);
  481. //printf_P(PSTR("MSLUTSEL=%08lx (x1=%d x2=%d x3=%d w0=%d w1=%d w2=%d w3=%d)\n"), val, x1, x2, x3, w0, w1, w2, w3);
  482. }
  483. void tmc2130_wr_MSLUT(uint8_t axis, uint8_t i, uint32_t val)
  484. {
  485. tmc2130_wr(axis, TMC2130_REG_MSLUT0 + (i & 7), val);
  486. //printf_P(PSTR("MSLUT[%d]=%08lx\n"), i, val);
  487. }
  488. void tmc2130_wr_CHOPCONF(uint8_t axis, uint8_t toff, uint8_t hstrt, uint8_t hend, uint8_t fd3, uint8_t disfdcc, uint8_t rndtf, uint8_t chm, uint8_t tbl, uint8_t vsense, uint8_t vhighfs, uint8_t vhighchm, uint8_t sync, uint8_t mres, uint8_t intpol, uint8_t dedge, uint8_t diss2g)
  489. {
  490. uint32_t val = 0;
  491. val |= (uint32_t)(toff & 15);
  492. val |= (uint32_t)(hstrt & 7) << 4;
  493. val |= (uint32_t)(hend & 15) << 7;
  494. val |= (uint32_t)(fd3 & 1) << 11;
  495. val |= (uint32_t)(disfdcc & 1) << 12;
  496. val |= (uint32_t)(rndtf & 1) << 13;
  497. val |= (uint32_t)(chm & 1) << 14;
  498. val |= (uint32_t)(tbl & 3) << 15;
  499. val |= (uint32_t)(vsense & 1) << 17;
  500. val |= (uint32_t)(vhighfs & 1) << 18;
  501. val |= (uint32_t)(vhighchm & 1) << 19;
  502. val |= (uint32_t)(sync & 15) << 20;
  503. val |= (uint32_t)(mres & 15) << 24;
  504. val |= (uint32_t)(intpol & 1) << 28;
  505. val |= (uint32_t)(dedge & 1) << 29;
  506. val |= (uint32_t)(diss2g & 1) << 30;
  507. tmc2130_wr(axis, TMC2130_REG_CHOPCONF, val);
  508. }
  509. //void tmc2130_wr_PWMCONF(uint8_t axis, uint8_t PWMautoScale, uint8_t PWMfreq, uint8_t PWMgrad, uint8_t PWMampl)
  510. void tmc2130_wr_PWMCONF(uint8_t axis, uint8_t pwm_ampl, uint8_t pwm_grad, uint8_t pwm_freq, uint8_t pwm_auto, uint8_t pwm_symm, uint8_t freewheel)
  511. {
  512. uint32_t val = 0;
  513. val |= (uint32_t)(pwm_ampl & 255);
  514. val |= (uint32_t)(pwm_grad & 255) << 8;
  515. val |= (uint32_t)(pwm_freq & 3) << 16;
  516. val |= (uint32_t)(pwm_auto & 1) << 18;
  517. val |= (uint32_t)(pwm_symm & 1) << 19;
  518. val |= (uint32_t)(freewheel & 3) << 20;
  519. tmc2130_wr(axis, TMC2130_REG_PWMCONF, val);
  520. // tmc2130_wr(axis, TMC2130_REG_PWMCONF, ((uint32_t)(PWMautoScale+PWMfreq) << 16) | ((uint32_t)PWMgrad << 8) | PWMampl); // TMC LJ -> For better readability changed to 0x00 and added PWMautoScale and PWMfreq
  521. }
  522. void tmc2130_wr_TPWMTHRS(uint8_t axis, uint32_t val32)
  523. {
  524. tmc2130_wr(axis, TMC2130_REG_TPWMTHRS, val32);
  525. }
  526. void tmc2130_wr_THIGH(uint8_t axis, uint32_t val32)
  527. {
  528. tmc2130_wr(axis, TMC2130_REG_THIGH, val32);
  529. }
  530. uint8_t tmc2130_usteps2mres(uint16_t usteps)
  531. {
  532. uint8_t mres = 8; while (mres && (usteps >>= 1)) mres--;
  533. return mres;
  534. }
  535. inline void tmc2130_cs_low(uint8_t axis)
  536. {
  537. switch (axis)
  538. {
  539. case X_AXIS: WRITE(X_TMC2130_CS, LOW); break;
  540. case Y_AXIS: WRITE(Y_TMC2130_CS, LOW); break;
  541. case Z_AXIS: WRITE(Z_TMC2130_CS, LOW); break;
  542. case E_AXIS: WRITE(E0_TMC2130_CS, LOW); break;
  543. }
  544. }
  545. inline void tmc2130_cs_high(uint8_t axis)
  546. {
  547. switch (axis)
  548. {
  549. case X_AXIS: WRITE(X_TMC2130_CS, HIGH); break;
  550. case Y_AXIS: WRITE(Y_TMC2130_CS, HIGH); break;
  551. case Z_AXIS: WRITE(Z_TMC2130_CS, HIGH); break;
  552. case E_AXIS: WRITE(E0_TMC2130_CS, HIGH); break;
  553. }
  554. }
  555. //spi
  556. #define TMC2130_SPI_ENTER() spi_setup(TMC2130_SPCR, TMC2130_SPSR)
  557. #define TMC2130_SPI_TXRX spi_txrx
  558. #define TMC2130_SPI_LEAVE()
  559. static void tmc2130_tx(uint8_t axis, uint8_t addr, uint32_t wval)
  560. {
  561. //datagram1 - request
  562. TMC2130_SPI_ENTER();
  563. tmc2130_cs_low(axis);
  564. TMC2130_SPI_TXRX(addr); // address
  565. TMC2130_SPI_TXRX((wval >> 24) & 0xff); // MSB
  566. TMC2130_SPI_TXRX((wval >> 16) & 0xff);
  567. TMC2130_SPI_TXRX((wval >> 8) & 0xff);
  568. TMC2130_SPI_TXRX(wval & 0xff); // LSB
  569. tmc2130_cs_high(axis);
  570. TMC2130_SPI_LEAVE();
  571. }
  572. static uint8_t tmc2130_rx(uint8_t axis, uint8_t addr, uint32_t* rval)
  573. {
  574. //datagram1 - request
  575. TMC2130_SPI_ENTER();
  576. tmc2130_cs_low(axis);
  577. TMC2130_SPI_TXRX(addr); // address
  578. TMC2130_SPI_TXRX(0); // MSB
  579. TMC2130_SPI_TXRX(0);
  580. TMC2130_SPI_TXRX(0);
  581. TMC2130_SPI_TXRX(0); // LSB
  582. tmc2130_cs_high(axis);
  583. TMC2130_SPI_LEAVE();
  584. //datagram2 - response
  585. TMC2130_SPI_ENTER();
  586. tmc2130_cs_low(axis);
  587. uint8_t stat = TMC2130_SPI_TXRX(0); // status
  588. uint32_t val32 = 0;
  589. val32 = TMC2130_SPI_TXRX(0); // MSB
  590. val32 = (val32 << 8) | TMC2130_SPI_TXRX(0);
  591. val32 = (val32 << 8) | TMC2130_SPI_TXRX(0);
  592. val32 = (val32 << 8) | TMC2130_SPI_TXRX(0); // LSB
  593. tmc2130_cs_high(axis);
  594. TMC2130_SPI_LEAVE();
  595. if (rval != 0) *rval = val32;
  596. return stat;
  597. }
  598. #define _GET_PWR_X (READ(X_ENABLE_PIN) == X_ENABLE_ON)
  599. #define _GET_PWR_Y (READ(Y_ENABLE_PIN) == Y_ENABLE_ON)
  600. #define _GET_PWR_Z (READ(Z_ENABLE_PIN) == Z_ENABLE_ON)
  601. #define _GET_PWR_E (READ(E0_ENABLE_PIN) == E_ENABLE_ON)
  602. #define _SET_PWR_X(ena) { WRITE(X_ENABLE_PIN, ena?X_ENABLE_ON:!X_ENABLE_ON); asm("nop"); }
  603. #define _SET_PWR_Y(ena) { WRITE(Y_ENABLE_PIN, ena?Y_ENABLE_ON:!Y_ENABLE_ON); asm("nop"); }
  604. #define _SET_PWR_Z(ena) { WRITE(Z_ENABLE_PIN, ena?Z_ENABLE_ON:!Z_ENABLE_ON); asm("nop"); }
  605. #define _SET_PWR_E(ena) { WRITE(E0_ENABLE_PIN, ena?E_ENABLE_ON:!E_ENABLE_ON); asm("nop"); }
  606. #define _GET_DIR_X (READ(X_DIR_PIN) == INVERT_X_DIR)
  607. #define _GET_DIR_Y (READ(Y_DIR_PIN) == INVERT_Y_DIR)
  608. #define _GET_DIR_Z (READ(Z_DIR_PIN) == INVERT_Z_DIR)
  609. #define _GET_DIR_E (READ(E0_DIR_PIN) == INVERT_E0_DIR)
  610. #define _SET_DIR_X(dir) { WRITE(X_DIR_PIN, dir?INVERT_X_DIR:!INVERT_X_DIR); asm("nop"); }
  611. #define _SET_DIR_Y(dir) { WRITE(Y_DIR_PIN, dir?INVERT_Y_DIR:!INVERT_Y_DIR); asm("nop"); }
  612. #define _SET_DIR_Z(dir) { WRITE(Z_DIR_PIN, dir?INVERT_Z_DIR:!INVERT_Z_DIR); asm("nop"); }
  613. #define _SET_DIR_E(dir) { WRITE(E0_DIR_PIN, dir?INVERT_E0_DIR:!INVERT_E0_DIR); asm("nop"); }
  614. #define _DO_STEP_X { WRITE(X_STEP_PIN, !INVERT_X_STEP_PIN); asm("nop"); WRITE(X_STEP_PIN, INVERT_X_STEP_PIN); asm("nop"); }
  615. #define _DO_STEP_Y { WRITE(Y_STEP_PIN, !INVERT_Y_STEP_PIN); asm("nop"); WRITE(Y_STEP_PIN, INVERT_Y_STEP_PIN); asm("nop"); }
  616. #define _DO_STEP_Z { WRITE(Z_STEP_PIN, !INVERT_Z_STEP_PIN); asm("nop"); WRITE(Z_STEP_PIN, INVERT_Z_STEP_PIN); asm("nop"); }
  617. #define _DO_STEP_E { WRITE(E0_STEP_PIN, !INVERT_E_STEP_PIN); asm("nop"); WRITE(E0_STEP_PIN, INVERT_E_STEP_PIN); asm("nop"); }
  618. uint16_t tmc2130_get_res(uint8_t axis)
  619. {
  620. return tmc2130_mres2usteps(tmc2130_mres[axis]);
  621. }
  622. void tmc2130_set_res(uint8_t axis, uint16_t res)
  623. {
  624. tmc2130_mres[axis] = tmc2130_usteps2mres(res);
  625. // uint32_t u = micros();
  626. tmc2130_setup_chopper(axis, tmc2130_mres[axis], tmc2130_current_h[axis], tmc2130_current_r[axis]);
  627. // u = micros() - u;
  628. // printf_P(PSTR("tmc2130_setup_chopper %c %lu us"), "XYZE"[axis], u);
  629. }
  630. uint8_t tmc2130_get_pwr(uint8_t axis)
  631. {
  632. switch (axis)
  633. {
  634. case X_AXIS: return _GET_PWR_X;
  635. case Y_AXIS: return _GET_PWR_Y;
  636. case Z_AXIS: return _GET_PWR_Z;
  637. case E_AXIS: return _GET_PWR_E;
  638. }
  639. return 0;
  640. }
  641. void tmc2130_set_pwr(uint8_t axis, uint8_t pwr)
  642. {
  643. switch (axis)
  644. {
  645. case X_AXIS: _SET_PWR_X(pwr); break;
  646. case Y_AXIS: _SET_PWR_Y(pwr); break;
  647. case Z_AXIS: _SET_PWR_Z(pwr); break;
  648. case E_AXIS: _SET_PWR_E(pwr); break;
  649. }
  650. }
  651. uint8_t tmc2130_get_inv(uint8_t axis)
  652. {
  653. switch (axis)
  654. {
  655. case X_AXIS: return INVERT_X_DIR;
  656. case Y_AXIS: return INVERT_Y_DIR;
  657. case Z_AXIS: return INVERT_Z_DIR;
  658. case E_AXIS: return INVERT_E0_DIR;
  659. }
  660. return 0;
  661. }
  662. uint8_t tmc2130_get_dir(uint8_t axis)
  663. {
  664. switch (axis)
  665. {
  666. case X_AXIS: return _GET_DIR_X;
  667. case Y_AXIS: return _GET_DIR_Y;
  668. case Z_AXIS: return _GET_DIR_Z;
  669. case E_AXIS: return _GET_DIR_E;
  670. }
  671. return 0;
  672. }
  673. void tmc2130_set_dir(uint8_t axis, uint8_t dir)
  674. {
  675. switch (axis)
  676. {
  677. case X_AXIS: _SET_DIR_X(dir); break;
  678. case Y_AXIS: _SET_DIR_Y(dir); break;
  679. case Z_AXIS: _SET_DIR_Z(dir); break;
  680. case E_AXIS: _SET_DIR_E(dir); break;
  681. }
  682. }
  683. void tmc2130_do_step(uint8_t axis)
  684. {
  685. switch (axis)
  686. {
  687. case X_AXIS: _DO_STEP_X; break;
  688. case Y_AXIS: _DO_STEP_Y; break;
  689. case Z_AXIS: _DO_STEP_Z; break;
  690. case E_AXIS: _DO_STEP_E; break;
  691. }
  692. }
  693. void tmc2130_do_steps(uint8_t axis, uint16_t steps, uint8_t dir, uint16_t delay_us)
  694. {
  695. tmc2130_set_dir(axis, dir);
  696. delayMicroseconds(100);
  697. while (steps--)
  698. {
  699. tmc2130_do_step(axis);
  700. delayMicroseconds(delay_us);
  701. }
  702. }
  703. void tmc2130_goto_step(uint8_t axis, uint8_t step, uint8_t dir, uint16_t delay_us, uint16_t microstep_resolution)
  704. {
  705. printf_P(PSTR("tmc2130_goto_step %d %d %d %d \n"), axis, step, dir, delay_us, microstep_resolution);
  706. uint8_t shift; for (shift = 0; shift < 8; shift++) if (microstep_resolution == (256 >> shift)) break;
  707. uint16_t cnt = 4 * (1 << (8 - shift));
  708. uint16_t mscnt = tmc2130_rd_MSCNT(axis);
  709. if (dir == 2)
  710. {
  711. dir = tmc2130_get_inv(axis)?0:1;
  712. int steps = (int)step - (int)(mscnt >> shift);
  713. if (steps < 0)
  714. {
  715. dir ^= 1;
  716. steps = -steps;
  717. }
  718. if (steps > (cnt / 2))
  719. {
  720. dir ^= 1;
  721. steps = cnt - steps;
  722. }
  723. cnt = steps;
  724. }
  725. tmc2130_set_dir(axis, dir);
  726. delayMicroseconds(100);
  727. mscnt = tmc2130_rd_MSCNT(axis);
  728. while ((cnt--) && ((mscnt >> shift) != step))
  729. {
  730. tmc2130_do_step(axis);
  731. delayMicroseconds(delay_us);
  732. mscnt = tmc2130_rd_MSCNT(axis);
  733. }
  734. }
  735. void tmc2130_get_wave(uint8_t axis, uint8_t* data, FILE* stream)
  736. {
  737. uint8_t pwr = tmc2130_get_pwr(axis);
  738. tmc2130_set_pwr(axis, 0);
  739. tmc2130_setup_chopper(axis, tmc2130_usteps2mres(256), tmc2130_current_h[axis], tmc2130_current_r[axis]);
  740. tmc2130_goto_step(axis, 0, 2, 100, 256);
  741. tmc2130_set_dir(axis, tmc2130_get_inv(axis)?0:1);
  742. for (int i = 0; i <= 255; i++)
  743. {
  744. uint32_t val = tmc2130_rd_MSCURACT(axis);
  745. uint16_t mscnt = tmc2130_rd_MSCNT(axis);
  746. int curA = (val & 0xff) | ((val << 7) & 0x8000);
  747. if (stream)
  748. {
  749. if (mscnt == i)
  750. fprintf_P(stream, PSTR("%d\t%d\n"), i, curA);
  751. else //TODO - remove this check
  752. fprintf_P(stream, PSTR("!! (i=%d MSCNT=%d)\n"), i, mscnt);
  753. }
  754. if (data) *(data++) = curA;
  755. tmc2130_do_step(axis);
  756. delayMicroseconds(100);
  757. }
  758. tmc2130_setup_chopper(axis, tmc2130_mres[axis], tmc2130_current_h[axis], tmc2130_current_r[axis]);
  759. }
  760. void tmc2130_set_wave(uint8_t axis, uint8_t amp, uint8_t fac1000)
  761. {
  762. // TMC2130 wave compression algorithm
  763. // optimized for minimal memory requirements
  764. // printf_P(PSTR("tmc2130_set_wave %hhd %hhd\n"), axis, fac1000);
  765. if (fac1000 < TMC2130_WAVE_FAC1000_MIN) fac1000 = 0;
  766. if (fac1000 > TMC2130_WAVE_FAC1000_MAX) fac1000 = TMC2130_WAVE_FAC1000_MAX;
  767. float fac = 0;
  768. if (fac1000) fac = ((float)((uint16_t)fac1000 + 1000) / 1000); //correction factor
  769. // printf_P(PSTR(" factor: %s\n"), ftostr43(fac));
  770. uint8_t vA = 0; //value of currentA
  771. uint8_t va = 0; //previous vA
  772. int8_t d0 = 0; //delta0
  773. int8_t d1 = 1; //delta1
  774. uint8_t w[4] = {1,1,1,1}; //W bits (MSLUTSEL)
  775. uint8_t x[3] = {255,255,255}; //X segment bounds (MSLUTSEL)
  776. uint8_t s = 0; //current segment
  777. int8_t b; //encoded bit value
  778. int8_t dA; //delta value
  779. int i; //microstep index
  780. uint32_t reg; //tmc2130 register
  781. tmc2130_wr_MSLUTSTART(axis, 0, amp);
  782. for (i = 0; i < 256; i++)
  783. {
  784. if ((i & 0x1f) == 0)
  785. reg = 0;
  786. // calculate value
  787. if (fac == 0) // default TMC wave
  788. vA = (uint8_t)((amp+1) * sin((2*PI*i + PI)/1024) + 0.5) - 1;
  789. else // corrected wave
  790. vA = (uint8_t)(amp * pow(sin(2*PI*i/1024), fac) + 0.5);
  791. dA = vA - va; // calculate delta
  792. va = vA;
  793. b = -1;
  794. if (dA == d0) b = 0; //delta == delta0 => bit=0
  795. else if (dA == d1) b = 1; //delta == delta1 => bit=1
  796. else
  797. {
  798. if (dA < d0) // delta < delta0 => switch wbit down
  799. {
  800. //printf("dn\n");
  801. b = 0;
  802. switch (dA)
  803. {
  804. case -1: d0 = -1; d1 = 0; w[s+1] = 0; break;
  805. case 0: d0 = 0; d1 = 1; w[s+1] = 1; break;
  806. case 1: d0 = 1; d1 = 2; w[s+1] = 2; break;
  807. default: b = -1; break;
  808. }
  809. if (b >= 0) { x[s] = i; s++; }
  810. }
  811. else if (dA > d1) // delta > delta0 => switch wbit up
  812. {
  813. //printf("up\n");
  814. b = 1;
  815. switch (dA)
  816. {
  817. case 1: d0 = 0; d1 = 1; w[s+1] = 1; break;
  818. case 2: d0 = 1; d1 = 2; w[s+1] = 2; break;
  819. case 3: d0 = 2; d1 = 3; w[s+1] = 3; break;
  820. default: b = -1; break;
  821. }
  822. if (b >= 0) { x[s] = i; s++; }
  823. }
  824. }
  825. if (b < 0) break; // delta out of range (<-1 or >3)
  826. if (s > 3) break; // segment out of range (> 3)
  827. //printf("%d\n", vA);
  828. if (b == 1) reg |= 0x80000000;
  829. if ((i & 31) == 31)
  830. tmc2130_wr_MSLUT(axis, (uint8_t)(i >> 5), reg);
  831. else
  832. reg >>= 1;
  833. // printf("%3d\t%3d\t%2d\t%2d\t%2d\t%2d %08x\n", i, vA, dA, b, w[s], s, reg);
  834. }
  835. tmc2130_wr_MSLUTSEL(axis, x[0], x[1], x[2], w[0], w[1], w[2], w[3]);
  836. }
  837. void bubblesort_uint8(uint8_t* data, uint8_t size, uint8_t* data2)
  838. {
  839. uint8_t changed = 1;
  840. while (changed)
  841. {
  842. changed = 0;
  843. for (uint8_t i = 0; i < (size - 1); i++)
  844. if (data[i] > data[i+1])
  845. {
  846. uint8_t register d = data[i];
  847. data[i] = data[i+1];
  848. data[i+1] = d;
  849. if (data2)
  850. {
  851. d = data2[i];
  852. data2[i] = data2[i+1];
  853. data2[i+1] = d;
  854. }
  855. changed = 1;
  856. }
  857. }
  858. }
  859. uint8_t clusterize_uint8(uint8_t* data, uint8_t size, uint8_t* ccnt, uint8_t* cval, uint8_t tol)
  860. {
  861. uint8_t cnt = 1;
  862. uint16_t sum = data[0];
  863. uint8_t cl = 0;
  864. for (uint8_t i = 1; i < size; i++)
  865. {
  866. uint8_t d = data[i];
  867. uint8_t val = sum / cnt;
  868. uint8_t dif = 0;
  869. if (val > d) dif = val - d;
  870. else dif = d - val;
  871. if (dif <= tol)
  872. {
  873. cnt += 1;
  874. sum += d;
  875. }
  876. else
  877. {
  878. if (ccnt) ccnt[cl] = cnt;
  879. if (cval) cval[cl] = val;
  880. cnt = 1;
  881. sum = d;
  882. cl += 1;
  883. }
  884. }
  885. if (ccnt) ccnt[cl] = cnt;
  886. if (cval) cval[cl] = sum / cnt;
  887. return ++cl;
  888. }
  889. bool tmc2130_home_calibrate(uint8_t axis)
  890. {
  891. uint8_t step[16];
  892. uint8_t cnt[16];
  893. uint8_t val[16];
  894. homeaxis(axis, 16, step);
  895. bubblesort_uint8(step, 16, 0);
  896. printf_P(PSTR("sorted samples:\n"));
  897. for (uint8_t i = 0; i < 16; i++)
  898. printf_P(PSTR(" i=%2d step=%2d\n"), i, step[i]);
  899. uint8_t cl = clusterize_uint8(step, 16, cnt, val, 1);
  900. printf_P(PSTR("clusters:\n"));
  901. for (uint8_t i = 0; i < cl; i++)
  902. printf_P(PSTR(" i=%2d cnt=%2d val=%2d\n"), i, cnt[i], val[i]);
  903. bubblesort_uint8(cnt, cl, val);
  904. tmc2130_home_origin[axis] = val[cl-1];
  905. printf_P(PSTR("result value: %d\n"), tmc2130_home_origin[axis]);
  906. if (axis == X_AXIS) eeprom_update_byte((uint8_t*)EEPROM_TMC2130_HOME_X_ORIGIN, tmc2130_home_origin[X_AXIS]);
  907. else if (axis == Y_AXIS) eeprom_update_byte((uint8_t*)EEPROM_TMC2130_HOME_Y_ORIGIN, tmc2130_home_origin[Y_AXIS]);
  908. return true;
  909. }
  910. #endif //TMC2130