14 November 2012

Quadrature Encoders in Arduino, done right. Done right.

Guys guys guys I should start doing things with my infinite motors! I will eventually, at some point, want to detect Quadrature Encoder feedback from multiple motors using an Arduino. Like in MASLAB or for Deltabot or something. So I'm doing it now. Because it's more fun than my homework.

http://www.hessmer.org/blog/2011/01/30/quadrature-encoder-too-fast-for-arduino/


Man, this is a great example. Like seriously read this guy's entire blog. It's not terribly long, and it's full of useful stuff. I'm a huge fun of everything he's done.


First thing I need to do to is get the provided code running. Taking a deeper look, it's unfortunately incomplete.


Let me explain:


Microcontrollers, like the AtXmega16a4u running TinyArmTroller, and the AtMega328 that's running in an Arduino Uno, have a feature called interrupts. An interrupt is a function that is called only when a certain event, usually pertaining to the voltage level at a certain pin, occurs. For example, suppose you want to make an LED turn on when a button is held down. You can approach this in two ways: 


Method 1 involves a loop that constantly checks whether or not a button is pressed. If the button is pressed, turn the light on. Otherwise, turn it off. This method is simple, and for the application of making a light turn on, is totally acceptable. 


However, if your microcontroller supports interrupts, you can create two interrupts. One of them will trigger when the button pin rises, that is, when it changes from low to high (Off to On). When the interrupt triggers, the interrupt handler function turning the LED on is immediately called. The second interrupt will trigger when the button pin undergoes a falling action (ie. The button goes from On to Off). When the interrupt triggers, The handler function turns the LED off. 




Now, in the context of a quadrature encoder, you traditionally need four interrupts: one to detect a channel A rising action, and channel A falling action, and a channel B rising and falling action. At each interrupt, you would know what the previous state of Channel A and Channel B were, and could iterate the odometry count appropriately (+1 or -1).

If you want some more reading on how Quadrature Encoders work, and how to use a Quadrature Decoder IC, check this page out:

http://www.robotoid.com/appnotes/circuits-quad-encoding.html
(The above diagram is grabbed from this page)

Also, let MIT Professor Harry Asada (the lecturer of 2.12: Intro to Robotics and my undergraduate academic adviser) teach you about encoders on page 13 of these free course notes: http://ocw.mit.edu/courses/mechanical-engineering/2-12-introduction-to-robotics-fall-2005/lecture-notes/chapter2.pdf


The code provided on Dr. Hessmer's site does not correctly implement a quadrature decoder. In that code, an interrupt triggers only when Channel A rises. He does achieve directionality, one of the primary advantages of using Quadrature encoders, by reading the state of Channel B each time the interrupt triggers, and either adding or subtracting to the count based on that reading. 




The issue with using this method is you achieve A QUARTER OF THE POSSIBLE RESOLUTION with a Quadrature encoder. Looking at the above diagram, suppose Channel A is on the outside and Channel B is on the inside. There are 12 black spaces per channel. If you account for both rising and falling of a single channel, you get a resolution of 24 counts per revolution [cpr]. Counting both Channels A and B, your resolution becomes 48 cpr! The code provided in the above example only takes into account when Channel A rises, cutting the resolution back down to 12. 

I want to correctly implement quadrature decoding, where interrupts trigger in both the A and B channel, and the resolution is four times that of that of just a single channel. Issue is, the Arduino Uno can only have two interrupts enabled at a time. I need four per motor, ideally.


Then I thought of some Code Magick: When one of four interrupts is triggered, I could reinitialize each interrupt to whatever two interrupts were possible at a current state!


Then I realized Arduino also could just trigger an interrupt when the state of a pin changed, not necessarily when it was Rising or Falling. That's good, because I can just check both pins at each interrupt, test it against the previous pin reading, and adjust the count accordingly. 


(If I wanted to keep only a single interrupt like in the above code, I could modify the interrupt to trigger when Channel A changes, read both channels A and B when this happens, and adjust the odometry count accordingly. This however would still keep me at half the total possible resolution. Using a second interrupt which triggers when Channel B changes is ideal).



Here's my setup: Arduino, Pittman 655 Motor, Computer with Arduino IDE. 




