Hardware Quadrature Code for Teensy 3.x


Well-known member
Attached is my code for utilizing the hardware quadrature decode for Teensy3.x. As others have noted, it is not straightforward. The marketing department must have decided the applications that utilized more than 64K encoder counts or were not at least piecewise uni-directional were a small part of the market.

I designed the code to minimize interrupt latency requirements and interrupt overhead. The code could have been a bit more straightforward if I had used both compare channels, but I wanted to save one for an exact position latch on hardware trigger feature (coming later).

There is the class QuadDecode<n> (it is templated to use either the FTM1 or FTM2 channel).
* The routines that can be called are:

* setup() - constructor order of execution is not guaranteed, this
* is called to set everyting up at the appropriate time. It overwrites some registers setup in
* teensy.c for the tone() library, so has to be called after that.

* start() - starts the counter counting at zero

* zeroFTM() - zeroes the counter

* calcPosn() - gets the current position. Needs to add the current
* counter position to the basePosn. Accurate to within
* interrupt latency and processing delay.

The programs attached are:

QuadDecode.h, QuadDecode_def.h - the important ones. Code to utilize the hardware quadrature decode channels on Teensy3.x

main.cpp - an example of use. As I mentioned, it is templated, and I did not Arduinofy it, so this shows how to use the templates.

GenEncoder.h, GenEncoder.cpp - program that generates simulated encoder signals for debug and development.

Have fun. Let me know of questions or comments.


P.S. Next step - hook up a 2nd Teensy thru I2C serial for additional quadrature channels. First action item - figure out how to program two Teeny's hooked up at the same time.


  • GenEncoder.cpp
    3 KB · Views: 5,336
  • GenEncoder.h
    3.5 KB · Views: 4,248
  • main.cpp
    11.2 KB · Views: 5,030
  • QuadDecode.h
    10.4 KB · Views: 4,699
  • QuadDecode_def.h
    11.9 KB · Views: 4,375

I've been looking for ways to decode 3 quadrature encoders with just 1 teensy 3.1, and I've been pointed to this topic.

It seems like the MK20DX256/MK20DX128 both have
"Two 2-channel quadrature decoder/general purpose timers."

Does this mean that the Teensy 3.1 can hardware decode two encoders? Even if so, this would probably not be useful.
Multiple teensies with I2C is not an option for me due to issues with speed, as well as space limitations.


Hi Brian,

Teensy 3.1 can decode two encoders. And from your other post you have an encoder edge rate of 53 kHz. This would give an interrupt rate of about 10 Hz to update position with the attached code. You don't say what your PID control loop rate is.

So for the 3rd encoder - put this code on one of your other Teensy's - it doesn't take much overhead, the hardware does all the work. And if your PID control speed accommodates it just read the position back on I2C. The I2C spec is 100kbit/s with max load or clock/20 (~2 Mhz) with less loading. A 32 bit position could be updated at about 50 kHz.

Or use the two hardware decoders, and do the third channel by direct sampling. So only one channel is interrupt intensive.


P.S. Code to read position back over I2C coming at some time.
Thanks. Hopefully one of the methods will work, the preferable being the second option (third channel by direct sampling).

One last question. I'm assuming the library doesn't support the index channel (I) from scanning through the code. Does the chip not support this?

My system does not have an index channel, so I did not write code for it.

As far as the hardware goes, there is not much it supports completely. There are enough pieces to do just about anything (while cursing the marketing department - a slight addition to the hardware would have made some things much easier) with enough code. There is a whole additional counter that doesn't have the quadrature front end that could be used to count index pulses (with the issue of how to deal with reversing). If all you want from the index channel is a zero, a hardware interrupt can call the zero function.

I don't think the index channel is actually useful to me, as I only need velocity.

Just to confirm:
Encoder X is connected to digital pins 3/4,
Encoder Y is connected to digital pins 32/25?

The following is from QuadDecode.h

 *    ENCXA	 3	PTA12	 28	Input	    FTM1    7
 *    ENCXB	 4	PTA13	 29	Input	    FTM1    7
 *    ENCYA	32	PTB18	 41	Input	    FTM2    6
 *    ENCYB	25	PTB19	 42	Input	    FTM2    6

