For my next robot project I am using Pittman Motors with 500 cnt/rev quadrature encoders mounted on the motor axle. Initially I used the same code that I described in my blog post ‘Building a Self-Balancing Robot: Motor Driver and Wheel Encoder’ for handling the encoder input. Everything worked fine when I turned the motors manually; the interrupts kicked in and the counters were correctly incremented / decremented. However, when I powered up the motors the Arduino program froze.

Others suffered from the same experience (see e.g., Missing counting encoder). One suggested solution is the use of dedicated quadrature decoder chips like the LS7266R1. More information about its usage in a robot project can be found in the thesis Design of a Wireless Control System for a Laboratory Planetary Rover by Eric Jamesson Wilhelm.

Fortunately no dedicated decoder hardware is necessary (at least in my case). It turns out that the Arduino function digitalRead() comes with a lot of overhead and is correspondingly slow. Both Jean-Claude Wippler (Pin I/O performance) and Bill Porter (Ready, Set, Oscillate! The Fastest Way to Change Arduino Pins) discuss the overhead and demonstrate how it can be avoided by directly reading from Atmel ports rather than going through the Arduino library, thus gaining a factor of about 50 in performance.

While directly reading from controller ports certainly solves the issue, it forces you to become familiar with port maps and write code that is specific to a given controller (no simple porting from Arduino Uno to Mega, etc.). This problem has been beautifully addressed by jrraines’ digitalwritefast library (download) which in turn is based on a suggestion from Paul Stoffregen (see http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1267553811/0).

So, with the open source community to the rescue my quadrature encoder problem is now solved. Here is my new quadrature encoder code wrapped in a little test sketch:

UPDATE: I incorporated Bill Porter’s feedback (see his comment for further detail) to further improve the performance of the code. Since the interrupt only fires on a rising change there is no need to read pin A; it’s value will always be ‘true’.  Furthermore by using pre-processor directives #ifdef … #else … #endif the evaluation of an if statement in each interrupt handler during runtime can be avoided. Thanks, Bill!

#include "WProgram.h"
#include <Servo.h>
#include <digitalWriteFast.h>  // library for high performance reads and writes by jrraines
                               // see http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1267553811/0
                               // and http://code.google.com/p/digitalwritefast/

// It turns out that the regular digitalRead() calls are too slow and bring the arduino down when
// I use them in the interrupt routines while the motor runs at full speed creating more than
// 40000 encoder ticks per second per motor.

// Quadrature encoders
// Left encoder
#define c_LeftEncoderInterrupt 4
#define c_LeftEncoderPinA 19
#define c_LeftEncoderPinB 25
#define LeftEncoderIsReversed
volatile bool _LeftEncoderBSet;
volatile long _LeftEncoderTicks = 0;

// Right encoder
#define c_RightEncoderInterrupt 5
#define c_RightEncoderPinA 18
#define c_RightEncoderPinB 24
volatile bool _RightEncoderBSet;
volatile long _RightEncoderTicks = 0;

Servo _RightServo;  // create servo object to control right motor
Servo _LeftServo;  // create servo object to control left motor

int potpin = 0;  // analog pin used to connect the potentiometer
int val;    // variable to read the value from the analog pin

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

  _RightServo.attach(2);  // attaches the servo on specified pin to the servo object
  _LeftServo.attach(3);  // attaches the servo on specified pin to the servo object

  // Quadrature encoders
  // Left encoder
  pinMode(c_LeftEncoderPinA, INPUT);      // sets pin A as input
  digitalWrite(c_LeftEncoderPinA, LOW);  // turn on pullup resistors
  pinMode(c_LeftEncoderPinB, INPUT);      // sets pin B as input
  digitalWrite(c_LeftEncoderPinB, LOW);  // turn on pullup resistors
  attachInterrupt(c_LeftEncoderInterrupt, HandleLeftMotorInterruptA, RISING);

  // Right encoder
  pinMode(c_RightEncoderPinA, INPUT);      // sets pin A as input
  digitalWrite(c_RightEncoderPinA, LOW);  // turn on pullup resistors
  pinMode(c_RightEncoderPinB, INPUT);      // sets pin B as input
  digitalWrite(c_RightEncoderPinB, LOW);  // turn on pullup resistors
  attachInterrupt(c_RightEncoderInterrupt, HandleRightMotorInterruptA, RISING);
}

