tmc2130.cpp 33 KB

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