Go Down

Topic: Using the Hardware Quadrature Encoder Channel on the DUE (Read 67091 times) previous topic - next topic

joker691

Here is my code:

Code: [Select]


#define ENC_A    2
#define ENC_B   13



void setup() {


  Serial.begin(115200);

  pinMode(ENC_A, INPUT);      // A pin encoder
  pinMode(ENC_B, INPUT);      // B pin encoder

  const unsigned int mask_ENC_A = digitalPinToBitMask(ENC_A);
  const unsigned int mask_ENC_B = digitalPinToBitMask(ENC_B);

  REG_PIOB_PDR = mask_ENC_A;     // activate peripheral function (disables all PIO functionality)
  REG_PIOB_ABSR |= mask_ENC_A;   // choose peripheral option B
  REG_PIOB_PDR = mask_ENC_B;     // activate peripheral function (disables all PIO functionality)
  REG_PIOB_ABSR |= mask_ENC_B;   // choose peripheral option B

  // Setup Quadrature Encoder
  REG_PMC_PCER0 = (1 << 27); // activate clock for TC0

  // select XC0 as clock source and set capture mode
  REG_TC0_CMR0 = 5; // continous count or
  //REG_TC0_CMR0 = (1 << 0) | (1 << 2) | (1 << 8) | (1 << 10); // reset counter on index

  // activate quadrature encoder and position measure mode, no filters
  REG_TC0_BMR = (1 << 9) | (1 << 8) | (1 << 12);

  // enable the clock (CLKEN=1) and reset the counter (SWTRG=1)
  // SWTRG = 1 necessary to start the clock!!
  REG_TC0_CCR0 = 5;
}


void loop() {

  Serial.println(REG_TC0_CV0, DEC);
  delay(500);
}




Thanx

ard_newbie

#76
Sep 16, 2017, 06:12 pm Last Edit: Sep 16, 2017, 08:04 pm by ard_newbie
The documentation for hardware quadrature decoder(s) in Sam3x datasheet is full of typos.

Anyway, to properly debug your sketch you need first to replace all magic numbers by their corresponding meaning. Search in datasheet and header files this information.

Here is a snippet which should be a good starting point to your PID programming for position measurement:

Code: [Select]

/*
  QDEC0:
  Timer Counter 0 channel 0 / TIOA and TIOB of channel 0
  PHA  TIOA0     = pin 2
  PHB  TIOB0     = pin 13
  Timer Counter 0 channel 1 / TIOB of channel 1
  INDEX  TIOB1 = pin A6

  A second hardware quadrature decoder "should" be available (not tested though)
  QDEC1:
  Timer Counter 2 channel 0 / TIOA and TIOB of channel 0
  PHA  TIOA6    = pin 5
  PHB  TIOB6     = pin 4
  Timer Counter 2 channel 1 / TIOB of channel 1
  INDEX  TIOB7 = pin 10

*/
void InitQDec0(void)
{

  TC0->TC_BMR = TC_BMR_QDEN
                | TC_BMR_POSEN
                | TC_BMR_INVA   // Depends of your configuration
                | TC_BMR_INVB   // Signals inverted or not
                | TC_BMR_INVIDX // Index available or not
                | TC_BMR_EDGPHA;

  // Select edges you want
  TC0->TC_CHANNEL[0].TC_CMR = TC_CMR_TCCLKS_TIMER_CLOCK1 // MCK/2
                              | TC_CMR_ETRGEDG_FALLING
                              | TC_CMR_ABETRG;

  TC0->TC_CHANNEL[1].TC_CMR = TC_CMR_TCCLKS_TIMER_CLOCK1;

  // All channels of TC0 are synchrone
  TC0->TC_BCR = TC_BCR_SYNC;

  // Software trigger and enable
  TC0->TC_CHANNEL[0].TC_CCR = TC_CCR_SWTRG | TC_CCR_CLKEN;
  TC0->TC_CHANNEL[1].TC_CCR = TC_CCR_SWTRG | TC_CCR_CLKEN;
}
void setup (void) {

  PMC->PMC_PCER0 |= PMC_PCER0_PID27    // TC0 channel 0 = TC0 power ON
                    | PMC_PCER0_PID28; // TC0 channel 1 = TC1 power ON

  PIOB->PIO_PDR = PIO_PDR_P25
                  | PIO_PDR_P27;

  PIOB->PIO_ABSR |= PIO_ABSR_P25
                    | PIO_ABSR_P27;

  PIOA->PIO_PDR = PIO_PDR_P3;
  PIOA->PIO_ABSR &= ~PIO_ABSR_P3;
  
  InitQDec0();
}
void loop(void) {

}




