   encoder_c = c;
 }
 
-// Where we currently are within the track
-byte trackPos = 0;
-// Track [trackLength] values of historic encoder positions
-unsigned int track_a[trackLength];
-unsigned int track_b[trackLength];
-unsigned int track_c[trackLength];
-
-/*
-  Handle timer interrupt
-  Each tick we stash the current value of the absolute position for each encoder, then walk
-  back in time in the buffer until we have at least a certain magnitude delta, at which point
-  we set the velocity value for that encoder to be the difference divided by the number of
-  steps back in time. The length of the buffer and the frequency of the timer along with the
-  minimum delta define the precision and latency of the velocity detection.
-*/
-ISR (TIMER1_COMPA_vect) {
-  // Write current absolute encoder values into the track arrays
-  track_a[trackPos] = pos_a;
-  track_b[trackPos] = pos_b;
-  track_c[trackPos] = pos_c;
-  // Try to work out velocity based on track history
-  vel_a = velocity(track_a);
-  vel_b = velocity(track_b);
-  vel_c = velocity(track_c);
-  // Increment and wrap track position
-  trackPos++;
-  if (trackPos >= trackLength) {
-    trackPos -= trackLength;
-  }
-}
-
-double velocity(unsigned int track[]) {
-  unsigned int current = track[trackPos];
-  unsigned int past;
-  double dif;
-  for (int d = 1; d < trackLength; d++) {
-    int pastIndex = trackPos - d;
-    if (pastIndex < 0) {
-      pastIndex += trackLength;
-    }
-    past = track[pastIndex];
-    dif = difference(past, current);
-    if (abs(dif) >= targetDelta) {
-      return dif / (double)d;
-    }
-  }
-  // Fallen off the end of the loop, return best guess
-  return dif / (trackLength - 1);
-}
-
 // Set colours based on the signal received from the Pi
 void setColours(byte hues[], int hueCount, int pixelsPerValue, int fromPixel) {
   int pixel = fromPixel;

    uint8_t i2c_command = receivedCommands[0];
    switch (i2c_command) {
      case MOTOR_SPEED_SET:
        for (int i = 1; i < MAX_SENT_BYTES; i++) {
          registerMap[i - 1] = receivedCommands[i];
        }

// Called on I2C data request
void requestEvent() {
  byte data[] = {(byte)((pos_a & 0xff00) >> 8),
                 (byte)(pos_a & 0xff),
                 (byte)((pos_b & 0xff00) >> 8),
                 (byte)(pos_b & 0xff),
                 (byte)((pos_c & 0xff00) >> 8),
                 (byte)(pos_c & 0xff)
                };
  Serial.println("a");
  Serial.println(data[0]);
  Serial.println("b");