void loop()
{
  val = analogRead(potpin);            // reads the value of the potentiometer (value between 0 and 1023)
  val = map(val, 0, 1023, 0, 179);     // scale it to use it with the servo (value between 0 and 180)

  _RightServo.write(val);
  _LeftServo.write(val);

  Serial.print(_LeftEncoderTicks);
  Serial.print("\t");
  Serial.print(_RightEncoderTicks);
  Serial.print("\n");

  delay(20);
}

// Interrupt service routines for the left motor's quadrature encoder
void HandleLeftMotorInterruptA()
{
  // Test transition; since the interrupt will only fire on 'rising' we don't need to read pin A
  _LeftEncoderBSet = digitalReadFast(c_LeftEncoderPinB);   // read the input pin

  // and adjust counter + if A leads B
  #ifdef LeftEncoderIsReversed
    _LeftEncoderTicks -= _LeftEncoderBSet ? -1 : +1;
  #else
    _LeftEncoderTicks += _LeftEncoderBSet ? -1 : +1;
  #endif
}

// Interrupt service routines for the right motor's quadrature encoder
void HandleRightMotorInterruptA()
{
  // Test transition; since the interrupt will only fire on 'rising' we don't need to read pin A
  _RightEncoderBSet = digitalReadFast(c_RightEncoderPinB);   // read the input pin

  // and adjust counter + if A leads B
  #ifdef RightEncoderIsReversed
    _RightEncoderTicks -= _RightEncoderBSet ? -1 : +1;
  #else
    _RightEncoderTicks += _RightEncoderBSet ? -1 : +1;
  #endif
}

Hopefully the code is fairly self explanatory. Since fast reads require that the pins are known at compile time I could no longer use my QuadratureEncoder class. Instead the code for calculating the encoder ticks is now part of the sketch itself and there is a little bit of duplicated code in the two interrupt handlers. For testing I used a 10k potentiometer at analog pin 0. The analog value is used to set the output of two servos that drive a Sabertooth 10 RC Dual Motor Speed Controller. This allowed me to put the motors through their paces and verify that the encoder ticks are correctly caught.