joker691

I tried your code and its working same way as mine. I found in datasheet that TC Counter Value Register is Read-only. So im confused. If it cannot go lower then 0 positon or to negative positions does everybody use only positive movement?

ard_newbie

#78
Sep 16, 2017, 08:11 pm Last Edit: Sep 16, 2017, 08:16 pm by ard_newbie
See Sam3x datasheet:

36.6.14.3 Direction Status and Change Detection

The direction status can be directly read at anytime in the TC_QISR. The polarity of the direction flag status
depends on the configuration written in TC_BMR. INVA, INVB, INVIDX, SWAP modify the polarity of DIR flag.
Any change in rotation direction is reported in the TC_QISR and can generate an interrupt.

36.6.14.4 Position and Rotation Measurement

Depending on the quadrature signals, the direction is decoded and allows to count up or down in timer/counter channels 0 and 1. The direction status is reported on TC_QISR.

joker691

I understand what you want to say to me but i think we misunderstood each other. When i startup arduino with encoder program then REG_TC0_CV0 == 0.

If I make one turn positive on encoder shaft then REG_TC0_CV0  == 8000.
But if I make one turn negative after startup on encoder shaft then REG_TC0_CV0  == 4294959294.

TC0 dont have negative numbers so it overflow and its counting down from max value. Thats why i cannot use it for PID input.

ard_newbie

#80
Sep 16, 2017, 08:29 pm Last Edit: Sep 16, 2017, 08:41 pm by ard_newbie
As I understand the datasheet, TC_CV counts down (from 2exp32  - 1   to 0) when direction is negative, whilst TC_CV counts up from 0 to 2 exp32  - 1 when direction is positive.

And there is an interrupt on counter overflow. Therefore in TC_Handler, check counter overflow bit and add 2exp32 - 1 to your software counter if necessary.

joker691

Yes exactly that is happening. Thats why i want to change TC_CV register or something else to get rig of calculations for every negative encoder positions for PID input. If i need to subtract value for negative positions than it will consume lot of cpu time and than its in fact useless or am i missing something?

Im not able to add any value to TC_CV. Its read-only.

ard_newbie

#82
Sep 17, 2017, 08:04 am Last Edit: Sep 17, 2017, 08:05 am by ard_newbie
This is not a big deal to update an "absolute" software  position counter, and it takes only 1 clock cycle to add or subtract.

Anyway, whatever the solution, you will meet the roll over issue. At 3000 RPM, assuming the shaft always turns in the same direction, 8000 ticks per revolution, there will be a roll over after three hours. Because the issue seems to be the shaft direction changes, I assume that it doesn't turn very fast. Set QDTRANS bit in TC_BMR then initialize the shaft position by turning it a few hundred revolutions to be far enough either from 0 or (2exp32  - 1) in TC_CV, this is your starting point. Then begin your real sketch. From now on, TC_CV should not initialize to 0 or (2 exp32  - 1) after each direction change.

joker691

Hi,

Actually she shaft turns also in minus direction. Motor is on linear guide rail. After startup i need to home axis and that time i need to go to minus direction to reach zero position. So i cant move shaft anywhere to plus to get rid of minus positions.

ard_newbie

In this thread, some users have been successful using QDEC0 (first Arduino DUE Quadrature decoder). 

