 }
 
 
-//Sends a write command
+//Write to register
 //*****************************************************
 void writeRegister(byte thisRegister, byte thisValue) 
 //*****************************************************

// OSRVR latency-testing hardware firmware source code.
// Author: Russell Taylor working for Sensics.com through Reliasolve.com
// LICENSE: XXX

// This code is based on the "Accelerometer_Gyro_Shield_test" example from
// Robogaia industries that ships along with their 6-axis Gyro/Accelerometer

void loop() 
//*****************************************************
{
  // We run a finite-state machine that cycles through cases of waiting for
  // a period of non motion, detecting a sudden motion, waiting until there
  // is a change in brightness, and reporting the time passed in microseconds
  // between the motion and the brightness change.
  static enum { S_CALM, S_MOTION, S_BRIGHTNESS } state = S_CALM;
  static unsigned long start;
  static int initial_brightness;
  
  switch (state) {
    case S_CALM:
      {
        // Wait for a period of at least 100 cycles where there is no motion above
        // the motion threshold.
        static int calm_cycles = 0;
        if (moving()) {
          calm_cycles = 0;
        } if (++calm_cycles >= 100) {
            state = S_MOTION;
            calm_cycles = 0;
        }
      }
      break;

    case S_MOTION:
      {
        // Wait for a sudden motion.  When we find it, record the time in microseconds
        // so we can compare it to when the brightness changes.  Also record the brightness
        // so we can look for changes.
        if (moving) {
          start = micros();
          initial_brightness = analogRead(A0);
          state = S_BRIGHTNESS;
        }
      }
      break;

    case S_BRIGHTNESS:
      {
        // Wait for a change in brightness compared to the original value that
        // passes a threshold.  When we get it, report the latency.
        // If it takes too long, then we time out and start over.
        int brightness = analogRead(A0);
        unsigned long now = micros();
        if (abs(brightness - initial_brightness > BRIGHTNESS_CHANGE_THRESHOLD)) {
          Serial.print("Latency ");
          Serial.print(now - start);
          Serial.print(" usecs\n");
          state = S_CALM;
        } else if (now - start > TIMEOUT_USEC) {
          Serial.print("Timeout: no brightness change after motion, restarting\n");
          state = S_CALM;
        }
      }
      break;
   
    default:
      Serial.print("Error: Unrecognized state; restarting\n");
      break;
  }
}


// Determine whether we're undergoing acceleration or rotation.
// Does this by comparing the gyros to a threshold and
// comparing accelerations to see if they are changing.
//*****************************************************
inline bool moving(void)
//*****************************************************
{
  byte accX, accY, accZ;
  byte gyroX, gyroY, gyroZ;
  getAccelerometerAndGyroHighCounts(accX, accY, accZ, gyroX, gyroY, gyroZ);        
  
  // If the gyros are above threshold, then we're moving.
  if (gyroX > GYRO_THRESHOLD) { return true; }
  if (gyroY > GYRO_THRESHOLD) { return true; }
  if (gyroZ > GYRO_THRESHOLD) { return true; }
  
  // See if the accelerations are sufficiently different from the
  // ones we read last time.  If so, we're moving.
  static int prevX = 0;
  static int prevY = 0;
  static int prevZ = 0;
  bool ret = false;
  if ( abs(accX - prevX) > ACCEL_CHANGE_THRESHOLD ) {
    ret = true;
  }
  if ( abs(accY - prevY) > ACCEL_CHANGE_THRESHOLD ) {
    ret = true;
  }
  if ( abs(accZ - prevZ) > ACCEL_CHANGE_THRESHOLD ) {
    ret = true;
  }
  prevX = accX;
  prevY = accY;
  prevZ = accZ;

  return ret;  
}

//*****************************************************
  // We only read the high-order bytes for two reasons:
  // 1) Speed.
  // 2) To avoid getting a measurement whose upper and lower bytes are
  // inconsisent.
inline void getAccelerometerAndGyroHighCounts(byte &accX, byte &accY, byte &accZ,
                                       byte &gyroX, byte &gyroY, byte &gyroZ)
//*****************************************************
{
    accX = readRegister(ACCEL_XOUT_H);
    accY = readRegister(ACCEL_YOUT_H);
    accZ = readRegister(ACCEL_ZOUT_H);
    gyroX = readRegister(GYRO_XOUT_H);
    gyroY = readRegister(GYRO_YOUT_H);
    gyroZ = readRegister(GYRO_ZOUT_H);
}


//Read from register
//*****************************************************
unsigned int readRegister(byte thisRegister) 
//*****************************************************
