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.