Forgot to ask. Which type of count mode does the library support by default (I don't mind but it's good to know)?
Not exactly sure what you are asking. The code assumes a quadrature input signal and determines position and direction from that.

Has anyone prototyped a "shield" for this role yet? ie: Connectors for a pair of Quadrature encoders, endstops, and either a pair of H-Bridges to drive DC motors or at least signals to an external Motor driver.

I'm switching up from a pair of Arduino Pro minis driving my DC motor REPSCRAP 3D printer to a single Teensy, and will likely be making a few boards if they don't already exist.
The quadrature decode hardware on the processor only has the capability for what would be 4x count mode, and that is what is supported. No other options.

I am also working on an optical encoder based repstrap. The Teensy3.1 has enough horse power to decode at least 4 channels of optical encoders with pulse frequencies of at least 20KHz without using hardware, which is plenty for my purposes (my motors can only move so fast). I do the encoder math in a 50KHz timer interrupt. Since I am not using hardware I can use whatever 5v tolerant pin I want.

I have attached the teensy 3.1 to one seeedstudio motor shield 2.0 (very good value for money). I can directly connect the outputs to the motor shield (the L298 works OK with 3.3V output I guess). The inputs are connected directly to the outputs of quadrature encoders like these: (http://reprap.org/wiki/Optical_encoders_01). Since the inputs are 5V tolerant, it works well. I am planning on connecting a second motor shield soon.


/* code by fdavies

   This example code is in the public domain.
   The goal is to have a complete 3D printer driver (gcode => motion)
   that uses optical encoders and brushed DC motors.

// definition of how the pins are set up

const int xQ1Pin = 7;  // pin for x axis quadrature encoder    J1 pin 4
const int xQ2Pin = 8;  // pin for x axis quadrature encoder    J1 pin 3
//(GPIOD_PDIR & 0x000C) >> 2    expression for reading the pins directly 
const int xHPin = 9;   // pin for x axis home signal               // not implemented yet  J1 pin 2
const int xM1Pin = 3;  // pin for x axis motor control M1    goes to IN1 pin of seeedstudio motor shield v2.0  J1 pin 6
const int xM2Pin = 4;  // pin for x axis motor control M2    goes to IN2 pin of seeedstudio motor shield v2.0  J1 pin 9

const int yQ1Pin = 10;  // pin for y axis quadrature encoder   J2 pin 4
const int yQ2Pin = 11;  // pin for y axis quadrature encoder   J2 pin 3
//((GPIOC_PDIR & 0x0010) >> 4) | ((GPIOC_PDIR & 0x0040) >> 5)   expression for reading the pins directly 
const int yHPin = 12;   // pin for y axis home signal            J2 pin 2
const int yM1Pin = 5;   // pin for y axis motor control M1        goes to IN3 pin of seeedstudio motor shield v2.0  J2 pin 6
const int yM2Pin = 6;   // pin for y axis motor control M2        goes to IN4 pin of seeedstudio motor shield v2.0  J2 pin 9

const int zQ1Pin = 14;  // pin for z axis quadrature channel 1       J3 pin 4
const int zQ2Pin = 15;  // pin for z axis quadrature channel 2       J3 pin 3
//((GPIOD_PDIR & 0x0002) >> 1) | ((GPIOC_PDIR & 0x0001) << 1)   expression for reading the pins directly 
const int zHPin = 24;   // pin for z axis home signal, may change 
const int zM1Pin = 20;  // pin for z axis motor control M1       will go to another seeedstudio motor shield when I get it hitched up
const int zM2Pin = 21;  // pin for z axis motor control M2        

const int eQ1Pin = 16;  // pin for e axis quadrature channel 1    J4 pin 4
const int eQ2Pin = 17;  // pin for e axis quadrature channel 2    J4 pin 4
//((GPIOB_PDIR & 0x0003) >> 0)   expression for reading the pins directly 
const int eHPin = 25;   // pin for e axis home signal  may change  25 B 19
const int eM1Pin = 22;  // pin for e axis motor control M1         
const int eM2Pin = 23;  // pin for e axis motor control M2         

const int motorEnable = 2; // pin to enable the motor drivers (all)  goes to EA and EB of seeedstudio motor shield v2.0 
const int outMin = 0; // minimum output level (PWM)
const int outMid = 128;  // middle output level (PWM)
const int outMax = 256;  // maximum output level (PWM)

// Teensy 3.0 has the LED on pin 13
const int ledPin = 13;
int p=0;
// this is the decode table for the optical encoder
signed long enc_TAB[]={0,1,-1,0,-1,0,0,1,1,0,0,-1,0,-1,1,0};

// the setup() method runs once, when the sketch starts

// Create an IntervalTimer object 
IntervalTimer encoder;

volatile unsigned long encoderState = 0; // use volatile for shared variables

volatile signed long xEncoder = 0; // This is the position based on counting encoder pulses
volatile signed long yEncoder = 0; // This is the position based on counting encoder pulses
volatile signed long zEncoder = 0; // This is the position based on counting encoder pulses
volatile signed long eEncoder = 0; // This is the position based on counting encoder pulses

volatile signed long xSetPoint = 0; // this is where the PID loop will try to move the axis
volatile signed long ySetPoint = 0;
volatile signed long zSetPoint = 0;
volatile signed long eSetPoint = 0;

unsigned int count1 = 0;  // so that the encoder counting is faster than the PID updating
unsigned int channelCount = 0;  // 

signed long xki=0;  // PID integration constant
signed long xkp=1024;  // PID proportional constant
signed long xkd=1024;  // PID derivative constant
signed long xITerm=0;
signed long xLastInput=0;

signed long yki=0;  // PID integration constant
signed long ykp=1024;  // PID proportional constant
signed long ykd=1024;  // PID derivative constant
signed long yITerm=0;
signed long yLastInput=0;

signed long zki=0;  // PID integration constant
signed long zkp=0;  // PID proportional constant
signed long zkd=0;  // PID derivative constant
signed long zITerm=0;
signed long zLastInput=0;

signed long eki=0;  // PID integration constant
signed long ekp=0;  // PID proportional constant
signed long ekd=0;  // PID derivative constant
signed long eITerm=0;
signed long eLastInput=0;

//unsigned long lastTime;

void setup() {
  // initialize pins for the motor control channels
  int k=0;
  Serial.println(" step 1");
  pinMode(xQ1Pin, INPUT);  
  pinMode(xQ2Pin, INPUT);  
  pinMode(xHPin, INPUT);  
  analogWriteFrequency(xM1Pin, 20000); // change PWM frequency out of audible range
  pinMode(xM1Pin, OUTPUT);  // default frequency and resolution for now
  pinMode(xM2Pin, OUTPUT);  // default frequency and resolution for now  
  pinMode(yQ1Pin, INPUT);  
  pinMode(yQ2Pin, INPUT);  
  pinMode(yHPin, INPUT);  
  analogWriteFrequency(yM1Pin, 20000); // change PWM frequency
  pinMode(yM1Pin, OUTPUT);  // default frequency and resolution for now
  pinMode(yM2Pin, OUTPUT);  // default frequency and resolution for now    

  pinMode(zQ1Pin, INPUT);  
  pinMode(zQ2Pin, INPUT);  
  pinMode(zHPin, INPUT);  
  pinMode(zM1Pin, OUTPUT);  // default frequency and resolution for now
  pinMode(zM2Pin, OUTPUT);  // default frequency and resolution for now    

  pinMode(eQ1Pin, INPUT);  
  pinMode(eQ2Pin, INPUT);  
  pinMode(eHPin, INPUT);  
  pinMode(eM1Pin, OUTPUT);  // default frequency and resolution for now 488.28 Hz
  pinMode(eM2Pin, OUTPUT);  // default frequency and resolution for now    
  pinMode(ledPin, OUTPUT);

  digitalWrite(motorEnable,HIGH);  // enable the motor drivers  


  /* move all the motors weakly negative for 2 seconds to home them.  Do this better later */
  Serial.println(" step 3");    
  encoder.begin(encoderUpdate, 20);  // blinkLED to run every 0.02 millisecond 50 KHz

// functions called by IntervalTimer should be short, run as quickly as
// possible, and should avoid calling other functions if possible.
void encoderUpdate(void) {
  unsigned long nowState = 0 ;
  unsigned long encIndex =0;
  signed long error=0; // for PID loops
  signed long dInput=0; // for PID loops
    signed long Output=0; // for PID loops
  digitalWriteFast(ledPin,1); // for measuring interrupt duration with an oscilloscope
  /* nowState = (digitalRead(xQ2Pin)<<1) | digitalRead(xQ1Pin)      | (digitalRead(yQ2Pin)<<5)  | (digitalRead(yQ1Pin)<<4) | \
             (digitalRead(zQ2Pin)<<9) | (digitalRead(zQ1Pin)<<8) | (digitalRead(eQ2Pin)<<13) | (digitalRead(eQ1Pin)<<12);  // 2.5 microseconds */

  nowState = (digitalReadFast(xQ2Pin)<<1) | digitalReadFast(xQ1Pin)      | (digitalReadFast(yQ2Pin)<<5)  | (digitalReadFast(yQ1Pin)<<4) | \
             (digitalReadFast(zQ2Pin)<<9) | (digitalReadFast(zQ1Pin)<<8) | (digitalReadFast(eQ2Pin)<<13) | (digitalReadFast(eQ1Pin)<<12); 
  /*nowState = (GPIOD_PDIR & 0x000C) >> 2  | \
             ((((GPIOC_PDIR & 0x0010) >> 4) | ((GPIOC_PDIR & 0x0040) >> 5) ) << 4) | \
             ((((GPIOD_PDIR & 0x0002) >> 1) | ((GPIOC_PDIR & 0x0001) << 1)) <<8 ) |  \
             (( (GPIOB_PDIR & 0x0003) >> 0) <<12) ;         */
  encIndex=nowState | (encoderState << 2);
  xEncoder = xEncoder + enc_TAB[ (encIndex >> 0) & 0x0f];  // these 4 lines take about 0.7 microseconds, maybe can be optimized
  yEncoder = yEncoder + enc_TAB[ (encIndex >> 4) & 0x0f];
  zEncoder = zEncoder + enc_TAB[ (encIndex >> 8) & 0x0f];
  eEncoder = eEncoder + enc_TAB[ (encIndex >> 12) & 0x0f];  

  encoderState = nowState; 

  if (count1 >= 50) {  // aiming for 1 khz update rate
    count1 = 0; // restart the count
    if (channelCount >= 4) {
     channelCount = 0;
    switch (channelCount) {  // actually each channel has its PID updated at 250Hz, which seems faste enough so far.
    case 0: // x channel
      **   PID stuff closely based on
      **   // brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/
      digitalWriteFast(ledPin,1);  // as it stands, this takes about 3 microseconds
      /*Compute all the working error variables*/
      error = xSetPoint - xEncoder;
      xITerm+= (xki * error*1024);  // scaling for fixed point 
      if(xITerm > outMax*1024) xITerm= outMax*1024;
      else if(xITerm < outMin*1024) xITerm= outMin*1024;
      Output = (xkp * error + xITerm- xkd * dInput)/1024;
      if(Output > outMax) Output = outMax;
      else if(Output < outMin) Output = outMin;
      xLastInput = xEncoder;  
    case 1: // y channel
      error = ySetPoint - yEncoder;
      yITerm+= (yki * error*1024);  // scaling for fixed point
      if(yITerm > outMax*1024) yITerm= outMax*1024;
      else if(yITerm < outMin*1024) yITerm= outMin*1024;
      Output = (ykp * error + yITerm- ykd * dInput)/1024;
      if(Output > outMax) Output = outMax;
      else if(Output < outMin) Output = outMin;
      yLastInput = yEncoder;  
    case 2: // z channel
    case 3: // e channel
    //Calculate the distance between the setpoint and encoder values
    // this is my first start at getting a way to tune the two channels so they work together
    // xSetPoint-xEncoder)*(xSetPoint-xEncoder) +  (ySetPoint-yEncoder)*(ySetPoint-yEncoder)
  digitalWriteFast(ledPin,0); // for measuring interrupt duration with an oscilloscope


// the loop() methor runs over and over again,
// as long as the board has power

void loop() {
  int i;
  signed long copy_xEncoder= 0;
  signed long copy_yEncoder= 0;
  signed long copy_zEncoder= 0;
  signed long copy_eEncoder= 0;

    cli(); // disable interrupt
    copy_xEncoder = xEncoder;
    copy_yEncoder = yEncoder;
    copy_zEncoder = zEncoder;
    copy_eEncoder = eEncoder;
    sei(); // reenable the interrupt 
    Serial.print(" ");
    Serial.print(" ");
    Serial.print(" ");
    Serial.println(" ");
  xSetPoint = 1000;  // this is how the PID control loop is told to move the axis.
  ySetPoint = 3000;
  delay(100); // 

  for (i=0;i<2000;i++)  {  // test profile
    xSetPoint = 3000 - i;
    ySetPoint = 1000 + i;
  delay(100); // 
  xSetPoint = 1000;    
  ySetPoint = 3000;

// see.stanford.edu/materials/aiircs223a/handout6_Trajectory.pdf
// developer.mbed.org/cookbook/PID
// brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/
// www.embeddedrelated.com/showarticle/121.php
Thanks TLB.

fdavies, 4 20KHz encoders without hardware is impressive.
Do you think it'll still work at several hundred Hz if you implemented the Teensy as an SPI slave and received commands for the target speed of each motor?
The computational power of the teensy is what is impressive.

I have not used the SPI bus on the teensy, but I expect that it would come down to how interrupts interact with each other. I do not know if the SPI system could put up with a delay of a microecond or two while the quadrature encoder interrupt updated. If it could not, then the quadrature encoder logic probably could.

Other than that, I think it could do it.


I'm working along similar lines, but using TLB's hardware implementation for EncX / EncY. Hardware pin interrupts for EncZ1 and EncZ2 (left and right). My theory being that the Z axis gets very little overall motion, and will contribute little to performance degradation. As I said... it's a theory... I'll let you know as soon as I have all three axis moving.
My build-Prep stage: Teensy 3.1 Repstrap printer with DC motor control and Flex Timer Quadrature Encoders

It's taken me a while to get the interrupts working without conflicting. I'm using a similar approach to fdavies and interestingly, using pure software decoding is faster than a mix of both hardware and software decoding. Thanks for all your help.

Currently SPI slave is also working very well. Basically, the teensy is sending/receiving at around 100kb/s whilst decoding 4 encoders at 52kHz (yes I've changed to 4 wheels from 3)
unix_guru, I suggest you try both pure software decoding or a mix of both. Right now I'm under the impression that software might just be better for encoder edge rates of <100kHz.

Many many thanks for your code. I have modified it slightly for my own use. All I am looking for is to output the encoder values as fast as possible over RS232. The maximum speed I am getting for the RS232 output is 64Hz. I adjusted the GUI Update Time to 5000 and was getting 200Hz but now can't seem to get it.

Can anybody suggest what may be limiting the serial output to 64Hz?

See code below:

To convert from raw encoder * 0.00137290914

Send back values to be read by Python pyserial
Send back 6 values for realtime X,Y,Z and latched X,Y,Z
On button push, latch X,Y,Z with realtime X,Y,Z

Send lines with
RX<value>\n (realtime)
LX<value>\n (latched)

Send back <value> as ascii string
Only send back latched value once after latching

Also send back comments with MS<string>

Check if any bytes received
If axis to be zeroed, expect string
ZZXY where ZXY are 1 if axis to be zeroed
0 otherwise

/* Use Teensy3.1 FTM (FlexTimerModule) Quadrature Decode
* to determine position on 2 axis
* (3rd channel with second Teensy TBD)
* Position Counter is 16-bit counter
* Read position every 100 mS and determine if counting up
* or down
* Count up in interrupt routine for 200 mS GUI update

/* Pinout
* Function Arduino K20ARM Pin Direction Pin Alternative
* LED 13 PTC5 50 Output
* ENCXA 3 PTA12 28 Input FTM1 7
* ENCXB 4 PTA13 29 Input FTM1 7
* ENCYA 16/A2 PTB0 35 Input FTM1 6
* ENCYB 17/A3 PTB1 36 Input FTM1 6
* ENCZA 32 PTB18 41 Input FTM2 6
* ENCZB 25 PTB19 42 Input FTM2 6
* ENCA 9 PTC3 46 Output
* ENCB 10 PTC4 49 Output
* Pin Control Registers PORTx_PCRn
* Bit 10-8 MUX Pin Mux Control 1-7 alternative
* Alternative 1 is GPIO
* Bit 4 PFE Passive Filter Enable 1 to enable
* Bit 1 PE Pull Enable 1 to enable
* Bit 0 PS Pull Select 0 is pulldown 1 is pullup
/* Quadrature Decoding with FTM1 Setup
* Filter A PHAFLTREN bit CH0FVAL[3:0] in FILTER0
* Filter B PHBFLTREN bit CH1FVAL[3:0] in FILTER0
* No channel logic used
* FTM channels in input capture or output compare modes
* PHAPOL and PHBPOL selects polarity
* QUADMODE = 0 for quadrature decode
* CNTIN=0 to count from 0
* MOD = 64K for maximum range
* TOF and TOFDIR indication Top Overflow and Direction
* Set CNTIN and MOD before set FTMEM
* The following registers are set in pins_teensy.c
* and need to be reset.
* FTM1_CNT Counter value
* Bit 15-0 is counter value
* Writing any value updates counter with CNTIN
* FTM1_MOD Modulo (Max value)
* Bit 15-0 is counter value - set to 0xFFFF
* Write to CNT first
* FTM1_MODE Features Mode Selection
* Bit0 FTMEN FTM Enable - [WPDIS must be 1 to write]
* Bit2 WPDIS Write Protect to Disable
* Set WPDIS, then set FTMEN - must be set to access FTM regs
* FTM1_FMS Fault Mode Status
* Bit 6 WPEN Write Protect Enable
* Write 1 to clear WPDIS
* Filter out pulses shorter than CHxFVALx 4 system clocks
* Bit 7-4 CH1FVAL for PHB
* Bit 3-0 CH0FVAL for PHA
* FTM1_QDCTRL Quadrature Decoder Control and Status
* Bit 7 PHAFLTREN Phase A Filter Enable
* Bit 6 PHBFLTREN Phase B Filter Enable
* Bit 5 PHAPOL Phase A Polarity
* Bit 4 PHBPOL Phase B Polarity
* Bit 3 QUADMODE Quadrature Decoder Mode
* 0 for Quadrature
* Bit 2 QUADIR Counting Direction
* Bit 1 TOFDIR Timer Overflow Dir
* 0 was set on bottom of counting
* 1 was set on top of counting
* Bit 0 QUADEN Quadrature Mode Enable
* 1 is quadrature mode enabled [WPDIS to write]
/* Generate 32 bit position value from 16 bit counter
* Keep track of BasePosition which indicates how many times wrapped
* around
* Generate TotalPosition by reading counter and adjusting from
* Base Position
* To generate Base Position interrupt at every quadrant
* This is to avoid issues if sitting and oscillating around either point
* Use every quadrant interrupt to determine direction
* To generate base position
* Interrupt on Timer Overflow (TOV)
* Look at current and previous overflow direction
* If overflow in same direction, add or subtract counter value (64K)
* If overflow in opposite direction, reversed direction
* BasePosition count remains the same
* Set Interrupt to Compare at quarter points +-(64K/2)
* Disable Overflow interrupt
* Leave Overflow bit set
* Will be cleared during Compare Interrupt or Position reading
* Need to check during Position reading
* Interrupt on Compare
* Save last overflow direction to know which way were going
* Clear overflow bit
* Far enough away so no oscillation
* Need to interrupt again when reach overflow point
* Set interrupt to overflow
* To get TotalPosition read counter and adjust from Base Position
* Read counter as unsigned integer
* Read overflow direction
* If last overflowed counting up, add to Base Position
* If last overflowed counting down, subtract 64K and add to
* Base Position
* Issues with Overflow interrupt occuring during reading
* To deal with this:
* If Interrupt on Compare clear Overflow bit
* Store Base Count
* Read Counter
* Read Overflow Direction
* Read Overflow bit
* If Overflow bit set re-do sequence 3x or until Overflow bit not set
* If 3x and still getting overflow, oscillating around zero
* Default to zero, and add zero to Base Count
* BaseCount should be remaining the same
/* Registers for 16 to 32 conversion interrupts
* FTM1_SC FTM1 Status and Control
* Bit 7 TOF Timer Overflow Flag
* Bit 6 TOIE Timer Overflow Interrupt Enable
* Bit 5 CPWMS Center Aligned PWM Select
* Resets to 0 (Write when WPDIS is 1)
* Rest of bits are 0 (Write when WPDIS is 1)
* FTM1_C0SC FTM1 Channel 0 Status and Control
* Set WPDIS before writing control bits
* Bit 7 CHF Channel Flag
* Channel event occured
* Read and write 0 to clear
* Bit 6 CHIE Channel Interrupt Enable
* Set for compare interrupt
* Bit 5 MSB Channel Mode Select B
* Set to 0
* Bit 4 MSA Channel Mode Select A
* Set to 1
* Bit 3:2 ELS Edge or Level Select
* Set to 0:0
* Set WPDIS to 1 before writing
* DECAPEN (Dual Edge Capture Enable)
* COMBINE (Combine Channels)
* Resets to all zero
* FTM1_C0V Channel 0 Value
* Channel Compare Value
* Set to 0x80 - halfway thru count
* Duplicate of CHnF bit for all channels
* Bit 0 CH0F
/* To zero and initialize channel
* Zero counter
* Set last Overflow Dir to Counting Direction

#include "WProgram.h"
#include "QuadDecode_def.h"
#include <stdbool.h>
#define INT_Pin0 23
#define INT_Pin1 22
#include <avr/io.h>
#include <avr/interrupt.h>
#include <Arduino.h>
// Timing for GUI update time, how often send back data values
#define GUI_UPDATE_TIME 10000 // 200 mSec update

// Command headers for X,Y,Z data channels
const char cmd1[] = "RX";
const char cmd2[] = "RY";
const char cmd3[] = "RZ";
const char* cmdPointers[]= {cmd1,cmd2,cmd3};

const uint8_t num_cmds = 3; // Only send up RXYZ continually

// Variables from CMM program
volatile int32_t rtX=0, rtY=0, rtZ=0; // Realtime values of X,Y,Z
float rtXD = 0, rtYD = 0;

volatile bool zero_rtX=0, zero_rtY=0, zero_rtZ=0; // Zero values of X,Y,Z

volatile bool doOutput=false; // Run output routine

IntervalTimer serialTimer; // How often to update serial

void timerInt(void); // Main timing loop interrupt

QuadDecode<1> xPosn; // Template using FTM1
QuadDecode<2> yPosn; // Template using FTM2

extern "C" int main(void)
//bool LED_ON=false;

char inbuf[3]; // Characters to be received
char inByte;
int32_t buffer[3]; // Values to send, X,Y,Z realtime and latched
uint8_t n; // Number of characters received
uint8_t j;

Serial1.begin(115200); // USB is always 12 Mbit/sec
pinMode(INT_Pin0, INPUT);
attachInterrupt(INT_Pin0, zerocounterX, RISING);

pinMode(INT_Pin1, INPUT);
attachInterrupt(INT_Pin1, zerocounterY, RISING);

xPosn.setup(); // Start Quad Decode position count
yPosn.setup(); // Start Quad Decode position count

serialTimer.begin(timerInt,GUI_UPDATE_TIME); // GUI Update time

xPosn.start(); // Start Quad Decode position count
yPosn.start(); // Start Quad Decode position count

while (1) {
if (doOutput){

// if (zero_rtX ){

// xPosn.zeroFTM();
// }

if (rtX < 0){

rtXD= rtX + 262144;
else {
rtXD = rtX;


rtYD = rtY;

// if (zero_rtZ ){
// rtZ=0;
// }

// Send out axis values
buffer[0] = rtXD;
buffer[1] = rtYD;
buffer[2] = rtZ;

rtXD = rtXD*0.001373;
rtYD = rtYD*0.001373;

// Send out rtX, rtY, rtZ values




void timerInt(void){

void zerocounterX(void)

void zerocounterY(void)
I am still confused on if the teensy3.1 has a hardware quadrature counter(s) in it or not. You say it is hardware based but then in the code it looks like we are using interrupts. If you had a high pulse count encoder spinning fast, and had some code just serially exporting the "count", would you ever run into an issue?