Next, I attached the encoder in the correct manner. Looking at the datasheet Tech Support Joe from Pittman scanned me from the 1990s, the lime green wire is ground, the red wire is Channel A, the black wire is +5V power (WTF?!), and the white wire is Channel B. The dark green wire is just shielding for the signal wire. 



I connected +5V and GND accordingly, and the Channel A and Channel B cables to pins 2 and 3 on my Arduino Uno. Pins 2 and 3 are the only pins able to trigger interrupts (Interrupt 0 and 1, respectively). 

Here's the code I used: 


/*
*Quadrature Decoder 
*/
#include "Arduino.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.

// Quadrature encoders
// Left encoder
#define c_LeftEncoderInterruptA 0
#define c_LeftEncoderInterruptB 1
#define c_LeftEncoderPinA 2
#define c_LeftEncoderPinB 3
#define LeftEncoderIsReversed

volatile bool _LeftEncoderASet;
volatile bool _LeftEncoderBSet;
volatile bool _LeftEncoderAPrev;
volatile bool _LeftEncoderBPrev;
volatile long _LeftEncoderTicks = 0;

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

  // 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_LeftEncoderInterruptA, HandleLeftMotorInterruptA, CHANGE);
  attachInterrupt(c_LeftEncoderInterruptB, HandleLeftMotorInterruptB, CHANGE);
}

void loop()

  Serial.print("Encoder Ticks: ");
  Serial.print(_LeftEncoderTicks);
  Serial.print("  Revolutions: ");
  Serial.print(_LeftEncoderTicks/4000.0);//4000 Counts Per Revolution
  Serial.print("\n");
}


// Interrupt service routines for the left motor's quadrature encoder
void HandleLeftMotorInterruptA(){
  _LeftEncoderBSet = digitalReadFast(c_LeftEncoderPinB);
  _LeftEncoderASet = digitalReadFast(c_LeftEncoderPinA);
  
  _LeftEncoderTicks+=ParseEncoder();
  
  _LeftEncoderAPrev = _LeftEncoderASet;
  _LeftEncoderBPrev = _LeftEncoderBSet;
}

// Interrupt service routines for the right motor's quadrature encoder
void HandleLeftMotorInterruptB(){
  // Test transition;
  _LeftEncoderBSet = digitalReadFast(c_LeftEncoderPinB);
  _LeftEncoderASet = digitalReadFast(c_LeftEncoderPinA);
  
  _LeftEncoderTicks+=ParseEncoder();
  
  _LeftEncoderAPrev = _LeftEncoderASet;
  _LeftEncoderBPrev = _LeftEncoderBSet;
}

int ParseEncoder(){
  if(_LeftEncoderAPrev && _LeftEncoderBPrev){
    if(!_LeftEncoderASet && _LeftEncoderBSet) return 1;
    if(_LeftEncoderASet && !_LeftEncoderBSet) return -1;
  }else if(!_LeftEncoderAPrev && _LeftEncoderBPrev){
    if(!_LeftEncoderASet && !_LeftEncoderBSet) return 1;
    if(_LeftEncoderASet && _LeftEncoderBSet) return -1;
  }else if(!_LeftEncoderAPrev && !_LeftEncoderBPrev){
    if(_LeftEncoderASet && !_LeftEncoderBSet) return 1;
    if(!_LeftEncoderASet && _LeftEncoderBSet) return -1;
  }else if(_LeftEncoderAPrev && !_LeftEncoderBPrev){
    if(_LeftEncoderASet && _LeftEncoderBSet) return 1;
    if(!_LeftEncoderASet && !_LeftEncoderBSet) return -1;
  }
}


I pull up the Serial monitor in the Arduino IDE and see: 



Running the program, I see all is well. 


To keep track of my motor's angle, I just used a sharpie on a point of the shaft and on the motor housing. Here is where I defined theta = 0. 


I moved the shaft 90 degrees, and expected the encoder count to be 250 (As the motor states it is 1000cpr). 



Interesting. They must have meant that there are 1000 dark slots per channel of the encoder! This encoder is actually 4000 counts per revolution! 

4000 counts/revolution*340 rpm/volt*12 Volts*1min/60sconds = 272000 interrupts/s
(When being driven at 12 volts)