29 Responses to “Quadrature Encoder too Fast for Arduino (with Solution)”

  1. Hi Dr. Hessmer,

    Nice post, and thanks for the link. I also built an Arduino powered motor-controller a while ago with high resolution encoders and ran into the same problem. I did not document it as well as you did, but I did post my code.

    http://www.billporter.info/s-a-g-a-r-s-smart-motor-controller/

    and rebuilt it later

    http://www.billporter.info/sagar-update-new-motor-controller-and-motors/

    It was before I found the easy to use digitalReadFast() so I used direct port access.

    I’m curious, you are attaching the interrupt for RISING only, but you still check the same pin (channel A, the one that triggered the interrupt ) anyway in your interrupt routine. Would it not be faster to assume the pin is HIGH since the interrupt is fired on the rising edge?

    Also, there’s another optimization you can make. Since ‘c_RightEncoderIsReversed’ is defined at compile time, you could change the ‘if(c_RightEncoderIsReversed)’ statements to pre-processor directives. Something like #ifdef RightEncoderIsReversed …. #else …. #endif

    This lets the pre-processor make the decision which set of code to compile for your sketch, instead the actual microprocessor running machine code to check a constant value EVERY time the interrupt fires. Should save a few cycles on every interrupt, and shrink your code size down a few bytes.

    Bill

  2. Bill,

    Thank you very much for your feedback! I already incorporated your suggestions and updated the blog entry.

    Rainer

  3. No problem, glad I could help.

    A self balancing platform looks like a fun challenge to tackle. I might have to add it to my list of future projects.

    Bill

  4. Dr Hessmer,

    First of all I must thank you for the help your blog has given me with ROS and Arduino. I’m also having the same problem with the wheel encoders and I understand your code apart from one small thing. My encoders have a channel A and channel B out. I see in your code you give these two pin assignments, what I don’t understand is what is attached to the interrupt. A response would be greatly appreciated.

    Mayur

  5. Hi Mayur,

    The tricky part is that by attaching to an interrupt you implicitly also specify an input pin. The correlation between interrupt number and input pin depends on the type of Arduino board you use. For the Arduino Mega the information is listed on the page http://arduino.cc/en/Main/ArduinoBoardMega.

    In my code I use interrupt 4 which uses pin 19 for the first encoder and interrupt 5 (pin 18) for the second. For simplicity, in the following I only discuss the first encoder (interrupt 4). The second encoder works identically but obviously with different pins.

    In order to determine the quadrature encoder transitions I need to know the values for channel A and channel B. Channel A is hooked up to pin 19; channel B to pin 25. I configure interrupt 4 (which acts on pin 19 / channel A) so that it only fires on ‘rising’; i.e., only when the value on pin 19 changes from low to high. So in the interrupt handler I don’t need to read the value of pin 19 (channel A); I already know that it is high. I only need to read channel B (pin 25) and, based on its value, increment / decrement the ticks. Bill Porter suggested this nifty optimization to me (see the first comment for this post).

  6. Dr. Hessmer,

    Yes I see, I was just being stupid. I forgot that interrupt 4 = pin 19 haha. I normally don’t use or prefer the Arduino.

    Thank you for your help,
    Mayur

  7. Great sample – just what I was looking for to get my robot up and running. Some questions though:
    What’s the reference to Pin26 in the setup routine? It also looks like c_RightEncoderPinA gets initialized twice and c_RightEncoderPinB never gets set to be an input:

    // Right encoder
    pinMode(c_RightEncoderPinA, INPUT); // sets pin A as input
    digitalWrite(c_RightEncoderPinA, LOW); // turn on pullup resistors
    pinMode(c_RightEncoderPinA, INPUT); // sets pin A as input
    digitalWrite(26, LOW); // turn on pullup resistors
    pinMode(26, INPUT); // sets pin B as input
    digitalWrite(c_RightEncoderPinB, LOW); // turn on pullup resistors

    I assumed this was a typo so I corrected the code and re-ran it. In either case, I get the pulse count I expect, but it doesn’t respect the motor direction. I checked the quadrature signals and they were as expected. If I output the sampled pin value from the interrupt routine, via a digitalWriteFast() call, it stays high all the time. It looks like I’ll have to settle for the pulse count and determine the direction from the motor controller outputs. Any suggestions where I’m going wrong?

  8. Sorry for the late response. Thanks for pointing out the typo. I will correct the post as soon as I can verify that the changed code works on my robot. I looked at the source code of the Arduino program that I actually run on my robot (see http://code.google.com/p/drh-robotics-ros/source/browse/trunk/Arduino/Robot/Robot.pde). It contains the same typo but it certainly works. I correctly get the speed and the direction from the encoders. You wrote that you checked the output via a digitalWriteFast() call. I guess you meant digitalReadFast() instead of digitalWriteFast(), correct?

    UPDATE: I cleaned up the encoder sample code. The variables _LeftEncoderASet and _LeftEncoderBSet were not used and are removed now. Also the code in Setup() has been cleaned up.

  9. Hello Dr Hessmer,
    I would like to know what is the upper limit of the counts/rev in order to have a properly worked counting?
    Can we calculate this limit. I think it would be helpful for those who want to have as much resolution as possible.

  10. It is difficult for me to give you an exact number since it depends heavily on how much else is running on the controller. In my case I seem to get accurate counts up to about 4000 ticks per second. This is based on the controller program I use on my robot. It includes three PI loops, odometry calculations, and quite a bit of serial communication. I know this is not a very satisfying answer but it provides at least a ballpark number.

  11. Hi.

    Nice code and well working. However by only interrupting on the rising edge and only on one of the two quadrature encoder signals, you will have reduced your resolution by a factor 4.

    I suggest using a board with at least 2 external interrupt signals for each quadrature encoder and then slightly rewrite the code, so that it interrupts on changes in the encoder signals.

  12. Lukas, only using the rising edge is very much on purpose since in my case the resolution of the wheel encoders combined with the speed of the motor produces more transitions than the Arduino can handle. This is the whole point of the post. For low resolution encoders and/or lower rpms, using two interrupts per encoder and triggering on all transitions (not just rising) is of course the way to go. I covered this in an older post.

  13. @Dr Hessmer – sry my bad, and thanks for the help.

  14. How would I use this on the UNO with just 2 encoders ?

    I have tried to adapt this, but having 50% luck.. I can get the left one working but not the right one.

    Thank you for any help you could give.

    JT

  15. Hi I have the same problem as JT.

    I am using it on Mega but can never get both the encoder counts at the same time and if i do i get ridiculously low values.

    My encoder is 1856 CPR and the speed is roughly 1.5 Rev/sec. Is this too low for this to method work?

    Thanks in advance for your method and any help you could give me.

    ksp

  16. Hey Dr. Hessmer,

    Thanks a lot for your tutorial. This greatly helped in my Balancing Project. Though I have a small problem. My code, as suggested by you, works well when directly accessing the port and reading the input value of Encoder B, while Encoder A is used as a triggering signal. But, when I try to apply this for two motors in parallel, Code starts to fail and gives erraneous values (velocity goes down with increasing voltage) which basically means that there is a delay in reading Encoder B input.

    Can you please suggest something?

    Following is my code:
    ——————————————————————-
    #define DirectionPin 3 // direction pin

    int encoder0Pos = 0,oldPos = 0; // encoder position
    int dir = 0;
    unsigned long oldTime, newTime, dt;
    float vel;

    void setup() {
    Serial.begin(9600); // set up Serial library at 9600 bps
    pinMode(DirectionPin, INPUT);

    // encoder pin on interrupt 0 (pin 2)
    attachInterrupt(5, doEncoder, RISING);
    oldTime = millis();
    }

    void loop() {
    newTime = millis(); // read the current time stamp
    dt = newTime – oldTime; // compute delta time
    oldTime = newTime;

    vel = (float) ((encoder0Pos – oldPos) * 4.25) / (dt); // velocity estimate in ms
    int temp = encoder0Pos – oldPos;
    oldPos = encoder0Pos;

    Serial.println(vel);
    }

    // Interrupt routine
    void doEncoder()
    {
    dir = (PINE & 0b00100000) >> 5;
    if (dir == HIGH) {encoder0Pos = encoder0Pos – 1;}
    else {encoder0Pos = encoder0Pos + 1;}
    }
    ———————————————————–

  17. By the way, forgot to mention, Interrupt frequency is around 12646 counts per sec per motor.

    (1024 Counts per Rotation and 741 Rotations per Minute (RPM))

  18. Hi,
    I had a problem with your code…My encoder works with 128 count/rev and trying to use your code, arduino only reads 32count/rev (1/4).
    Have you some idea to help me?
    I will wate your help at my e-mail…
    Thanks!

  19. Bruno, that’s right, the code is only listening to one of the 2 encoder outputs and only catching the rising edge of that so it is counting exactly 1/4th of the actual count. They made this shortcut because catching high speed pulses was more important to them than precision for this code and they weren’t concerned with direction, only that it is moving.

  20. Thank you for the code snippet. It’s helping greatly. One question concerning your assumption for one of the encoders inputs already high, I have a 1024 step/Rev encoder I am using to determine to a fairly precise angular movement of a shaft. In the spec sheet they list three inputs, The interrupt and the two standard A,B type inputs. For CCW rotation the interrupt and the B input fire simultaneous. For CW rotation the B output seems to show a delay before the going active. I’m new to rotary encoders so maybe this is a known thing and used to detect a change of direction? Would you still need to sample the (A or B) even with the interrupt to properly decode this? Thanks in advance.

    Jim

  21. Thankyou very much for this.

    For those of you who want to count 1/2 the ticks instead of 1/4 because you need just a bit more precision here is a quick rewrite for you.

    Note this uses 4 interrupt pins, 18 and 19 are one encoder and 20 and 21 are the other.

    #include
    #include // library for high performance reads and writes by jrraines
    // see http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1267553811/0
    // and http://code.google.com/p/digitalwritefast/

    // It turns out that the regular digitalRead() calls are too slow and bring the arduino down when
    // I use them in the interrupt routines while the motor runs at full speed creating more than
    // 40000 encoder ticks per second per motor.

    // Quadrature encoders
    // Left encoder
    #define c_LeftEncoderInterrupt 3
    #define d_LeftEncoderInterrupt 2
    #define c_LeftEncoderPinA 20
    #define c_LeftEncoderPinB 21
    #define LeftEncoderIsReversed
    volatile bool _LeftEncoderBSet;
    volatile long _LeftEncoderTicks = 0;

    // Right encoder
    #define c_RightEncoderInterrupt 5
    #define d_RightEncoderInterrupt 4
    #define c_RightEncoderPinA 18
    #define c_RightEncoderPinB 19
    volatile bool _RightEncoderBSet;
    volatile long _RightEncoderTicks = 0;

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

    // Quadrature encoders
    // Left encoder
    pinMode(c_LeftEncoderPinA, INPUT); // sets pin A as input
    digitalWrite(c_LeftEncoderPinA, LOW); // turn on pullup resistors
    pinMode(c_LeftEncoderPinB, INPUT); // sets pin B as input
    digitalWrite(c_LeftEncoderPinB, LOW); // turn on pullup resistors
    attachInterrupt(c_LeftEncoderInterrupt, HandleLeftMotorInterruptA, RISING);
    attachInterrupt(d_LeftEncoderInterrupt, HandleLeftMotorInterruptB, RISING);

    // Right encoder
    pinMode(c_RightEncoderPinA, INPUT); // sets pin A as input
    digitalWrite(c_RightEncoderPinA, LOW); // turn on pullup resistors
    pinMode(c_RightEncoderPinB, INPUT); // sets pin B as input
    digitalWrite(c_RightEncoderPinB, LOW); // turn on pullup resistors
    attachInterrupt(c_RightEncoderInterrupt, HandleRightMotorInterruptA, RISING);
    attachInterrupt(d_RightEncoderInterrupt, HandleRightMotorInterruptB, RISING);
    }

    void loop()
    {

    Serial.print(_LeftEncoderTicks);
    Serial.print(“\t”);
    Serial.print(_RightEncoderTicks);
    Serial.print(“\n”);

    delay(20);
    }

    // Interrupt service routines for the left motor’s quadrature encoder
    void HandleLeftMotorInterruptA()
    {
    // Test transition; since the interrupt will only fire on ‘rising’ we don’t need to read pin A
    _LeftEncoderBSet = digitalReadFast(c_LeftEncoderPinB); // read the input pin

    // and adjust counter + if A leads B
    #ifdef LeftEncoderIsReversed
    _LeftEncoderTicks -= _LeftEncoderBSet ? -1 : +1;
    #else
    _LeftEncoderTicks += _LeftEncoderBSet ? -1 : +1;
    #endif
    }

    // Interrupt service routines for the left motor’s quadrature encoder
    void HandleLeftMotorInterruptB()
    {
    // Test transition; since the interrupt will only fire on ‘rising’ we don’t need to read pin A
    _LeftEncoderBSet = digitalReadFast(c_LeftEncoderPinA); // read the input pin

    // and adjust counter + if A leads B
    #ifdef LeftEncoderIsReversed
    _LeftEncoderTicks += _LeftEncoderBSet ? -1 : +1;
    #else
    _LeftEncoderTicks -= _LeftEncoderBSet ? -1 : +1;
    #endif
    }

    // Interrupt service routines for the right motor’s quadrature encoder
    void HandleRightMotorInterruptA()
    {
    // Test transition; since the interrupt will only fire on ‘rising’ we don’t need to read pin A
    _RightEncoderBSet = digitalReadFast(c_RightEncoderPinB); // read the input pin

    // and adjust counter + if A leads B
    #ifdef RightEncoderIsReversed
    _RightEncoderTicks -= _RightEncoderBSet ? -1 : +1;
    #else
    _RightEncoderTicks += _RightEncoderBSet ? -1 : +1;
    #endif
    }

    // Interrupt service routines for the right motor’s quadrature encoder
    void HandleRightMotorInterruptB()
    {
    // Test transition; since the interrupt will only fire on ‘rising’ we don’t need to read pin A
    _RightEncoderBSet = digitalReadFast(c_RightEncoderPinA); // read the input pin

    // and adjust counter + if A leads B
    #ifdef RightEncoderIsReversed
    _RightEncoderTicks += _RightEncoderBSet ? -1 : +1;
    #else
    _RightEncoderTicks -= _RightEncoderBSet ? -1 : +1;
    #endif
    }

    On a side note to add all of the ticks, you have to add interrupts to the falling edge of the signals as well.

  22. I’ll appreciate if you continue this in future. Many people will be benefited from your writing.

  23. [...] Potential issue (& solution) for Quadrature Encoders being too fast for Arduino to processQuadrature Encoder too Fast for Arduino (with Solution) » Dr. Rainer Hessmer [...]

  24. [...] Potential issue (& solution) for Quadrature Encoders being too fast for Arduino to process.Quadrature Encoder too Fast for Arduino (with Solution) » Dr. Rainer Hessmer [...]

  25. How about ‘OR ing” the rising edges ; feeding it to one interrupt and reading the state ?
    I don’t know this micro-processor but this would be half the interrupt service routine overhead for
    the same amount of work ?

  26. Thank you, I’ve been searching for this solution quite some time..

  27. i want to use the rotary encoder to measure and display revolutions per minute. it has a resolution of 200p/r and has three outputs..can anyone help??

  28. Howdy! Quick question that’s entirely off topic. Do you know how to
    make your site mobile friendly? My website looks weird when browsing
    from my apple iphone. I’m trying to find a template or plugin
    that might be able to resolve this issue. If you have any suggestions, please share.
    With thanks!

  29. Hi there everyone, it’s my first pay a quick visit at this
    site, and post is in fact fruijtful for me, keep up posting these
    articles or reviews.

    Here is myy homepage Example

Sorry, the comment form is closed at this time.

© 2014 Dr. Rainer Hessmer Suffusion theme by Sayontan Sinha