-package org.usfirst.frc.team3501.robot.sensors;
-/**
- * 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;
- }
- }
- }
-}