Saturday 19 May 2012

Quadrature Encoder

In the last post I began adding some more sensors, the last of which was a rotary encoder. This device is used to measure the speed of a spinning wheel, using a black+white striped disk and an infra red reflectivity sensor:

Wheel with disk
Sensor pointing at disk
Connected to oscilloscope

On the left you can see the coloured disk, centre is a cross section to show the sensor and wheel, and right shows the actual square wave signal coming out as I spin the wheel. For more info on the basic setup, see my last post (More Sensors).

Now it's time for something more interesting. My next goal is to be able to identify whether the motor is going forwards or backwards. With just 1 sensor this isn't possible, as I just get a binary value back. However it occurred to me that using 2 out-of-phase waves I'll be able to work out which way the wheel is turning. My original idea was to have a disk with 2 waves on, and have 2 sensors - one for each wave. You can see this disk on the right hand side below:

3 potential wheel disks - single wave low res (left), single wave high res (centre), dual wave (right)
However after talking to Alex about it, he pointed out that you can achieve the same result with a single wave disk, provided you have 2 IR sensors that are positioned out of phase with the square wave printed on the disk. I put together 2 extra disks as you can see above - a hi res one (middle), and in case the sensor can't handle it, a lower resolution one (left).

This diagram shows how I'll mount the sensors to be 'out of phase' with the wave represented by the black/white stripes on the disk:

2 sensors for quadrature encoder mounted out of phase with the pattern on the disk
You can see when the disk is lined up so the left sensor is in the centre of the black section, the right sensor is exactly on the boundary between the black section and the next white section. So how does this help? Well, hopefully this wave shows things a bit more clearly:

Wave's generated from each sensor by the turning wheel
Here you can see the waves generated by both sensors as the wheel rotates. Note how they are the same wave, just out of phase by 1/4 of a wave length. The red line shows where the wheel is right now, and you can see from the reading that Wave 1 is low (over black), and Wave 2 is on the boundary between black and white - just like in the earlier cross section. Now, if the wheel is turning, one of 2 things can happen:
  • The wheel can turn left (red line moves towards green dashed line). When the red line hits the green line, a change in wave 1 is detected (goes from low to high), and wave 2 is high
  • The wheel can turn right (red line moves towards blue dashed line). When the red line hits the blue line, a change in wave 1 is detected (goes from low to high), and wave 2 is low
So, we were in the centre of a low point in wave 1. We waited until a change was detected in wave 1, and then the reading from wave 2 tells us which direction we went in! This principle can be applied at any point in time - we wait for a change in a wave, and then the reading from the other wave gives you direction. On top of that, the frequency of either wave tells you speed. So... speed and direction! Hurray.

Next step, build something. To start, I solder cables to 4 sensors (2 for each wheel):

2 sensors mounted on wood (right), my awesome soldering (left)
The sensors on the right are mounted on some wood ready for attaching. The ones on the left are just there so you can admire my awesome soldering :)

Sensors connected to oscilloscope
Close up of sensors pointing at disk
I now build a simple construction (mostly out of blu-tac, cardboard and tape) to mount a motor on the side of my desk, with the 2 sensors held directly in front of it. The motor has virtually no resistance and a full 12V going through, so is running at around 360rpm. This video shows the system at work, with one of the infra red sensors connected to an oscilloscope:


That's step 1 done - I can take readings from 2 sensors in front of a wheel. Next up, I connect it to an Arduino - initially just into analogue inputs 1 and 2:

