Arduino Forum

Products => Arduino Due => Topic started by: dreschel on Jan 02, 2013, 12:45 am

Title: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: dreschel on Jan 02, 2013, 12:45 am
Hi,
I would like to start working with the quadrature encoder hardware in the DUE. It appears you can use the pins TIOA0, TIOB0 and TIOA1 to decode 2 channel quadrature plus an index. (SAM3X manual p. 855) If my reading of the pinout is correct, this would be pins 2, 13 and AD7.

Could someone point me to any existing low level calls that might make this easier? I know, for instance, the Arduino code writes to the DAC using:
dacc_write_conversion_data(DACC_INTERFACE, 0x0000);

Is there a similar function for the decoder channels and where might I find these calls.
THANKS IN ADVANCE.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: fiddler on Jan 10, 2013, 07:58 pm
dreschel

Did you get the encoder to work ?
It appear that only 1 of the the 2 Quadrature channels pins have been connected to pins.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: pingflip on Jan 16, 2013, 02:06 pm
@fiddler:
All three QDEC pins are on a connector, the pinout is as dreschel wrote

@dreschel:

I came to the same point like you and nothing further  :(
This is a header file from Keil, it defines allot of registers and peripheral base adresses: http://www.keil.com/dd/docs/arm/atmel/sam3x/sam3x.h (http://www.keil.com/dd/docs/arm/atmel/sam3x/sam3x.h)

This part could be interesting for you:

Code: [Select]
#define TC_BMR_QDEN (0x1u << 8) /**< \brief (TC_BMR) Quadrature Decoder ENabled */
#define TC_BMR_POSEN (0x1u << 9) /**< \brief (TC_BMR) POSition ENabled */
#define TC_BMR_SPEEDEN (0x1u << 10) /**< \brief (TC_BMR) SPEED ENabled */
#define TC_BMR_QDTRANS (0x1u << 11) /**< \brief (TC_BMR) Quadrature Decoding TRANSparent */
#define TC_BMR_EDGPHA (0x1u << 12) /**< \brief (TC_BMR) EDGe on PHA count mode */
#define TC_BMR_INVA (0x1u << 13) /**< \brief (TC_BMR) INVerted phA */
#define TC_BMR_INVB (0x1u << 14) /**< \brief (TC_BMR) INVerted phB */
#define TC_BMR_INVIDX (0x1u << 15) /**< \brief (TC_BMR) INVerted InDeX */
#define TC_BMR_SWAP (0x1u << 16) /**< \brief (TC_BMR) SWAP PHA and PHB */
#define TC_BMR_IDXPHB (0x1u << 17) /**< \brief (TC_BMR) InDeX pin is PHB pin */
#define TC_BMR_FILTER (0x1u << 19) /**< \brief (TC_BMR)  */
#define TC_BMR_MAXFILT_Pos 20
#define TC_BMR_MAXFILT_Msk (0x3fu << TC_BMR_MAXFILT_Pos) /**< \brief (TC_BMR) MAXimum FILTer */
#define TC_BMR_MAXFILT(value) ((TC_BMR_MAXFILT_Msk & ((value) << TC_BMR_MAXFILT_Pos)))
/* -------- TC_QIER : (TC Offset: 0xC8) QDEC Interrupt Enable Register -------- */
#define TC_QIER_IDX (0x1u << 0) /**< \brief (TC_QIER) InDeX */
#define TC_QIER_DIRCHG (0x1u << 1) /**< \brief (TC_QIER) DIRection CHanGe */
#define TC_QIER_QERR (0x1u << 2) /**< \brief (TC_QIER) Quadrature ERRor */
/* -------- TC_QIDR : (TC Offset: 0xCC) QDEC Interrupt Disable Register -------- */
#define TC_QIDR_IDX (0x1u << 0) /**< \brief (TC_QIDR) InDeX */
#define TC_QIDR_DIRCHG (0x1u << 1) /**< \brief (TC_QIDR) DIRection CHanGe */
#define TC_QIDR_QERR (0x1u << 2) /**< \brief (TC_QIDR) Quadrature ERRor */
/* -------- TC_QIMR : (TC Offset: 0xD0) QDEC Interrupt Mask Register -------- */
#define TC_QIMR_IDX (0x1u << 0) /**< \brief (TC_QIMR) InDeX */
#define TC_QIMR_DIRCHG (0x1u << 1) /**< \brief (TC_QIMR) DIRection CHanGe */
#define TC_QIMR_QERR (0x1u << 2) /**< \brief (TC_QIMR) Quadrature ERRor */
/* -------- TC_QISR : (TC Offset: 0xD4) QDEC Interrupt Status Register -------- */
#define TC_QISR_IDX (0x1u << 0) /**< \brief (TC_QISR) InDeX */
#define TC_QISR_DIRCHG (0x1u << 1) /**< \brief (TC_QISR) DIRection CHanGe */
#define TC_QISR_QERR (0x1u << 2) /**< \brief (TC_QISR) Quadrature ERRor */
#define TC_QISR_DIR (0x1u << 8) /**< \brief (TC_QISR) Direction */


it's from line 6484 to line 6514.

I do not know if this header file works with the arduino IDE or Atmel Studio or if it only works with the keil compiler but maybe it is a starting point.

I tried to implement the Quadrature Decoder in Atmel Studio but I cant program the Due from within Atmel Studio(yet) and since I'm not really great in programming this could be quite a task for me anyway

I hope some one here on the forum will come up with an example so we can use this neat feature of this nice new Arduino :)

Cheers

Pingflip
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: aethaniel on Jan 16, 2013, 02:14 pm
It seems that this file \hardware\arduino\sam\system\CMSIS\Device\ATMEL\sam3xa\include\component\component_tc.h present the same information.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: pingflip on Jan 18, 2013, 12:30 am
thats great news :)

I also found it in "Contents/Resources/Java/hardware/arduino/sam/system/CMSIS/Device/ATMEL/sam3xa/include/component/components_tc.h"

I am trying to figure out how to include all necessary files with all the typedefs and stuff to not get compiling errors
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: pingflip on Jan 20, 2013, 10:51 pm
I'm currently working my self through the datasheet and the Timer/Counter relating header and source files and I think I'm almost there :)
But: which T/C channel should I use to not generate conflicts with a PWM output or any other peripheral.

Does any one know which channels are already in use by the PWM and / or other functions?
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: schwingkopf on Feb 13, 2013, 08:48 pm
I got the hardware quadrature encoder running partially...

Using the following code and a motor with a 2 trace quadrature encoder I manage to make the REG_TC0_CV0 register measure the position of the motor. To use the hardware quadrature encoder the two traces need to be connected to digital pins 2 and 13 which correspond to the TIOA0 and TIOB0 peripheral pin function of the SAM3x8e. I didn't test using the Index pin for revolution counting yet...

Code: [Select]

const int quad_A = 2;
const int quad_B = 13;
const unsigned int mask_quad_A = digitalPinToBitMask(quad_A);
const unsigned int mask_quad_B = digitalPinToBitMask(quad_B); 

void setup() {
    Serial.begin(115200); 
    delay(100);
   
    // activate peripheral functions for quad pins
    REG_PIOB_PDR = mask_quad_A;     // activate peripheral function (disables all PIO functionality)
    REG_PIOB_ABSR |= mask_quad_A;   // choose peripheral option B   
    REG_PIOB_PDR = mask_quad_B;     // activate peripheral function (disables all PIO functionality)
    REG_PIOB_ABSR |= mask_quad_B;   // choose peripheral option B
   
    // activate clock for TC0
    REG_PMC_PCER0 = (1<<27);
    // select XC0 as clock source and set capture mode
    REG_TC0_CMR0 = 5;
    // activate quadrature encoder and position measure mode, no filters
    REG_TC0_BMR = (1<<9)|(1<<8);
    // 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);
}


The problem I encountered is that the TC0_CV0 register only counts half of the position steps that could actually be measured when evaluating all slopes on quad_A and quad_B. I investigated further and the problem seems to be that the slope detection on the digital pin 13 doesn't work. The position value is changed only when a slope on the digital pin 2 is send. A slope on pin 13 changes nothing.
However the level detection on pin 13 seems to work fine because otherwise the quadrature encoder wouldn't count all the way up when turning the motor in one direction. If the level at pin 13 was fixed, the counter would count one up at a positive slope on pin 2 and then down on the following negative slope, so the position value would alternate between two values...

anyone ideas that could explain this behaviour? maybe the quadrature encoder is not build to evaluate all slopes (which would be kinda stupid)?

Matthias
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: retrolefty on Feb 13, 2013, 08:51 pm
Quote
anyone ideas that could explain this behaviour? maybe the quadrature encoder is not build to evaluate all slopes (which would be kinda stupid)?


But possible until proven otherwise I guess? Shouldn't the datasheet shed light on how it responds to the four transitions of each encoder step is handled?

Lefty
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: schwingkopf on Feb 13, 2013, 09:15 pm
I checked, but I couldn't find a clear statement. In Figure 37-15 the wire going out the Quadrature Decoder block is labeled "PHEdges" (and not particularly PHAEdges or PHBedges, they name the quadrature signals PHA and PHB in the datasheet), which kind of makes me think there should be edges on both detected.

update: from the datasheet:
" Position channel 0 accumulates the edges of PHA, PHB input signals giving a high accuracy on motor position..."
and
"After filtering, the quadrature signals are analyzed to extract the rotation direction and edges of
the 2 quadrature signals detected in order to be counted by timer/counter logic downstream."

sounds like edges should be detected on both signals.. will run some more tests tomorrow..
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: schwingkopf on Feb 14, 2013, 11:34 am
just solved the problem.. the EDGPHA bit in the TC_BMR register controls whether the edges are detected on both PHA and PHB or just on PHA. I had this bit set to 0 before which according to the datasheet was the good choice.