I tested QDEC1(second Arduino DUE Quadrature decoder) and it works nicely.

In the code below, I have been using a Quadrature encoder emulation with 2 synchro PWM signals, the second one 90° out of phase from the first one to simulate a 600 PPR quadrature encoder between 800 RPM (8000 edges/s) and 20000 RPM (200000 edges /s). Synchro PWM channels duty cycles are updated with a PDC DMA to reduce the CPU load.

For this test, connect PWML0 (pin 34) to TIOA6 (pin 5) and PWML1 (pin 36) to TIOB6 (pin 4).

To be continued because of the 9000 character limitation !

ard_newbie

#85
Nov 12, 2018, 09:20 am Last Edit: Nov 13, 2018, 03:17 pm by ard_newbie
Code: [Select]

/***************************************************************************************************/
/*      QDEC1 test with an emulation of a Quadrature encoder between 800 RPM and 20000 RPM         */
/*                          QDEC Speed mode by polling in loop()                                   */
/*      Connect PWML0(pin 34) to TIOA6(pin 5) and PWML1(pin 36) to TIOB6(pin 4)                    */
/***************************************************************************************************/

/*
  In speed mode, the results are accumulated within the time base (100 ms in this example).
  Every 100 ms, we get an edge that is connected internally thru TIOA8 to TC6. Depending on the user
  application and the requirement, the user must modify the time base with respect to their speed range.
  For example, if the application requires low speed measurement, the time base can be increased to get
  the fine results. For higher speed measurement, the user must choose the lesser time base so that the
  number of pulses counted does not overflow beyond the channel 0 counter (32-bit). If this is not taken
  care, the results may be inaccurate.
*/
const uint32_t ENCODER_CPR = 150;                            // Cycles per revolution; this depends on your encoder
const uint32_t ENCODER_EDGES_PER_ROTATION = ENCODER_CPR * 4; // PPR = CPR * 4
const uint32_t ENCODER_SAMPLES_PER_SECOND = 10;              // this will need to be tuned depending on your use case...
const uint32_t LOOP_DELAY_MS = 1 * 1000;                     // ... as will this

/************************************************************************/
/*    This part before setup() is for a Quadrature encoder emulation    */
/************************************************************************/
#define Wavesize  (16)                // Sample number (a power of 2 is better)
#define _Wavesize (Wavesize - 1)

// Comment out the RPM you want to test
//#define PERIOD_VALUE   (2625)        // ****** For a 2000 Hz waveform ---> 8000 edges per second  = 600 PPR * 800 RPM/60 Second
//#define PERIOD_VALUE   (525)         // ****** For a 10000 Hz waveform ---> 40000 edges per second  = 600 PPR * 4000 RPM/60 Second
#define PERIOD_VALUE   (105)           // ****** For a 50000 Hz waveform ---> 200000 edges per second  = 600 PPR * 20000 RPM/60 Second


#define NbCh      (2)                // If Only channel 0 ---> Number of channels = 1, etc..
#define DUTY_BUFFER_LENGTH      (Wavesize * NbCh) // Half words

#define UpdatePeriod_Msk (0b1111)
#define UpdatePeriod    (UpdatePeriod_Msk & 0b0000) //Defines the time between each duty cycle update of the synchronous channels
  //This time equals to (UpdatePeriod + 1) periods of the Reference channel 0

uint16_t Duty_Buffer[DUTY_BUFFER_LENGTH];
uint16_t Wave_Duty[Wavesize];

