[3501/stronghold-2016] / src / org / usfirst / frc / team3501 / robot / GyroLib.java

package org.usfirst.frc.team3501.robot;
/**
 * Copyright (c) 2015, www.techhounds.com
 * All rights reserved.
 *
 * <p>
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 * </p>
 * <ul>
 * <li>Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.</li>
 * <li>Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.</li>
 * <li>Neither the name of the www.techhounds.com nor the
 *     names of its contributors may be used to endorse or promote products
 *     derived from this software without specific prior written permission.</li>
 * </ul>
 *
 * <p>
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * </p>
 */

import java.io.File;
import java.io.IOException;
import java.io.PrintStream;

import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.PIDSourceType;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 * A Java wrapper around the ITC based ITG-3200 triple axis gryo.
 * 
 * <p>
 * Typical usage:
 * </p>
 * <ul>
 * <li>Make sure your ITG-3200 is connected to the I2C bus on the roboRIO and
 * mounted flat (so Z axis is used to track direction robot is facing).</li>
 * <li>Construct a single instance of the {@link GyroLib} class to be
 * shared throughout your Robot code.</li>
 * <li>Use the {@link #getRotationZ()} method create "trackers" that allow you
 * to keep track of how much your robot has rotated (direction your robot is
 * facing).</li>
 * </ul>
 * <p>
 * Be aware of the following:
 * </p>
 * <ul>
 * <li>Angles are in signed degrees (both positive and negative values are
 * possible) and not necessarily normalized (large values like -1203 degrees are
 * possible).</li>
 * <li>The {@link #reset} method is called once initially at construction.
 * Reseting the gyro should only be done when your robot is stationary and it
 * may take up to one second. You should not need to do this and should avoid
 * doing this during the autonomous or teleop periods (unless you know your
 * robot won't be moving).</li>
 * <li>There is a background thread that is automatically started that keeps
 * reading and accumulating values from the ITG-3200. You should not need to use
 * the {@link #start()} or {@link #stop()} methods during normal matches.
 * However, if you only use the gyro during the autonomous period, you can use
 * the {@link #stop()} method at the end of the autonomous period to save some
 * CPU.</li>
 * </ul>
 * 
 * <h2>Suggested Usage</h2>
 * <p>
 * You should be able to use this class to aid your robot in making relative
 * turns. For example, if you want to create a command which rotates your robot
 * 90 degrees.
 * </p>
 * <ul>
 * <li>In your command's initialize method, use the {@link #getRotationZ()} and
 * the {@link Rotation#zero()} method on the returned {@link Rotation} object to
 * track how much you have turned.</li>
 * <li>Use the {@link Rotation} object as a PID source and/or check the current
 * angle reported by the {@link Rotation} object in your isFinished() method.
 * </li>
 * </ul>
 */
public final class GyroLib {

  /**
   * Object used to monitor robot rotation.
   * 
   * <ul>
   * <li>Use this class to track how much your robot has rotated.</li>
   * <li>Use the {@link GyroLib#getRotationZ()} to create an instance
   * to track how much your robot has rotated around the z-axis (direction robot
   * is facing - useful for making turns)..</li>
   * <li>Use the {@link GyroLib#getRotationX()} to create an instance
   * to track how much your robot has rotated around the x-axis (hopefully not
   * much unless you are tipping).</li>
   * <li>Use the {@link GyroLib#getRotationY()} to create an instance
   * to track how much your robot has rotated around the y-axis (hopefully not
   * much unless you are tipping).</li>
   * <li>Use the {@link GyroLib#getRo
   * </ul>
   */
  public final class Rotation implements RotationTracker {
    /** Raw axis accumulator on gyro associated with this rotation tracker. */
    private Accumulator m_axis;
    /**
     * The degrees reported by the accumulator the last time this tracker was
     * zeroed.
     */
    private double m_zeroDeg;
    /**
     * The number of readings reported by the accumulator the last time this
     * tracker was zeroed.
     */
    private int m_zeroCnt;

    /**
     * Constructor is protected, instances are created through the
     * {@link GyroLib} methods.
     * 
     * @param axis
     *          An accumulator from the gyro for the axis to be tracked.
     */
    private Rotation(Accumulator axis) {
      m_axis = axis;
      m_zeroDeg = 0;
      m_zeroCnt = 0;
    }

    /**
     * Zero the tracker (sets the current heading/direction as the zero point).
     */
    public void zero() {
      m_zeroDeg = m_axis.getDegrees();
      m_zeroCnt = m_axis.getReadings();
    }

    /**
     * Get the number of degrees rotated since last zeroed.
     * 
     * @return A signed number of degrees [-INF, +INF].
     */
    public double getAngle() {
      double angle = m_axis.getDegrees() - m_zeroDeg;
      return angle;
    }

    /**
     * Get the total number of times the raw values from the gyro have been read
     * since zeroed.
     * 
     * @return A diagnostic count that can be used to make sure the angle is
     *         still being updated.
     */
    public int getReadings() {
      return m_axis.getReadings() - m_zeroCnt;
    }

    /**
     * Returns the last raw (integer) value read from the gyro for the axis.
     * 
     * @return An integer value from the ITG-3200 for the associated axis.
     */
    public int getAngleRateRaw() {
      return m_axis.getRaw();
    }

    /**
     * Returns the current rotation rate in degrees/second from the last
     * reading.
     * 
     * @return How quickly the system is rotating about the axis in
     *         degrees/second.
     */
    public double getAngleRate() {
      return getAngleRateRaw() * COUNT_TO_DEGSEC;
    }

    /**
     * Returns the angle value from {@link #getAngle()} so object can be used as
     * a source to a PID controller.
     * 
     * @return See {@link #getAngle()}.
     * 
     * @see edu.wpi.first.wpilibj.PIDSource#pidGet()
     */
    public double pidGet() {
      return getAngle();
    }

    public void setPIDSourceType(PIDSourceType pidSource) {
      // TODO Auto-generated method stub

    }

    public PIDSourceType getPIDSourceType() {
      // TODO Auto-generated method stub
      return null;
    }
  }

  //
  // List of I2C registers which the ITG-3200 uses from the datasheet
  //
  private static final byte WHO_AM_I = 0x00;
  private static final byte SMPLRT_DIV = 0x15;
  private static final byte DLPF_FS = 0x16;
  // private static final byte INT_CFG = 0x17;
  // private static final byte INT_STATUS = 0x1A;
  // private static final byte TEMP_OUT_H = 0x1B;
  // private static final byte TEMP_OUT_L = 0x1C;
  private static final byte GYRO_XOUT_H = 0x1D;
  // private static final byte GYRO_XOUT_L = 0x1E;
  // private static final byte GYRO_YOUT_H = 0x1F;
  // private static final byte GYRO_YOUT_L = 0x20;
  // private static final byte GYRO_ZOUT_H = 0x21;
  // private static final byte GYRO_ZOUT_L = 0x22;

  //
  // Bit flags used for interrupt operation
  //

  /*
   * Set this bit in INT_CFG register for logic level of INT output pin to be
   * low when interrupt is active (leave 0 if you want it high).
   */
  // private static final byte INT_CFG_ACTL_LOW = (byte) (1 << 7);

  /*
   * Set drive type for interrupt to open drain mode, omit if you want push-pull
   * mode (what does this mean?).
   */
  // private static final byte INT_CFG_OPEN_DRAIN = 1 << 6;

  /*
   * Interrupt latch mode (remains set until you clear it), omit this flag to
   * get a 0-50us interrupt pulse.
   */
  // private static final byte INT_CFG_LATCH_INT_EN = 1 << 5;

  /*
   * Allow any read operation of data to clear the interrupt flag (otherwise it
   * is only cleared after reading status register).
   */
  // private static final byte INT_CFG_ANYRD_2CLEAR = 1 << 4;

  /*
   * Enable interrup when device is ready (PLL ready after changing clock
   * source). Hmmm?
   */
  // private static final byte INT_CFG_RDY_EN = 1 << 3;

  /* Enable interrupt when new data is available. */
  // private static final byte INT_CFG_RAW_RDY_EN = 1 << 1;

  /* Guess at mode to use if we want to try enabling interrupts. */
  // private static final byte INT_CFG_SETTING = (INT_CFG_LATCH_INT_EN |
  // INT_CFG_ANYRD_2CLEAR | INT_CFG_RAW_RDY_EN);

  //
  // The bit flags that can be set in the DLPF register on the ITG-3200
  // as specified in the ITG-3200 data sheet.
  //

  //
  // The low pass filter bandwidth settings
  //

  // private static final byte DLPF_LOWPASS_256HZ = 0 << 0;
  private static final byte DLPF_LOWPASS_188HZ = 1 << 0;
  // private static final byte DLPF_LOWPASS_98HZ = 2 << 0;
  // private static final byte DLPF_LOWPASS_42HZ = 3 << 0;
  // private static final byte DLPF_LOWPASS_20HZ = 4 << 0;
  // private static final byte DLPF_LOWPASS_10HZ = 5 << 0;
  // private static final byte DLPF_LOWPASS_5HZ = 6 << 0;

  /** Select range of +/-2000 deg/sec. (only range supported). */
  private static final byte DLPF_FS_SEL_2000 = 3 << 3;

  /**
   * The I2C address of the ITG-3200 when AD0 (pin 9) is jumpered to logic high.
   */
  private static final byte itgAddressJumper = 0x69;

  /**
   * The I2C address of the ITG-3200 when AD0 (pin 9) is jumpered to logic low.
   */
  private static final byte itgAddressNoJumper = 0x68;

  /** Multiplier to convert raw integer values returned to degrees/sec. */
  private static final float COUNT_TO_DEGSEC = (float) (1.0 / 14.375);

  /** Set this to true for lots of diagnostic output. */
  private static final boolean DEBUG = false;

  /**
   * How many sample readings to make to determine the bias value for each axis.
   */
  private static final int MIN_READINGS_TO_SET_BIAS = 50;

  /** I2C Address to use to communicate with the ITG-3200. */
  private byte m_addr;

  /** I2C object used to communicate with Gyro. */
  private I2C m_i2c;

  /**
   * Background thread responsible for accumulating angle data from the sensor.
   */
  private Thread m_thread;

  /** Flag used to signal background thread that the gyro should be reset. */
  private boolean m_need_reset;

  /** Flag used to signal background thread that it's time to stop. */
  private boolean m_run_thread;

  /** Accumulator for rotation around the x-axis. */
  private Accumulator m_x;

  /** Accumulator for rotation around the y-axis. */
  private Accumulator m_y;

  /** Accumulator for rotation around the z-axis. */
  private Accumulator m_z;
  private int[] m_xBuffer;
  private int[] m_yBuffer;
  private int[] m_zBuffer;
  private int m_cntBuffer;
  private int m_sizeBuffer;
  private double[] m_timeBuffer;

  /**
   * Construct a new instance of the ITG-3200 gryo class.
   * 
   * <p>
   * IMPORTANT
   * 
   * @param port
   *          This should be {@link I2C.Port#kOnboard} if the ITG-3200 is
   *          connected to the main I2C bus on the roboRIO. This should be
   *          {@link I2C.Port#kMXP} if it is connected to the I2C bus on the MXP
   *          port on the roboRIO.
   * @param jumper
   *          This should be true if the ITG-3200 has the AD0 jumpered to logic
   *          level high and false if not.
   */
  public GyroLib(I2C.Port port, boolean jumper) {
    m_addr = (jumper ? itgAddressJumper : itgAddressNoJumper);
    m_i2c = new I2C(port, m_addr);
    m_thread = new Thread(new Runnable() {
      @Override
      public void run() {
        accumulateData();
      }
    });
    if (DEBUG) {
      check();
    }
    m_x = new Accumulator();
    m_y = new Accumulator();
    m_z = new Accumulator();
    m_need_reset = true;

    start();
  }

  /**
   * Construct a {@link Rotation} object used to monitor rotation about the
   * Z-axis.
   * 
   * @return A rotation object that very useful for checking the direction your
   *         robot is facing.
   */
  public Rotation getRotationZ() {
    return new Rotation(m_z);
  }

  /**
   * Construct a {@link Rotation} object used to monitor rotation about the
   * X-axis.
   * 
   * @return A rotation object that is probably only useful for checking if your
   *         robot is starting to tip over.
   */
  public Rotation getRotationX() {
    return new Rotation(m_x);
  }

  /**
   * Construct a {@link Rotation} object used to monitor rotation about the
   * Y-axis.
   * 
   * @return A rotation object that is probably only useful for checking if your
   *         robot is starting to tip over.
   */
  public Rotation getRotationY() {
    return new Rotation(m_y);
  }

  /**
   * Returns string representation of the object for debug purposes.
   */
  public String toString() {
    return "Gyro[0x" + Integer.toHexString(m_addr & 0xff) + "]";
  }

  /**
   * Dumps information about the state of the Gyro to the smart dashboard.
   * 
   * @param tag
   *          Short name like "Gyro" to prefix each label with on the dashboard.
   * @param debug
   *          Pass true if you want a whole lot of details dumped onto the
   *          dashboard, pass false if you just want the direction of each axis
   *          and the temperature.
   */
  public void updateDashboard(String tag, boolean debug) {
    SmartDashboard.putNumber(tag + " x-axis degrees", m_x.getDegrees());
    SmartDashboard.putNumber(tag + " y-axis degrees", m_y.getDegrees());
    SmartDashboard.putNumber(tag + " z-axis degrees", m_z.getDegrees());

    if (debug) {
      SmartDashboard.putNumber(tag + " x-axis raw", m_x.getRaw());
      SmartDashboard.putNumber(tag + " y-axis raw", m_y.getRaw());
      SmartDashboard.putNumber(tag + " z-axis raw", m_z.getRaw());

      SmartDashboard.putNumber(tag + " x-axis count", m_x.getReadings());
      SmartDashboard.putNumber(tag + " y-axis count", m_y.getReadings());
      SmartDashboard.putNumber(tag + " z-axis count", m_z.getReadings());

      SmartDashboard.putString(tag + " I2C Address",
          "0x" + Integer.toHexString(m_addr));
    }
  }

  /**
   * Internal method that runs in the background thread to accumulate data from
   * the Gyro.
   */
  private void accumulateData() {
    m_run_thread = true;
    int resetCnt = 0;

    while (m_run_thread) {
      if (m_need_reset) {
        // Set gyro to the proper mode
        performResetSequence();

        // Reset accumulators and set the number of readings to take to
        // compute bias values
        resetCnt = MIN_READINGS_TO_SET_BIAS;
        m_x.reset();
        m_y.reset();
        m_z.reset();
        m_need_reset = false;
      } else {
        // Go read raw values from ITG-3200 and update our accumulators
        readRawAngleBytes();

        if (resetCnt > 0) {
          // If we were recently reset, and have made enough initial
          // readings,
          // then go compute and set our new bias (correction) values
          // for each accumulator
          resetCnt--;
          if (resetCnt == 0) {
            m_x.setBiasByAccumulatedValues();
            m_y.setBiasByAccumulatedValues();
            m_z.setBiasByAccumulatedValues();
          }
        }
      }

      // Short delay between each reading
      if (m_run_thread) {
        Timer.delay(.01);
      }
    }
  }

  /**
   * Singles the gyro's background thread that we want to reset the gyro.
   * 
   * <p>
   * You don't typically need to call this during a match. If you do call it,
   * you should only do so when the robot is stationary and will remain
   * stationary for a short time.
   * </p>
   */
  public void reset() {
    m_need_reset = true;
  }

  /**
   * Starts up the background thread that accumulates gyro statistics.
   * 
   * <p>
   * You never need to call this method unless you have stopped the gyro and now
   * want to start it up again. If you do call this method, you should probably
   * also call the {@link #reset} method.
   * </p>
   */
  public void start() {
    if (!m_thread.isAlive()) {
      m_thread.start();
    }
  }

  /**
   * Stops the background thread from accumulating angle information (turns OFF
   * gyro!).
   * 
   * <p>
   * This method is not typically called as it stops the gyro from accumulating
   * statistics essentially turning it off. The only time you might want to do
   * this is if you are done using the gyro for the rest of the match and want
   * to save some CPU cyles (for example, if you only needed the gyro during the
   * autonomous period).
   * </p>
   */
  public void stop() {
    m_run_thread = false;
  }

  /**
   * Sends commands to configure the ITG-3200 the way we need it to run.
   */
  private void performResetSequence() {
    // Configure the gyroscope
    // Set the gyroscope scale for the outputs to +/-2000 degrees per second
    m_i2c.write(DLPF_FS, (DLPF_FS_SEL_2000 | DLPF_LOWPASS_188HZ));
    // Set the sample rate to 100 hz
    m_i2c.write(SMPLRT_DIV, 9);
  }

  /**
   * Enables the buffering of the next "n" data samples (which can then be saved
   * for analysis).
   * 
   * @param samples
   *          Maximum number of samples to read.
   */
  public void enableBuffer(int samples) {
    double[] timeBuffer = new double[samples];
    int[] xBuffer = new int[samples];
    int[] yBuffer = new int[samples];
    int[] zBuffer = new int[samples];
    synchronized (this) {
      m_timeBuffer = timeBuffer;
      m_xBuffer = xBuffer;
      m_yBuffer = yBuffer;
      m_zBuffer = zBuffer;
      m_cntBuffer = 0;
      m_sizeBuffer = samples;
    }
  }

  /**
   * Check to see if the buffer is full.
   * 
   * @return true if buffer is at capacity.
   */
  public boolean isBufferFull() {
    boolean isFull;
    synchronized (this) {
      isFull = (m_cntBuffer == m_sizeBuffer);
    }
    return isFull;
  }

  /**
   * Writes any raw buffered data to the file "/tmp/gyro-data.csv" for
   * inspection via Excel.
   */
  public void saveBuffer() {
    double[] timeBuffer;
    int[] xBuffer;
    int[] yBuffer;
    int[] zBuffer;
    int size;

    // Transfer buffer info to local variables and turn off buffering in a
    // thread safe way.
    synchronized (this) {
      timeBuffer = m_timeBuffer;
      xBuffer = m_xBuffer;
      yBuffer = m_yBuffer;
      zBuffer = m_zBuffer;
      size = m_cntBuffer;
      m_sizeBuffer = 0;
      m_cntBuffer = 0;
    }

    if (size > 0) {
      try {
        PrintStream out = new PrintStream(new File("/tmp/gryo-data.csv"));
        out.println("\"FPGA Time\",\"x-axis\",\"y-axis\",\"z-axis\"");
        for (int i = 0; i < size; i++) {
          out.println(timeBuffer[i] + "," + xBuffer[i] + "," + yBuffer[i] + ","
              + zBuffer[i]);
        }
        out.close();
        SmartDashboard.putBoolean("Gyro Save OK", true);
      } catch (IOException ignore) {
        SmartDashboard.putBoolean("Gyro Save OK", false);
      }
    }
  }

  /**
   * Internal method run in the background thread that reads values from the
   * ITG-3200 and updates the accumulators.
   */
  private void readRawAngleBytes() {
    double now = Timer.getFPGATimestamp();

    byte[] buffer = new byte[6];
    boolean rc = m_i2c.read(GYRO_XOUT_H, buffer.length, buffer);

    if (rc) {
      // Got a good read, get 16 bit integer values for each axis and
      // update accumulated values
      int x = (buffer[0] << 8) | (buffer[1] & 0xff);
      int y = (buffer[2] << 8) | (buffer[3] & 0xff);
      int z = (buffer[4] << 8) | (buffer[5] & 0xff);

      m_x.update(x, now);
      m_y.update(y, now);
      m_z.update(z, now);

      // If buffered enabled, then save values in a thread safe way
      if (m_sizeBuffer > 0) {
        synchronized (this) {
          int i = m_cntBuffer;
          if (i < m_sizeBuffer) {
            m_timeBuffer[i] = now;
            m_xBuffer[i] = x;
            m_yBuffer[i] = y;
            m_zBuffer[i] = z;
            m_cntBuffer++;
          }
        }
      }
    }

    if (DEBUG) {
      String name = toString();
      String[] labels = { "XOUT_H", "XOUT_L", "YOUT_H", "YOUT_L", "ZOUT_H",
          "ZOUT_L" };
      for (int i = 0; i < labels.length; i++) {
        SmartDashboard.putString(name + " " + labels[i],
            "0x" + Integer.toHexString(0xff & buffer[i]));
      }
    }
  }

  /**
   * Helper method to check that we can communicate with the gyro.
   */
  private void check() {
    byte[] buffer = new byte[1];
    boolean rc = m_i2c.read(WHO_AM_I, buffer.length, buffer);
    if (DEBUG) {
      String name = toString();
      SmartDashboard.putBoolean(name + " Check OK?", rc);
      SmartDashboard.putNumber(name + " WHO_AM_I", buffer[0]);
    }
  }

  /**
   * Private helper class to accumulate values read from the gryo and convert
   * degs/sec into degrees.
   */
  private class Accumulator {
    /** Accumulated degrees since zero. */
    private double m_accumulatedDegs;
    /**
     * 2 times the computed bias value that is used when getting average of
     * readings.
     */
    private double m_bias2;
    /** The prior raw value read from the gyro. */
    private int m_lastRaw;
    /** The prior time stamp the last raw value was read. */
    private double m_lastTime;
    /** The total count of time the gyro value has been read. */
    private int m_cnt;
    /** The sum of all of the raw values read. */
    private long m_sum;

    /** Multipler to covert 2*Count to degrees/sec (optimization). */
    private static final double COUNT2_TO_DEGSEC = (COUNT_TO_DEGSEC / 2.0);

    /**
     * Returns the accumulated degrees.
     * 
     * @return Accumulated signed degrees since last zeroed.
     */
    public synchronized double getDegrees() {
      return m_accumulatedDegs;
    }

    /**
     * @return The raw integer reading from the ITG-3200 associated with the
     *         axis.
     */
    public int getRaw() {
      return m_lastRaw;
    }

    /**
     * Returns the number or readings that went into the accumulated degrees.
     * 
     * @return Count of readings since last zeroed.
     */
    public synchronized int getReadings() {
      return m_cnt;
    }

    /**
     * Constructs a new instance.
     */
    private Accumulator() {
      m_bias2 = 0;
      zero();
    }

    /**
     * Zeros out accumulated information.
     */
    private void zero() {
      m_lastRaw = 0;
      m_lastTime = 0;
      m_sum = 0;
      synchronized (this) {
        m_cnt = 0;
        m_accumulatedDegs = 0;
      }
    }

    /**
     * Zeros out accumulated information and clears (zeros) the internal bias
     * value.
     */
    private void reset() {
      zero();
      m_bias2 = 0;
    }

    /**
     * Computes new bias value from accumulated values and then zeros.
     */
    private void setBiasByAccumulatedValues() {
      m_bias2 = 2.0 * ((double) m_sum) / ((double) m_cnt);
      zero();
    }

    /**
     * Updates (accumulates) new value read from axis.
     * 
     * @param raw
     *          Raw signed 16 bit value read from gyro for axis.
     * @param time
     *          The time stamp when the value was read.
     */
    private void update(int raw, double time) {
      double degs = 0;

      if (m_cnt != 0) {
        // Get average of degrees per second over the time span
        double degPerSec = (m_lastRaw + raw - m_bias2) * COUNT2_TO_DEGSEC;
        // Get time span this rate occurred for
        double secs = (m_lastTime - time);
        // Get number of degrees rotated for time period
        degs = degPerSec * secs;
      }

      // Update our thread shared values
      synchronized (this) {
        m_accumulatedDegs += degs;
        m_sum += raw;
        m_cnt++;
        m_lastRaw = raw;
        m_lastTime = time;
      }
    }
  }
}