Sensors connected to analog inputs on Arduino
This simple program is the first test I do (I left out setup as it doesn't do anything except init the serial connection):

int left_buff[256];
int buff_pos = 0;

void loop()
{
  left_buff[buff_pos] = analogRead(1);
  buff_pos++;
  if(buff_pos >= 256)
  {
     for(int i = 0; i < buff_pos; i++)
       Serial.println(left_buff[i]);
     buff_pos = 0;
  }
 
  delay(5);
}

This is basically reading values from the left sensor every 5ms (significantly less than 1/4 of a wave length at 360rpm) into a buffer. When the buffer fills, it gets dumped to the serial monitor and the process begins again. The resulting output isn't very pretty:

721
584
32
195
827
38
47
858
376
43
698
752
43
233
879
... and lots more!...

But what happens if we plonk it into excel and draw a graph?

Output from Arduino when reading a sensor as the wheel slows down
This shows the output as the motor slows down - we're still getting that wave. It's not square as we aren't sampling often enough, so I decide to try another approach:

void loop()
{
  int new_left_state = analogRead(1) > 100 ? 1 : 0;
  int new_right_state = analogRead(2) > 100 ? 1 : 0;
  
  digitalWrite(3,new_left_state);
  digitalWrite(4,new_right_state); 
}

Now I'm reading the wave constantly, with no delays whatsoever. I couldn't output to serial fast enough to display these readings, however what I can do is output digital results to the digital pins, and monitor the output with an oscilloscope:

Square wave coming out of digital pin, generated by Arduino reading sensors
As you can see, I now get a perfectly regular square wave. If you can read the settings you'll see the wave alternates about once every 10ms. With 16 coloured strips on the card this means it takes 160ms for the wheel to turn once, thus the wheel turns 6.25 times per second. Multiply by 60 and you're at 375 rpm - just 15 off what I expected!

The Arduino is reading sensor data now, and I've proved it works with the short program above. However this code (and anything that used it) needs to be updating constantly so it can detect changes in sensor output as soon as they occur. This won't fit into any useful program, so I need to switch the code to use interrupts. This is an event triggered by the hardware that interupts whatever is happening and runs some code.

Unfortunately interupts on the Arduino are only available for digital IO, so the first problem is how to convert the analog output from the sensors into a digital input for the Arduino. After worrying about this for a while, I just plug the sensors directly into the digital IO. Fortunately enough the range coming from the analog sensor is wide enough to cross the digital IO reference voltage so I get a nice solid square wave coming out straight away. Now some modifications to code to use interupts instead of constant polling.

//PIN Definitions
#define PIN_IRLEFT 2
#define PIN_IRRIGHT 3
#define PIN_OUTLEFT 4
#define PIN_OUTRIGHT 5

void leftChange()
{
  digitalWrite(PIN_OUTLEFT,digitalRead(PIN_IRLEFT));
}

void rightChange()
{
  digitalWrite(PIN_OUTRIGHT,digitalRead(PIN_IRRIGHT));
}

///////////////////////////////////////////////////////////////////
//init
///////////////////////////////////////////////////////////////////
unsigned long last_print = 0;
void setup()  
{
  //init pins
  pinMode(PIN_IRLEFT,INPUT);
  pinMode(PIN_IRRIGHT,INPUT);
  pinMode(PIN_OUTLEFT,OUTPUT);
  pinMode(PIN_OUTRIGHT,OUTPUT);
  pinMode(PIN_OUTXOR,OUTPUT);
    
  //init serial port for debugging and print message  
  Serial.begin(115200);
  Serial.println("Hello");
  
  attachInterrupt(0, leftChange, CHANGE);
  attachInterrupt(1, rightChange, CHANGE);
}

As you can see here, there's no actual need for an loop function at all. I attach the interrupt 0 (which is on pin 2) to leftChange, and interrupt 1 (which is on pin 3) to rightChange. When an interupt is triggered I read the corresponding pin and output it's value just as with the earlier example. Once again, the oscilllosope shows me a square wave - things are working!

Now to get some useful data out (with the help of this tutorial by OddBot). The main thing he points out is how to convert the encoder signal into a direction. I end up with this code which is based off his examples:

byte current_encoder = 0;
int QEM [16] = {0,-1,1,2,1,0,2,-1,-1,2,0,1,2,1,-1,0};
long encoder_pos = 0;
byte readEncoder()
{
   byte left = digitalRead(PIN_IRLEFT);
   byte right = digitalRead(PIN_IRRIGHT);
   digitalWrite(PIN_OUTLEFT,left);
   digitalWrite(PIN_OUTRIGHT,right);
   return left | (right << 1);
}

void updateEncoder()
{
  byte new_encoder = readEncoder();
  int dir = QEM[current_encoder*4+new_encoder];
  encoder_pos += dir;
  current_encoder = new_encoder;
}

I'm doing 3 things here:
  • Reading a number from 0 to 3 to indicate the 2 bit encoder value (1 bit per sensor)
  • Using the QEM (quadrature encoder matrix) lookup table to convert a previous+current encoder value into a direction
  • Incrementing or decrementing the current encoder position based on direction
In other words, each time the interupt is triggered, I either increment or decrement the current encoder position.

Next up, I add my loop function as follows:

void loop()
{  
  long new_time         = millis();
  long new_encoder_pos  = encoder_pos;
  long pos_delta        = new_encoder_pos - last_encoder_pos;
  long time_delta       = new_time-last_time;
  
  last_time             = new_time;
  last_encoder_pos      = new_encoder_pos;
  
  long rpm              = ((pos_delta * 60000) / time_delta)/32;
  
  Serial.println(rpm);
  
  delay(100);
}

This is basically taking a change in encoder position and the time passed to work out the speed the wheel is turning. Just pos_delta / time_delta would give me stripes-per-millisecond which isn't too useful, but multiply it by 60000 (milliseconds -> minutes), and divide by 32 (number of interrupts per revolution) and you get rpm! And surprise surprise, if I print out the rpm and graph it in excel:

RPM graph coming from Arduino
It's spikier than I'd like (although that's probably due to badly positioned sensors and a wobbly blu-tac attached wheel), but it's clearly giving a solid reading of just around 400rpm - probably what I'd expect from this motor given all that's currently attached is a small wooden disk!

Well, that's it for now. I have a quadrature encoder built. Next up - fit the disk and sensors to MmBot. Once that's done I can take the rpm of each wheel and use it to work out the the actual velocity of the robot at the point the wheel is touching the ground, eventually giving me the ability to reliably say 'go forwards 5m', or 'turn 90 degrees'.

-Chris


2 comments:

  1. Quick question, what type of IR sensor did you use? I am currently using the QR1113 digital breakout board. Are you using the analog ones so you just get a low and a high?

    ReplyDelete
  2. Any chance you can post your final code? I'm having trouble following when you call your interrupt functions.

    ReplyDelete