visus
/
framework-arduinoteensy-cxx20
Sledovat 1
Oblíbit 0
Rozštěpit 0
Zdrojový kód Úkoly 0 Požadavky na natažení 0 Vydání 0 Wiki Aktivita
PlatformIO package of the Teensy core framework compatible with GCC 10 & C++20
Nelze vybrat více než 25 témat Téma musí začínat písmenem nebo číslem, může obsahovat pomlčky („-“) a může být dlouhé až 35 znaků.
3 Revize
1 Větev
Strom: 8b2d08b377
Větve Značky
${ item.name }
Vytvořit větev ${ searchTerm }
z „8b2d08b377“
${ noResults }
framework-arduinoteensy-cxx20/libraries/QuadEncoder/QuadEncoder.cpp

540 lines
17KB
Surový Blame Historie 

  1. #include "Arduino.h"

  2. #include "QuadEncoder.h"

  3. 

  4. //#define DEBUG_OUTPUT

  5. 

  6. const QuadEncoder::ENC_Channel_t QuadEncoder::channel[] = {	

  7. 	{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

  8. 	{1, &IMXRT_ENC1, IRQ_ENC1, isrEnc1, 66, 67, 68, 69, 70,&CCM_CCGR4,CCM_CCGR4_ENC1(CCM_CCGR_ON)},

  9. 	{2, &IMXRT_ENC2, IRQ_ENC2, isrEnc2, 71, 72, 73, 74, 75,&CCM_CCGR4,CCM_CCGR4_ENC2(CCM_CCGR_ON)},

  10. 	{3, &IMXRT_ENC3, IRQ_ENC3, isrEnc3, 76, 77, 78, 79, 80,&CCM_CCGR4,CCM_CCGR4_ENC3(CCM_CCGR_ON)},

  11. 	{4, &IMXRT_ENC4, IRQ_ENC4, isrEnc4, 81, 82, 83, 84, 95,&CCM_CCGR4,CCM_CCGR4_ENC4(CCM_CCGR_ON)}

  12. };

  13. const uint8_t QuadEncoder::_channel_count =  (sizeof(QuadEncoder::channel)/sizeof(QuadEncoder::channel[0]));

  14. 

  15. //xbara1 pin config

  16. //pin, idx, *reg, alt

  17. #ifdef ARDUINO_TEENSY40

  18. const  QuadEncoder::ENC_Hardware_t QuadEncoder::hardware[] = {	

  19. 	{0, 0, &CORE_XIO_PIN0, 1, 17, 1},	{1, 1, &CORE_XIO_PIN1, 1, 16, 0},

  20. 	{2, 2, &CORE_XIO_PIN2, 3, 6, 0},	{3, 3, &CORE_XIO_PIN3, 3, 7, 0},

  21. 	{4, 4, &CORE_XIO_PIN4,3, 8, 0},		{5, 5, &CORE_XIO_PIN5, 3, 17, 0},

  22. 	{7, 7, &CORE_XIO_PIN7, 1, 15, 1},	{8, 8, &CORE_XIO_PIN8, 1, 14, 1},

  23. 	{9, 30, &CORE_XIO_PIN30, 1, 23, 0},	{10, 31, &CORE_XIO_PIN31, 1, 22, 0},

  24. 	{11, 33, &CORE_XIO_PIN33, 3, 9, 0}

  25. };

  26. #endif

  27. #ifdef ARDUINO_TEENSY41

  28. const  QuadEncoder::ENC_Hardware_t QuadEncoder::hardware[] = {	

  29. 	{0, 0, &CORE_XIO_PIN0, 1, 17, 1},	{1, 1, &CORE_XIO_PIN1, 1, 16, 0},

  30. 	{2, 2, &CORE_XIO_PIN2, 3, 6, 0},	{3, 3, &CORE_XIO_PIN3, 3, 7, 0},

  31. 	{4, 4, &CORE_XIO_PIN4,3, 8, 0},		{5, 5, &CORE_XIO_PIN5, 3, 17, 0},

  32. 	{7, 7, &CORE_XIO_PIN7, 1, 15, 1},	{8, 8, &CORE_XIO_PIN8, 1, 14, 1},

  33. 	{9, 30, &CORE_XIO_PIN30, 1, 23, 0},	{10, 31, &CORE_XIO_PIN31, 1, 22, 0},

  34. 	{11, 33, &CORE_XIO_PIN33, 3, 9, 0},	{12, 36, &CORE_XIO_PIN36, 1, 16, 1},

  35. 	{13, 37, &CORE_XIO_PIN37, 1, 17, 3}

  36. 

  37. };

  38. #endif

  39. const uint8_t QuadEncoder::_hardware_count =  (sizeof(QuadEncoder::hardware)/sizeof(QuadEncoder::hardware[0]));

  40. 

  41. QuadEncoder * QuadEncoder::list[5];

  42. 

  43.   uint8_t QuadEncoder::compareValueFlag;

  44.  uint32_t QuadEncoder::homeCounter;

  45.  uint32_t QuadEncoder::indexCounter;

  46. 

  47.  

  48. void QuadEncoder::setInitConfig() {

  49.   getConfig1(&EncConfig);

  50. }

  51. 

  52. void QuadEncoder::init()

  53. {

  54.   Init(&EncConfig);

  55.   setConfigInitialPosition();

  56.   enableInterrupts(&EncConfig);

  57. }

  58. 

  59. 

  60. 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){

  61. 	if(encoder_ch >= 0){

  62. 	  _encoder_ch = encoder_ch;

  63. 	

  64. 	CCM_CCGR2 |= CCM_CCGR2_XBAR1(CCM_CCGR_ON);   //turn clock on for xbara1

  65.   

  66. #ifdef DEBUG_OUTPUT

  67. 	Serial.printf("begin: encoder channel-> %d\n",_encoder_ch);

  68. 	Serial.printf("begin: pinA-> %d, pinB-> %d\n", PhaseA_pin, PhaseB_pin);

  69. #endif

  70. 	  if(PhaseA_pin != 255 )

  71. 		enc_xbara_mapping(PhaseA_pin, PHASEA, pin_pus);

  72. 	  if(PhaseA_pin != 255 )

  73. 		enc_xbara_mapping(PhaseB_pin, PHASEB, pin_pus);

  74. 	  if(home_pin != 255 )

  75. 		 enc_xbara_mapping(home_pin, HOME, pin_pus);

  76. 	  if(index_pin != 255 )

  77. 		 enc_xbara_mapping(index_pin, INDEX, pin_pus);

  78. 	  if(trigger_pin != 255 )

  79. 		 enc_xbara_mapping(trigger_pin, TRIGGER, pin_pus);

  80. 	} else {

  81. 	  Serial.println("No Encoder Channel Selected!");

  82. 	}

  83. 	

  84. 	disableInterrupts(_positionROEnable);

  85. 	disableInterrupts(_positionRUEnable);

  86. }

  87. 

  88. void QuadEncoder::getConfig1(enc_config_t *config)

  89. {

  90.     /* Initializes the configure structure to zero. */

  91.     memset(config, 0, sizeof(*config));

  92. 

  93.     config->enableReverseDirection = DISABLE;

  94.     config->decoderWorkMode = DISABLE;

  95.     config->HOMETriggerMode = DISABLE;

  96.     config->INDEXTriggerMode = DISABLE;

  97. 	config->IndexTrigger = DISABLE;

  98. 	config->HomeTrigger = DISABLE;

  99.     config->clearCounter = DISABLE;

  100.     config->clearHoldCounter = DISABLE;

  101.     config->filterCount = 0;

  102.     config->filterSamplePeriod = 0;

  103.     config->positionMatchMode = DISABLE;

  104.     config->positionCompareValue = 0xffffffff;

  105.     config->revolutionCountCondition = DISABLE;

  106.     config->enableModuloCountMode = DISABLE;

  107.     config->positionModulusValue = 0;

  108.     config->positionInitialValue = 0;

  109. 	config->positionROIE = DISABLE;

  110. 	config->positionRUIE = DISABLE;

  111. 	

  112. }

  113. 

  114. void QuadEncoder::printConfig(enc_config_t *config)

  115. {

  116. 	Serial.printf("\tenableReverseDirection: %d\n",config->enableReverseDirection);

  117. 	Serial.printf("\tdecoderWorkMode: %d\n",config->decoderWorkMode);

  118. 	Serial.printf("\tHOMETriggerMode: %d\n",config->HOMETriggerMode);

  119. 	Serial.printf("\tINDEXTriggerMode: %d\n",config->INDEXTriggerMode);

  120. 	Serial.printf("\tIndexTrigger: %d\n",config->IndexTrigger);

  121. 	Serial.printf("\tHomeTrigger: %d\n",config->HomeTrigger);	

  122. 	

  123. 	Serial.printf("\tclearCounter: %d\n",config->clearCounter);

  124. 	Serial.printf("\tclearHoldCounter: %d\n",config->clearHoldCounter);

  125. 	

  126. 	Serial.printf("\tfilterCount: %d\n",config->filterCount);

  127. 	Serial.printf("\tfilterSamplePeriod: %d\n",config->filterSamplePeriod);

  128. 	Serial.printf("\tpositionCompareValue: %x\n",config->positionCompareValue);

  129. 	Serial.printf("\trevolutionCountCondition: %d\n",config->clearCounter);

  130. 	Serial.printf("\tenableModuloCountMode: %d\n",config->clearHoldCounter);

  131. 	

  132. 	Serial.printf("\tpositionInitialValue: %d\n",config->positionInitialValue);

  133. 	Serial.printf("\tpositionROIE: %d\n",config->positionROIE);

  134. 	Serial.printf("\tpositionRUIE: %x\n",config->positionRUIE);

  135. 	Serial.printf("\n");

  136. }

  137. 

  138. void QuadEncoder::setConfigInitialPosition()

  139. {

  140.     uint16_t tmp16 = channel[_encoder_ch].ENC->CTRL & (uint16_t)(~ENC_CTRL_W1C_FLAGS);

  141. 

  142.     tmp16 |= ENC_CTRL_SWIP_MASK; /* Write 1 to trigger the command for loading initial position value. */

  143.     channel[_encoder_ch].ENC->CTRL = tmp16;

  144. }

  145. 

  146. void QuadEncoder::Init(const enc_config_t *config)

  147. {

  148.     uint32_t tmp16;

  149. 

  150. 	// make sure the appropriate clock gate is enabled.

  151. 	*channel[_encoder_ch].clock_gate_register |= channel[_encoder_ch].clock_gate_mask;

  152. 

  153.     /* ENC_CTRL. */

  154.     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));

  155. 	

  156.     /* For HOME trigger. */

  157.     if (config->HOMETriggerMode != DISABLE)

  158.     {

  159.         tmp16 |= ENC_CTRL_HIP_MASK;

  160.         if (FALLING_EDGE == config->HOMETriggerMode)

  161.         {

  162.             tmp16 |= ENC_CTRL_HNE_MASK;

  163.         }

  164.     }

  165. 	

  166.     /* For encoder work mode. */

  167.     if (config->enableReverseDirection)

  168.     {

  169.         tmp16 |= ENC_CTRL_REV_MASK;

  170.     }

  171.     if (true == config->decoderWorkMode)

  172.     {

  173.         tmp16 |= ENC_CTRL_PH1_MASK;

  174.     }

  175.     /* For INDEX trigger. */

  176.     if (DISABLE != config->INDEXTriggerMode)

  177.     {

  178.         tmp16 |= ENC_CTRL_XIP_MASK;

  179.         if (FALLING_EDGE == config->INDEXTriggerMode)

  180.         {

  181.             tmp16 |= ENC_CTRL_XNE_MASK;

  182.         }

  183.     }

  184. 

  185.     channel[_encoder_ch].ENC->CTRL = tmp16;

  186. 	

  187.     /* ENC_FILT. */

  188.     channel[_encoder_ch].ENC->FILT = ENC_FILT_FILT_CNT(config->filterCount) | ENC_FILT_FILT_PER(config->filterSamplePeriod);

  189. 

  190.     /* ENC_CTRL2. */

  191.     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));

  192. 	

  193.     if (1 == config->positionMatchMode)

  194.     {

  195.         tmp16 |= ENC_CTRL2_OUTCTL_MASK;

  196.     }

  197.     if (1 == config->revolutionCountCondition)

  198.     {

  199.         tmp16 |= ENC_CTRL2_REVMOD_MASK;

  200.     }

  201.     if (config->enableModuloCountMode)

  202.     {

  203.         tmp16 |= ENC_CTRL2_MOD_MASK;

  204.         /* Set modulus value. */

  205.         channel[_encoder_ch].ENC->UMOD = (uint16_t)(config->positionModulusValue >> 16U); /* Upper 16 bits. */

  206.         channel[_encoder_ch].ENC->LMOD = (uint16_t)(config->positionModulusValue);        /* Lower 16 bits. */

  207.     }

  208.     if (config->clearCounter)

  209.     {

  210.         tmp16 |= ENC_CTRL2_UPDPOS_MASK;

  211.     }

  212.     if (config->clearHoldCounter)

  213.     {

  214.         tmp16 |= ENC_CTRL2_UPDHLD_MASK;

  215.     }

  216.     channel[_encoder_ch].ENC->CTRL2 = tmp16;

  217. 

  218.     /* ENC_UCOMP & ENC_LCOMP. */

  219.     channel[_encoder_ch].ENC->UCOMP = (uint16_t)(config->positionCompareValue >> 16U); /* Upper 16 bits. */

  220.     channel[_encoder_ch].ENC->LCOMP = (uint16_t)(config->positionCompareValue);        /* Lower 16 bits. */

  221. 

  222.     /* ENC_UINIT & ENC_LINIT. */

  223.     channel[_encoder_ch].ENC->UINIT = (uint16_t)(config->positionInitialValue >> 16U); /* Upper 16 bits. */

  224.     channel[_encoder_ch].ENC->LINIT = (uint16_t)(config->positionInitialValue);        /* Lower 16 bits. */

  225. 	

  226. }

  227. 

  228. int32_t QuadEncoder::read()

  229. {

  230.     uint32_t ret32;

  231. 

  232.     ret32 = channel[_encoder_ch].ENC->UPOS; /* Get upper 16 bits and make a snapshot. */

  233.     ret32 <<= 16U;

  234.     ret32 |= channel[_encoder_ch].ENC->LPOSH; /* Get lower 16 bits from hold register. */

  235. 

  236.     return (int32_t) ret32;

  237. }

  238. 

  239. void QuadEncoder::write(uint32_t value)

  240. {

  241.     channel[_encoder_ch].ENC->UINIT = (uint16_t)(value >> 16U); /* Set upper 16 bits. */

  242.     channel[_encoder_ch].ENC->LINIT = (uint16_t)(value);        /* Set lower 16 bits. */

  243. 	

  244. 	setConfigInitialPosition();

  245. }

  246. 

  247. uint32_t QuadEncoder::getHoldPosition()

  248. {

  249.     uint32_t ret32;

  250. 

  251.     ret32 = channel[_encoder_ch].ENC->UPOSH; /* Get upper 16 bits and make a snapshot. */

  252.     ret32 <<= 16U;

  253.     ret32 |= channel[_encoder_ch].ENC->LPOSH; /* Get lower 16 bits from hold register. */

  254. 

  255.     return ret32;

  256. }

  257. 

  258. 

  259. uint16_t QuadEncoder::getPositionDifference()

  260. {

  261.     return channel[_encoder_ch].ENC->POSD;

  262. }

  263. 

  264. 

  265. uint16_t QuadEncoder::getHoldDifference()

  266. {

  267.     return channel[_encoder_ch].ENC->POSDH;

  268. }

  269. 

  270. 

  271. uint16_t QuadEncoder::getRevolution()

  272. {

  273.     return channel[_encoder_ch].ENC->REV;

  274. }

  275. 

  276. uint16_t QuadEncoder::getHoldRevolution()

  277. {

  278.     return channel[_encoder_ch].ENC->REVH;

  279. }

  280. 

  281. void QuadEncoder::enc_xbara_mapping(uint8_t pin, uint8_t PHASE, uint8_t PUS){ 

  282. 

  283.   const struct digital_pin_bitband_and_config_table_struct *p;

  284. 

  285.   for (int idx_channel = 0; idx_channel < _hardware_count; idx_channel++) {

  286. 	if (hardware[idx_channel].pin == pin) {

  287. 		_pin_idx = idx_channel;

  288. 		break;	

  289. 	}

  290.   }

  291. #ifdef DEBUG_OUTPUT

  292.   Serial.printf("\nxbara_mapping: pin-> %d, idx-> %d\n", pin, _pin_idx);

  293.   Serial.printf("xbara_mapping: hw_count-> %d\n",_hardware_count); 

  294.   Serial.printf("xbara_mapping: hardware[_pin_idx].pin-> %d, .select_val-> %d\n", hardware[_pin_idx].pin, hardware[_pin_idx].select_val);

  295.   Serial.printf("xbara_mapping: Encoder-> %d\n", _encoder_ch);

  296. #endif

  297. 

  298.   if (_pin_idx == _hardware_count) return;

  299.     

  300.   p = digital_pin_to_info_PGM + hardware[_pin_idx].pin;

  301.   //mux is ctrl config for pin

  302.   //pad is pad config

  303.   //pinmode = *(p->reg + 1);

  304.   

  305.   //Pin ctrl configuration for encoder/xbara1

  306.     *(p->mux) = hardware[_pin_idx].select_val;

  307.   

  308.   //Pad configuration for encoder/xbara1

  309.   if(PUS == 0){

  310.     *(p->pad) = 0x10B0;

  311.   } else {

  312.     *(p->pad) = 0x1f038;

  313.   }

  314.   

  315.   //x = xio_pin_to_info_PGM + pin;

  316.   //*(x->reg) = xbara1_mux[pin];

  317.   *hardware[_pin_idx].reg = hardware[_pin_idx].xbarMUX;

  318. #ifdef DEBUG_OUTPUT

  319.   if(PHASE == 1) Serial.printf("xbarIO-> %d, PhaseA-> %d\n",hardware[_pin_idx].xbarIO, channel[_encoder_ch].phaseA);

  320.   if(PHASE == 2)Serial.printf("xbarIO-> %d, PhaseB-> %d\n",hardware[_pin_idx].xbarIO, channel[_encoder_ch].phaseB);

  321.   if(PHASE == 3)Serial.printf("xbarIO-> %d, INDEX-> %d\n",hardware[_pin_idx].xbarIO, channel[_encoder_ch].index);

  322.   if(PHASE == 4)Serial.printf("xbarIO-> %d, HOME-> %d\n",hardware[_pin_idx].xbarIO, channel[_encoder_ch].home);

  323. #endif

  324. 

  325.   //XBARA1 Connection to encoder

  326.   if(PHASE == 1) xbar_connect(hardware[_pin_idx].xbarIO, channel[_encoder_ch].phaseA);

  327.   if(PHASE == 2) xbar_connect(hardware[_pin_idx].xbarIO, channel[_encoder_ch].phaseB);

  328.   if(PHASE == 3) xbar_connect(hardware[_pin_idx].xbarIO, channel[_encoder_ch].index);

  329.   if(PHASE == 4) xbar_connect(hardware[_pin_idx].xbarIO, channel[_encoder_ch].home);

  330.   if(PHASE == 5) xbar_connect(hardware[_pin_idx].xbarIO, channel[_encoder_ch].trigger);

  331. }

  332. 

  333. void QuadEncoder::xbar_connect(unsigned int input, unsigned int output)

  334. {

  335.   if (input >= 88) return;

  336.   if (output >= 132) return;

  337. #if 1

  338.   volatile uint16_t *xbar = &XBARA1_SEL0 + (output / 2);

  339.   uint16_t val = *xbar;

  340.   if (!(output & 1)) {

  341.     val = (val & 0xFF00) | input;

  342.   } else {

  343.     val = (val & 0x00FF) | (input << 8);

  344.   }

  345.   *xbar = val;

  346. #else

  347.   // does not work, seems 8 bit access is not allowed

  348.   volatile uint8_t *xbar = (volatile uint8_t *)XBARA1_SEL0;

  349.   xbar[output] = input;

  350. #endif

  351. }

  352. 

  353. void QuadEncoder::enableInterrupts(const enc_config_t *config)

  354. {

  355.     uint32_t tmp16 = 0;

  356. 	

  357. 	//enable interrupt

  358. 	NVIC_SET_PRIORITY(channel[_encoder_ch].interrupt, 32);

  359. 	NVIC_ENABLE_IRQ(channel[_encoder_ch].interrupt);

  360. 	attachInterruptVector(channel[_encoder_ch].interrupt, channel[_encoder_ch].isr);

  361. 	

  362.     /* ENC_CTRL. */

  363.     if (config->HomeTrigger != DISABLE)

  364.     {

  365.         tmp16 |= ENC_CTRL_HIE_MASK;

  366.     }

  367.     if (config->IndexTrigger != DISABLE)

  368.     {

  369.         tmp16 |= ENC_CTRL_XIE_MASK;

  370.     }

  371. 

  372.     if (config->positionMatchMode == ENABLE)

  373.     {

  374.         tmp16 |= ENC_CTRL_CMPIE_MASK;

  375.     }

  376.     if (tmp16 != 0)

  377.     {

  378.         channel[_encoder_ch].ENC->CTRL = (channel[_encoder_ch].ENC->CTRL & (uint16_t)(~ENC_CTRL_W1C_FLAGS)) | tmp16;

  379.     }

  380. 	/* ENC_CTRL2. */

  381.     tmp16 = 0U;

  382.     if (config->positionROIE != DISABLE)

  383.     {

  384.         tmp16 |= ENC_CTRL2_ROIRQ_MASK;

  385.     }

  386.     if (config->positionRUIE != DISABLE)

  387.     {

  388.         tmp16 |= ENC_CTRL2_RUIRQ_MASK;

  389.     }

  390.     if (tmp16 != 0U)

  391.     {

  392.         channel[_encoder_ch].ENC->CTRL2 = (uint16_t)(channel[_encoder_ch].ENC->CTRL2 & (uint16_t)(~ENC_CTRL2_W1C_FLAGS)) & (uint16_t)(~tmp16);

  393.     }

  394. 	

  395. }

  396. 

  397. 

  398. void QuadEncoder::disableInterrupts(uint32_t flag)

  399. {

  400.     uint16_t tmp16 = 0U;

  401. 

  402.     /* ENC_CTRL. */

  403.     if (_HOMETransitionEnable == (_HOMETransitionEnable & flag))

  404.     {

  405.         tmp16 |= ENC_CTRL_HIE_MASK;

  406.     }

  407.     if (_INDEXPulseEnable == (_INDEXPulseEnable & flag))

  408.     {

  409.         tmp16 |= ENC_CTRL_XIE_MASK;

  410.     }

  411.     if (_positionCompareEnable == (_positionCompareEnable & flag))

  412.     {

  413.         tmp16 |= ENC_CTRL_CMPIE_MASK;

  414.     }

  415.     if (0U != tmp16)

  416.     {

  417.         channel[_encoder_ch].ENC->CTRL = (uint16_t)(channel[_encoder_ch].ENC->CTRL & (uint16_t)(~ENC_CTRL_W1C_FLAGS)) & (uint16_t)(~tmp16);

  418.     }

  419.     /* ENC_CTRL2. */

  420.     tmp16 = 0U;

  421.     if (_positionRollOverFlag == (_positionRollOverFlag & flag))

  422.     {

  423.         tmp16 |= ENC_CTRL2_ROIRQ_MASK;

  424.     }

  425.     if (_positionRollUnderFlag == (_positionRollUnderFlag & flag))

  426.     {

  427.         tmp16 |= ENC_CTRL2_RUIRQ_MASK;

  428.     }

  429.     if (tmp16 != 0U)

  430.     {

  431.         channel[_encoder_ch].ENC->CTRL2 = (uint16_t)(channel[_encoder_ch].ENC->CTRL2 & (uint16_t)(~ENC_CTRL2_W1C_FLAGS)) & (uint16_t)(~tmp16);

  432.     }

  433. }

  434. 

  435. 

  436. void QuadEncoder::clearStatusFlags(uint32_t flag, uint8_t index)

  437. {

  438.     uint32_t tmp16 = 0U;

  439. 

  440.     /* ENC_CTRL. */

  441.     if (_HOMETransitionFlag == (_HOMETransitionFlag & flag))

  442.     {

  443.         tmp16 |= ENC_CTRL_HIRQ_MASK;

  444.     }

  445.     if (_INDEXPulseFlag == (_INDEXPulseFlag & flag))

  446.     {

  447.         tmp16 |= ENC_CTRL_XIRQ_MASK;

  448.     }

  449.     if (_positionCompareFlag == (_positionCompareFlag & flag))

  450.     {

  451.         tmp16 |= ENC_CTRL_CMPIRQ_MASK;

  452.     }

  453.     if (0U != tmp16)

  454.     {

  455.         channel[index].ENC->CTRL = (channel[index].ENC->CTRL & (uint16_t)(~ENC_CTRL_W1C_FLAGS)) | tmp16;

  456.     }

  457.     /* ENC_CTRL2. */

  458.     tmp16 = 0U;

  459.     if (_positionRollOverFlag == (_positionRollOverFlag & flag))

  460.     {

  461.         tmp16 |= ENC_CTRL2_ROIRQ_MASK;

  462.     }

  463.     if (_positionRollUnderFlag == (_positionRollUnderFlag & flag))

  464.     {

  465.         tmp16 |= ENC_CTRL2_RUIRQ_MASK;

  466.     }

  467.     if (0U != tmp16)

  468.     {

  469.         channel[index].ENC->CTRL2 = (channel[index].ENC->CTRL2 & (uint16_t)(~ENC_CTRL2_W1C_FLAGS)) | tmp16;

  470.     }

  471. }

  472. 

  473. inline void QuadEncoder::checkAndProcessInterrupt(uint8_t index) 

  474. {

  475.  	list[index]->isr(index);

  476. }

  477. 

  478. 

  479. void QuadEncoder::isrEnc1()

  480. {

  481. 	checkAndProcessInterrupt(1);

  482. 	asm volatile ("dsb");  // wait for clear  memory barrier

  483. }

  484. 

  485. void QuadEncoder::isrEnc2()

  486. {

  487. 	checkAndProcessInterrupt(2);

  488. 	asm volatile ("dsb");  // wait for clear  memory barrier

  489. 

  490. }

  491. 

  492. void QuadEncoder::isrEnc3()

  493. {

  494. 	checkAndProcessInterrupt(3);

  495. 	asm volatile ("dsb");  // wait for clear  memory barrier

  496. }

  497. void QuadEncoder::isrEnc4()

  498. {

  499. 	checkAndProcessInterrupt(4);

  500. 	asm volatile ("dsb");  // wait for clear  memory barrier

  501. }

  502. 

  503. void QuadEncoder::isr(uint8_t index) 

  504. {	

  505.     if (ENC_CTRL_XIRQ_MASK == (ENC_CTRL_XIRQ_MASK & channel[index].ENC->CTRL) && (ENC_CTRL_XIE_MASK & channel[index].ENC->CTRL))

  506.     {

  507. 		indexCounter = indexCounter + 1;

  508. 		clearStatusFlags(_INDEXPulseFlag, index);

  509. 		

  510. 		if(ENC_CTRL2_ROIRQ_MASK == (ENC_CTRL2_ROIRQ_MASK & channel[index].ENC->CTRL2))

  511. 		{

  512. 			//Serial.println("ROLL OVER INDICATED!!!!!");

  513. 			//Serial.println("=========================");

  514. 			//Serial.println();

  515. 			clearStatusFlags(_positionRollOverFlag, index);

  516. 		}

  517. 		if(ENC_CTRL2_RUIRQ_MASK == (ENC_CTRL2_RUIRQ_MASK & channel[index].ENC->CTRL2))

  518. 		{

  519. 			//Serial.println("ROLL UNDER INDICATED!!!!!");

  520. 			//Serial.println("=========================");

  521. 			//Serial.println();

  522. 			clearStatusFlags(_positionRollUnderFlag, index);

  523. 		}

  524. 

  525. 	}

  526. 	

  527.     if (ENC_CTRL_HIRQ_MASK == (ENC_CTRL_HIRQ_MASK & channel[index].ENC->CTRL) && (ENC_CTRL_HIE_MASK & channel[index].ENC->CTRL))

  528.     {

  529. 

  530. 		homeCounter++;

  531. 		clearStatusFlags(_HOMETransitionFlag, index);

  532.     }

  533. 	

  534. 	if (ENC_CTRL_CMPIRQ_MASK == (ENC_CTRL_CMPIRQ_MASK & channel[index].ENC->CTRL))

  535.     {

  536. 		compareValueFlag = 1;

  537. 		clearStatusFlags(_positionCompareFlag, index);

  538. 	}

  539. }