void setup() {

  Serial.begin(250000);

  PMC->PMC_PCER1 = PMC_PCER1_PID33                  // TC6 power ON ; Timer counter 2 channel 0 is TC6
                   | PMC_PCER1_PID34                // TC7 power ON ; Timer counter 2 channel 1 is TC7
                   | PMC_PCER1_PID35;               // TC8 power ON ; Timer counter 2 channel 2 is TC8

  // TC8 in waveform mode
  TC2->TC_CHANNEL[2].TC_CMR = TC_CMR_TCCLKS_TIMER_CLOCK4  // Select Mck/128
                              | TC_CMR_WAVE               // Waveform mode
                              | TC_CMR_ACPC_TOGGLE        // Toggle TIOA of TC2 (TIOA8) on RC compare match
                              | TC_CMR_WAVSEL_UP_RC;      // UP mode with automatic trigger on RC Compare match

  TC2->TC_CHANNEL[2].TC_RC = F_CPU / 128 / ENCODER_SAMPLES_PER_SECOND;  // F_CPU = 84 MHz
  // Final TC frequency is 84 MHz/128/TC_RC

  // TC6 in capture mode
  // Timer Counter 2 channel 0 is internally clocked by TIOA8
  TC2->TC_CHANNEL[0].TC_CMR = TC_CMR_ABETRG               // TIOA8 is used as an external trigger.
                              | TC_CMR_TCCLKS_XC0         // External clock selected
                              | TC_CMR_LDRA_EDGE          // RA loading on each edge of TIOA6
                              | TC_CMR_ETRGEDG_RISING     // External TC trigger by edge selection of TIOA8
                              | TC_CMR_CPCTRG;            // RC Compare match resets the counter and starts the counter clock

  TC2->TC_BMR = TC_BMR_QDEN                               // Enable QDEC (filter, edge detection and quadrature decoding)
                | TC_BMR_SPEEDEN                          // Enable the speed measure on channel 0, the time base being provided by channel 2.
                | TC_BMR_EDGPHA                           // Edges are detected on both PHA and PHB
                | TC_BMR_SWAP                             // Swap PHA and PHB if necessary
                | TC_BMR_MAXFILT(1);                      // Pulses with a period shorter than MAXFILT+1 peripheral clock cycles are discarded

  TC2->TC_CHANNEL[0].TC_CCR = TC_CCR_CLKEN | TC_CCR_SWTRG;
  TC2->TC_CHANNEL[1].TC_CCR = TC_CCR_CLKEN | TC_CCR_SWTRG;
  TC2->TC_CHANNEL[2].TC_CCR = TC_CCR_CLKEN | TC_CCR_SWTRG;

  /***********************************************************************************************************/
  /*   This part of setup() simulates PHA and PHB of a quadrature encoder of 150 CPR = 600 PPR               */
  /***********************************************************************************************************/

  PMC->PMC_PCER1 |= PMC_PCER1_PID36;       // PWM power ON

  // PWML0 on PC2, peripheral type B
  PIOC->PIO_PDR |= PIO_PDR_P2;                        // Set PWM pin to an output
  PIOC->PIO_ABSR |= PIO_PC2B_PWML0;                   // Set PWM pin perhipheral

  // PWML1 on PC4 ; Peripheral=B
  PIOC->PIO_PDR |= PIO_PDR_P4;                        // Set PWM pin to an output
  PIOC->PIO_ABSR |= PIO_PC4B_PWML1;                   // Set PWM pin perhipheral

  // Set synchro channels list : Channel 0 and channel 1
  PWM->PWM_DIS = PWM_DIS_CHID0 | PWM_DIS_CHID1;

  PWM->PWM_SCM  = PWM_SCM_SYNC0          // Add SYNCx accordingly, at least SYNC0 plus SYNC1
                  | PWM_SCM_SYNC1
                  | PWM_SCM_UPDM_MODE2;  //Automatic write of duty-cycle update registers by the PDC DMA

  // Set duty cycle update period
  PWM->PWM_SCUP = PWM_SCUP_UPR(UpdatePeriod);

  // Set the PWM Reference channel 0 i.e. : Clock/Frequency/Alignment
  PWM->PWM_CLK = PWM_CLK_PREB(0b0000) | PWM_CLK_DIVB(1);        // Set the PWM clock rate for 84 MHz
  PWM->PWM_CH_NUM[0].PWM_CMR = PWM_CMR_CPRE_CLKB;               // The period is left aligned, clock source as CLKB on PWM channel 0
  PWM->PWM_CH_NUM[0].PWM_CPRD = PERIOD_VALUE;                   // Set the PWM frequency (84MHz)/PERIOD_VALUE

  /****  Final frequency = MCK/DIVB/PRES/CPRD/(UPR + 1)/Wavesize    ****/

  // Set Interrupt events
  PWM->PWM_IER2 = PWM_IER2_WRDY;   //Write Ready for Synchronous Channels Update Interrupt Enable
  //synchro with ENDTX End of TX Buffer Interrupt Enable

  // Fill duty cycle buffer for channels 0, x, y ...
  // Duty_Buffer is a buffer of Half Words(H_W) composed of N lines whose structure model for each duty cycle update is :
  // [ H_W: First synchro channel 0 duty cycle **Mandatory** ]/[ H_W: Second synchro channel duty cycle ] ... and so on

  /*      PWML0 waveform     */
  for (int i = 0; i < Wavesize / 2; i++) {
    Wave_Duty[i] = 4095;
  }
  for (int i = Wavesize / 2; i < Wavesize; i++) {
    Wave_Duty[i] = 0;
  }

  /*     Fill Duty_Buffer for all synchro channels  */
  for (uint32_t i = 0; i < Wavesize; i++) {
    Duty_Buffer[i * NbCh + 0] =  Wave_Duty[i];
    Duty_Buffer[i * NbCh + 1] =  Wave_Duty[((i + (Wavesize / 4)) & _Wavesize)];
  }

  PWM->PWM_ENA = PWM_ENA_CHID0;                 // Enable PWM for all channels, channel 0 Enable is sufficient

  PWM->PWM_TPR  = (uint32_t)Duty_Buffer;        // FIRST DMA buffer
  PWM->PWM_TCR  = DUTY_BUFFER_LENGTH;           // Number of Half words
  PWM->PWM_TNPR = (uint32_t)Duty_Buffer;        // Next DMA buffer
  PWM->PWM_TNCR = DUTY_BUFFER_LENGTH;
  PWM->PWM_PTCR = PWM_PTCR_TXTEN;               // Enable PDC Transmit channel request

  NVIC_EnableIRQ(PWM_IRQn);
}

