tmc2130.cpp 36 KB

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