   Serial.println((double)(micros()-start)/n);
   while ( Serial.available() > 0 ) Serial.read();
 }
-  
+
+// Interrupt service routines for the left motor's quadrature encoder
+void HandleInterruptA()
+{
+  // Test transition; since the interrupt will only fire on 'rising' we don't need to read pin A
+  _EncoderBSet = digitalRead2(d_EncoderPinB);   // read the input pin
+ 
+  // and adjust counter + if A leads B
+    _EncoderTicks += _EncoderBSet ? -1 : +1;
+    
+    long us = micros();
+    if(_last != 0) _speed = 5000.0f/(double)(us-_last);
+    _last = us;
+}

 
  // Quadrature encoders
  // Left encoder
  digitalWrite(A0, HIGH);
  digitalWrite(A1, HIGH);
  digitalWrite(A2, HIGH);
}
 
void loop()

#include <arduino2.h>

#define c_EncoderPinA A0
#define c_EncoderPinB A1
#define c_EncoderPinZ A2

#define d_EncoderPinZ 2
#define d_EncoderPinA 3
#define d_EncoderPinB 4

#define d_EncoderInterruptZ 0
#define d_EncoderInterruptA 1

volatile bool _EncoderBSet;
volatile long _EncoderTicks = 0;
long _last = 0;
double _speed = 0;
long _CentreCount = 0;
bool positionKnown = false;

#define	GPIO2_PREFER_SPEED	1

void setup()
{
