PlatformIO package of the Teensy core framework compatible with GCC 10 & C++20
Nie możesz wybrać więcej, niż 25 tematów Tematy muszą się zaczynać od litery lub cyfry, mogą zawierać myślniki ('-') i mogą mieć do 35 znaków.

QuadEncoder.cpp 17KB

3 lat temu
123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539
  1. #include "Arduino.h"
  2. #include "QuadEncoder.h"
  3. //#define DEBUG_OUTPUT
  4. const QuadEncoder::ENC_Channel_t QuadEncoder::channel[] = {
  5. {0,&IMXRT_ENC1, IRQ_ENC1, isrEnc1, 66, 67, 68, 69, 70,&CCM_CCGR4,CCM_CCGR4_ENC1(CCM_CCGR_ON)}, //this is a dummy entry - use 1-4 for channels
  6. {1, &IMXRT_ENC1, IRQ_ENC1, isrEnc1, 66, 67, 68, 69, 70,&CCM_CCGR4,CCM_CCGR4_ENC1(CCM_CCGR_ON)},
  7. {2, &IMXRT_ENC2, IRQ_ENC2, isrEnc2, 71, 72, 73, 74, 75,&CCM_CCGR4,CCM_CCGR4_ENC2(CCM_CCGR_ON)},
  8. {3, &IMXRT_ENC3, IRQ_ENC3, isrEnc3, 76, 77, 78, 79, 80,&CCM_CCGR4,CCM_CCGR4_ENC3(CCM_CCGR_ON)},
  9. {4, &IMXRT_ENC4, IRQ_ENC4, isrEnc4, 81, 82, 83, 84, 95,&CCM_CCGR4,CCM_CCGR4_ENC4(CCM_CCGR_ON)}
  10. };
  11. const uint8_t QuadEncoder::_channel_count = (sizeof(QuadEncoder::channel)/sizeof(QuadEncoder::channel[0]));
  12. //xbara1 pin config
  13. //pin, idx, *reg, alt
  14. #ifdef ARDUINO_TEENSY40
  15. const QuadEncoder::ENC_Hardware_t QuadEncoder::hardware[] = {
  16. {0, 0, &CORE_XIO_PIN0, 1, 17, 1}, {1, 1, &CORE_XIO_PIN1, 1, 16, 0},
  17. {2, 2, &CORE_XIO_PIN2, 3, 6, 0}, {3, 3, &CORE_XIO_PIN3, 3, 7, 0},
  18. {4, 4, &CORE_XIO_PIN4,3, 8, 0}, {5, 5, &CORE_XIO_PIN5, 3, 17, 0},
  19. {7, 7, &CORE_XIO_PIN7, 1, 15, 1}, {8, 8, &CORE_XIO_PIN8, 1, 14, 1},
  20. {9, 30, &CORE_XIO_PIN30, 1, 23, 0}, {10, 31, &CORE_XIO_PIN31, 1, 22, 0},
  21. {11, 33, &CORE_XIO_PIN33, 3, 9, 0}
  22. };
  23. #endif
  24. #ifdef ARDUINO_TEENSY41
  25. const QuadEncoder::ENC_Hardware_t QuadEncoder::hardware[] = {
  26. {0, 0, &CORE_XIO_PIN0, 1, 17, 1}, {1, 1, &CORE_XIO_PIN1, 1, 16, 0},
  27. {2, 2, &CORE_XIO_PIN2, 3, 6, 0}, {3, 3, &CORE_XIO_PIN3, 3, 7, 0},
  28. {4, 4, &CORE_XIO_PIN4,3, 8, 0}, {5, 5, &CORE_XIO_PIN5, 3, 17, 0},
  29. {7, 7, &CORE_XIO_PIN7, 1, 15, 1}, {8, 8, &CORE_XIO_PIN8, 1, 14, 1},
  30. {9, 30, &CORE_XIO_PIN30, 1, 23, 0}, {10, 31, &CORE_XIO_PIN31, 1, 22, 0},
  31. {11, 33, &CORE_XIO_PIN33, 3, 9, 0}, {12, 36, &CORE_XIO_PIN36, 1, 16, 1},
  32. {13, 37, &CORE_XIO_PIN37, 1, 17, 3}
  33. };
  34. #endif
  35. const uint8_t QuadEncoder::_hardware_count = (sizeof(QuadEncoder::hardware)/sizeof(QuadEncoder::hardware[0]));
  36. QuadEncoder * QuadEncoder::list[5];
  37. uint8_t QuadEncoder::compareValueFlag;
  38. uint32_t QuadEncoder::homeCounter;
  39. uint32_t QuadEncoder::indexCounter;
  40. void QuadEncoder::setInitConfig() {
  41. getConfig1(&EncConfig);
  42. }
  43. void QuadEncoder::init()
  44. {
  45. Init(&EncConfig);
  46. setConfigInitialPosition();
  47. enableInterrupts(&EncConfig);
  48. }
  49. QuadEncoder::QuadEncoder(uint8_t encoder_ch, uint8_t PhaseA_pin, uint8_t PhaseB_pin, uint8_t pin_pus, uint8_t index_pin, uint8_t home_pin, uint8_t trigger_pin){
  50. if(encoder_ch >= 0){
  51. _encoder_ch = encoder_ch;
  52. CCM_CCGR2 |= CCM_CCGR2_XBAR1(CCM_CCGR_ON); //turn clock on for xbara1
  53. #ifdef DEBUG_OUTPUT
  54. Serial.printf("begin: encoder channel-> %d\n",_encoder_ch);
  55. Serial.printf("begin: pinA-> %d, pinB-> %d\n", PhaseA_pin, PhaseB_pin);
  56. #endif
  57. if(PhaseA_pin != 255 )
  58. enc_xbara_mapping(PhaseA_pin, PHASEA, pin_pus);
  59. if(PhaseA_pin != 255 )
  60. enc_xbara_mapping(PhaseB_pin, PHASEB, pin_pus);
  61. if(home_pin != 255 )
  62. enc_xbara_mapping(home_pin, HOME, pin_pus);
  63. if(index_pin != 255 )
  64. enc_xbara_mapping(index_pin, INDEX, pin_pus);
  65. if(trigger_pin != 255 )
  66. enc_xbara_mapping(trigger_pin, TRIGGER, pin_pus);
  67. } else {
  68. Serial.println("No Encoder Channel Selected!");
  69. }
  70. disableInterrupts(_positionROEnable);
  71. disableInterrupts(_positionRUEnable);
  72. }
  73. void QuadEncoder::getConfig1(enc_config_t *config)
  74. {
  75. /* Initializes the configure structure to zero. */
  76. memset(config, 0, sizeof(*config));
  77. config->enableReverseDirection = DISABLE;
  78. config->decoderWorkMode = DISABLE;
  79. config->HOMETriggerMode = DISABLE;
  80. config->INDEXTriggerMode = DISABLE;
  81. config->IndexTrigger = DISABLE;
  82. config->HomeTrigger = DISABLE;
  83. config->clearCounter = DISABLE;
  84. config->clearHoldCounter = DISABLE;
  85. config->filterCount = 0;
  86. config->filterSamplePeriod = 0;
  87. config->positionMatchMode = DISABLE;
  88. config->positionCompareValue = 0xffffffff;
  89. config->revolutionCountCondition = DISABLE;
  90. config->enableModuloCountMode = DISABLE;
  91. config->positionModulusValue = 0;
  92. config->positionInitialValue = 0;
  93. config->positionROIE = DISABLE;
  94. config->positionRUIE = DISABLE;
  95. }
  96. void QuadEncoder::printConfig(enc_config_t *config)
  97. {
  98. Serial.printf("\tenableReverseDirection: %d\n",config->enableReverseDirection);
  99. Serial.printf("\tdecoderWorkMode: %d\n",config->decoderWorkMode);
  100. Serial.printf("\tHOMETriggerMode: %d\n",config->HOMETriggerMode);
  101. Serial.printf("\tINDEXTriggerMode: %d\n",config->INDEXTriggerMode);
  102. Serial.printf("\tIndexTrigger: %d\n",config->IndexTrigger);
  103. Serial.printf("\tHomeTrigger: %d\n",config->HomeTrigger);
  104. Serial.printf("\tclearCounter: %d\n",config->clearCounter);
  105. Serial.printf("\tclearHoldCounter: %d\n",config->clearHoldCounter);
  106. Serial.printf("\tfilterCount: %d\n",config->filterCount);
  107. Serial.printf("\tfilterSamplePeriod: %d\n",config->filterSamplePeriod);
  108. Serial.printf("\tpositionCompareValue: %x\n",config->positionCompareValue);
  109. Serial.printf("\trevolutionCountCondition: %d\n",config->clearCounter);
  110. Serial.printf("\tenableModuloCountMode: %d\n",config->clearHoldCounter);
  111. Serial.printf("\tpositionInitialValue: %d\n",config->positionInitialValue);
  112. Serial.printf("\tpositionROIE: %d\n",config->positionROIE);
  113. Serial.printf("\tpositionRUIE: %x\n",config->positionRUIE);
  114. Serial.printf("\n");
  115. }
  116. void QuadEncoder::setConfigInitialPosition()
  117. {
  118. uint16_t tmp16 = channel[_encoder_ch].ENC->CTRL & (uint16_t)(~ENC_CTRL_W1C_FLAGS);
  119. tmp16 |= ENC_CTRL_SWIP_MASK; /* Write 1 to trigger the command for loading initial position value. */
  120. channel[_encoder_ch].ENC->CTRL = tmp16;
  121. }
  122. void QuadEncoder::Init(const enc_config_t *config)
  123. {
  124. uint32_t tmp16;
  125. // make sure the appropriate clock gate is enabled.
  126. *channel[_encoder_ch].clock_gate_register |= channel[_encoder_ch].clock_gate_mask;
  127. /* ENC_CTRL. */
  128. tmp16 = channel[_encoder_ch].ENC->CTRL & (uint16_t)(~(ENC_CTRL_W1C_FLAGS | ENC_CTRL_HIP_MASK | ENC_CTRL_HNE_MASK | ENC_CTRL_REV_MASK | ENC_CTRL_PH1_MASK | ENC_CTRL_XIP_MASK | ENC_CTRL_XNE_MASK | ENC_CTRL_WDE_MASK));
  129. /* For HOME trigger. */
  130. if (config->HOMETriggerMode != DISABLE)
  131. {
  132. tmp16 |= ENC_CTRL_HIP_MASK;
  133. if (FALLING_EDGE == config->HOMETriggerMode)
  134. {
  135. tmp16 |= ENC_CTRL_HNE_MASK;
  136. }
  137. }
  138. /* For encoder work mode. */
  139. if (config->enableReverseDirection)
  140. {
  141. tmp16 |= ENC_CTRL_REV_MASK;
  142. }
  143. if (true == config->decoderWorkMode)
  144. {
  145. tmp16 |= ENC_CTRL_PH1_MASK;
  146. }
  147. /* For INDEX trigger. */
  148. if (DISABLE != config->INDEXTriggerMode)
  149. {
  150. tmp16 |= ENC_CTRL_XIP_MASK;
  151. if (FALLING_EDGE == config->INDEXTriggerMode)
  152. {
  153. tmp16 |= ENC_CTRL_XNE_MASK;
  154. }
  155. }
  156. channel[_encoder_ch].ENC->CTRL = tmp16;
  157. /* ENC_FILT. */
  158. channel[_encoder_ch].ENC->FILT = ENC_FILT_FILT_CNT(config->filterCount) | ENC_FILT_FILT_PER(config->filterSamplePeriod);
  159. /* ENC_CTRL2. */
  160. tmp16 = channel[_encoder_ch].ENC->CTRL2 & (uint16_t)(~(ENC_CTRL2_W1C_FLAGS | ENC_CTRL2_OUTCTL_MASK | ENC_CTRL2_REVMOD_MASK | ENC_CTRL2_MOD_MASK | ENC_CTRL2_UPDPOS_MASK | ENC_CTRL2_UPDHLD_MASK | ENC_CTRL2_ROIRQ_MASK | ENC_CTRL2_RUIRQ_MASK));
  161. if (1 == config->positionMatchMode)
  162. {
  163. tmp16 |= ENC_CTRL2_OUTCTL_MASK;
  164. }
  165. if (1 == config->revolutionCountCondition)
  166. {
  167. tmp16 |= ENC_CTRL2_REVMOD_MASK;
  168. }
  169. if (config->enableModuloCountMode)
  170. {
  171. tmp16 |= ENC_CTRL2_MOD_MASK;
  172. /* Set modulus value. */
  173. channel[_encoder_ch].ENC->UMOD = (uint16_t)(config->positionModulusValue >> 16U); /* Upper 16 bits. */
  174. channel[_encoder_ch].ENC->LMOD = (uint16_t)(config->positionModulusValue); /* Lower 16 bits. */
  175. }
  176. if (config->clearCounter)
  177. {
  178. tmp16 |= ENC_CTRL2_UPDPOS_MASK;
  179. }
  180. if (config->clearHoldCounter)
  181. {
  182. tmp16 |= ENC_CTRL2_UPDHLD_MASK;
  183. }
  184. channel[_encoder_ch].ENC->CTRL2 = tmp16;
  185. /* ENC_UCOMP & ENC_LCOMP. */
  186. channel[_encoder_ch].ENC->UCOMP = (uint16_t)(config->positionCompareValue >> 16U); /* Upper 16 bits. */
  187. channel[_encoder_ch].ENC->LCOMP = (uint16_t)(config->positionCompareValue); /* Lower 16 bits. */
  188. /* ENC_UINIT & ENC_LINIT. */
  189. channel[_encoder_ch].ENC->UINIT = (uint16_t)(config->positionInitialValue >> 16U); /* Upper 16 bits. */
  190. channel[_encoder_ch].ENC->LINIT = (uint16_t)(config->positionInitialValue); /* Lower 16 bits. */
  191. }
  192. int32_t QuadEncoder::read()
  193. {
  194. uint32_t ret32;
  195. ret32 = channel[_encoder_ch].ENC->UPOS; /* Get upper 16 bits and make a snapshot. */
  196. ret32 <<= 16U;
  197. ret32 |= channel[_encoder_ch].ENC->LPOSH; /* Get lower 16 bits from hold register. */
  198. return (int32_t) ret32;
  199. }
  200. void QuadEncoder::write(uint32_t value)
  201. {
  202. channel[_encoder_ch].ENC->UINIT = (uint16_t)(value >> 16U); /* Set upper 16 bits. */
  203. channel[_encoder_ch].ENC->LINIT = (uint16_t)(value); /* Set lower 16 bits. */
  204. setConfigInitialPosition();
  205. }
  206. uint32_t QuadEncoder::getHoldPosition()
  207. {
  208. uint32_t ret32;
  209. ret32 = channel[_encoder_ch].ENC->UPOSH; /* Get upper 16 bits and make a snapshot. */
  210. ret32 <<= 16U;
  211. ret32 |= channel[_encoder_ch].ENC->LPOSH; /* Get lower 16 bits from hold register. */
  212. return ret32;
  213. }
  214. uint16_t QuadEncoder::getPositionDifference()
  215. {
  216. return channel[_encoder_ch].ENC->POSD;
  217. }
  218. uint16_t QuadEncoder::getHoldDifference()
  219. {
  220. return channel[_encoder_ch].ENC->POSDH;
  221. }
  222. uint16_t QuadEncoder::getRevolution()
  223. {
  224. return channel[_encoder_ch].ENC->REV;
  225. }
  226. uint16_t QuadEncoder::getHoldRevolution()
  227. {
  228. return channel[_encoder_ch].ENC->REVH;
  229. }
  230. void QuadEncoder::enc_xbara_mapping(uint8_t pin, uint8_t PHASE, uint8_t PUS){
  231. const struct digital_pin_bitband_and_config_table_struct *p;
  232. for (int idx_channel = 0; idx_channel < _hardware_count; idx_channel++) {
  233. if (hardware[idx_channel].pin == pin) {
  234. _pin_idx = idx_channel;
  235. break;
  236. }
  237. }
  238. #ifdef DEBUG_OUTPUT
  239. Serial.printf("\nxbara_mapping: pin-> %d, idx-> %d\n", pin, _pin_idx);
  240. Serial.printf("xbara_mapping: hw_count-> %d\n",_hardware_count);
  241. Serial.printf("xbara_mapping: hardware[_pin_idx].pin-> %d, .select_val-> %d\n", hardware[_pin_idx].pin, hardware[_pin_idx].select_val);
  242. Serial.printf("xbara_mapping: Encoder-> %d\n", _encoder_ch);
  243. #endif
  244. if (_pin_idx == _hardware_count) return;
  245. p = digital_pin_to_info_PGM + hardware[_pin_idx].pin;
  246. //mux is ctrl config for pin
  247. //pad is pad config
  248. //pinmode = *(p->reg + 1);
  249. //Pin ctrl configuration for encoder/xbara1
  250. *(p->mux) = hardware[_pin_idx].select_val;
  251. //Pad configuration for encoder/xbara1
  252. if(PUS == 0){
  253. *(p->pad) = 0x10B0;
  254. } else {
  255. *(p->pad) = 0x1f038;
  256. }
  257. //x = xio_pin_to_info_PGM + pin;
  258. //*(x->reg) = xbara1_mux[pin];
  259. *hardware[_pin_idx].reg = hardware[_pin_idx].xbarMUX;
  260. #ifdef DEBUG_OUTPUT
  261. if(PHASE == 1) Serial.printf("xbarIO-> %d, PhaseA-> %d\n",hardware[_pin_idx].xbarIO, channel[_encoder_ch].phaseA);
  262. if(PHASE == 2)Serial.printf("xbarIO-> %d, PhaseB-> %d\n",hardware[_pin_idx].xbarIO, channel[_encoder_ch].phaseB);
  263. if(PHASE == 3)Serial.printf("xbarIO-> %d, INDEX-> %d\n",hardware[_pin_idx].xbarIO, channel[_encoder_ch].index);
  264. if(PHASE == 4)Serial.printf("xbarIO-> %d, HOME-> %d\n",hardware[_pin_idx].xbarIO, channel[_encoder_ch].home);
  265. #endif
  266. //XBARA1 Connection to encoder
  267. if(PHASE == 1) xbar_connect(hardware[_pin_idx].xbarIO, channel[_encoder_ch].phaseA);
  268. if(PHASE == 2) xbar_connect(hardware[_pin_idx].xbarIO, channel[_encoder_ch].phaseB);
  269. if(PHASE == 3) xbar_connect(hardware[_pin_idx].xbarIO, channel[_encoder_ch].index);
  270. if(PHASE == 4) xbar_connect(hardware[_pin_idx].xbarIO, channel[_encoder_ch].home);
  271. if(PHASE == 5) xbar_connect(hardware[_pin_idx].xbarIO, channel[_encoder_ch].trigger);
  272. }
  273. void QuadEncoder::xbar_connect(unsigned int input, unsigned int output)
  274. {
  275. if (input >= 88) return;
  276. if (output >= 132) return;
  277. #if 1
  278. volatile uint16_t *xbar = &XBARA1_SEL0 + (output / 2);
  279. uint16_t val = *xbar;
  280. if (!(output & 1)) {
  281. val = (val & 0xFF00) | input;
  282. } else {
  283. val = (val & 0x00FF) | (input << 8);
  284. }
  285. *xbar = val;
  286. #else
  287. // does not work, seems 8 bit access is not allowed
  288. volatile uint8_t *xbar = (volatile uint8_t *)XBARA1_SEL0;
  289. xbar[output] = input;
  290. #endif
  291. }
  292. void QuadEncoder::enableInterrupts(const enc_config_t *config)
  293. {
  294. uint32_t tmp16 = 0;
  295. //enable interrupt
  296. NVIC_SET_PRIORITY(channel[_encoder_ch].interrupt, 32);
  297. NVIC_ENABLE_IRQ(channel[_encoder_ch].interrupt);
  298. attachInterruptVector(channel[_encoder_ch].interrupt, channel[_encoder_ch].isr);
  299. /* ENC_CTRL. */
  300. if (config->HomeTrigger != DISABLE)
  301. {
  302. tmp16 |= ENC_CTRL_HIE_MASK;
  303. }
  304. if (config->IndexTrigger != DISABLE)
  305. {
  306. tmp16 |= ENC_CTRL_XIE_MASK;
  307. }
  308. if (config->positionMatchMode == ENABLE)
  309. {
  310. tmp16 |= ENC_CTRL_CMPIE_MASK;
  311. }
  312. if (tmp16 != 0)
  313. {
  314. channel[_encoder_ch].ENC->CTRL = (channel[_encoder_ch].ENC->CTRL & (uint16_t)(~ENC_CTRL_W1C_FLAGS)) | tmp16;
  315. }
  316. /* ENC_CTRL2. */
  317. tmp16 = 0U;
  318. if (config->positionROIE != DISABLE)
  319. {
  320. tmp16 |= ENC_CTRL2_ROIRQ_MASK;
  321. }
  322. if (config->positionRUIE != DISABLE)
  323. {
  324. tmp16 |= ENC_CTRL2_RUIRQ_MASK;
  325. }
  326. if (tmp16 != 0U)
  327. {
  328. channel[_encoder_ch].ENC->CTRL2 = (uint16_t)(channel[_encoder_ch].ENC->CTRL2 & (uint16_t)(~ENC_CTRL2_W1C_FLAGS)) & (uint16_t)(~tmp16);
  329. }
  330. }
  331. void QuadEncoder::disableInterrupts(uint32_t flag)
  332. {
  333. uint16_t tmp16 = 0U;
  334. /* ENC_CTRL. */
  335. if (_HOMETransitionEnable == (_HOMETransitionEnable & flag))
  336. {
  337. tmp16 |= ENC_CTRL_HIE_MASK;
  338. }
  339. if (_INDEXPulseEnable == (_INDEXPulseEnable & flag))
  340. {
  341. tmp16 |= ENC_CTRL_XIE_MASK;
  342. }
  343. if (_positionCompareEnable == (_positionCompareEnable & flag))
  344. {
  345. tmp16 |= ENC_CTRL_CMPIE_MASK;
  346. }
  347. if (0U != tmp16)
  348. {
  349. channel[_encoder_ch].ENC->CTRL = (uint16_t)(channel[_encoder_ch].ENC->CTRL & (uint16_t)(~ENC_CTRL_W1C_FLAGS)) & (uint16_t)(~tmp16);
  350. }
  351. /* ENC_CTRL2. */
  352. tmp16 = 0U;
  353. if (_positionRollOverFlag == (_positionRollOverFlag & flag))
  354. {
  355. tmp16 |= ENC_CTRL2_ROIRQ_MASK;
  356. }
  357. if (_positionRollUnderFlag == (_positionRollUnderFlag & flag))
  358. {
  359. tmp16 |= ENC_CTRL2_RUIRQ_MASK;
  360. }
  361. if (tmp16 != 0U)
  362. {
  363. channel[_encoder_ch].ENC->CTRL2 = (uint16_t)(channel[_encoder_ch].ENC->CTRL2 & (uint16_t)(~ENC_CTRL2_W1C_FLAGS)) & (uint16_t)(~tmp16);
  364. }
  365. }
  366. void QuadEncoder::clearStatusFlags(uint32_t flag, uint8_t index)
  367. {
  368. uint32_t tmp16 = 0U;
  369. /* ENC_CTRL. */
  370. if (_HOMETransitionFlag == (_HOMETransitionFlag & flag))
  371. {
  372. tmp16 |= ENC_CTRL_HIRQ_MASK;
  373. }
  374. if (_INDEXPulseFlag == (_INDEXPulseFlag & flag))
  375. {
  376. tmp16 |= ENC_CTRL_XIRQ_MASK;
  377. }
  378. if (_positionCompareFlag == (_positionCompareFlag & flag))
  379. {
  380. tmp16 |= ENC_CTRL_CMPIRQ_MASK;
  381. }
  382. if (0U != tmp16)
  383. {
  384. channel[index].ENC->CTRL = (channel[index].ENC->CTRL & (uint16_t)(~ENC_CTRL_W1C_FLAGS)) | tmp16;
  385. }
  386. /* ENC_CTRL2. */
  387. tmp16 = 0U;
  388. if (_positionRollOverFlag == (_positionRollOverFlag & flag))
  389. {
  390. tmp16 |= ENC_CTRL2_ROIRQ_MASK;
  391. }
  392. if (_positionRollUnderFlag == (_positionRollUnderFlag & flag))
  393. {
  394. tmp16 |= ENC_CTRL2_RUIRQ_MASK;
  395. }
  396. if (0U != tmp16)
  397. {
  398. channel[index].ENC->CTRL2 = (channel[index].ENC->CTRL2 & (uint16_t)(~ENC_CTRL2_W1C_FLAGS)) | tmp16;
  399. }
  400. }
  401. inline void QuadEncoder::checkAndProcessInterrupt(uint8_t index)
  402. {
  403. list[index]->isr(index);
  404. }
  405. void QuadEncoder::isrEnc1()
  406. {
  407. checkAndProcessInterrupt(1);
  408. asm volatile ("dsb"); // wait for clear memory barrier
  409. }
  410. void QuadEncoder::isrEnc2()
  411. {
  412. checkAndProcessInterrupt(2);
  413. asm volatile ("dsb"); // wait for clear memory barrier
  414. }
  415. void QuadEncoder::isrEnc3()
  416. {
  417. checkAndProcessInterrupt(3);
  418. asm volatile ("dsb"); // wait for clear memory barrier
  419. }
  420. void QuadEncoder::isrEnc4()
  421. {
  422. checkAndProcessInterrupt(4);
  423. asm volatile ("dsb"); // wait for clear memory barrier
  424. }
  425. void QuadEncoder::isr(uint8_t index)
  426. {
  427. if (ENC_CTRL_XIRQ_MASK == (ENC_CTRL_XIRQ_MASK & channel[index].ENC->CTRL) && (ENC_CTRL_XIE_MASK & channel[index].ENC->CTRL))
  428. {
  429. indexCounter = indexCounter + 1;
  430. clearStatusFlags(_INDEXPulseFlag, index);
  431. if(ENC_CTRL2_ROIRQ_MASK == (ENC_CTRL2_ROIRQ_MASK & channel[index].ENC->CTRL2))
  432. {
  433. //Serial.println("ROLL OVER INDICATED!!!!!");
  434. //Serial.println("=========================");
  435. //Serial.println();
  436. clearStatusFlags(_positionRollOverFlag, index);
  437. }
  438. if(ENC_CTRL2_RUIRQ_MASK == (ENC_CTRL2_RUIRQ_MASK & channel[index].ENC->CTRL2))
  439. {
  440. //Serial.println("ROLL UNDER INDICATED!!!!!");
  441. //Serial.println("=========================");
  442. //Serial.println();
  443. clearStatusFlags(_positionRollUnderFlag, index);
  444. }
  445. }
  446. if (ENC_CTRL_HIRQ_MASK == (ENC_CTRL_HIRQ_MASK & channel[index].ENC->CTRL) && (ENC_CTRL_HIE_MASK & channel[index].ENC->CTRL))
  447. {
  448. homeCounter++;
  449. clearStatusFlags(_HOMETransitionFlag, index);
  450. }
  451. if (ENC_CTRL_CMPIRQ_MASK == (ENC_CTRL_CMPIRQ_MASK & channel[index].ENC->CTRL))
  452. {
  453. compareValueFlag = 1;
  454. clearStatusFlags(_positionCompareFlag, index);
  455. }
  456. }