void loop() {
  double dSpeedRPS, dSpeedRPM;
  int32_t __dSpeedRPM;
  int32_t iSpeedPPP;

  iSpeedPPP = TC2->TC_CHANNEL[0].TC_RA;

  dSpeedRPS = ((iSpeedPPP / (ENCODER_EDGES_PER_ROTATION * 1.0)) * ENCODER_SAMPLES_PER_SECOND);
  dSpeedRPM =  dSpeedRPS * 60;
  __dSpeedRPM = (int32_t) dSpeedRPM;

  printf(
    " Speed rpm: %d \n",
    __dSpeedRPM
  );
  delay(LOOP_DELAY_MS);
}

void PWM_Handler() {  // move PDC DMA pointers to next buffer

  PWM->PWM_ISR2;      // Clear status register

  PWM->PWM_TNPR = (uint32_t)Duty_Buffer;
  PWM->PWM_TNCR = DUTY_BUFFER_LENGTH;
}






ard_newbie


I wondered what would be the limit of the hardware quadrature decoder in speed mode.....


I had to simulate an expensive 10000 PPR quadrature encoder ( ~ $700) running at 4200 RPM and, you know what ? I was not able to make it crash even at 42000000 edges per second ....

Note: I did this test by using TC_SMMR instead of the PWM peripheral to simulate 2 square waves 90° out of phase.

ard_newbie

#87
Nov 15, 2018, 07:35 am Last Edit: Nov 15, 2018, 08:22 am by ard_newbie
For absolute position measurement, TC_CV register (32-bit) provides all you need, however care must be taken whenever TC_CV overflows or underflows. To deal with that, an int64_t (8 bytes) is handy, as well as interrupts on TC_CV overflow and underflow.