Whatever hardware is running the software running this (like my Arduino) should be ideally running at a clock speed an order of magnitude higher than this in order to keep up. 


One full rotation. Yup, makes sense. 



When I move the motor shaft really fast, though, it picks up a lot of error. I should write a filter to be able to keep up with really fast motor movements....

Not now though. I'm late for class -_-

12 comments:

  1. Likely you have a gearmotor with a 4:1 ratio output to input. So the encoder is on the motor, however you're counting rotations of the output of the gera reduced shaft which rotates 1 time for every 4 rotations of the motor.

    ReplyDelete
  2. Nope, I assure you there is no gearing on this motor, thought that would totally make sense if there were 500 counts per track revolution. This behavior is consistent with the encoder having two tracks, each with 1000 ticks each. With quadrature overlap and parsing, this produces 4000 total counts per revolution: the total potential resolution.

    ReplyDelete
  3. Do you have to use an older arduino IDE to get the library to work? I am trying to play with the library but I am getting errors when trying to compile on 1.0.3.

    ReplyDelete
  4. Thanks for your write-up. I believe that you are correct about the motor specs indicating the number of full pulse cycles( HIGH to LOW then back again) per revolution. That usually misleads people as when you take the QUADrature you yield four times as many states per revolution so you get four times the counts.!!

    I often trip up on this when shopping for an encoder and forget that it will actually have 4x the resolution that my quick assumption makes. It is a common mistake.

    ReplyDelete
  5. Still too slow for my application; reading a DRO/CNC glass scale slide. I'm looking at implementing a D flip-flop as a go-between so as to eliminate all logic about direction on the Arduino.

    ReplyDelete
  6. Thanks for sharing your! In my case, I was reading quadrature output from a DRO glass scale. The Arduino couldn't keep up ... even when I used the pair of D flip-flops in a 7474 to only send the Arduino up and down pulses (on separate pins). I found out about quadrature decoder chips, like the HCTL family, but they require a clock. What worked for me was a LS7166. Write up here: http://forum.freetronics.com/viewtopic.php?f=6&t=5387

    ReplyDelete
  7. where did you buy the ls7166, thanks for the hints.

    ReplyDelete
    Replies
    1. I google'd LSI7166 and a company in Los Angeles came up.
      -Chris

      Delete
  8. I'm trying to get revs when with this code is when the encoder is rotated backwards even thou I have a Z channel. I tried doing a interrupt for when the B pin (pin3) rises also at the same time if A pin(pin2) is low, detach the earlier interrupts and subtract one from the revolutions. I tried doing this by an if statement I kept getting an error, I think the way I was writing the code was wrong, or maybe I was missing something with in the code I dont know.

    What show I Do?

    ReplyDelete
  9. Hi all, OP here. It's sure been a while since I've posted on my blog!
    I want to say that the way to do quadrature encoders done right, done right, done right would be to use an FPGA-based decoder that keeps count offline from the main microcontroller control loop or to use one of these Encoder Buffers: http://www.superdroidrobots.com/shop/item.aspx/dual-ls7366r-quadrature-encoder-buffer-breakout-board/1523/
    With this, you can get up to 40MHz of encoder ticks without skipping a beat, but the Arduino can request the current count via i2c at whatever speed it wants. It works wonders!

    Glad you all still find the code useful, happy robot-ing!

    ReplyDelete
    Replies
    1. So I was was inspired to make a few modifications to your code... and this is what I ended up liking. It will build small enough to fit in an ATtiny13 with this in the interrupt handler...

      stateCode = digitalRead(EncoderPinB) & 0x01;
      stateCode += (digitalRead(EncoderPinA) << 1) & 0x02;

      char state = (prevStateCode << 2) + stateCode ;
      char tmpOutput = 0;
      switch (state )
      {
      case 1: //0b00_00_00_01
      case 7: //0b00_00_01_11
      case 8: //0b00_00_10_00
      case 14: //0b00_00_11_10
      EncoderTicks += -1;
      break;
      case 2: //0b00_00_00_10
      case 4: //0b00_00_01_00
      case 11: //0b00_00_10_11
      case 13: //0b00_00_11_01
      EncoderTicks += 1;
      break;
      default:
      break;
      }

      Delete