("0: edges are detected on both PHA and PHB. 1: edges are detected on PHA only.")

BUT!! the description in the datasheet is WRONG!! its inverted!

so the code that works fine is the following:
Code: [Select]

const int quad_A = 2;
const int quad_B = 13;
const unsigned int mask_quad_A = digitalPinToBitMask(quad_A);
const unsigned int mask_quad_B = digitalPinToBitMask(quad_B); 

void setup() {
    Serial.begin(115200); 
    delay(100);
   
    // activate peripheral functions for quad pins
    REG_PIOB_PDR = mask_quad_A;     // activate peripheral function (disables all PIO functionality)
    REG_PIOB_ABSR |= mask_quad_A;   // choose peripheral option B   
    REG_PIOB_PDR = mask_quad_B;     // activate peripheral function (disables all PIO functionality)
    REG_PIOB_ABSR |= mask_quad_B;   // choose peripheral option B
   
    // activate clock for TC0
    REG_PMC_PCER0 = (1<<27);
    // select XC0 as clock source and set capture mode
    REG_TC0_CMR0 = 5;
    // 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);
}
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: spencoid on Feb 14, 2013, 06:57 pm
so for us dummies, does this code print the accumulated count from quadrature input on digital pins 2 and 13? not too good at complicated datasheets. also, are there only two pins that can be used as quadrature input and are they D2 and D13? i have a TFT shield that is wired to pin 2 among many others and can probably cut and jump to make pin 2 available but would prefer to not. my not very good interpretation of the data sheets suggests that three timers are available for quad processing but only two are brought out on the DUE board. is this correct?
The Timer Counter (TC) embeds a quadrature decoder logic connected in front of the 3 timers

and driven by TIOA0, TIOB0 and TIOA1 inputs. When enabled, the quadrature decoder per-
forms the input lines filtering, decoding of quadrature signals and connects to the
3 timers/counters in order to read the position and speed of the motor through user interface.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: pingflip on Feb 15, 2013, 12:52 am
@schwingkopf:

great work, works really nice

@spencoid:

unfortunately you have to use Pin2, Pin13 and PinA7 for the quadrature encoder (PinA7 is the third pin/index Pin for the necoder).
I dont know if there is something like virtual pins on the SAM3X like on the XMega line. If there is something like that, the TIOA0, TIOB0 and TIOA1 lines could be routed to other pins I think
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: jchalo99 on Feb 20, 2013, 08:20 pm
Ok i dont have a DUE yet, but im coding ahead of time. how about handling 2 or 3 encoders? and how can i level shift encoders? i have multiple 5v encoders. extremely fast 250lines.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: spencoid on Feb 20, 2013, 09:22 pm
i don't know if you can do it fast enough for high speed operation but the last encoders i bought (made by Bourns) have a chip select line so you could conceivably stick them all on a bus and sweep through them looking for signal changes. the programming might get a little confusing or might even be impossible.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: Arctic_Eddie on Feb 20, 2013, 09:48 pm
A hex buffer, CD4050, will work nicely. Power the Vss pin with 3V3 to establish the output level. The inputs will take 5V signals.

Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: schwingkopf on Feb 20, 2013, 10:05 pm
As far as I understand the due has only one hardware encoder, but you can still use interrupt versions of software encoders to handle almost as many encoders as you wish.

in terms of speed you will then be limited by your interrupt handlers. If you programm cleverly you can get the time to handle the interrupts well below 1us (I can provide code if necessary), meaning that you can detect slopes at rates above 1MHz. If you use more than one encoder you just divide the 1MHz by the number of encoders you'd like to use.

I don't know exactly how fast the hardware encoder can go but since it uses the master clock (84MHz) as a reference I guess it can take at least more than 10MHz slope rate, probably even much faster.

For your application this would mean that you connect the encoder with the highest speed demands on the hardware encoder and you programm two interrupt based software encoders for the other 2. Those would then be able to detect slope rates of more than 500kHz.

Would that fit your "extremely fast" application?

for level shifting you can use chips like 74LVCC4245 and others. they're convenient to handle and super fast (10ns delay)


Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: spencoid on Feb 20, 2013, 10:17 pm
the buffer is a better way but i am using my 5 volt encoders with a voltage divider on the output. just a 10k and 20k resistor in series. easier to stick in as an afterthought than wiring in the buffer.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: pingflip on Feb 20, 2013, 11:28 pm
I am using a 500 pulse/rev encoder from maxon motors (I think they are rebranded AvagoTech Encoders), rated for 5V and 3,3kOhm Pullups on the signal lines. I use it with 3.3V and 10k Pullups and it works just fine. Maybe just give it a try.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: jchalo99 on Feb 21, 2013, 12:07 am
Thank you all. Im going to buy the next Arduino Due ASAP. and replace my MEGA 1280. and get 3 buffers. its going to be interesting. next question how can i get a 5v pwm signal out?
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: Arctic_Eddie on Feb 21, 2013, 01:16 am
Use one of the same buffer chips but connect Vss to 5V. As long as your input signal crosses around the 50% point, the output will switch states. Use the B version of the chip as it's switching level is better defined. Check the data sheet on AllDataSheet.com.

http://pdf1.alldatasheet.com/datasheet-pdf/view/26876/TI/CD4050B.html


Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: schwingkopf on Feb 21, 2013, 09:08 am
you might actually run into troubles using the hardware quadrature encoder simultaneously with the standard PWM outputs.
The hardware encoder uses the timer counter TC0 and the standard PWM uses it partially too, as far as I know..

anyone with more insight on how the standard PWM would interfere with the settings on TC0 channel 0 and 1?

I circumvent this problem by not using the arduino defined PWM outputs (that use the timer counter channels) but by using the SAM3x8e internal PWM channels (labeled PWMHx and PWMLx in the due pinout diagramm http://arduino.cc/forum/index.php?PHPSESSID=cddf2a518756c532262dde0095dd89df&topic=132130.0). They actually exceed the functionality of the standard ones in terms of resolution and available PWM frequencies. The drawback is that you have to do a bit of reading in the datasheet and to programm some registers.
I will probably post some code examples when I have a bit more time..
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: skyAlex on Feb 23, 2013, 07:29 pm

just solved the problem.. the EDGPHA bit in the TC_BMR register controls whether the edges are detected on both PHA and PHB or just on PHA. I had this bit set to 0 before which according to the datasheet was the good choice.

("0: edges are detected on both PHA and PHB. 1: edges are detected on PHA only.")

BUT!! the description in the datasheet is WRONG!! its inverted!

so the code that works fine is the following:
Code: [Select]

const int quad_A = 2;
const int quad_B = 13;
const unsigned int mask_quad_A = digitalPinToBitMask(quad_A);
const unsigned int mask_quad_B = digitalPinToBitMask(quad_B); 

void setup() {
    Serial.begin(115200); 
    delay(100);
   
    // activate peripheral functions for quad pins
    REG_PIOB_PDR = mask_quad_A;     // activate peripheral function (disables all PIO functionality)
    REG_PIOB_ABSR |= mask_quad_A;   // choose peripheral option B   
    REG_PIOB_PDR = mask_quad_B;     // activate peripheral function (disables all PIO functionality)
    REG_PIOB_ABSR |= mask_quad_B;   // choose peripheral option B
   
    // activate clock for TC0
    REG_PMC_PCER0 = (1<<27);
    // select XC0 as clock source and set capture mode
    REG_TC0_CMR0 = 5;
    // 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);
}


I tested the proposed code at up to 60 kHz, works great! But the index is not work at A7. Does anyone understand? My knowledge there is no longer enough.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: pingflip on Feb 23, 2013, 08:16 pm
To enable the Index Signal you have to configure the input pin to the right peripheral function
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: garthn on Feb 28, 2013, 01:10 pm
After an hour studying the SAM3X manual, I am getting a vague idea of how the decoding works - but can an example of how to also count the index be added to the working code example?

I am trying with an interrupt for the index, but the counts are low and drop more when the encoder is spun faster (TS5303 - can handle 5000RPM, pulses dropped at perhaps 100RPM).

I'm using 4N35s - perhaps they aren't switching fast enough?

Last question : the count keeps increasing, does it eventually overflow? can it be reset (per rev)? how is this normally handled?

I would be very appreciative of help here
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: jchalo99 on Feb 28, 2013, 01:37 pm
When using interup try to keep the statments as quick and short as you can. my interupt code ive only been able to run at 0.1-150RPM with 250Pulses/rev. (1000counts) and that is with an arduino mega 1280, with encoder A+B+Z channels on interupts@change.

my interrupts are simple:
Code: [Select]

X_EncoderA(){
if (digitalRead(encoderXPinA)==HIGH) {
   if (digitalRead(encoderXPinB) == LOW)  encoderXPos++;         // CW
   if (digitalRead(encoderXPinB) == HIGH)  encoderXPos--;         // CCW
} else {
   if (digitalRead(encoderXPinB) == HIGH)  encoderXPos++;        // CW
   if (digitalRead(encoderXPinB) == LOW)  encoderXPos--;         // CCW
}
}
X_EncoderB(){
if (digitalRead(encoderXPinB)==HIGH) {
   if (digitalRead(encoderXPinA) == LOW)  encoderXPos++;         // CW
   if (digitalRead(encoderXPinA) == HIGH)  encoderXPos--;         // CCW
} else {
   if (digitalRead(encoderXPinA) == HIGH)  encoderXPos++;         // CW
   if (digitalRead(encoderXPinA) == LOW)  encoderXPos--;          // CCW
}
}



if you keep spinning the encoder in one direction it will overflow. depending on what data type you are using to store them. i use "unsigned int" that gives me 0 to 4,294,967,295 (2^32 - 1). i use unsigned because when i zero it, it is at it's left most extreme location. (i used to use "int" because i zero'ed it in the middle and needed the negative numbers.)