I tested the position measurement with a simulation of an 10000 PPR quadrature encoder, 42000000 edges per second and a reverse direction firstly after 5 seconds then every 10 seconds. You can see oscillations around the zero position such as a point above a linear rail by selecting tools>Serial Plotter and 250000 bauds.

Code: [Select]

/***************************************************************************************************/
/*                   QDEC1 test with an emulation of a 10000PPR Quadrature encoder                 */
/*                          QDEC position mode by polling in loop()                                */
/*               Connect TIOA0(pin 2) to TIOA6(pin 5), TIOB0(pin 13) to TIOB6(pin 4)               */
/*                     Select Serial Plotter to show scillations around the zero position          */
/***************************************************************************************************/

const uint32_t LOOP_DELAY_MS = 1 * 100;
volatile int64_t AbsPos , AbsPosBase;

/************************************************************************/
/*    This part before setup() is for a Quadrature encoder emulation    */
/************************************************************************/
#define PERIOD_VALUE   (1)           // 42000000 edges per second simulation by TC_SMMR

void setup() {

  Serial.begin(250000);

  PMC->PMC_PCER1 = PMC_PCER1_PID33                  // TC6 power ON ; Timer counter 2 channel 0 is TC6
                   | PMC_PCER1_PID34;               // TC7 power ON ; Timer counter 2 channel 1 is TC7

  // TC6 in capture mode
  TC2->TC_CHANNEL[0].TC_CMR = TC_CMR_TCCLKS_TIMER_CLOCK1   // capture mode, MCK/2, clk on rising edge
                              | TC_CMR_ABETRG              // TIOA6 is used as an external trigger.
                              | TC_CMR_LDRA_EDGE           // RA loading on each edge of TIOA6
                              | TC_CMR_ETRGEDG_RISING;     // External TC trigger by edge selection of TIOA

  TC2->TC_BMR = TC_BMR_QDEN                                // Enable QDEC (filter, edge detection and quadrature decoding)
                | TC_BMR_POSEN                             // Enable the position measure on channel 0
                | TC_BMR_EDGPHA                            // Edges are detected on both PHA and PHB
                // | TC_BMR_SWAP                              // Swap PHA and PHB if necessary
                | TC_BMR_MAXFILT(1);                       // Pulses with a period shorter than MAXFILT+1 peripheral clock cycles are discarded

  TC2->TC_CHANNEL[0].TC_IER = TC_IER_COVFS                 // Interruption enable on TC_CV overflow ( TC_CV = 0xFFFFFFFF)
                              | TC_IER_CPCS;               // Interruption enable on TC_CV underflow ( TC_CV = 0)

  // Enable Compare Fault Channel 0
  TC2->TC_FMR = TC_FMR_ENCF0;                              // To trigger an interrupt whenever TC_CV = TC_RC = 0

  NVIC_EnableIRQ(TC6_IRQn);

  TC2->TC_CHANNEL[0].TC_CCR = TC_CCR_CLKEN | TC_CCR_SWTRG;
  TC2->TC_CHANNEL[1].TC_CCR = TC_CCR_CLKEN | TC_CCR_SWTRG;

  /*************************************************************************************/
  /*   This part of setup() simulates PHA and PHB of a quadrature encoder              */
  /*************************************************************************************/
  PMC->PMC_PCER0 |= PMC_PCER0_PID27;                      // TC0 power ON : Timer Counter 0 channel 0 IS TC0

  PIOB->PIO_PDR = PIO_PDR_P25
                  | PIO_PDR_P27;
  PIOB->PIO_ABSR = PIO_PB25B_TIOA0
                   | PIO_PB27B_TIOB0;

  TC0->TC_CHANNEL[0].TC_SMMR = TC_SMMR_GCEN;              // Gray Code divides TC frequency by 4

  TC0->TC_CHANNEL[0].TC_CMR = TC_CMR_TCCLKS_TIMER_CLOCK1  // MCK/2, clk on rising edge
                              | TC_CMR_EEVT_XC0           // TIOB0 in output
                              | TC_CMR_WAVE;              // Waveform mode

  TC0->TC_CHANNEL[0].TC_RC = PERIOD_VALUE;  //<*********************  TIOA0 Frequency = (Mck/2/TC_RC/4
  TC0->TC_CHANNEL[0].TC_CCR = TC_CCR_SWTRG | TC_CCR_CLKEN; // Software trigger TC0 counter and enable
}

