tmc2130.cpp 30 KB

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