Sorry i cant help with any SAM3X information, or buffers. i dont use them YET.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: garthn on Feb 28, 2013, 03:53 pm
Ok, that would give me more than 24 hours before it overflowed (at 1800RPM) so not a problem

My interrupt is even simpler, just detects RISING and sets a flag

I'm using the flag in loop to display the number of pulses. So the serial comms should not be interfering, unless the previous print has not completed before the flag is being set again

And if I just count high flag state and only display the pulses after 100 revs, the count is still low and differening. I only get the expected count when rotating very slowly
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: garthn on Feb 28, 2013, 07:32 pm
schwingkopf da boss....

Now running up to 4000RPM with consistent pulsing (1440 state changes per rev, 96000 per second). Problem - bad breadboard connection somewhere  :smiley-red:

Ready to solder :)

I'd like to get the Z (index) pin working instead of using an interrupt - as I sometimes get a count +1 or -1, I guess a race situation because both index and a pulse pin are changing more or less at the same time. Funny that an encoder does that in a way - if the Due hardware logic sorts it out, it really can't be much better
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: jchalo99 on Feb 28, 2013, 07:38 pm
you guys have got to be jokeing!!! 
Quote
4000RPM with consistent pulsing (1440 state changes per rev, 96000 per second)


I cant wait to get my DUE. how are you getting feedback, serial?
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: garthn on Feb 28, 2013, 07:51 pm
const int quad_A = 2;
const int quad_B = 13;
const unsigned int mask_quad_A = digitalPinToBitMask(quad_A);
const unsigned int mask_quad_B = digitalPinToBitMask(quad_B); 
const int z = 10;
volatile int reg_at_z=0;
volatile int zcount=0;
volatile int  lag;
unsigned int prevmillis;
unsigned int thismillis;
int t;
int prevt=0;
float rpm=0.0;

volatile int firsttime=1;

void setup() {
    Serial.begin(115200); 
    delay(100);
   
    // activate peripheral functions for quad pins
    REG_PIOB_PDR = mask_quad_A;     // activate peripheral function (disables all PIO functionality)
    REG_PIOB_ABSR |= mask_quad_A;   // choose peripheral option B   
    REG_PIOB_PDR = mask_quad_B;     // activate peripheral function (disables all PIO functionality)
    REG_PIOB_ABSR |= mask_quad_B;   // choose peripheral option B
   
    // activate clock for TC0
    REG_PMC_PCER0 = (1<<27);
    // select XC0 as clock source and set capture mode
    REG_TC0_CMR0 = 5;
    // 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;   
 
   pinMode(z, INPUT);       
   attachInterrupt(z, zrising, RISING); 

}   
void loop() {
  if (reg_at_z==1) {
    t=REG_TC0_CV0;   
    Serial.println((t-prevt),DEC);   
     thismillis=millis();
     rpm = 1.0/((thismillis-prevmillis)/60000.0);
    Serial.println(rpm,DEC);         
     reg_at_z=0;
     prevt=t;
     prevmillis=thismillis;
  }

}

void zrising() {
  reg_at_z=1;
}

Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: garthn on Feb 28, 2013, 08:12 pm
For what it's worth - I suggest some careful checking before trying this.

Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: garthn on Feb 28, 2013, 08:14 pm
Pin connections need to be adjusted according to those used in the sketch (or whatever pins are used)
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: exedor on Jun 11, 2013, 02:38 am
Quote

I would like to start working with the quadrature encoder hardware in the DUE. It appears you can use the pins TIOA0, TIOB0 and TIOA1 to decode 2 channel quadrature plus an index. (SAM3X manual p. 855) If my reading of the pinout is correct, this would be pins 2, 13 and AD7.