void loop() {

  static uint32_t Oldmillis;
  static uint32_t Timestamp = 5000;

  // Reverse CW <--> CCW every 10 seconds
  if ((millis() - Oldmillis) > Timestamp) {
    Timestamp = 10000;
    Oldmillis = millis();
    TC0->TC_CHANNEL[0].TC_SMMR ^= TC_SMMR_DOWN;
  }
  AbsPos = AbsPosBase + (uint32_t)TC2->TC_CHANNEL[0].TC_CV;
  printf( // Print a 64-bit variable
    "%lld\n",
    AbsPos
  );
  delay(LOOP_DELAY_MS);
}

void TC6_Handler() {

  uint32_t status = TC2->TC_CHANNEL[0].TC_SR;

  if (status & TC_SR_COVFS) {
    // TC_CV overflow --> TC_CV = 0xFFFFFFFF
    AbsPosBase += 0xFFFFFFFFllu;
  }
  else  { // if (status & TC_SR_CPCS)
    // TC_CV underflow --> TC_CV = 0
    AbsPosBase -= 0xFFFFFFFFllu;
  }
}




dougcl

Hi folks, summarizing from above, this is the minimal setup for using a two pin encoder as a knob on channel TC 0, Channel 0:

Code: [Select]

void setup() {

  pmc_enable_periph_clk(ID_TC0);
  TC_Configure(TC0,0, TC_CMR_TCCLKS_XC0); 
 
  TC0->TC_BMR = TC_BMR_QDEN          // Enable QDEC (filter, edge detection and quadrature decoding)
                | TC_BMR_POSEN       // Enable the position measure on channel 0
                //| TC_BMR_EDGPHA    // Edges are detected on PHA only
                //| TC_BMR_SWAP      // Swap PHA and PHB if necessary (reverse acting)
                | TC_BMR_FILTER      // Enable filter
                | TC_BMR_MAXFILT(63) // Set MAXFILT value
               ;                       

  // Enable and trigger reset
  TC0->TC_CHANNEL[0].TC_CCR = TC_CCR_SWTRG | TC_CCR_CLKEN;
 
}

void loop() {
  int16_t count = REG_TC0_CV0;
}



This will return a positive or negative count. Note it will roll over at plus or minus 32767. In theory, you can avoid rollover by resetting the count to zero by calling

TC0->TC_CHANNEL[0].TC_CCR = TC_CCR_SWTRG | TC_CCR_CLKEN;

But in reality, this does not seem to quite get back to zero all the time. I just check to see if I am above 32000, or below -32000, and reset.

Hope this helps.
Doug

ard_newbie

#89
Nov 17, 2018, 07:20 pm Last Edit: Nov 18, 2018, 05:37 am by ard_newbie
A pulse on the INDEX pin (TIOB1 for QDEC0, TIOB7 for QDEC1) resets TC_CV0 at each full revolution, and meanwhile TC_CV1 accumulates the number of full revolutions. If your quadrature encoder does not provide the INDEX pulse, AFAIK you want to accumulate PPR numbers in a greater size variable as an Absolute Position Base each time TC_CV0 crosses  Zero.

BTW, I noticed that TC_BMR_FILTER is not present in the datasheet but in the header file...
Nevertheless, when QDEN  bit is set, filter, edge detection and quadrature decoding are enable.

IMO, the FILTER bit (1<<19) is usefull when you want to chain timers (page 856).

Go Up