+#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()
 {

  Serial.println((double)(micros()-start)/n);
  while ( Serial.available() > 0 ) Serial.read();
}
  

 
  // Quadrature encoders
  // Left encoder
  pinMode2(c_EncoderPinA, INPUT_PULLUP);      // sets pin A as input
  pinMode2(c_EncoderPinB, INPUT_PULLUP);      // sets pin B as input
  pinMode2(c_EncoderPinZ, INPUT_PULLUP);      // sets pin B as input
  digitalWrite(A0, HIGH);
  digitalWrite(A1, HIGH);
  digitalWrite(A2, HIGH);
  
  attachInterrupt(d_EncoderInterruptA, HandleInterruptA, RISING);
  attachInterrupt(d_EncoderInterruptZ, HandleInterruptZ, RISING);
}
 
void loop()