As a point of clarification I would restate that it would be pins 2, 13, and A7 since technically AD7 as can be found on the wonderful pin-out diagram that Gray Nomad put together and the SAM3X8e datasheet that the SAM3X8e pin AD7 is actually on A0 which is how I originally interpreted the post that started this thread :(

I've checked and the original code for using the bit masks to select peripherals and such is not working.  You can verify that by looking at the status registers after executing the commands.  Additionally the quad decoding still works after commenting out all the mask _PDR AND _ABSR variables and assignment statements.  I'm even wondering if turning clocks on and off is working.  I think the Arduino init code/IDE is probably turning on write protect registers after initializing and before calling our setup function.

Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: Designservicecorp on Jun 17, 2013, 07:02 pm
For those that are still trying to get the encoder Index to work, here is what I did:

I started with the code from Schwinghopf in Reply#6 and as Exedor suggested in Reply#31, I removed the peripheral function setup and found it still worked. After reading through the SAM3x document many times I eventually discovered that the line: REG_PMC_PCER0 = (1<<27); was not turning on all of TC0 but rather only turning on channel0 of TC0. In order to start counting indexes, channel1 of TC0 needs to be turned on also.

Another issue was that I had to connect the Index wire to A6 (not A7 as the documentation shows). Not sure why this is, but since I wasn't using A6 for anything else I didn't look into it.

In the code below, I enabled interrupts for the Index (Z). Every index, REG_TC0_CV1 will reset back to zero. If you want it to continue counting, remove the interrupt (last 4 lines of setup() ).

Hope this helps.

Code: [Select]


volatile int z_Total=0;

void setup() {

   // Setup Quadrature Encoder with Marker

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

 REG_PMC_PCER0 = (1<<28); // activate clock for TC1

 // select XC0 as clock source and set capture mode

 REG_TC0_CMR0 = 5;

  // activate quadrature encoder and position measure mode, no filters

 REG_TC0_BMR = (1<<8)|(1<<9)|(1<<12);

 // enable the clock (CLKEN=1) and reset the counter (SWTRG=1)

 REG_TC0_CCR0 = 5;  

 REG_TC0_CCR1 = 5;

 //Remark out the next 4 lines to remove index interrupt and
  // accumulate index count
  REG_TC0_CMR1 = (1<<8); // Set rising edge of Z
 REG_TC0_IER1=0b10000000; // enable interrupt on Z
 REG_TC0_IDR1=0b01111111; // disable other interrupts
 NVIC_EnableIRQ(TC1_IRQn);

}

void loop() {
//REG_TC0_CV0 Stores count from encoder
//REG_TC0_CV1 Stores count from index if interrupts are off

}

void TC1_Handler() {

 z_Total++;
 long dummy=REG_TC0_SR1; // vital - reading this clears some flag
                           // otherwise you get infinite interrupts
}

Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: exedor on Jun 18, 2013, 09:44 am
I'm not able to read the value on REG_TC0_CV1 even when interrupts are off.  How did you turn interrupts off?  I'm wondering if rather than actually using the index line, it is instead recognizing an "external" trigger which happens to be coming through A6 rather than the actual index line.  From the data sheet it sounds like the TC0_CV0 register is supposed to reset to 0 each time an index pulse comes through.



Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: Designservicecorp on Jun 18, 2013, 10:33 pm

I updated my earlier post to include remarking out one more line (REG_TC0_CMR1 =) in order to allow the accumulation of index pulses.

I'll also go back and rearrange the code to make it easier to see the interrupt lines.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: schwingkopf on Jun 19, 2013, 12:32 am
with the updated version of your code I still dont see the index pulses being counted on REG_TC0_CV1.

just to clearify the functionality of the hardware quadrature decoder logic:
every index pulse should increase or decrease the value in the REG_TC0_CV1 register by one and at the same time restart or zero the position value in the REG_TC0_CV0 register. Please dont state you have solved the problem until you really see this behaviour!

As far as I understand your code, it just counts manually the index pulses using the interrupt handler, which is definitely not the same as the automatic counting using the hardware decoder logic.

Concerning the fact that the index pin might be on A6 and not on A7: In the figure 37-15 in the datasheet the index pin is drawn to be connected to TIOB1 (and not to TIOA1 as stated thoughout the rest of the datasheet) which corresponds to A6.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: Designservicecorp on Jun 19, 2013, 12:45 am
Did you remark out the 4 lines to stop the interrupt on the index? I copied the code to my DUE after editing my post and tried it both ways(with/without interrupts) and it functions. With interrupts enabled CV1 stays at zero but the interrupt handler occurs each index. With the interrupts remarked out CV1 counts up each index.

I'm running an encoder emulator box that only drives one direction, so I can't attest to lowering REG_TC0_CV1 only that mine adds each index. I'll have to swap my A & B wires and see what happens.

As for zeroing REG_TC0_CV0 on each index, I understood that to be a setting that needed to be enabled. My project didn't require it, so I didn't look into setting it up.

Thanks for spotting the A6 figure. This is not the worst document I have had the privilege of deciphering, but it's real close.

BTW, I never stated I solved the problem. Only sharing what I did to get the index to accumulate.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: Designservicecorp on Jun 19, 2013, 12:56 am

You got me curious on the direction issue.

I reloaded the code after remarking out the index interrupt and added a print statement for CV1 and started it.

I let the value increase to about 80 rotations and then pulled and swapped the A & B wires. The index count did start to decrease so direction does work. The number after passing zero became very large, however.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: exedor on Jun 19, 2013, 01:16 am
Thanks for that.  It still isn't doing what the data sheet says it will do.  According to the datasheet, when an index pulse comes along, it will supposedly reset the value in the TC0_CV0 register and start the count over at 0.  Um, nope.  That ain't happening!  Either data sheet lies or we have peripheral usage/configuration issues.  The fact that pulses are coming in A6 tells me something is amiss....nevertheless, I'm grateful to have at least something.   As I understand it, the index pulses are designed to help keep your stuff in sync.  I have seen for extremely fast pulse rates, pulses do get dropped and I've seen a tiny amount of drift.  I'm working with an extremely high precision application and that is a problem so I was really hoping to get the hardware quadrature decoder working as documented.  It's almost like the clock is actually using TIOB1 and not TIOA1 like the data sheet says or maybe for that port, the other peripheral is selected??

Also, for anyone looking to obtain the direction, you can do this:

Code: [Select]
dir = (REG_TC0_QISR >> 8) & 0x1

Assuming you have of course declared "dir"
I have it declared as a regular int and it is 0 when clockwise and 1 when counter-clockwise assuming you have ChA connected to pin 2 and ChB connected to pin 13.

Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: Designservicecorp on Jun 19, 2013, 05:36 pm

For those that want the index to trigger a reset of the A/B count register this works using the code from my earlier post (Reply #32):

Replace the line: REG_TC0_CMR0 = 5;

With this:

Code: [Select]
REG_TC0_CMR0 = (1<<0)|(1<<2)|(1<<8)|(1<<10);

This works with interrupts enabled on the index or not.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: schwingkopf on Jun 20, 2013, 12:18 am
That one's working fine! good job!
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: exedor on Jun 20, 2013, 04:43 am
I bow to the master an with humble respect!  You da man!

Seriously though, thank you.  I will have to try playing around with the edge triggering because in my test, for very slow rotations of the motor, the index pulse fails to increment the index counter and thereby creates some seriously erroneous readings.


Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: Designservicecorp on Jun 26, 2013, 12:25 am
I have been attempting to use Channel2 as a clock to determine speed and I believe I finally got it.

The code below is from the initial program I posted (Reply #32) but with Channel2 enabled and set to TCLK4. I also added more remarks and cleaned it up a bit.

Index interrupts must be enabled for the speed portion to work.

Code: [Select]

/*
 Quadrature Encoder with Marker via hardware
*/

volatile int z_Total=0;
volatile double TPR=0;

void setup() {

 Serial.begin(115200);  
 delay(100);

 // Setup Quadrature Encoder

 REG_PMC_PCER0 = (1<<27)|(1<<28)|(1<<29); // activate clock for TC0,TC1,TC2

 // select XC0 as clock source and set capture mode
 REG_TC0_CMR0 = 5;
     //Resets the count every index. Rem out the line above,
       //and use the line below instead.
     //REG_TC0_CMR0 = (1<<0)|(1<<2)|(1<<8)|(1<<10);

 REG_TC0_CMR2 = (1<<0)|(1<<1); // Set to TCLK4

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

 // Enable the clock (CLKEN=1) and reset the counter (SWTRG=1)
 REG_TC0_CCR0 = 5;  
 REG_TC0_CCR1 = 5;
 REG_TC0_CCR2 = 5;

 //Remark out these 4 lines to disable Index interrupts
 REG_TC0_CMR1 = (1<<8); // Set rising edge of Z
 REG_TC0_IER1=0b10000000; // enable interrupt on Z
 REG_TC0_IDR1=0b01111111; // disable other interrupts
 NVIC_EnableIRQ(TC1_IRQn);

}

void loop() {

 //REG_TC0_CV0 Stores count from encoder
 Serial.println(REG_TC0_CV0);

 //REG_TC0_CV1 Stores count from index if interrupts are off
   //if interrupts are on, CV1 will always reset every cycle to zero.
 if (REG_TC0_CV1 == 0)
 {
   Serial.print(z_Total);
   Serial.println("         -With Int");
 }
 else
 {
   Serial.print(REG_TC0_CV1);
   Serial.println("         -No Int");
 }
 //TPR holds the quantity of ticks every Index. Index interrupts must be on.
 //TCLK4 is set so 128 is the divisor.
 Serial.print((F_CPU/128/TPR)*60);Serial.println(" RPM");
 Serial.println("-------");

 delay(500);

}



void TC1_Handler() {
 //This won't fire unless the Index interrupt is enabled above
 z_Total++;

 long dummy=REG_TC0_SR1; // vital - reading this clears some flag
                           // otherwise you get infinite interrupts

 TPR=REG_TC0_CV2; //Store ticks
 REG_TC0_CCR2 = 4; //Reset counter
}
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: jchalo99 on Jul 23, 2013, 06:39 pm
OK sorry to bring up a dieing thread but...

I am about to wire up my arduino DUE to my table and i want to make sure i am getting this right. So from the Encoder i have 3.3v in (where 5v should be) and gnd connected. i take channel A+, B+, and Z+ and connect them directly to 2,13,A6 respectively. (my encoder has A-,B-, Z- aswell).

than i can upload the below code straight from arduino 1.5._?
Code: [Select]
/*
  Quadrature Encoder with Marker via hardware
*/

volatile int z_Total=0;
volatile double TPR=0;

void setup() {

  Serial.begin(115200); 
  delay(100);

  // Setup Quadrature Encoder

  REG_PMC_PCER0 = (1<<27)|(1<<28)|(1<<29); // activate clock for TC0,TC1,TC2

  // select XC0 as clock source and set capture mode
  REG_TC0_CMR0 = 5;
      //Resets the count every index. Rem out the line above,
        //and use the line below instead.
      //REG_TC0_CMR0 = (1<<0)|(1<<2)|(1<<8)|(1<<10);

  REG_TC0_CMR2 = (1<<0)|(1<<1); // Set to TCLK4

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

  // Enable the clock (CLKEN=1) and reset the counter (SWTRG=1)
  REG_TC0_CCR0 = 5; 
  REG_TC0_CCR1 = 5;
  REG_TC0_CCR2 = 5;

  //Remark out these 4 lines to disable Index interrupts
  REG_TC0_CMR1 = (1<<8); // Set rising edge of Z
  REG_TC0_IER1=0b10000000; // enable interrupt on Z
  REG_TC0_IDR1=0b01111111; // disable other interrupts
  NVIC_EnableIRQ(TC1_IRQn);

}

void loop() {

  //REG_TC0_CV0 Stores count from encoder
  Serial.println(REG_TC0_CV0);

  //REG_TC0_CV1 Stores count from index if interrupts are off
    //if interrupts are on, CV1 will always reset every cycle to zero.
  if (REG_TC0_CV1 == 0)
  {
    Serial.print(z_Total);
    Serial.println("         -With Int");
  }
  else
  {
    Serial.print(REG_TC0_CV1);
    Serial.println("         -No Int");
  }
  //TPR holds the quantity of ticks every Index. Index interrupts must be on.
  //TCLK4 is set so 128 is the divisor.
  Serial.print((F_CPU/128/TPR)*60);Serial.println(" RPM");
  Serial.println("-------");

  delay(500);

}



void TC1_Handler() {
  //This won't fire unless the Index interrupt is enabled above
  z_Total++;

  long dummy=REG_TC0_SR1; // vital - reading this clears some flag
                            // otherwise you get infinite interrupts

  TPR=REG_TC0_CV2; //Store ticks
  REG_TC0_CCR2 = 4; //Reset counter
}
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: Designservicecorp on Jul 26, 2013, 01:43 am

Code-wise you're correct with the pin numbers. Not sure about voltage because you didn't mention what encoder you're using.

I used an encoder with A+, A-, B+, B-, Z+, Z- and ran it into a Maxim MAX3096 422 receiver. This allowed me to run the encoder with 24vdc and interface it to the DUE 3.3v. It also gives great noise immunity.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: jchalo99 on Jul 26, 2013, 01:56 pm
Just an update, hardware wise it works!! the 5v encoder works just fine at 3.3v, i am getting some feedback.

The code works OK (havent verifyed . it gives me number of pulses for what i see correctly. i am thinking of hooking up my O-Scope and verifying it.

I dont know much about working directly with registers, but what type of data is REG_TC0_CV0? i am looking to use it in a PID loop. and how do i manualy reset it to 0? I have "home" switchs on my machine that i want to make 0,0.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: Designservicecorp on Jul 26, 2013, 05:08 pm

CV0 stores the current pulse count from A&B. The code your using is currently set to reset this value every Z. If you would like it to accumulate without resetting every rotation, read post #39 and reverse the code.

As for using your home switch, there might be a hardware method of tying that input to the counter but I'm not sure.

I would probably create an interrupt routine for that input and in the routine use "REG_TC0_CCR0 = 4;"  This does a software trigger and will reset CV0 once your have reversed the modification done in post #39. (Info located at 37.7.3 in the ARM docs)
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: jchalo99 on Jul 28, 2013, 09:09 pm
well modified the code... eliminated the z interup stuff. and allowed the counter to continuity count up. what type of data is CV0 + CV1. i still havent hooked up my o-scope to verify everything. but everytime the index pulse comes in the counter is going up 999, so i have strong feelings that its working.

thank you everyone for the wonderfull code. you should button it up as a library. define the library as with z axis. alow them to reset the counter. etc.
but the biggest thing is thank you all, this is a very wonderfull comuinity and always has been.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: bungle99 on Feb 23, 2014, 02:37 pm
Its a six month old thread, but I wanted to try digging it back up...

I have the code working in post #32 and post #42 - thank you.

Reading the SAM3X manual (http://www.atmel.com/Images/doc11057.pdf) page 891 section 37.6.14.5, it looks like this can be put into a Speed Measurement Mode rather than a Position Measurement Mode, allowing you to grab the speed directly, rather than having to calculate it (e.g. in theory it should give an answer similar to post #42 except more work is offloaded and I suspect no interrupts are required).

However, this is not my area of expertise; I've tried to understand the code so far combined with the docs, but have got well and truly lost. What I have so far is;

Code: [Select]

/*
  Quadrature Encoder with Marker via hardware
*/

void setup() {

  Serial.begin(9600); 
  delay(10);

  // Setup Quadrature Encoder
  // http://www.atmel.com/Images/doc11057.pdf
  // Speed measurement is p891

  // Activate clock for TC0,TC1,TC2
  REG_PMC_PCER0 = (1<<27)|(1<<28)|(1<<29); 

  // Page 895 (Enable features)
  REG_TC0_BMR = TC_BMR_QDEN
              | TC_BMR_SPEEDEN;
  
  // Page 891,
  REG_TC0_CMR2 = TC_CMR_TCCLKS_TIMER_CLOCK2 
               | TC_CMR_WAVE
               | TC_CMR_ACPA_TOGGLE 
               | TC_CMR_WAVSEL_UPDOWN;

  //Page 904
  REG_TC0_CMR0 = TC_CMR_ABETRG
               | TC_CMR_LDRA_RISING 
               | TC_CMR_LDRB_RISING
               | TC_CMR_ETRGEDG_RISING;

  // Enable the clock (CLKEN=1) and reset the counter (SWTRG=1)
  REG_TC0_CCR0 = TC_CCR_CLKEN | TC_CCR_SWTRG;  
  REG_TC0_CCR1 = TC_CCR_CLKEN | TC_CCR_SWTRG;
  REG_TC0_CCR2 = TC_CCR_CLKEN | TC_CCR_SWTRG;
}

void loop() {
  Serial.print(REG_TC0_CV0);
  Serial.print(", ");
  Serial.print(REG_TC0_CV1);
  Serial.print(", ");
  Serial.print(REG_TC0_CV2);
  Serial.println();

  Serial.print("  ");
  Serial.print(REG_TC0_CMR0);
  Serial.print(", ");
  Serial.print(REG_TC0_RA0);
  Serial.println();
  
  delay(1000);
}



If you try this example, and turn the encoder, things "tick", but the all important "speed" is doing nothing.

Code: [Select]

1282, 6527, 1565282450
  328960, 0
2388, 6531, 1576160954
  328960, 0
6496, 6533, 1587038366
  328960, 0
6496, 6533, 1597916870
  328960, 0


As I mentioned, I don't really know what I am doing; a combination of Google, manuals and educated guesses have got me this far, but I am obviously doing something plain wrong!

If any one has this Speed Measurement working, I would be interested to know.

Thanks
bungle99
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: Rgnazar on Feb 24, 2014, 02:44 pm
Using DUE
How do I use 02 encoders?
If possible with the pins too.
I'm a newbie at this.  :~ :~ :~
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: MarkT on Feb 24, 2014, 05:34 pm
O2 encoder?
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: bungle99 on Feb 24, 2014, 10:00 pm
I'm not sure what was meant by the "O2 Encoder" either. However, if this is some form / make of quadrature encoder, then I can confirm that the code in post #32 and post #42 work out of the box (including the pin assignments). However, I'd recommend reading the whole thread as there are some interesting nuggets on how to alter the code to do specific tasks.

If anyone has any ideas on enabling the  hardware "speed measure" mode, that would be appreciated as I'm still stumped after a good few hours of playing!
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: Designservicecorp on Feb 25, 2014, 12:39 am
Have you tried enabling the interrupts?

You also need to enter a timebase value before it will start counting (see 37.6.14.1 in the Arm Doc and also 37.6.14.5)

It's been awhile since I went through this. Hope it helps.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: bungle99 on Feb 28, 2014, 09:22 pm

Many thanks for the feedback.

I don't think interrupts should need to be enabled for this particular feature; the data sheet implies that Speed can be calculated "passively".

The timebase is the bit I think might be awry. Staring at the data sheet again now  :smiley-eek:
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: bungle99 on Feb 28, 2014, 10:02 pm
I've got this working. There were two big mistakes (and some minor things) in my original code



The code's a mess; let me tidy it up and I will repost it.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: bungle99 on Feb 28, 2014, 10:41 pm
This example uses the Due's hardware to decode a Quadrature Encoder input to determine speed.

No interrupts required. Can deal with higher speeds due to being a hardware based solution.

Most other samples in this thread are Position based, whereas this is Speed based. If you want position, then you need to look at Post #32. Calculating speed is a little harder; for example, see post #42 that does this using Position and an interrupt to give some concept of time. However, I believe this example simplifies things and improves the speed calculation by using the hardware for its calc (e.g. no need to wait for an Index interrupt to fire).

Many thanks to @Designservicecorp (notably post #32 and post #42) that gave me the base to work from.

See code for notes / comments.

Has had some basic testing, but YMMV. Don't blame me if it blows something up!

Code: [Select]

/*
** Quadrature Encoder on Arduino Due in "Speed Mode"
** Will not work on other Arduino types because of a specific hardware requirement.
**
** @bungle99 / 2014-02-28
**
** Many thanks to @Designservicecorp (notably post #32 and post #42) that gave me the base to work from.
**
** Uses *hardware* to do the heavy lifting of interpreting the Encoder output, which gives it significant
** advantage over other Arduino's for this use case. Eg many people have reported their Unos maxing out
** or skipping counts when interpretting high RPMs, whereas people using the Due in hardware mode have
** reported better success with high RPMs.
**
** This particular example puts the hardware into "Speed" mode, whereas other examples in this thread use
** "Position" mode (see posts 32, 42). The one example in the thread that attempts Speed calcs uses Position
** Mode and uses an interrupt on the Z Index axis to manually apply a time period (needed to caculate speed)
** on each rotation.
**
** This code below has been used in tests (using Lego NXT and a gear train for driving / comparison) to
** ~3,500 RPM with a 1,024 PPR Encoder (4 x 1,024 Edges). The Arduino didn't seem to struggle and the
** numbers (eg RPM) matched fairly closely with the Lego (eg as a sanity check, the calcs in the code look
** correct).
**
** I bought one of these encoders, which seems ok;
**
**   * http://proto-pic.co.uk/rotary-encoder-1024-p-r-quadrature/
**
** Great service from proto-pic. The encoder specs are officially
**
**   * http://dlnmh9ip6v2uc.cloudfront.net/datasheets/Robotics/E6B2Encoders.pdf
**
** But look an awful lot like this one too (which is easier to read)
**
**   * http://www.ia.omron.com/product/item/e6b27032r/index.html
**
** Note that this Encoder is rated at 5V min. I've successully powered it off the 5V Arduino pin header,
** and it doesn't work off the 3.3V one. At 5V, the outputs need to be run through some resistors to bring
** them into tolerance of the Arduino inputs.
**
** The inputs are 2 and 13 (and optionally A6 for the index).
**
** The test hardware (some Lego NXT servos and gear train) was unable to turn any faster (more gears
** stalled the servos) - eg the Arduino Due was not the limiting factor. At this speed, the Due Hardware
** is dealing with approx 238,000 edges per second.
**
** This example offloads the speed calculation onto the hardware, meaning no interrupt is required.
** In addition, the index is not required either, so it allows speeds to be seen even for rotations that
** don't pass the index marker (for example, slow rotations needing a speed before a complete rotation).
** That said, index is available if you want it and the docs suggest you can still use it (and others) as
** interrupt if you wish.
**
** NB There appears to be a myriad of features on the Chip that have not been touched. Eg you can still add
** interrupts to this code to do other things; eg passing the Z index, direction changes. There are also
** filters available if your encoder suffers from noise (eg vibrations). All this is available to play with
** another day :)
**
** Code appears to work, but no guarantees. Understand it. Test it. Feedback to the group. YMMV. Don't blame
** me if it doesn't work or expolodes!
*/

const int ENCODER_EDGES_PER_ROTATION = 1024 * 4;  // this depends on your encoder
const int ENCODER_SAMPLES_PER_SECOND = 10;        // this will need to be tuned depending on your use case...
const int LOOP_DELAY_MS = 1 * 1000;               // ... as will this (see comments in main code)

void setup() {
 
  Serial.begin(115200); 
  delay(10);


  // Setup Quadrature Encoder
  // http://www.atmel.com/Images/doc11057.pdf
  // Section 37 p869
  // Section 37.6.14 p885
  // Section 37.6.14.2 p890 Position and Rotation Measurement
  // Section 37.6.14.5 p891 Speed Measurement (what this is about)

  REG_PMC_PCER0 = PMC_PCER0_PID27
                | PMC_PCER0_PID28
                | PMC_PCER0_PID29;

  // Setup a channel in waveform mode (eg an input into the encoder to trigger the time based sampling)
  // Note some of the choices here impact calculations below and if you change them, you need to change
  // the next section to suit.
  // Section 37.7.11 p906 (also refer to Section 37.6.14.5 p891 for detail of what to set)
  REG_TC0_CMR2 = TC_CMR_TCCLKS_TIMER_CLOCK4
               | TC_CMR_WAVE
               | TC_CMR_ACPC_TOGGLE
               | TC_CMR_WAVSEL_UP_RC;

  // Now define the sample period, using the clock chosen above as the basis
  // Note that REG_TC0_CMR2 above is using CLOCK4; this is an 128 divisor. You need to change the
  // divisor below if you change the clock above. You could change the input clock and the RC mode to
  // suit your app (eg how many pulses are you  expecting - depends on encoder type
  // and slowest/normal/fastest rotation speed and what you want to do with the result).
  // Section 37.6.14.5 p891 notes you need to set this up, otherwise 0 comes out all the time :-)
  REG_TC0_RC2 = F_CPU / 128 / ENCODER_SAMPLES_PER_SECOND;
 
  // Setup a channel in capture mode
  // Section 37.7.10 p904 (also refer to Section 37.6.14.5 p891 for detail of what to set)
  REG_TC0_CMR0 = TC_CMR_ABETRG
               | TC_CMR_LDRA_EDGE
               | TC_CMR_LDRB_EDGE
               | TC_CMR_ETRGEDG_EDGE
               | TC_CMR_CPCTRG;

  // Enable features, noting Speed not Position is chosen
  // Section 37.7.2 p895 (also refer to Section 37.6.14.5 p891 for detail of what to set)
  REG_TC0_BMR = TC_BMR_QDEN
              | TC_BMR_SPEEDEN
              | TC_BMR_EDGPHA;
 
  // Set everything going
  REG_TC0_CCR0 = TC_CCR_CLKEN | TC_CCR_SWTRG; 
  REG_TC0_CCR1 = TC_CCR_CLKEN | TC_CCR_SWTRG; 
  REG_TC0_CCR2 = TC_CCR_CLKEN | TC_CCR_SWTRG; 
}

void loop() {

  int iIndexCount = REG_TC0_CV1;  // Don't need this, but manual notes its available 
  int iSpeedPPP = REG_TC0_RA0;    // This is what we're really after (speed in Pulses Per sample Period)
 
  // which we can convert to rps or rpm easily
  double dSpeedRPS = ((iSpeedPPP / (ENCODER_EDGES_PER_ROTATION * 1.0)) * ENCODER_SAMPLES_PER_SECOND);
  double dSpeedRPM =  dSpeedRPS * 60;
 
  Serial.print("Speed ppp: ");
  Serial.print(iSpeedPPP);
  Serial.print(", ");
  Serial.print("Speed rps: ");
  Serial.print(dSpeedRPS);
  Serial.print(", ");
  Serial.print("Speed rpm: ");
  Serial.print(dSpeedRPM);
  Serial.print(", ");
  Serial.print("Indexes: ");
  Serial.print(iIndexCount);
  Serial.print(". ");
  Serial.println();

  // Slow down the main loop noting the Encoder can update independently in the background (we're
  // effectively polling it to find out the latest and greatest info on each loop around).
  delay(LOOP_DELAY_MS);
}


Example output should look something like

Code: [Select]

// note 3,500 rpm
Speed ppp: 0, Speed rps: 0.00, Speed rpm: 0.00, Indexes: 0.
Speed ppp: -24351, Speed rps: -59.45, Speed rpm: -3567.04, Indexes: -52.
Speed ppp: -25007, Speed rps: -61.05, Speed rpm: -3663.13, Indexes: -113.
// snip
Speed ppp: -672, Speed rps: -1.64, Speed rpm: -98.44, Indexes: 61.
Speed ppp: -693, Speed rps: -1.69, Speed rpm: -101.51, Indexes: 59.
// snip - note direction change (+ve rpm)
Speed ppp: 1355, Speed rps: 3.31, Speed rpm: 198.49, Indexes: 63.
Speed ppp: 1344, Speed rps: 3.28, Speed rpm: 196.87, Indexes: 66.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: Rgnazar on Mar 11, 2014, 01:20 pm

Using DUE
How do I use 02 encoders?
If possible with the pins too.
I'm a newbie at this.  :~ :~ :~


I have a project to manage a telescope with Arduino.

I need to read 4 encoders.
2 encoder are the axes of the telescope (slow). I managed to read using this library (http://www.pjrc.com/teensy/td_libs_Encoder.html).
2 encoder on the motor shaft are (fast). I have lost these encoder ticks.

I do not understand almost anything to arduino low level programming.

Please, someone would be possible for a complete example, with the pins to be used ...

Well from what I understand is to use Encoder1A Encoder1B on pin 2 and pin 13.
And the second encoder which pin do I use?

Thanks for any help.

More information: (Pt-Br)
http://www.cosmobrain.com.br/cosmoforum/viewtopic.php?f=32&t=19190&sid=19398e95634d9f04a70536c6536305b0
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: iosugbakedano on May 06, 2014, 12:11 pm

I have been attempting to use Channel2 as a clock to determine speed and I believe I finally got it.

The code below is from the initial program I posted (Reply #32) but with Channel2 enabled and set to TCLK4. I also added more remarks and cleaned it up a bit.

Index interrupts must be enabled for the speed portion to work.

Code: [Select]

/*
 Quadrature Encoder with Marker via hardware
*/

volatile int z_Total=0;
volatile double TPR=0;

void setup() {

 Serial.begin(115200);  
 delay(100);

 // Setup Quadrature Encoder

 REG_PMC_PCER0 = (1<<27)|(1<<28)|(1<<29); // activate clock for TC0,TC1,TC2

 // select XC0 as clock source and set capture mode
 REG_TC0_CMR0 = 5;
     //Resets the count every index. Rem out the line above,
       //and use the line below instead.
     //REG_TC0_CMR0 = (1<<0)|(1<<2)|(1<<8)|(1<<10);

 REG_TC0_CMR2 = (1<<0)|(1<<1); // Set to TCLK4

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

 // Enable the clock (CLKEN=1) and reset the counter (SWTRG=1)
 REG_TC0_CCR0 = 5;  
 REG_TC0_CCR1 = 5;
 REG_TC0_CCR2 = 5;

 //Remark out these 4 lines to disable Index interrupts
 REG_TC0_CMR1 = (1<<8); // Set rising edge of Z
 REG_TC0_IER1=0b10000000; // enable interrupt on Z
 REG_TC0_IDR1=0b01111111; // disable other interrupts
 NVIC_EnableIRQ(TC1_IRQn);

}

void loop() {

 //REG_TC0_CV0 Stores count from encoder
 Serial.println(REG_TC0_CV0);

 //REG_TC0_CV1 Stores count from index if interrupts are off
   //if interrupts are on, CV1 will always reset every cycle to zero.
 if (REG_TC0_CV1 == 0)
 {
   Serial.print(z_Total);
   Serial.println("         -With Int");
 }
 else
 {
   Serial.print(REG_TC0_CV1);
   Serial.println("         -No Int");
 }
 //TPR holds the quantity of ticks every Index. Index interrupts must be on.
 //TCLK4 is set so 128 is the divisor.
 Serial.print((F_CPU/128/TPR)*60);Serial.println(" RPM");
 Serial.println("-------");

 delay(500);

}



void TC1_Handler() {
 //This won't fire unless the Index interrupt is enabled above
 z_Total++;

 long dummy=REG_TC0_SR1; // vital - reading this clears some flag
                           // otherwise you get infinite interrupts

 TPR=REG_TC0_CV2; //Store ticks
 REG_TC0_CCR2 = 4; //Reset counter
}



Hi, good job here!! I just have a question, the F_CPU variable is already defined? If it is not, what is the value of that parameter?
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: Designservicecorp on May 07, 2014, 12:21 am

F_CPU is the Frequency of the CPU.

In this case (DUE) it would equal 84000000 ( or 84MHZ)
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: Jasonrao on Aug 20, 2014, 03:26 pm
Hi Everyone,

My name is Jasonrao and this is my first time posting a comment on Arduino Due forum. This is to acknowledge that I have successfully configured Quadrature encoder (Avago Incremental Quadrature encoder with 500 CPR) by using the hardware Quadrature Encoder on Due. At the moment, I am having some difficulties in configuring another similar encoder on Due. In other words, I am trying to connect 2 quadrature encoders with 4 channels in total. As I know, Due hardware for capture and compare mode can support for only one encoder. By default, PWM Pin 2 and Pin 13 have been reserved for this purpose.

Can anyone help me with the codes to configure my second encoder on Arduino Due?

Many thanks,

Jasonrao
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: tuxcnc on Jan 27, 2015, 01:12 am
Hi.

First, there is error in SAM3X manual.
The descriptions are saying TIOA1 but drawings show TIOB1.
It seems correct is TIOB1 and arduino pin for index is A6, not A7.

Second, if you use encoder with open collector outputs, you can set pullups on SAM inputs and you don't need external resistors.
I have Omron E6C2-CWZ6C and it works fine, supplied from +5V arduino pin and without any extra elements.


Below is my program, shows shaft position, intended for dividing head.

Regards.

Code: [Select]
#define encoder_cpr 2000 // change to your encoder resolution
#define encoder_quad (encoder_cpr * 4); // quadrature counts is 4*CPR

void setup() {
  Serial.begin(115200);
  // set pullups on inputs
  pinMode(2, OUTPUT);
  digitalWrite(2, 1);
  pinMode(13, OUTPUT);
  digitalWrite(13, 1);
  pinMode(A6, OUTPUT);
  digitalWrite(A6, 1);

 // Setup Quadrature Encoder with index

 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<<8)|(1<<9)|(1<<12);

 // enable the clock (CLKEN=1) and reset the counter (SWTRG=1)

 REG_TC0_CCR0 = 5;  //
}

void loop() {
//REG_TC0_CV0 Stores count from encoder
//(REG_TC0_QISR >> 8) & 0x1; gives 0 or 1 depends of direction

  int position = REG_TC0_CV0;
  if (position < 0) { position += encoder_quad; } 
  Serial.println(position);
  delay(500);
}
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: Orion12 on Feb 07, 2016, 10:30 am
Hi tuxcnc

void setup() {
 Serial.begin(115200);
 // set pullups on inputs
 pinMode(2, OUTPUT);
 digitalWrite(2, 1);
 pinMode(13, OUTPUT);
 digitalWrite(13, 1);
 pinMode(A6, OUTPUT);
 digitalWrite(A6, 1);

I've just recently started coding in Arduino / Due & have a basic question . . .

In the above code section I notice you are using a "pinMode(2, OUTPUT)" instruction which I believe sets that Pin as an OUTPUT Pin . . . Seems to me that when Reading an External Encoder - The Encoder would OUTPUT it's signal to a Due INPUT Pin . . .

Can ANYONE Please Offer some additional Information on this question . . . As apparently this code works.

Thanks,
Orion12
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: Masood_Salik on Aug 10, 2016, 06:50 am
Does anyone have tried reading 2 Quadrature encoder from an arduino. I have 2 motors of 200 rpm and encoder of 3200 ticks per revolution. I wanted to read both encoder and determine its speed.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: AdderD on Aug 10, 2016, 02:49 pm
Does anyone have tried reading 2 Quadrature encoder from an arduino. I have 2 motors of 200 rpm and encoder of 3200 ticks per revolution. I wanted to read both encoder and determine its speed.
Well, the Due itself only has a single hardware quadrature decoding module. You could read a second one by using another timer/capture input and just counting the number of pulses. I don't know how easy it'd be to get direction but I suppose you could enable two TC inputs for the second encoder and have them interrupt when they transition state. Then, figure out which order they're transitioning in and you have direction. But, basically you'll have to create your own quadrature decoder in software and then use two TC inputs. It should be possible, just not as easy.

But, you said you want to know the speed, not necessarily the direction or exact quadrature position. I guess that could be a little easier. Set up a TC input and then time the gap between pulses and you have speed.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: Masood_Salik on Aug 11, 2016, 08:03 pm
Set up a TC input and then time the gap between pulses and you have speed.
I am new to Arduino Due Micro-controller programming. I have got to know that Sam3x has 2 quadrature built in.

http://arduino.stackexchange.com/questions/28214/reading-two-quadrature-encoder-using-a-single-arduino-due

But i dont know how to configure those pins. And if i use your idea How can i find the time gap between two pulses.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: AdderD on Aug 12, 2016, 08:58 pm
Well, the SAM3X documentation, the CMSIS header files, and I all respectfully disagree. I don't know what sort of drugs the writer of that Excel spreadsheet was on but the SAM3X has only a single quadrature encoder interface. There are not and never were two. My guess is that the spreadsheet has a copy/paste error or they just plain made an honest mistake. There are plenty of chips out there that do have two or more QEI. You can find several of them in the dsPIC30 and dsPIC33 line. Those are *super* popular for motor controllers.

Or, if you do still need to use the Arduino Due then just capture the second encoder in only the A channel, tie it into a TC channel other than 0, 1, 2 (the QEI) and then generate an interrupt when it triggers. Time how long the duration between interrupts is and you've got your speed.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: dlovy on Dec 01, 2016, 03:20 pm
Well, the SAM3X documentation, the CMSIS header files, and I all respectfully disagree. I don't know what sort of drugs the writer of that Excel spreadsheet was on but the SAM3X has only a single quadrature encoder interface. There are not and never were two.

There are three quadrature counters on the SAM3X !!!
But only two are connected to Due pins:   pins 2, 13 and A6 for TC0, pins 5, 4 and 3 for TC2.
I am running both simultaneously right now...
But the indexes do not work as expected, and there are too many errors in the SAM3X doc. Very painful...
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: octopic on Feb 14, 2017, 02:21 pm
Hi dlovy
Have you an example code for the double quad encoder measuring ?
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: Ajss on Mar 09, 2017, 07:31 pm
I see a lot of you guys have problem with second encoder and it's really hard to find decent answer on internet so I'll be glad to share my working code with you.Pins used for first encoder are 2 and 13, for second encoder 5 and 4.

Code: [Select]
const int quad_A = 2;
const int quad_B = 13;
const unsigned int mask_quad_A = digitalPinToBitMask(quad_A);
const unsigned int mask_quad_B = digitalPinToBitMask(quad_B);

void initQDec(void)
{
  // activate peripheral functions for quad pins first decoder
  REG_PIOB_PDR = mask_quad_A;     // activate peripheral function (disables all PIO functionality)
  REG_PIOB_ABSR |= mask_quad_A;   // choose peripheral option B
  REG_PIOB_PDR = mask_quad_B;     // activate peripheral function (disables all PIO functionality)
  REG_PIOB_ABSR |= mask_quad_B;   // choose peripheral option B

  // activate peripheral functions for quad pins second decoder
  PIO_Configure(PIOC, PIO_PERIPH_B, PIO_PC25B_TIOA6, PIO_DEFAULT);
  PIO_Configure(PIOC, PIO_PERIPH_B, PIO_PC26B_TIOB6, PIO_DEFAULT);

  // activate clock for TC0
  REG_PMC_PCER0 = (1<<27);
  // select XC0 as clock source and set capture mode
  REG_TC0_CMR0 = 5;
  // activate quadrature encoder and position measure mode, no filters
  REG_TC0_BMR = (1<<9)|(1<<8)|(1<<12)|(1<<13);
  // enable the clock (CLKEN=1) and reset the counter (SWTRG=1)
  // SWTRG = 1 necessary to start the clock!!
  REG_TC0_CCR0 = 5;

  // activate clock for TC2
  REG_PMC_PCER0 = PMC_PCER0_PID27 | PMC_PCER0_PID28 | PMC_PCER0_PID29 | PMC_PCER0_PID30 | PMC_PCER0_PID31;
  REG_PMC_PCER1 = PMC_PCER1_PID32 | PMC_PCER1_PID33 | PMC_PCER1_PID34 | PMC_PCER1_PID35;
  // select XC0 as clock source and set capture mode
  REG_TC2_CMR0 = 5;
  // activate quadrature encoder and position measure mode, no filters
  REG_TC2_BMR = (1<<9)|(1<<8)|(1<<12)|(1<<13);
  // enable the clock (CLKEN=1) and reset the counter (SWTRG=1)
  // SWTRG = 1 necessary to start the clock!!
  REG_TC2_CCR0 = 5;

  ////////
  REG_TC0_IER1=0b10000000; // enable overflow interrupt TC0
  REG_TC2_IER1=0b10000000; // enable overflow interrupt TC2
  REG_TC0_IDR1=0b01111111; // disable other interrupts TC0
  REG_TC2_IDR1=0b01111111; // disable other interrupts TC2

  NVIC_EnableIRQ(TC0_IRQn); // enable TC0 interrupts
  NVIC_EnableIRQ(TC2_IRQn); // enable TC2 interrupts
}
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: ard_newbie on Mar 09, 2017, 08:36 pm

A good starting point to see if it works with TIOA6 (pin 5) and TIOB6 (pin 4) would be to adapt the sketch you can find in this thread reply #55. AFAIK this is the most documented you could find.

Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: digibluh on Jun 10, 2017, 06:17 am

The number of encoders is based on the pin count of the chip... 144 pin have 3, lower ones have one.  the firmware and docs is sort of made to cover the base SAM3X product... but there are indeed 3 of them on the 144 pin... but i guess only two are exposed on the due.   course you could get adventurous and solder the wires on the chip :)
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: joker691 on Sep 16, 2017, 04:00 pm
Hi,

Encoder program is working well, but I have a problem. At startup REG_TC0_BMR == 0. When i turn encoder shaft on one side position encrease that is ok. but If REG_TC0_BMR == 0 and i turn shaft to other side than value is max unsigned Long 4294967295. Is there possibility to initialize REG_TC0_BMR to different number then zero at stratup or change register to signed Long?


Thanx.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: ard_newbie on Sep 16, 2017, 04:18 pm

As far as I can understand the datasheet, for position measurements, POSEN bit has to be set in TC_BMR.

When POSEN bit is set in the TC_BMR, the motor axis position is processed on channel 0 (by means of the PHA/ PHB edge detections on TIOA channel 0/TIOB channel 0) and the number of motor revolutions are recorded on channel 1 if the IDX signal is provided on the TIOB channel 1 input. TIOA channel 0 is leveraged to accumulate edges detection and TIOA/TIOB channel 0 are leveraged to deduce direction.
The position measurement can be read in the TC_CV0 register and the rotation measurement can be read in the TC_CV1 register.

Channel 0 and 1 must be configured in Capture mode (TC_CMR0.WAVE). 'Rising edge' must be selected as the External Trigger Edge (TC_CMR.ETRGEDG) and TIOA must be selected as the External Trigger (TC_CMR.ABETRG).

In parallel, the number of edges are accumulated on Timer Counter channel 0 and can be read on the TC_CV0 register then divided by 4 (Rising and Falling edges of PHA and PHB). Therefore, the accurate position can be read on both TC_CV registers and concatenated to form a 32-bit word.

The Timer Counter channel 0 is cleared for each increment of IDX count value.

Depending on the quadrature signals, the direction is decoded and allows counting up or down in Timer Counter channels 0 and 1. The direction status is reported on TC_QISR.

The process must be started by configuring bits CLKEN and SWTRG in the TC_CCR.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: joker691 on Sep 16, 2017, 04:27 pm
I think program is counting correctly, but the problem is that the register REG_TC0_CV0 is 32-bit word. So after powerup arduino REG_TC0_CV0 is set to 0. If i will turn motor to minus direction REG_TC0_CV0 will count down from 4294967295 or so. I want to use it for PID input and it will go crazy if motor will go to minus positions. So best will be set value to register manualy but i dont know how to modify it 
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: ard_newbie on Sep 16, 2017, 05:17 pm

Post your code.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: joker691 on Sep 16, 2017, 05:24 pm
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
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: ard_newbie on Sep 16, 2017, 06:12 pm
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) {

}



Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: joker691 on Sep 16, 2017, 08:01 pm
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?
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: ard_newbie on Sep 16, 2017, 08:11 pm
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.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: joker691 on Sep 16, 2017, 08:22 pm
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.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: ard_newbie on Sep 16, 2017, 08:29 pm
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.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: joker691 on Sep 16, 2017, 08:47 pm
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.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: ard_newbie on Sep 17, 2017, 08:04 am
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.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: joker691 on Sep 17, 2017, 01:10 pm
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.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: ard_newbie on Nov 12, 2018, 09:19 am
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 !
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: ard_newbie on Nov 12, 2018, 09:20 am
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;
}





Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: ard_newbie on Nov 14, 2018, 06:17 am

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.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: ard_newbie on Nov 15, 2018, 07:35 am
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;
  }
}



Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: dougcl on Nov 17, 2018, 06:12 pm
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
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: ard_newbie on Nov 17, 2018, 07:20 pm
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).
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: dougcl on Nov 18, 2018, 12:08 am
Ah, yes I could just accumulate a counter in a larger integer and do a carry at the overflow.

I just need a delta count though. So reset at plus or minus 32K works great for me.

I admit to using the SAM3S datasheet
http://ww1.microchip.com/downloads/en/DeviceDoc/Atmel-6500-32-bit-Cortex-M3-Microcontroller-SAM3S4-SAM3S2-SAM3S1_Datasheet.pdf

which shows the FILTER bit on page 768.

• FILTER:
0 = IDX,PHA, PHB input pins are not filtered.
1 = IDX,PHA, PHB input pins are filtered using MAXFILT value.

Looks like SAM3X is different. I suspect that it has no effect (except perhaps to disable), because the SAM3S datasheet also says that the filter is enabled when QDEC is enabled.

Regarding reset to avoid overflow using
TC0->TC_CHANNEL[0].TC_CCR = TC_CCR_SWTRG | TC_CCR_CLKEN;

It seems that the reset does indeed work, but the counter value of zero is not active until the encoder has been touched again. At least I think that's what is happening.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: ard_newbie on Nov 19, 2018, 08:11 pm
Looks like SAM3X is different. I suspect that it has no effect (except perhaps to disable), because the SAM3S datasheet also says that the filter is enabled when QDEC is enabled.

If you look at Sam3x, Sam4 or Sam7 datasheets, the quadrature decoders are all the same, FILTER bit is missing in the documentation BUT present in the header files, therefore IMO the right version of TC_BMR register is in Sam3s datasheet and the same for all Sam versions.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: Val69-Fr on Jan 23, 2019, 01:57 pm
Hello,

To begin, I don't have much knowledge in Aurduino DUE.

I bought an arduino DUE in order to get the speed of 2 RENISHAW Atom quadrature encoders with a Lowest recommended counter input frequency of 25MHz.
https://www.renishaw.com/en/atom-incremental-encoder-system-with-rtlf-linear-scale--24183

First of all, I read the various datasheet and previous post to understand what you did.

However, I wish in my case connect on the same arduino DUE my 2 encoders (without index) to know their speed (negative or positive) and save them or print them on serial port. Is that possible with DUE ?

Thanks in advance
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: ard_newbie on Jan 23, 2019, 04:38 pm
You can use both quadrature decoder QDEC0 and QDEC1 at the same time, for speed or position measurement of rotary quadrature encoders.

Two hardware quadrature decoder QDEC0 and QDEC1:
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

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


A minimal example sketch for speed measurement with QDEC1 (For QDEC0, replace TC2 accordingly):

Code: [Select]

/***************************************************************************************************/
/*                          QDEC Speed mode by polling in loop()                                   */
/***************************************************************************************************/

/*
  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 * 100;                     // ... as will this

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_CLOCK1  // Select Mck/2
                              | 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 / 2 / ENCODER_SAMPLES_PER_SECOND;  // F_CPU = 84 MHz
  // Final TC frequency is 84 MHz/2/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;

}

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);
}



Your encoders doc is unclear to me, Your encoders seem to run either as linear or rotary encoders. If they are used as rotary encoders, the speed measurement code is relevant, providing you know the number of pulses per revolution. For a linear use, search the code for Position measurement (an example in this thread, reply #87).

The DUE is 3.3V compliant, be careful with the voltage of the encoders output signals.

The "Master" clock of the DUE Timer Counters is 42 MHz (84 MHz/2).
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: ASK_ME on Sep 22, 2019, 01:39 am
Is it possibile to run 3 Encoders on DUE at the same time?
Now I have running 2 Encoders and it works fine.
The speed is very slow, max. 0.2 turns / Sec.
The Encoder sending 2500 Pulses / Revolution, that is 500 Pulses / Sec. maximum.


Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: ard_newbie on Sep 22, 2019, 06:58 am

For the third quadrature encoder, a simple attachInterrupt() should work fine since the speed is low. Attach a pull down resistor to the attachInterrupt pin you will select to avoid EMI.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: ASK_ME on Sep 23, 2019, 03:24 am
This is my code for two encoders that works fine (attached File).
How can I configure Timer 1 for the third Encoder wit the corresponding Ports?




Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: mikeyBoyAus on Oct 11, 2019, 01:19 pm
bungle99
I realise that this thread is quite old but in post #55 you posted some code for getting the speed of an encoder. I have been looking at you code for the hardware interrupts and speed calculation for quadrature encoders with a Due and have difficulty getting the RPM to match the values found just using a normal interrupt driven sketch which makes me think that I haven't really understood what is going on here. I have tried finding the referenced PDF document (doc11057.pdf) but can't find it.
I am using the encoder and disc from an old printer as well as a single slit disc with a simple photo interrupter on the same motor shaft and a simple sketch to get the time between breaking of the beam to work out the speed of the motor. Also by counting the quadrature RISING pulses (2 per interruption of the quadrature beam) between photointerrupter signals will give the encoder edges per rotation * 2 (4200 on my version). However I don't seem to be able to get values anything near this with the sketch in post #55. My motor is spinning at 840 rpm (according to my sketch) but yours gives something like 2000. I don't know enough about the inner workings of the SAM3 to get any further. Any suggestions would be helpful. Could you possibly suggest where the PDF is located in case this sheds some light on the problem.

Regards

Mike
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: ASK_ME on Oct 15, 2019, 12:47 am
I'm using the encoders for my hobby milling machine, to get the precise position (+/- 0.003mm) of the platform.
X and Y acsis works fine but I need the Z axis to!

The doc11057.pdf you can get here:
https://ww1.microchip.com/downloads/en/DeviceDoc/Atmel-11057-32-bit-Cortex-M3-Microcontroller-SAM3X-SAM3A_Datasheet.pd
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: ASK_ME on Oct 23, 2019, 06:14 pm
Hey Users!

I don't believe it, nobody using Arduino DUE with 3 Axis encoder ???
Ok, I will do it myself, it is a big challenge for me ( mechanical designer) to learn programming, but I'll do it!!!
   
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: ASK_ME on Oct 23, 2019, 06:20 pm
Thanks for all comments (and helps ???)...

:)  :)  :)  :)

Unable to publish the post. Please notice you can only post once every 5 minutes and only edit posts after 30 seconds. Once you reach 100 published posts this limit will be removed.

Shame on you Arduino Forum !!!

BAD, BAD, BAD, BAD !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: heliro on Feb 14, 2020, 10:25 am
Hello ASK_ME, have try your Rotary_Encoder_3_DUO.ino very nice, I have code for 3 axis with arduino mega2560 and display SSD1963 code working nice with 3 axis but problem is refreshing display nummbers of axis dont turn in real time so I got today ARDUINO DUE SAM3X and try with your code but problem is becouse I have 7" SSD1963 i try repair code for SSD1963  or You have maybe code for rotary encoder with 7" SSD1963?
If can You help I will happy!
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: ASK_ME on Feb 20, 2020, 07:06 am
@heliro
The mega 2560 has no Hardware-Quadrature-Encoder!
mega2560 is to slow to handle 3 encoders at the same time but this is dependent of the rotations speed!

For your Display, get this Library: http://www.rinkydinkelectronics.com/download.php?f=UTFT.zip

Best Regards
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: heliro on Feb 23, 2020, 02:20 pm
Code encoder ARDUINO DUE SAM3X for SSD1963 display x and y axis DRO
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: chamarthi on Feb 27, 2020, 04:41 pm
I learned Quadrature decoder usage from the code of ard_newbie, I measured the speed of the shaft by using signals QA and QB only.

what I understood is..................

The rotary encoder will have three signals as per my knowledge, which are named as QA,QB and Index. The signals QA and QB  signals are enough for the measurement of speed. In ATSAM3X8E their is an option of using the hardware Quadrature decoder module.

There are two quadrature decoders modules in the device which are incorporated in Timer/Counter-0 and Timer/Counter-2. Therefore by manipulating the registers of each channel in the timer/counter segment the speed or position of the shaft can be calculated.

steps to measure the speed.
1. Initialize the channel-0 in counter mode. To count the number pulses for a particular time interval.
2. Initialize the channel-2 in timer mode. For generating the triggering pulses for the channel-0, such that counter value will be stored into the register and then reset to zero every time.

The code is available at ard_newbie posts. If anybody wants my code just put a message to me. mine is a bit modified version of ard_newbie code.

If their is any mistake in my analysis, please inform me.
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: al_iraqi on Jul 22, 2020, 02:00 pm
(http://file:///C:/Users/AL-IRAQI/Desktop/gggg.jpg)

please help me can change the #define PERIOD_VALUE   (250)  to be #define PERIOD_VALUE   (value of speed from the rotary encoder)             
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: al_iraqi on Jul 22, 2020, 09:41 pm
please help me, can change the #define PERIOD_VALUE   (250)  to be #define PERIOD_VALUE   (value of speed from the rotary encoder)   
Title: Re: Using the Hardware Quadrature Encoder Channel on the DUE
Post by: dooeh on Sep 16, 2020, 10:45 pm
I just wanted to add (for anyone who finds this thread in the future) that you can deal with the underflow by type-casting the register value to a signed 32 bit number.

Code: [Select]
encoderPosition = (int32_t) REG_TC2_CV0;

This will give you negative values as the encoder rotates past zero with the upper and lower limits of int32.