You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

436 lines
15 KiB

12 months ago
  1. #include "tmc51x0.hpp"
  2. #include "reg/TMC5130_Constants.h"
  3. #include "reg/TMC5130_Fields.h"
  4. #include "reg/TMC5130_Register.h"
  5. //
  6. using namespace iflytop;
  7. #if 0
  8. // Register access permissions:
  9. // 0x00: none (reserved)
  10. // 0x01: read
  11. // 0x02: write
  12. // 0x03: read/write
  13. // 0x13: read/write, separate functions/values for reading or writing
  14. // 0x21: read, flag register (read to clear)
  15. // 0x42: write, has hardware presets on reset
  16. static const uint8_t tmc5130_defaultRegisterAccess[TMC5130_REGISTER_COUNT] = {
  17. // 0 1 2 3 4 5 6 7 8 9 A B C D E F
  18. 0x03, 0x21, 0x01, 0x02, 0x13, 0x02, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, // 0x00 - 0x0F
  19. 0x02, 0x02, 0x01, 0x02, 0x02, 0x02, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, // 0x10 - 0x1F
  20. 0x03, 0x03, 0x01, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, ____, 0x02, 0x02, 0x02, 0x03, ____, ____, // 0x20 - 0x2F
  21. ____, ____, ____, 0x02, 0x03, 0x21, 0x01, ____, 0x03, 0x03, 0x02, 0x21, 0x01, ____, ____, ____, // 0x30 - 0x3F
  22. ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, // 0x40 - 0x4F
  23. ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, // 0x50 - 0x5F
  24. 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x01, 0x01, 0x03, 0x02, 0x02, 0x01, // 0x60 - 0x6F
  25. 0x42, 0x01, 0x02, 0x01, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____ // 0x70 - 0x7F
  26. };
  27. #endif
  28. /***********************************************************************************************************************
  29. * *
  30. ***********************************************************************************************************************/
  31. #define FIELD_SET(data, mask, shift, value) (data = (((data) & (~(mask))) | (((value) << (shift)) & (mask))))
  32. #define FIELD_GET(data, mask, shift) (((data) & (mask)) >> (shift))
  33. #define BYTE(value, n) (((value) >> ((n) << 3)) & 0xFF)
  34. #define NIBBLE(value, n) (((value) >> ((n) << 2)) & 0x0F)
  35. #define SHORT(value, n) (((value) >> ((n) << 4)) & 0xFFFF)
  36. #define WORD(value, n) (((value) >> ((n) << 5)) & 0xFFFFFFFF)
  37. #define TMC51x0_ADDRESS(x) ((x) & (TMC5130_ADDRESS_MASK))
  38. #define TAG "TMC51X0"
  39. void TMC51X0::initialize(TMC51X0Cfg cfg) {
  40. m_cfg = cfg;
  41. m_hspi = cfg.hspi;
  42. ZLOGI(TAG, "SPI:%p CSPin:%s,enPIn:%s", cfg.hspi, pinname(cfg.csnpin), pinname(cfg.ennpin));
  43. m_mutex.init();
  44. m_csnpin.initAsOutput(m_cfg.csnpin, kxs_gpio_nopull, false, true);
  45. m_ennpin.initAsOutput(m_cfg.ennpin, kxs_gpio_nopull, false, true);
  46. enable(false);
  47. stop();
  48. writeInt(TMC5130_PWMCONF, 0x000500C8);
  49. writeInt(TMC5130_CHOPCONF, 0x000100c3);
  50. writeInt(TMC5130_IHOLD_IRUN, 0x00051A00);
  51. writeInt(TMC5130_PWMCONF, 0x000401c8);
  52. writeInt(TMC5130_XTARGET, 0);
  53. writeInt(TMC5130_XACTUAL, 0x00000000);
  54. writeInt(TMC5130_VACTUAL, 0x00000000);
  55. writeInt(TMC5130_TZEROWAIT, 0);
  56. setVstart(100);
  57. setA1(30);
  58. setAmax(50);
  59. setV1(500);
  60. setDmax(50);
  61. setD1(30);
  62. setVstop(100);
  63. setTzerowait(100);
  64. setIHOLD_IRUN(0, 30, 100);
  65. enable(false);
  66. }
  67. /*******************************************************************************
  68. * *
  69. *******************************************************************************/
  70. void TMC51X0::readWriteArray(uint8_t *data, size_t length) {
  71. portENTER_CRITICAL();
  72. m_csnpin.write(false);
  73. HAL_SPI_TransmitReceive(m_hspi, data, data, length, 100);
  74. m_csnpin.write(true);
  75. portEXIT_CRITICAL();
  76. }
  77. void TMC51X0::writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t x4) {
  78. uint8_t data[5] = {static_cast<uint8_t>(address | static_cast<uint8_t>(TMC5130_WRITE_BIT)), x1, x2, x3, x4};
  79. readWriteArray(&data[0], 5);
  80. }
  81. void TMC51X0::writeInt(uint8_t address, int32_t value) { //
  82. writeDatagram(address, BYTE(value, 3), BYTE(value, 2), BYTE(value, 1), BYTE(value, 0));
  83. }
  84. void TMC51X0::writeField(uint8_t add, uint32_t mask, uint32_t shift, uint32_t value) {
  85. uint32_t regval = readUInt(add);
  86. regval = (regval & ~mask) | ((value << shift) & mask);
  87. writeInt(add, regval);
  88. }
  89. int32_t TMC51X0::readInt(uint8_t address) {
  90. /**
  91. * @WARNING:
  92. *
  93. */
  94. address = TMC51x0_ADDRESS(address);
  95. uint8_t data[5] = {0, 0, 0, 0, 0};
  96. data[0] = address;
  97. readWriteArray(&data[0], 5);
  98. data[0] = address;
  99. readWriteArray(&data[0], 5);
  100. return ((uint32_t)data[1] << 24) | ((uint32_t)data[2] << 16) | (data[3] << 8) | data[4];
  101. }
  102. uint32_t TMC51X0::readUInt(uint8_t address) {
  103. int32_t val = readInt(address);
  104. uint32_t ret;
  105. memcpy(&ret, &val, sizeof(uint32_t));
  106. return ret;
  107. }
  108. /***********************************************************************************************************************
  109. * BASIC *
  110. ***********************************************************************************************************************/
  111. bool TMC51X0::ping() {
  112. zlock_guard lkg(m_mutex);
  113. uint32_t regval = readUInt(TMC5130_IOIN);
  114. uint32_t chipId = FIELD_GET(regval, TMC5130_VERSION_MASK, TMC5130_VERSION_SHIFT);
  115. return chipId != 0;
  116. }
  117. void TMC51X0::enable(bool enable) {
  118. zlock_guard lkg(m_mutex);
  119. m_ennpin.write(!enable);
  120. }
  121. int32_t TMC51X0::to_motor_acc(int32_t acc) { //
  122. int32_t val = acc / 60.0 * m_onecirclepulse; // 65535
  123. if (val > 65535) val = 65535;
  124. return val;
  125. }
  126. int32_t TMC51X0::to_motor_vel(int32_t vel) { //
  127. int32_t val = vel / 60.0 * m_onecirclepulse;
  128. if (val > 8388095 /*2^23-512*/) {
  129. val = 8388095;
  130. }
  131. return val;
  132. } // rpm
  133. int32_t TMC51X0::to_motor_pos(int32_t pos) { //
  134. int32_t val = pos * 1.0 / m_scale * m_scale_deceleration * m_onecirclepulse + 0.5;
  135. return val;
  136. } //
  137. int32_t TMC51X0::to_user_pos(int32_t pos) { //
  138. int32_t val = pos / m_onecirclepulse * m_scale / m_scale_deceleration + 0.5;
  139. // ZLOGI("TMC5130", "to_user_pos %d,%d", pos, val);
  140. return val;
  141. } //
  142. int32_t TMC51X0::to_user_vel(int32_t vel) { //
  143. int32_t val = vel * 60.0 / m_onecirclepulse;
  144. return val;
  145. }
  146. /***********************************************************************************************************************
  147. * CTRL *
  148. ***********************************************************************************************************************/
  149. void TMC51X0::stop() {
  150. zlock_guard lkg(m_mutex);
  151. rotate(0);
  152. }
  153. void TMC51X0::rotate(int32_t velocity) {
  154. zlock_guard lkg(m_mutex);
  155. velocity = to_motor_vel(velocity);
  156. writeInt(TMC5130_VMAX, abs(velocity));
  157. writeInt(TMC5130_RAMPMODE, (velocity >= 0) ? TMC5130_MODE_VELPOS : TMC5130_MODE_VELNEG);
  158. }
  159. void TMC51X0::moveTo(int32_t position, uint32_t velocityMax) {
  160. zlock_guard lkg(m_mutex);
  161. position = to_motor_pos(position);
  162. velocityMax = to_motor_vel(velocityMax);
  163. writeInt(TMC5130_RAMPMODE, TMC5130_MODE_POSITION);
  164. writeInt(TMC5130_VMAX, velocityMax);
  165. writeInt(TMC5130_XTARGET, position);
  166. }
  167. void TMC51X0::moveToEnd(int32_t direction, uint32_t velocityMax) {
  168. zlock_guard lkg(m_mutex);
  169. if (direction >= 0) {
  170. writeInt(TMC5130_RAMPMODE, TMC5130_MODE_POSITION);
  171. writeInt(TMC5130_VMAX, to_motor_vel(velocityMax));
  172. writeInt(TMC5130_XTARGET, INT32_MAX / 1.5 - 1000);
  173. } else {
  174. writeInt(TMC5130_RAMPMODE, TMC5130_MODE_POSITION);
  175. writeInt(TMC5130_VMAX, to_motor_vel(velocityMax));
  176. writeInt(TMC5130_XTARGET, INT32_MIN / 1.5 + 1000);
  177. }
  178. }
  179. void TMC51X0::moveBy(int32_t relativePosition, uint32_t velocityMax) { // determine actual position and add numbers of ticks to move
  180. zlock_guard lkg(m_mutex);
  181. int32_t pos = getXACTUAL();
  182. int32_t target = pos + relativePosition;
  183. moveTo(target, velocityMax);
  184. }
  185. /***********************************************************************************************************************
  186. * GET STATE *
  187. ***********************************************************************************************************************/
  188. int32_t TMC51X0::getXACTUAL() {
  189. zlock_guard lkg(m_mutex);
  190. return to_user_pos(readInt(TMC5130_XACTUAL));
  191. }
  192. int32_t TMC51X0::getVACTUAL() {
  193. zlock_guard lkg(m_mutex);
  194. return to_user_pos(readInt(TMC5130_VACTUAL));
  195. }
  196. int32_t TMC51X0::getEncVal() {
  197. zlock_guard lkg(m_mutex);
  198. int32_t enc_val = to_user_pos(readInt(TMC5130_XENC));
  199. if (m_enc_resolution < 0) {
  200. enc_val = -enc_val;
  201. }
  202. return enc_val;
  203. }
  204. TMC5130RampStat TMC51X0::getRampStat() {
  205. zlock_guard lkg(m_mutex);
  206. int32_t val = readInt(TMC5130_RAMPSTAT);
  207. return TMC5130RampStat(val);
  208. }
  209. bool TMC51X0::isReachTarget(TMC5130RampStat *state) {
  210. zlock_guard lkg(m_mutex);
  211. int mode = readInt(TMC5130_RAMPMODE);
  212. if (mode == TMC5130_MODE_POSITION) {
  213. return state->isSetted(TMC5130RampStat::ktmc5130_rs_posreached);
  214. } else {
  215. return state->isSetted(TMC5130RampStat::ktmc5130_rs_vzero) && state->isSetted(TMC5130RampStat::ktmc5130_rs_velreached);
  216. }
  217. }
  218. bool TMC51X0::isTMC5130() {
  219. zlock_guard lkg(m_mutex);
  220. uint32_t regval = readUInt(TMC5130_IOIN);
  221. uint32_t chipId = FIELD_GET(regval, TMC5130_VERSION_MASK, TMC5130_VERSION_SHIFT);
  222. return chipId == kTMC5130;
  223. }
  224. TMC5130DevStatusReg_t TMC51X0::getDevStatus() { // R 读后不清
  225. zlock_guard lkg(m_mutex);
  226. static_assert(sizeof(TMC5130DevStatusReg_t) == 4);
  227. uint32_t value = readInt(TMC5130_DRVSTATUS);
  228. TMC5130DevStatusReg_t val;
  229. memcpy(&val, &value, sizeof(TMC5130DevStatusReg_t));
  230. return val;
  231. }
  232. TMC5130GState_t TMC51X0::getGState() { // R+C 读后自动清除
  233. zlock_guard lkg(m_mutex);
  234. static_assert(sizeof(TMC5130GState_t) == 4);
  235. uint32_t value = readInt(TMC5130_GSTAT);
  236. TMC5130GState_t val;
  237. memcpy(&val, &value, sizeof(TMC5130GState_t));
  238. return val;
  239. }
  240. int32_t TMC51X0::readICVersion() {
  241. zlock_guard lkg(m_mutex);
  242. uint32_t regval = readUInt(TMC5130_IOIN);
  243. uint32_t chipId = FIELD_GET(regval, TMC5130_VERSION_MASK, TMC5130_VERSION_SHIFT);
  244. return chipId;
  245. }
  246. /***********************************************************************************************************************
  247. * set state *
  248. ***********************************************************************************************************************/
  249. void TMC51X0::setXACTUAL(int32_t value) {
  250. zlock_guard lkg(m_mutex);
  251. writeInt(TMC5130_XACTUAL, to_motor_pos(value));
  252. }
  253. void TMC51X0::setMotorShaft(bool reverse) {
  254. zlock_guard lkg(m_mutex);
  255. uint32_t regval = readUInt(TMC5130_GCONF);
  256. FIELD_SET(regval, TMC5130_SHAFT_MASK, TMC5130_SHAFT_SHIFT, reverse ? 1 : 0);
  257. writeInt(TMC5130_GCONF, regval);
  258. // uint32_t readbak = readUInt(TMC5130_GCONF);
  259. // ZLOGI(TAG, "%x,%x", regval, readbak);
  260. }
  261. void TMC51X0::setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) {
  262. zlock_guard lkg(m_mutex);
  263. writeInt(TMC5130_IHOLD_IRUN, (iholddelay << TMC5130_IHOLDDELAY_SHIFT) | (irun << TMC5130_IRUN_SHIFT) | (ihold << TMC5130_IHOLD_SHIFT));
  264. }
  265. void TMC51X0::setGlobalScale(uint8_t globalscale) {
  266. #define TMC5160_GLOBAL_SCALER 0x0B
  267. #define TMC5160_GLOBAL_SCALER_MASK 0xFF
  268. #define TMC5160_GLOBAL_SCALER_SHIFT 0
  269. zlock_guard lkg(m_mutex);
  270. if (isTMC5130()) {
  271. return;
  272. }
  273. if (globalscale == 0) {
  274. globalscale = 0;
  275. } else if (globalscale <= 31 && globalscale >= 1) {
  276. globalscale = 32;
  277. } else {
  278. globalscale = globalscale;
  279. }
  280. writeInt(TMC5160_GLOBAL_SCALER, (readInt(TMC5160_GLOBAL_SCALER) & ~TMC5160_GLOBAL_SCALER_MASK) | (globalscale << TMC5160_GLOBAL_SCALER_SHIFT));
  281. }
  282. void TMC51X0::setScale(int32_t scale) {
  283. zlock_guard lkg(m_mutex);
  284. m_scale = scale;
  285. }
  286. void TMC51X0::setScaleDenominator(int32_t scale) {
  287. zlock_guard lkg(m_mutex);
  288. m_scale_deceleration = scale;
  289. }
  290. void TMC51X0::setVstart(int32_t val) {
  291. zlock_guard lkg(m_mutex);
  292. writeInt(TMC5130_VSTART, to_motor_vel(val));
  293. }
  294. void TMC51X0::setA1(int32_t val) {
  295. zlock_guard lkg(m_mutex);
  296. writeInt(TMC5130_A1, to_motor_acc(val));
  297. }
  298. void TMC51X0::setAmax(int32_t val) {
  299. zlock_guard lkg(m_mutex);
  300. writeInt(TMC5130_AMAX, to_motor_acc(val));
  301. }
  302. void TMC51X0::setV1(int32_t val) {
  303. zlock_guard lkg(m_mutex);
  304. writeInt(TMC5130_V1, to_motor_vel(val));
  305. }
  306. void TMC51X0::setDmax(int32_t val) {
  307. zlock_guard lkg(m_mutex);
  308. writeInt(TMC5130_DMAX, to_motor_acc(val));
  309. }
  310. void TMC51X0::setD1(int32_t val) {
  311. zlock_guard lkg(m_mutex);
  312. writeInt(TMC5130_D1, to_motor_acc(val));
  313. }
  314. void TMC51X0::setVstop(int32_t val) {
  315. zlock_guard lkg(m_mutex);
  316. writeInt(TMC5130_VSTOP, to_motor_vel(val));
  317. }
  318. void TMC51X0::setTzerowait(int32_t val) {
  319. zlock_guard lkg(m_mutex);
  320. writeInt(TMC5130_TZEROWAIT, val);
  321. }
  322. bool TMC51X0::setEncResolution(int32_t enc_resolution) {
  323. zlock_guard lkg(m_mutex);
  324. /**
  325. * @brief
  326. *
  327. * 1.256
  328. * 2.TMC5130_ENC_CONST是十进制模式
  329. * 3.
  330. */
  331. int32_t enc_resolution_tmp = enc_resolution * 4;
  332. int16_t enc_const_integral = 0;
  333. int16_t enc_const_fractional = 0;
  334. switch (abs(enc_resolution_tmp)) {
  335. case 1000:
  336. enc_const_integral = 51;
  337. enc_const_fractional = 0.2 * 10000;
  338. break;
  339. case 1024:
  340. enc_const_integral = 50;
  341. enc_const_fractional = 0 * 10000;
  342. break;
  343. case 4000:
  344. enc_const_integral = 12;
  345. enc_const_fractional = 0.8 * 10000;
  346. break;
  347. case 4096:
  348. enc_const_integral = 12;
  349. enc_const_fractional = 0.5 * 10000;
  350. break;
  351. case 16384:
  352. enc_const_integral = 3;
  353. enc_const_fractional = 0.125 * 10000;
  354. break;
  355. case 0:
  356. m_enc_resolution = 0;
  357. return true;
  358. default:
  359. return false;
  360. break;
  361. }
  362. m_enc_resolution = enc_resolution;
  363. uint32_t setval = 0;
  364. uint8_t *psetval = (uint8_t *)&setval;
  365. memcpy(psetval + 2, &enc_const_integral, 2);
  366. memcpy(psetval, &enc_const_fractional, 2);
  367. writeInt(TMC5130_ENCMODE, 0x1 << 10);
  368. writeInt(TMC5130_ENC_CONST, setval);
  369. return true;
  370. }
  371. void TMC51X0::setEncVal(int32_t enc_val) {
  372. zlock_guard lkg(m_mutex);
  373. int32_t enc_set_val = to_motor_pos(enc_val);
  374. if (m_enc_resolution < 0) {
  375. enc_set_val = -enc_set_val;
  376. }
  377. writeInt(TMC5130_XENC, enc_set_val);
  378. }
  379. /***********************************************************************************************************************
  380. * reg operation *
  381. ***********************************************************************************************************************/
  382. void TMC51X0::writeIntExt(uint8_t address, int32_t value) {
  383. zlock_guard lkg(m_mutex);
  384. writeInt(address, value);
  385. }
  386. int32_t TMC51X0::readIntExt(uint8_t address) {
  387. zlock_guard lkg(m_mutex);
  388. return readInt(address);
  389. }