Permalink
Cannot retrieve contributors at this time
Fetching contributors…
| /** | |
| * 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> | |
| */ | |
| package com.techhounds.gyro; | |
| 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 GyroItg3200} 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 GyroItg3200 { | |
| /** | |
| * Object used to monitor robot rotation. | |
| * | |
| * <ul> | |
| * <li>Use this class to track how much your robot has rotated.</li> | |
| * <li>Use the {@link GyroItg3200#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 GyroItg3200#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 GyroItg3200#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 GyroItg3200#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 GyroItg3200} 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() | |
| */ | |
| @Override | |
| public double pidGet() { | |
| return getAngle(); | |
| } | |
| @Override | |
| public void setPIDSourceType(PIDSourceType pidSource) { | |
| // TODO Auto-generated method stub | |
| } | |
| @Override | |
| 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 GyroItg3200(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; | |
| } | |
| } | |
| } | |
| } |