+++ /dev/null
-// GyroITG3200 I2C device class file
-// Based on InvenSense ITG-3200 datasheet rev. 1.4, 3/30/2010 (PS-ITG-3200A-00-01.4)
-// Original work by 7/31/2011 by Jeff Rowberg <jeff@rowberg.net>
-// Java implementation for First Robotics Competition Team 2521 using WPILibj
-// 1/27/2015 by Joe Bussell <joe dot bussell at gmail dot com>
-// Updates should (hopefully) always be available at https://github.com/bussell
-//
-// Changelog:
-// 2011-07-31 - initial release
-// 2015-01-30 - Java FRC revision
-
-/* ============================================
-GyroITG3200 device library code is placed under the MIT license
-Copyright (c) 2011 by Jeff Rowberg
-Copyright (c) 2015 Joe Bussell
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-The above copyright notice and this permission notice shall be included in
-all copies or substantial portions of the Software.
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-THE SOFTWARE.
-===============================================
-*/
-
-package org.usfirst.frc.team3501.robot;
-
-import java.util.Arrays;
-
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.I2C;
-import edu.wpi.first.wpilibj.PIDSource;
-import edu.wpi.first.wpilibj.PIDSourceType;
-import edu.wpi.first.wpilibj.SensorBase;
-import edu.wpi.first.wpilibj.livewindow.LiveWindow;
-import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
-import edu.wpi.first.wpilibj.tables.ITable;
-
-/**
- * @author Joe Bussell Team 2521 Mentor With thanks to the c++ version authors
- * at: https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/ITG3200
- *
- */
-public class FirebotGyro extends SensorBase
- implements PIDSource, LiveWindowSendable {
- int devAddr;
- byte buffer[] = new byte[7];
-
- public static final boolean DEBUG = true;
-
- I2C m_i2c;
-
- /**
- * Default constructor, uses default I2C address.
- *
- * @see ITG3200_DEFAULT_ADDRESS
- */
- public FirebotGyro(I2C.Port port) {
- devAddr = ITG3200_DEFAULT_ADDRESS;
-
- m_i2c = new I2C(port, devAddr);
-
- // TODO: This report is incorrect. Need to create instance for I2C ITG3200
- // Gyro
- // UsageReporting.report( tResourceType.kResourceType_I2C, tInstances.?? );
- LiveWindow.addSensor("ITG3200_Gyro_I2C", port.getValue(), this);
- }
-
- /**
- * Specific address constructor.
- *
- * @param address
- * I2C address
- * @see ITG3200_DEFAULT_ADDRESS
- * @see ITG3200_ADDRESS_AD0_LOW
- * @see ITG3200_ADDRESS_AD0_HIGH
- */
- public FirebotGyro(I2C.Port port, byte address) {
- devAddr = address;
-
- m_i2c = new I2C(port, address);
-
- // TODO: This report is incorrect. Need to create instance for I2C ITG3200
- // Gyro
- // UsageReporting.report( tResourceType.kResourceType_I2C, tInstances.?? );
- LiveWindow.addSensor("ITG3200_Gyro_I2C", port.getValue(), this);
- }
-
- /**
- * Power on and prepare for general usage. This will activate the gyroscope,
- * so be sure to adjust the power settings after you call this method if you
- * want it to enter standby mode, or another less demanding mode of operation.
- * This also sets the gyroscope to use the X-axis gyro for a clock source.
- * Note that it doesn't have any delays in the routine, which means you might
- * want to add ~50ms to be safe if you happen to need to read gyro data
- * immediately after initialization. The data will flow in either case, but
- * the first reports may have higher error offsets.
- */
- public void initialize() {
- if (!testConnection()) {
- DriverStation.reportError("Test connection failed!", false);
- }
- setFullScaleRange(ITG3200_FULLSCALE_2000);
- setClockSource(ITG3200_CLOCK_PLL_XGYRO);
- setIntDeviceReadyEnabled(true);
- setIntDataReadyEnabled(true);
- }
-
- /**
- * Verify the I2C connection. Make sure the device is connected and responds
- * as expected.
- *
- * @return True if connection is valid, false otherwise
- */
- public boolean testConnection() {
- return getDeviceID() == 0b110100;
- }
-
- private void writeBit(int register, byte bit, boolean value) {
- byte[] buf = new byte[1];
- ReadI2CBuffer(register, 1, buf);
- byte newValue = (byte) (value ? (buf[0] | (1 << bit))
- : (buf[0] & ~(1 << bit)));
- writeI2CBuffer(register, newValue);
-
- if (DEBUG) {
- ReadI2CBuffer(register, 1, buf);
- if (newValue != buf[0]) {
- System.out.println("Expected " + newValue + " seeing " + buf[0]);
- }
- }
- }
-
- // this routine should update the original byte with the new data properly
- // shifted to the correct bit location
- public static byte updateByte(byte original, int bit, int numBits,
- byte value) {
- if (numBits > 7) {
- throw new IllegalArgumentException(
- "This routine is intended to use 8-bit bytes. \n Value: "
- + GetBinaryString(value) + "\n Number bits: " + numBits);
- }
- if (bit > 7) {
- throw new IllegalArgumentException(
- "This routine is intended to use 8-bit bytes. \n Value: "
- + GetBinaryString(value) + "\n Bit: " + bit);
- }
- if (bit < numBits - 1) {
- throw new IllegalArgumentException(
- "This routine is intended to use 8-bit bytes. \n Value: "
- + GetBinaryString(value) + "\n Bit: " + bit + "\n Number bits: "
- + numBits);
- }
- if (value > Math.pow(2, numBits)) {
- throw new IllegalArgumentException(
- "Cannot encode a number this big using the number of bits requested \n Value: "
- + GetBinaryString(value) + "\n Number bits: " + numBits);
- }
- if (bit < 0 || numBits < 0 || value < 0) {
- throw new IllegalArgumentException(
- "This routine is intended to use 8-bit bytes. "
- + "\n All inputs should be greater than 0. " + "\n Value: "
- + GetBinaryString(value) + "\n Bit: " + bit + "\n Number bits: "
- + numBits);
-
- }
- byte mask = getMask(bit, numBits);
- byte maskedOriginal = (byte) ((original & mask) & 0xFF);
- byte shiftedValue = (byte) ((value << (1 + bit - numBits)) & 0xFF);
- byte result = (byte) ((shiftedValue | maskedOriginal) & 0xFF);
- /*
- * // Debug code System.out.println( "bit = " + bit );
- * System.out.println( "num bits = " + numBits ); System.out.println(
- * "original = " + GetBinaryString(original) ); System.out.println(
- * " Value = " + GetBinaryString(value) ); System.out.println( "" );
- * System.out.println( "mask = " + GetBinaryString(mask) );
- * System.out.println( "maskedOriginal = " + GetBinaryString(maskedOriginal)
- * ); System.out.println( "shifted Value = " +
- * GetBinaryString(shiftedValue) ); System.out.println( "" );
- * System.out.println( "result = " + GetBinaryString(result) );
- */
- return result;
- }
-
- public static String GetBinaryString(byte value) {
- return String.format("%8s", Integer.toBinaryString(value & 0xFF))
- .replace(' ', '0');
- }
-
- public boolean writeI2CBuffer(int registerAddress, int data) {
- boolean retVal = false;
- try {
- retVal = m_i2c.write(registerAddress, data);
- if (DEBUG) {
- byte[] buf = new byte[1];
- ReadI2CBuffer(registerAddress, 1, buf);
- if (data != buf[0]) {
- DriverStation.reportError(
- "Expected " + data + "\nseeing " + buf[0] + "\n", false);
- }
- }
- } catch (Throwable t) {
- DriverStation.reportError("ERROR Unhandled exception: " + t.toString()
- + " at " + Arrays.toString(t.getStackTrace()), false);
- }
- return retVal;
- }
-
- //
- // I2Cdev::writeBits(devAddr, ITG3200_RA_WHO_AM_I, ITG3200_DEVID_BIT,
- // ITG3200_DEVID_LENGTH, id);
- private void writeBits(int register, int bit, int numBits, byte value) {
- try {
- byte[] rawData = new byte[1];
- ReadI2CBuffer(register, 1, rawData);
- byte newValue = updateByte(rawData[0], bit, numBits, value);
- writeI2CBuffer(register, newValue);
- } catch (Throwable t) {
- DriverStation.reportError("ERROR Unhandled exception: " + t.toString()
- + " at " + Arrays.toString(t.getStackTrace()), false);
- }
- }
-
- private boolean readBit(int register, byte bit) {
- byte buf[] = new byte[1];
- ReadI2CBuffer(register, 1, buf);
- return (buf[0] & bit) != 0;
- }
-
- // Get n bits from the byte to form a byte slice
- private static byte getBits(byte bitField, int bit, int numBits) {
-
- if (numBits > 7) {
- throw new IllegalArgumentException(
- "This routine is intended to use 8-bit bytes." + "\n Number bits: "
- + numBits);
- }
- if (bit > 7) {
- throw new IllegalArgumentException(
- "This routine is intended to use 8-bit bytes. " + "\n Bit: " + bit);
- }
- if (bit < numBits - 1) {
- throw new IllegalArgumentException(
- "This routine is intended to use 8-bit bytes. " + "\n Bit: " + bit
- + "\n Number bits: " + numBits);
- }
- if (bit < 0 || numBits < 0) {
- throw new IllegalArgumentException(
- "This routine is intended to use 8-bit bytes. "
- + "\n All inputs should be greater than 0. " + "\n Bit: " + bit
- + "\n Number bits: " + numBits);
-
- }
- byte result = 0;
-
- byte mask = (byte) (~getMask(bit, numBits) & 0xFF);
- byte maskedInput = (byte) ((bitField & mask) & 0xFF);
- result = (byte) ((maskedInput >>> (1 + bit - numBits)) & 0xFF);
-
- /*
- * // Debug code System.out.println( "mask = " +
- * GetBinaryString(mask) ); System.out.println( "maskedInput = " +
- * GetBinaryString(maskedInput) ); System.out.println( "result = " +
- * GetBinaryString(result) );
- */
-
- return result;
- }
-
- // Gets the bit mask for the given bit and number of bits
- private static byte getMask(int bit, int numBits) {
- int newMask = 0;
- for (int i = 0; i <= 7; i++) {
- if (i > bit || i <= bit - numBits) {
- // set the mask bit
- newMask = (int) (newMask + Math.pow(2, i));
- }
- }
- byte mask = (byte) (newMask & 0xFF);
- return mask;
- }
-
- private byte getRegisterByte(int register) {
- byte[] buf = new byte[1];
- ReadI2CBuffer(register, 1, buf);
- return buf[0];
- }
-
- /**
- * Get specified bits from the specified register. Form a new value from a
- * byte (0b10110100) get the 3rd bit request 6 bits and you should get a new
- * byte (0b00110100).
- */
- private byte getRegisterBits(int register, int bit, int numBits) {
- byte containingByte = getRegisterByte(register);
- return getBits(containingByte, bit, numBits);
- }
-
- // WHO_AM_I register
- /**
- * Get Device ID. This register is used to verify the identity of the device
- * (0b110100).
- *
- * @return Device ID (should be 0x34, 52 dec, 64 oct)
- * @see ITG3200_RA_WHO_AM_I
- * @see ITG3200_RA_DEVID_BIT
- * @see ITG3200_RA_DEVID_LENGTH
- */
- public byte getDeviceID() {
- return getRegisterBits(ITG3200_RA_WHO_AM_I, ITG3200_DEVID_BIT,
- ITG3200_DEVID_LENGTH);
- }
-
- /**
- * Set Device ID. Write a new ID into the WHO_AM_I register (no idea why this
- * should ever be necessary though).
- *
- * @param id
- * New device ID to set.
- * @see getDeviceID()
- * @see ITG3200_RA_WHO_AM_I
- * @see ITG3200_RA_DEVID_BIT
- * @see ITG3200_RA_DEVID_LENGTH
- */
- public void setDeviceID(byte id) {
- writeBits(ITG3200_RA_WHO_AM_I, ITG3200_DEVID_BIT, ITG3200_DEVID_LENGTH, id);
- }
-
- // SMPLRT_DIV register
- /**
- * Get sample rate. This register determines the sample rate of the ITG-3200
- * gyros. The gyros' outputs are sampled internally at either 1kHz or 8kHz,
- * determined by the DLPF_CFG setting (see register 22). This sampling is then
- * filtered digitally and delivered into the sensor registers after the number
- * of cycles determined by this register. The sample rate is given by the
- * following formula:
- *
- * F_sample = F_internal / (divider+1), where F_internal is either 1kHz or
- * 8kHz
- *
- * As an example, if the internal sampling is at 1kHz, then setting this
- * register to 7 would give the following:
- *
- * F_sample = 1kHz / (7 + 1) = 125Hz, or 8ms per sample
- *
- * @return Current sample rate
- * @see setDLPFBandwidth()
- * @see ITG3200_RA_SMPLRT_DIV
- */
- public byte getRate() {
- return getRegisterByte(ITG3200_RA_SMPLRT_DIV);
- }
-
- /**
- * Set sample rate.
- *
- * @param rate
- * New sample rate
- * @see getRate()
- * @see setDLPFBandwidth()
- * @see ITG3200_RA_SMPLRT_DIV
- */
- public void setRate(byte rate) {
- writeI2CBuffer(ITG3200_RA_SMPLRT_DIV, rate);
- }
-
- // DLPF_FS register
- /**
- * Full-scale range. The FS_SEL parameter allows setting the full-scale range
- * of the gyro sensors, as described in the table below. The power-on-reset
- * value of FS_SEL is 00h. Set to 03h for proper operation.
- *
- * 0 = Reserved 1 = Reserved 2 = Reserved 3 = +/- 2000 degrees/sec
- *
- * @return Current full-scale range setting
- * @see ITG3200_FULLSCALE_2000
- * @see ITG3200_RA_DLPF_FS
- * @see ITG3200_DF_FS_SEL_BIT
- * @see ITG3200_DF_FS_SEL_LENGTH
- */
- public byte getFullScaleRange() {
- return getRegisterBits(ITG3200_RA_DLPF_FS, ITG3200_DF_FS_SEL_BIT,
- ITG3200_DF_FS_SEL_LENGTH);
- }
-
- /**
- * Set full-scale range setting.
- *
- * @param range
- * New full-scale range value
- * @see getFullScaleRange()
- * @see ITG3200_FULLSCALE_2000
- * @see ITG3200_RA_DLPF_FS
- * @see ITG3200_DF_FS_SEL_BIT
- * @see ITG3200_DF_FS_SEL_LENGTH
- */
- public void setFullScaleRange(byte range) {
- writeBits(ITG3200_RA_DLPF_FS, ITG3200_DF_FS_SEL_BIT,
- ITG3200_DF_FS_SEL_LENGTH, range);
- }
-
- /**
- * Get digital low-pass filter bandwidth. The DLPF_CFG parameter sets the
- * digital low pass filter configuration. It also determines the internal
- * sampling rate used by the device as shown in the table below.
- *
- * DLPF_CFG | Low-Pass Filter Bandwidth | Internal Sample Rate
- * ---------+---------------------------+--------------------- 0 | 256Hz |
- * 8kHz 1 | 188Hz | 1kHz 2 | 98Hz | 1kHz 3 | 42Hz | 1kHz 4 | 20Hz | 1kHz 5 |
- * 10Hz | 1kHz 6 | 5Hz | 1kHz 7 | Reserved | Reserved
- *
- * @return DLFP bandwidth setting
- * @see ITG3200_RA_DLPF_FS
- * @see ITG3200_DF_DLPF_CFG_BIT
- * @see ITG3200_DF_DLPF_CFG_LENGTH
- */
- public byte getDLPFBandwidth() {
- return getRegisterBits(ITG3200_RA_DLPF_FS, ITG3200_DF_DLPF_CFG_BIT,
- ITG3200_DF_DLPF_CFG_LENGTH);
- }
-
- /**
- * Set digital low-pass filter bandwidth.
- *
- * @param bandwidth
- * New DLFP bandwidth setting
- * @see getDLPFBandwidth()
- * @see ITG3200_DLPF_BW_256
- * @see ITG3200_RA_DLPF_FS
- * @see ITG3200_DF_DLPF_CFG_BIT
- * @see ITG3200_DF_DLPF_CFG_LENGTH
- */
- public void setDLPFBandwidth(byte bandwidth) {
- writeBits(ITG3200_RA_DLPF_FS, ITG3200_DF_DLPF_CFG_BIT,
- ITG3200_DF_DLPF_CFG_LENGTH, bandwidth);
- }
-
- // INT_CFG register
-
- /**
- * Get interrupt logic level mode. Will be set 0 for active-high, 1 for
- * active-low.
- *
- * @return Current interrupt mode (0=active-high, 1=active-low)
- * @see ITG3200_RA_INT_CFG
- * @see ITG3200_INTCFG_ACTL_BIT
- */
- public boolean getInterruptMode() {
- return readBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_ACTL_BIT);
- }
-
- /**
- * Set interrupt logic level mode.
- *
- * @param mode
- * New interrupt mode (0=active-high, 1=active-low)
- * @see getInterruptMode()
- * @see ITG3200_RA_INT_CFG
- * @see ITG3200_INTCFG_ACTL_BIT
- */
- public void setInterruptMode(boolean mode) {
- writeBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_ACTL_BIT, mode);
- }
-
- /**
- * Get interrupt drive mode. Will be set 0 for push-pull, 1 for open-drain.
- *
- * @return Current interrupt drive mode (0=push-pull, 1=open-drain)
- * @see ITG3200_RA_INT_CFG
- * @see ITG3200_INTCFG_OPEN_BIT
- */
- public boolean getInterruptDrive() {
- return readBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_OPEN_BIT);
- }
-
- /**
- * Set interrupt drive mode.
- *
- * @param drive
- * New interrupt drive mode (0=push-pull, 1=open-drain)
- * @see getInterruptDrive()
- * @see ITG3200_RA_INT_CFG
- * @see ITG3200_INTCFG_OPEN_BIT
- */
- public void setInterruptDrive(boolean drive) {
- writeBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_OPEN_BIT, drive);
- }
-
- /**
- * Get interrupt latch mode. Will be set 0 for 50us-pulse, 1 for
- * latch-until-int-cleared.
- *
- * @return Current latch mode (0=50us-pulse, 1=latch-until-int-cleared)
- * @see ITG3200_RA_INT_CFG
- * @see ITG3200_INTCFG_LATCH_INT_EN_BIT
- */
- public boolean getInterruptLatch() {
- return readBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_LATCH_INT_EN_BIT);
- }
-
- /**
- * Set interrupt latch mode.
- *
- * @param latch
- * New latch mode (0=50us-pulse, 1=latch-until-int-cleared)
- * @see getInterruptLatch()
- * @see ITG3200_RA_INT_CFG
- * @see ITG3200_INTCFG_LATCH_INT_EN_BIT
- */
- public void setInterruptLatch(boolean latch) {
- writeBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_LATCH_INT_EN_BIT, latch);
- }
-
- /**
- * Get interrupt latch clear mode. Will be set 0 for status-read-only, 1 for
- * any-register-read.
- *
- * @return Current latch clear mode (0=status-read-only, 1=any-register-read)
- * @see ITG3200_RA_INT_CFG
- * @see ITG3200_INTCFG_INT_ANYRD_2CLEAR_BIT
- */
- public boolean getInterruptLatchClear() {
- return readBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_INT_ANYRD_2CLEAR_BIT);
- }
-
- /**
- * Set interrupt latch clear mode.
- *
- * @param clear
- * New latch clear mode (0=status-read-only, 1=any-register-read)
- * @see getInterruptLatchClear()
- * @see ITG3200_RA_INT_CFG
- * @see ITG3200_INTCFG_INT_ANYRD_2CLEAR_BIT
- */
- public void setInterruptLatchClear(boolean clear) {
- writeBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_INT_ANYRD_2CLEAR_BIT, clear);
- }
-
- /**
- * Get "device ready" interrupt enabled setting. Will be set 0 for disabled, 1
- * for enabled.
- *
- * @return Current interrupt enabled setting
- * @see ITG3200_RA_INT_CFG
- * @see ITG3200_INTCFG_ITG_RDY_EN_BIT
- */
- public boolean getIntDeviceReadyEnabled() {
- return readBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_ITG_RDY_EN_BIT);
- }
-
- /**
- * Set "device ready" interrupt enabled setting.
- *
- * @param enabled
- * New interrupt enabled setting
- * @see getIntDeviceReadyEnabled()
- * @see ITG3200_RA_INT_CFG
- * @see ITG3200_INTCFG_ITG_RDY_EN_BIT
- */
- public void setIntDeviceReadyEnabled(boolean enabled) {
- writeBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_ITG_RDY_EN_BIT, enabled);
- }
-
- /**
- * Get "data ready" interrupt enabled setting. Will be set 0 for disabled, 1
- * for enabled.
- *
- * @return Current interrupt enabled setting
- * @see ITG3200_RA_INT_CFG
- * @see ITG3200_INTCFG_RAW_RDY_EN_BIT
- */
- public boolean getIntDataReadyEnabled() {
- return readBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_RAW_RDY_EN_BIT);
- }
-
- /**
- * Set "data ready" interrupt enabled setting.
- *
- * @param enabled
- * New interrupt enabled setting
- * @see getIntDataReadyEnabled()
- * @see ITG3200_RA_INT_CFG
- * @see ITG3200_INTCFG_RAW_RDY_EN_BIT
- */
- public void setIntDataReadyEnabled(boolean enabled) {
- writeBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_RAW_RDY_EN_BIT, enabled);
- }
-
- // INT_STATUS register
-
- /**
- * Get Device Ready interrupt status. The ITG_RDY interrupt indicates that the
- * PLL is ready and gyroscopic data can be read.
- *
- * @return Device Ready interrupt status
- * @see ITG3200_RA_INT_STATUS
- * @see ITG3200_INTSTAT_RAW_DATA_READY_BIT
- */
- public boolean getIntDeviceReadyStatus() {
- return readBit(ITG3200_RA_INT_STATUS, ITG3200_INTSTAT_ITG_RDY_BIT);
- }
-
- /**
- * Get Data Ready interrupt status. In normal use, the RAW_DATA_RDY interrupt
- * is used to determine when new sensor data is available in and of the sensor
- * registers (27 to 32).
- *
- * @return Data Ready interrupt status
- * @see ITG3200_RA_INT_STATUS
- * @see ITG3200_INTSTAT_RAW_DATA_READY_BIT
- */
- public boolean getIntDataReadyStatus() {
- return readBit(ITG3200_RA_INT_STATUS, ITG3200_INTSTAT_RAW_DATA_READY_BIT);
- }
-
- // TEMP_OUT_* registers
- /**
- * Get current internal temperature.
- *
- * @return Temperature reading in 16-bit 2's complement format
- * @see ITG3200_RA_TEMP_OUT_H
- */
- public short getTemperature() {
- byte[] buf = new byte[2];
- ReadI2CBuffer(ITG3200_RA_TEMP_OUT_H, 2, buf);
- return (short) (((buf[0]) << 8) | buf[1]);
- }
-
- // GYRO_*OUT_* registers
-
- public static class AllAxes {
- public short XAxis;
- public short YAxis;
- public short ZAxis;
- }
-
- /**
- * Get 3-axis gyroscope readings.
- *
- * @param x
- * 16-bit signed integer container for X-axis rotation
- * @param y
- * 16-bit signed integer container for Y-axis rotation
- * @param z
- * 16-bit signed integer container for Z-axis rotation
- * @see ITG3200_RA_GYRO_XOUT_H
- */
- public AllAxes getRotation() {
- AllAxes data = new AllAxes();
- byte[] buffer = new byte[6];
- ReadI2CBuffer(ITG3200_RA_GYRO_XOUT_H, 6, buffer);
- data.XAxis = (short) (((buffer[0]) << 8) | buffer[1]);
- data.YAxis = (short) (((buffer[2]) << 8) | buffer[3]);
- data.ZAxis = (short) (((buffer[4]) << 8) | buffer[5]);
- return data;
- }
-
- public void ReadI2CBuffer(int registerAddress, int count, byte[] buffer) {
- try {
- m_i2c.read(registerAddress, count, buffer);
- } catch (Throwable t) {
- DriverStation.reportError("ERROR Unhandled exception in I2C Read: "
- + t.toString() + " at " + Arrays.toString(t.getStackTrace()), false);
- }
- }
-
- public short ReadShortFromRegister(byte register, int count) {
- byte[] buffer = new byte[count];
- ReadI2CBuffer(register, count, buffer);
- return (short) (((buffer[0]) << 8) | buffer[1]);
- }
-
- /**
- * Get X-axis gyroscope reading.
- *
- * @return X-axis rotation measurement in 16-bit 2's complement format
- * @see ITG3200_RA_GYRO_XOUT_H
- */
- public short getRotationX() {
- return ReadShortFromRegister(ITG3200_RA_GYRO_XOUT_H, 2);
- }
-
- /**
- * Get Y-axis gyroscope reading.
- *
- * @return Y-axis rotation measurement in 16-bit 2's complement format
- * @see ITG3200_RA_GYRO_YOUT_H
- */
- public short getRotationY() {
- return ReadShortFromRegister(ITG3200_RA_GYRO_YOUT_H, 2);
- }
-
- /**
- * Get Z-axis gyroscope reading.
- *
- * @return Z-axis rotation measurement in 16-bit 2's complement format
- * @see ITG3200_RA_GYRO_ZOUT_H
- */
- public short getRotationZ() {
- return ReadShortFromRegister(ITG3200_RA_GYRO_ZOUT_H, 0);
- }
-
- // PWR_MGM register
-
- /**
- * Trigger a full device reset. A small delay of ~50ms may be desirable after
- * triggering a reset.
- *
- * @see ITG3200_RA_PWR_MGM
- * @see ITG3200_PWR_H_RESET_BIT
- */
-
- public void reset() {
- writeBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_H_RESET_BIT, true);
- }
-
- /**
- * Get sleep mode status. Setting the SLEEP bit in the register puts the
- * device into very low power sleep mode. In this mode, only the serial
- * interface and internal registers remain active, allowing for a very low
- * standby current. Clearing this bit puts the device back into normal mode.
- * To save power, the individual standby selections for each of the gyros
- * should be used if any gyro axis is not used by the application.
- *
- * @return Current sleep mode enabled status
- * @see ITG3200_RA_PWR_MGM
- * @see ITG3200_PWR_SLEEP_BIT
- */
- public boolean getSleepEnabled() {
- return readBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_SLEEP_BIT);
- }
-
- /**
- * Set sleep mode status.
- *
- * @param enabled
- * New sleep mode enabled status
- * @see getSleepEnabled()
- * @see ITG3200_RA_PWR_MGM
- * @see ITG3200_PWR_SLEEP_BIT
- */
- public void setSleepEnabled(boolean enabled) {
- writeBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_SLEEP_BIT, enabled);
- }
-
- /**
- * Get X-axis standby enabled status. If enabled, the X-axis will not gather
- * or report data (or use power).
- *
- * @return Current X-axis standby enabled status
- * @see ITG3200_RA_PWR_MGM
- * @see ITG3200_PWR_STBY_XG_BIT
- */
- public boolean getStandbyXEnabled() {
- return readBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_STBY_XG_BIT);
- }
-
- /**
- * Set X-axis standby enabled status.
- *
- * @param New
- * X-axis standby enabled status
- * @see getStandbyXEnabled()
- * @see ITG3200_RA_PWR_MGM
- * @see ITG3200_PWR_STBY_XG_BIT
- */
- public void setStandbyXEnabled(boolean enabled) {
- writeBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_STBY_XG_BIT, enabled);
- }
-
- /**
- * Get Y-axis standby enabled status. If enabled, the Y-axis will not gather
- * or report data (or use power).
- *
- * @return Current Y-axis standby enabled status
- * @see ITG3200_RA_PWR_MGM
- * @see ITG3200_PWR_STBY_YG_BIT
- */
- public boolean getStandbyYEnabled() {
- return readBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_STBY_YG_BIT);
- }
-
- /**
- * Set Y-axis standby enabled status.
- *
- * @param New
- * Y-axis standby enabled status
- * @see getStandbyYEnabled()
- * @see ITG3200_RA_PWR_MGM
- * @see ITG3200_PWR_STBY_YG_BIT
- */
- public void setStandbyYEnabled(boolean enabled) {
- writeBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_STBY_YG_BIT, enabled);
- }
-
- /**
- * Get Z-axis standby enabled status. If enabled, the Z-axis will not gather
- * or report data (or use power).
- *
- * @return Current Z-axis standby enabled status
- * @see ITG3200_RA_PWR_MGM
- * @see ITG3200_PWR_STBY_ZG_BIT
- */
- public boolean getStandbyZEnabled() {
- return readBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_STBY_ZG_BIT);
- }
-
- /**
- * Set Z-axis standby enabled status.
- *
- * @param New
- * Z-axis standby enabled status
- * @see getStandbyZEnabled()
- * @see ITG3200_RA_PWR_MGM
- * @see ITG3200_PWR_STBY_ZG_BIT
- */
- public void setStandbyZEnabled(boolean enabled) {
- writeBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_STBY_ZG_BIT, enabled);
- }
-
- /**
- * Get clock source setting.
- *
- * @return Current clock source setting
- * @see ITG3200_RA_PWR_MGM
- * @see ITG3200_PWR_CLK_SEL_BIT
- * @see ITG3200_PWR_CLK_SEL_LENGTH
- */
- public byte getClockSource() {
- byte[] buf = new byte[1];
- ReadI2CBuffer(ITG3200_RA_PWR_MGM, 1, buf);
- // I2Cdev::readBits(devAddr, ITG3200_RA_PWR_MGM, ITG3200_PWR_CLK_SEL_BIT,
- // ITG3200_PWR_CLK_SEL_LENGTH, buffer);
- return (byte) (buf[0] & ITG3200_PWR_CLK_SEL_BIT);
- }
-
- /**
- * Set clock source setting. On power up, the ITG-3200 defaults to the
- * internal oscillator. It is highly recommended that the device is configured
- * to use one of the gyros (or an external clock) as the clock reference, due
- * to the improved stability.
- *
- * The CLK_SEL setting determines the device clock source as follows:
- *
- * CLK_SEL | Clock Source --------+-------------------------------------- 0 |
- * Internal oscillator 1 | PLL with X Gyro reference 2 | PLL with Y Gyro
- * reference 3 | PLL with Z Gyro reference 4 | PLL with external 32.768kHz
- * reference 5 | PLL with external 19.2MHz reference 6 | Reserved 7 | Reserved
- *
- * @param source
- * New clock source setting
- * @see getClockSource()
- * @see ITG3200_RA_PWR_MGM
- * @see ITG3200_PWR_CLK_SEL_BIT
- * @see ITG3200_PWR_CLK_SEL_LENGTH
- */
- public void setClockSource(byte source) {
- writeBits(ITG3200_RA_PWR_MGM, ITG3200_PWR_CLK_SEL_BIT,
- ITG3200_PWR_CLK_SEL_LENGTH, source);
- }
-
- private ITable m_table;
-
- /**
- * {@inheritDoc}
- */
- @Override
- public void initTable(ITable subtable) {
- m_table = subtable;
- updateTable();
- }
-
- /**
- * {@inheritDoc}
- */
- @Override
- public ITable getTable() {
- return m_table;
- }
-
- /**
- * {@inheritDoc}
- */
- @Override
- public void updateTable() {
- if (m_table != null) {
- m_table.putNumber("GyroX", getRotationX());
- m_table.putNumber("GyroY", getRotationY());
- m_table.putNumber("GyroZ", getRotationZ());
- m_table.putNumber("GyroPIDValue", pidGet());
- }
- }
-
- /*
- * (non-Javadoc)
- *
- * @see edu.wpi.first.wpilibj.Sendable#getSmartDashboardType()
- */
- @Override
- public String getSmartDashboardType() {
- return "Gyro";
- }
-
- /*
- * (non-Javadoc)
- *
- * @see
- * edu.wpi.first.wpilibj.livewindow.LiveWindowSendable#startLiveWindowMode()
- */
- @Override
- public void startLiveWindowMode() {
- }
-
- /*
- * (non-Javadoc)
- *
- * @see
- * edu.wpi.first.wpilibj.livewindow.LiveWindowSendable#stopLiveWindowMode()
- */
- @Override
- public void stopLiveWindowMode() {
- }
-
- /*
- * (non-Javadoc)
- *
- * @see edu.wpi.first.wpilibj.PIDSource#pidGet()
- */
- @Override
- public double pidGet() {
- // TODO We likely want to return one of the axes based on a setup option.
- AllAxes var = getRotation();
- double result = Math.cbrt(
- var.XAxis * var.XAxis + var.YAxis * var.YAxis + var.ZAxis * var.ZAxis);
- return result;
- // return 0;
- }
-
- public static final byte ITG3200_ADDRESS_AD0_LOW = 0x68; // address pin low
- // (GND), default for
- // SparkFun IMU
- // Digital Combo
- // board
- public static final byte ITG3200_ADDRESS_AD0_HIGH = 0x69; // address pin high
- // (VCC), default
- // for SparkFun
- // ITG-3200 Breakout
- // board
-
- public static final int ITG3200_SPARKFUN_ADDRES = 0xD2;
-
- public static final int ITG3200_DEFAULT_ADDRESS = ITG3200_ADDRESS_AD0_LOW; // ITG3200_ADDRESS_AD0_HIGH;
-
- public static final byte ITG3200_RA_WHO_AM_I = 0x00;
- public static final byte ITG3200_RA_SMPLRT_DIV = 0x15;
- public static final byte ITG3200_RA_DLPF_FS = 0x16;
- public static final byte ITG3200_RA_INT_CFG = 0x17;
- public static final byte ITG3200_RA_INT_STATUS = 0x1A;
- public static final byte ITG3200_RA_TEMP_OUT_H = 0x1B;
- public static final byte ITG3200_RA_TEMP_OUT_L = 0x1C;
- public static final byte ITG3200_RA_GYRO_XOUT_H = 0x1D;
- public static final byte ITG3200_RA_GYRO_XOUT_L = 0x1E;
- public static final byte ITG3200_RA_GYRO_YOUT_H = 0x1F;
- public static final byte ITG3200_RA_GYRO_YOUT_L = 0x20;
- public static final byte ITG3200_RA_GYRO_ZOUT_H = 0x21;
- public static final byte ITG3200_RA_GYRO_ZOUT_L = 0x22;
- public static final byte ITG3200_RA_PWR_MGM = 0x3E;
-
- public static final short ITG3200_DEVID_BIT = 6;
- public static final short ITG3200_DEVID_LENGTH = 6;
-
- public static final short ITG3200_DF_FS_SEL_BIT = 4;
- public static final short ITG3200_DF_FS_SEL_LENGTH = 2;
- public static final short ITG3200_DF_DLPF_CFG_BIT = 2;
- public static final short ITG3200_DF_DLPF_CFG_LENGTH = 3;
-
- public static final byte ITG3200_FULLSCALE_2000 = 0x03;
-
- public static final byte ITG3200_DLPF_BW_256 = 0x00;
- public static final byte ITG3200_DLPF_BW_188 = 0x01;
- public static final byte ITG3200_DLPF_BW_98 = 0x02;
- public static final byte ITG3200_DLPF_BW_42 = 0x03;
- public static final byte ITG3200_DLPF_BW_20 = 0x04;
- public static final byte ITG3200_DLPF_BW_10 = 0x05;
- public static final byte ITG3200_DLPF_BW_5 = 0x06;
-
- public static final byte ITG3200_INTCFG_ACTL_BIT = 7;
- public static final byte ITG3200_INTCFG_OPEN_BIT = 6;
- public static final byte ITG3200_INTCFG_LATCH_INT_EN_BIT = 5;
- public static final byte ITG3200_INTCFG_INT_ANYRD_2CLEAR_BIT = 4;
- public static final byte ITG3200_INTCFG_ITG_RDY_EN_BIT = 2;
- public static final byte ITG3200_INTCFG_RAW_RDY_EN_BIT = 0;
-
- public static final byte ITG3200_INTMODE_ACTIVEHIGH = 0x00;
- public static final byte ITG3200_INTMODE_ACTIVELOW = 0x01;
-
- public static final byte ITG3200_INTDRV_PUSHPULL = 0x00;
- public static final byte ITG3200_INTDRV_OPENDRAIN = 0x01;
-
- public static final byte ITG3200_INTLATCH_50USPULSE = 0x00;
- public static final byte ITG3200_INTLATCH_WAITCLEAR = 0x01;
-
- public static final byte ITG3200_INTCLEAR_STATUSREAD = 0x00;
- public static final byte ITG3200_INTCLEAR_ANYREAD = 0x01;
-
- public static final byte ITG3200_INTSTAT_ITG_RDY_BIT = 2;
- public static final byte ITG3200_INTSTAT_RAW_DATA_READY_BIT = 0;
-
- public static final byte ITG3200_PWR_H_RESET_BIT = 7;
- public static final byte ITG3200_PWR_SLEEP_BIT = 6;
- public static final byte ITG3200_PWR_STBY_XG_BIT = 5;
- public static final byte ITG3200_PWR_STBY_YG_BIT = 4;
- public static final byte ITG3200_PWR_STBY_ZG_BIT = 3;
- public static final byte ITG3200_PWR_CLK_SEL_BIT = 2;
- public static final byte ITG3200_PWR_CLK_SEL_LENGTH = 3;
-
- public static final byte ITG3200_CLOCK_INTERNAL = 0x00;
- public static final byte ITG3200_CLOCK_PLL_XGYRO = 0x01;
- public static final byte ITG3200_CLOCK_PLL_YGYRO = 0x02;
- public static final byte ITG3200_CLOCK_PLL_ZGYRO = 0x03;
- public static final byte ITG3200_CLOCK_PLL_EXT32K = 0x04;
- public static final byte ITG3200_CLOCK_PLL_EXT19M = 0x05;
-
- @Override
- public void setPIDSourceType(PIDSourceType pidSource) {
- // TODO Auto-generated method stub
-
- }
-
- @Override
- public PIDSourceType getPIDSourceType() {
- // TODO Auto-generated method stub
- return null;
- }
-
-}
--- /dev/null
+// GyroITG3200 I2C device class file
+// Based on InvenSense ITG-3200 datasheet rev. 1.4, 3/30/2010 (PS-ITG-3200A-00-01.4)
+// Original work by 7/31/2011 by Jeff Rowberg <jeff@rowberg.net>
+// Java implementation for First Robotics Competition Team 2521 using WPILibj
+// 1/27/2015 by Joe Bussell <joe dot bussell at gmail dot com>
+// Updates should (hopefully) always be available at https://github.com/bussell
+//
+// Changelog:
+// 2011-07-31 - initial release
+// 2015-01-30 - Java FRC revision
+
+/* ============================================
+GyroITG3200 device library code is placed under the MIT license
+Copyright (c) 2011 by Jeff Rowberg
+Copyright (c) 2015 Joe Bussell
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+===============================================
+*/
+
+package org.usfirst.frc.team3501.robot;
+
+import java.util.Arrays;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.I2C;
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.PIDSourceType;
+import edu.wpi.first.wpilibj.SensorBase;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
+import edu.wpi.first.wpilibj.tables.ITable;
+
+/**
+ * @author Joe Bussell Team 2521 Mentor With thanks to the c++ version authors
+ * at: https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/ITG3200
+ *
+ */
+public class GyroClass extends SensorBase
+ implements PIDSource, LiveWindowSendable {
+ int devAddr;
+ byte buffer[] = new byte[7];
+
+ public static final boolean DEBUG = true;
+
+ I2C m_i2c;
+
+ /**
+ * Default constructor, uses default I2C address.
+ *
+ * @see ITG3200_DEFAULT_ADDRESS
+ */
+ public GyroClass(I2C.Port port) {
+ devAddr = ITG3200_DEFAULT_ADDRESS;
+
+ m_i2c = new I2C(port, devAddr);
+
+ // TODO: This report is incorrect. Need to create instance for I2C ITG3200
+ // Gyro
+ // UsageReporting.report( tResourceType.kResourceType_I2C, tInstances.?? );
+ LiveWindow.addSensor("ITG3200_Gyro_I2C", port.getValue(), this);
+ }
+
+ /**
+ * Specific address constructor.
+ *
+ * @param address
+ * I2C address
+ * @see ITG3200_DEFAULT_ADDRESS
+ * @see ITG3200_ADDRESS_AD0_LOW
+ * @see ITG3200_ADDRESS_AD0_HIGH
+ */
+ public GyroClass(I2C.Port port, byte address) {
+ devAddr = address;
+
+ m_i2c = new I2C(port, address);
+
+ // TODO: This report is incorrect. Need to create instance for I2C ITG3200
+ // Gyro
+ // UsageReporting.report( tResourceType.kResourceType_I2C, tInstances.?? );
+ LiveWindow.addSensor("ITG3200_Gyro_I2C", port.getValue(), this);
+ }
+
+ /**
+ * Power on and prepare for general usage. This will activate the gyroscope,
+ * so be sure to adjust the power settings after you call this method if you
+ * want it to enter standby mode, or another less demanding mode of operation.
+ * This also sets the gyroscope to use the X-axis gyro for a clock source.
+ * Note that it doesn't have any delays in the routine, which means you might
+ * want to add ~50ms to be safe if you happen to need to read gyro data
+ * immediately after initialization. The data will flow in either case, but
+ * the first reports may have higher error offsets.
+ */
+ public void initialize() {
+ if (!testConnection()) {
+ DriverStation.reportError("Test connection failed!", false);
+ }
+ setFullScaleRange(ITG3200_FULLSCALE_2000);
+ setClockSource(ITG3200_CLOCK_PLL_XGYRO);
+ setIntDeviceReadyEnabled(true);
+ setIntDataReadyEnabled(true);
+ }
+
+ /**
+ * Verify the I2C connection. Make sure the device is connected and responds
+ * as expected.
+ *
+ * @return True if connection is valid, false otherwise
+ */
+ public boolean testConnection() {
+ return getDeviceID() == 0b110100;
+ }
+
+ private void writeBit(int register, byte bit, boolean value) {
+ byte[] buf = new byte[1];
+ ReadI2CBuffer(register, 1, buf);
+ byte newValue = (byte) (value ? (buf[0] | (1 << bit))
+ : (buf[0] & ~(1 << bit)));
+ writeI2CBuffer(register, newValue);
+
+ if (DEBUG) {
+ ReadI2CBuffer(register, 1, buf);
+ if (newValue != buf[0]) {
+ System.out.println("Expected " + newValue + " seeing " + buf[0]);
+ }
+ }
+ }
+
+ // this routine should update the original byte with the new data properly
+ // shifted to the correct bit location
+ public static byte updateByte(byte original, int bit, int numBits,
+ byte value) {
+ if (numBits > 7) {
+ throw new IllegalArgumentException(
+ "This routine is intended to use 8-bit bytes. \n Value: "
+ + GetBinaryString(value) + "\n Number bits: " + numBits);
+ }
+ if (bit > 7) {
+ throw new IllegalArgumentException(
+ "This routine is intended to use 8-bit bytes. \n Value: "
+ + GetBinaryString(value) + "\n Bit: " + bit);
+ }
+ if (bit < numBits - 1) {
+ throw new IllegalArgumentException(
+ "This routine is intended to use 8-bit bytes. \n Value: "
+ + GetBinaryString(value) + "\n Bit: " + bit + "\n Number bits: "
+ + numBits);
+ }
+ if (value > Math.pow(2, numBits)) {
+ throw new IllegalArgumentException(
+ "Cannot encode a number this big using the number of bits requested \n Value: "
+ + GetBinaryString(value) + "\n Number bits: " + numBits);
+ }
+ if (bit < 0 || numBits < 0 || value < 0) {
+ throw new IllegalArgumentException(
+ "This routine is intended to use 8-bit bytes. "
+ + "\n All inputs should be greater than 0. " + "\n Value: "
+ + GetBinaryString(value) + "\n Bit: " + bit + "\n Number bits: "
+ + numBits);
+
+ }
+ byte mask = getMask(bit, numBits);
+ byte maskedOriginal = (byte) ((original & mask) & 0xFF);
+ byte shiftedValue = (byte) ((value << (1 + bit - numBits)) & 0xFF);
+ byte result = (byte) ((shiftedValue | maskedOriginal) & 0xFF);
+ /*
+ * // Debug code System.out.println( "bit = " + bit );
+ * System.out.println( "num bits = " + numBits ); System.out.println(
+ * "original = " + GetBinaryString(original) ); System.out.println(
+ * " Value = " + GetBinaryString(value) ); System.out.println( "" );
+ * System.out.println( "mask = " + GetBinaryString(mask) );
+ * System.out.println( "maskedOriginal = " + GetBinaryString(maskedOriginal)
+ * ); System.out.println( "shifted Value = " +
+ * GetBinaryString(shiftedValue) ); System.out.println( "" );
+ * System.out.println( "result = " + GetBinaryString(result) );
+ */
+ return result;
+ }
+
+ public static String GetBinaryString(byte value) {
+ return String.format("%8s", Integer.toBinaryString(value & 0xFF))
+ .replace(' ', '0');
+ }
+
+ public boolean writeI2CBuffer(int registerAddress, int data) {
+ boolean retVal = false;
+ try {
+ retVal = m_i2c.write(registerAddress, data);
+ if (DEBUG) {
+ byte[] buf = new byte[1];
+ ReadI2CBuffer(registerAddress, 1, buf);
+ if (data != buf[0]) {
+ DriverStation.reportError(
+ "Expected " + data + "\nseeing " + buf[0] + "\n", false);
+ }
+ }
+ } catch (Throwable t) {
+ DriverStation.reportError("ERROR Unhandled exception: " + t.toString()
+ + " at " + Arrays.toString(t.getStackTrace()), false);
+ }
+ return retVal;
+ }
+
+ //
+ // I2Cdev::writeBits(devAddr, ITG3200_RA_WHO_AM_I, ITG3200_DEVID_BIT,
+ // ITG3200_DEVID_LENGTH, id);
+ private void writeBits(int register, int bit, int numBits, byte value) {
+ try {
+ byte[] rawData = new byte[1];
+ ReadI2CBuffer(register, 1, rawData);
+ byte newValue = updateByte(rawData[0], bit, numBits, value);
+ writeI2CBuffer(register, newValue);
+ } catch (Throwable t) {
+ DriverStation.reportError("ERROR Unhandled exception: " + t.toString()
+ + " at " + Arrays.toString(t.getStackTrace()), false);
+ }
+ }
+
+ private boolean readBit(int register, byte bit) {
+ byte buf[] = new byte[1];
+ ReadI2CBuffer(register, 1, buf);
+ return (buf[0] & bit) != 0;
+ }
+
+ // Get n bits from the byte to form a byte slice
+ private static byte getBits(byte bitField, int bit, int numBits) {
+
+ if (numBits > 7) {
+ throw new IllegalArgumentException(
+ "This routine is intended to use 8-bit bytes." + "\n Number bits: "
+ + numBits);
+ }
+ if (bit > 7) {
+ throw new IllegalArgumentException(
+ "This routine is intended to use 8-bit bytes. " + "\n Bit: " + bit);
+ }
+ if (bit < numBits - 1) {
+ throw new IllegalArgumentException(
+ "This routine is intended to use 8-bit bytes. " + "\n Bit: " + bit
+ + "\n Number bits: " + numBits);
+ }
+ if (bit < 0 || numBits < 0) {
+ throw new IllegalArgumentException(
+ "This routine is intended to use 8-bit bytes. "
+ + "\n All inputs should be greater than 0. " + "\n Bit: " + bit
+ + "\n Number bits: " + numBits);
+
+ }
+ byte result = 0;
+
+ byte mask = (byte) (~getMask(bit, numBits) & 0xFF);
+ byte maskedInput = (byte) ((bitField & mask) & 0xFF);
+ result = (byte) ((maskedInput >>> (1 + bit - numBits)) & 0xFF);
+
+ /*
+ * // Debug code System.out.println( "mask = " +
+ * GetBinaryString(mask) ); System.out.println( "maskedInput = " +
+ * GetBinaryString(maskedInput) ); System.out.println( "result = " +
+ * GetBinaryString(result) );
+ */
+
+ return result;
+ }
+
+ // Gets the bit mask for the given bit and number of bits
+ private static byte getMask(int bit, int numBits) {
+ int newMask = 0;
+ for (int i = 0; i <= 7; i++) {
+ if (i > bit || i <= bit - numBits) {
+ // set the mask bit
+ newMask = (int) (newMask + Math.pow(2, i));
+ }
+ }
+ byte mask = (byte) (newMask & 0xFF);
+ return mask;
+ }
+
+ private byte getRegisterByte(int register) {
+ byte[] buf = new byte[1];
+ ReadI2CBuffer(register, 1, buf);
+ return buf[0];
+ }
+
+ /**
+ * Get specified bits from the specified register. Form a new value from a
+ * byte (0b10110100) get the 3rd bit request 6 bits and you should get a new
+ * byte (0b00110100).
+ */
+ private byte getRegisterBits(int register, int bit, int numBits) {
+ byte containingByte = getRegisterByte(register);
+ return getBits(containingByte, bit, numBits);
+ }
+
+ // WHO_AM_I register
+ /**
+ * Get Device ID. This register is used to verify the identity of the device
+ * (0b110100).
+ *
+ * @return Device ID (should be 0x34, 52 dec, 64 oct)
+ * @see ITG3200_RA_WHO_AM_I
+ * @see ITG3200_RA_DEVID_BIT
+ * @see ITG3200_RA_DEVID_LENGTH
+ */
+ public byte getDeviceID() {
+ return getRegisterBits(ITG3200_RA_WHO_AM_I, ITG3200_DEVID_BIT,
+ ITG3200_DEVID_LENGTH);
+ }
+
+ /**
+ * Set Device ID. Write a new ID into the WHO_AM_I register (no idea why this
+ * should ever be necessary though).
+ *
+ * @param id
+ * New device ID to set.
+ * @see getDeviceID()
+ * @see ITG3200_RA_WHO_AM_I
+ * @see ITG3200_RA_DEVID_BIT
+ * @see ITG3200_RA_DEVID_LENGTH
+ */
+ public void setDeviceID(byte id) {
+ writeBits(ITG3200_RA_WHO_AM_I, ITG3200_DEVID_BIT, ITG3200_DEVID_LENGTH, id);
+ }
+
+ // SMPLRT_DIV register
+ /**
+ * Get sample rate. This register determines the sample rate of the ITG-3200
+ * gyros. The gyros' outputs are sampled internally at either 1kHz or 8kHz,
+ * determined by the DLPF_CFG setting (see register 22). This sampling is then
+ * filtered digitally and delivered into the sensor registers after the number
+ * of cycles determined by this register. The sample rate is given by the
+ * following formula:
+ *
+ * F_sample = F_internal / (divider+1), where F_internal is either 1kHz or
+ * 8kHz
+ *
+ * As an example, if the internal sampling is at 1kHz, then setting this
+ * register to 7 would give the following:
+ *
+ * F_sample = 1kHz / (7 + 1) = 125Hz, or 8ms per sample
+ *
+ * @return Current sample rate
+ * @see setDLPFBandwidth()
+ * @see ITG3200_RA_SMPLRT_DIV
+ */
+ public byte getRate() {
+ return getRegisterByte(ITG3200_RA_SMPLRT_DIV);
+ }
+
+ /**
+ * Set sample rate.
+ *
+ * @param rate
+ * New sample rate
+ * @see getRate()
+ * @see setDLPFBandwidth()
+ * @see ITG3200_RA_SMPLRT_DIV
+ */
+ public void setRate(byte rate) {
+ writeI2CBuffer(ITG3200_RA_SMPLRT_DIV, rate);
+ }
+
+ // DLPF_FS register
+ /**
+ * Full-scale range. The FS_SEL parameter allows setting the full-scale range
+ * of the gyro sensors, as described in the table below. The power-on-reset
+ * value of FS_SEL is 00h. Set to 03h for proper operation.
+ *
+ * 0 = Reserved 1 = Reserved 2 = Reserved 3 = +/- 2000 degrees/sec
+ *
+ * @return Current full-scale range setting
+ * @see ITG3200_FULLSCALE_2000
+ * @see ITG3200_RA_DLPF_FS
+ * @see ITG3200_DF_FS_SEL_BIT
+ * @see ITG3200_DF_FS_SEL_LENGTH
+ */
+ public byte getFullScaleRange() {
+ return getRegisterBits(ITG3200_RA_DLPF_FS, ITG3200_DF_FS_SEL_BIT,
+ ITG3200_DF_FS_SEL_LENGTH);
+ }
+
+ /**
+ * Set full-scale range setting.
+ *
+ * @param range
+ * New full-scale range value
+ * @see getFullScaleRange()
+ * @see ITG3200_FULLSCALE_2000
+ * @see ITG3200_RA_DLPF_FS
+ * @see ITG3200_DF_FS_SEL_BIT
+ * @see ITG3200_DF_FS_SEL_LENGTH
+ */
+ public void setFullScaleRange(byte range) {
+ writeBits(ITG3200_RA_DLPF_FS, ITG3200_DF_FS_SEL_BIT,
+ ITG3200_DF_FS_SEL_LENGTH, range);
+ }
+
+ /**
+ * Get digital low-pass filter bandwidth. The DLPF_CFG parameter sets the
+ * digital low pass filter configuration. It also determines the internal
+ * sampling rate used by the device as shown in the table below.
+ *
+ * DLPF_CFG | Low-Pass Filter Bandwidth | Internal Sample Rate
+ * ---------+---------------------------+--------------------- 0 | 256Hz |
+ * 8kHz 1 | 188Hz | 1kHz 2 | 98Hz | 1kHz 3 | 42Hz | 1kHz 4 | 20Hz | 1kHz 5 |
+ * 10Hz | 1kHz 6 | 5Hz | 1kHz 7 | Reserved | Reserved
+ *
+ * @return DLFP bandwidth setting
+ * @see ITG3200_RA_DLPF_FS
+ * @see ITG3200_DF_DLPF_CFG_BIT
+ * @see ITG3200_DF_DLPF_CFG_LENGTH
+ */
+ public byte getDLPFBandwidth() {
+ return getRegisterBits(ITG3200_RA_DLPF_FS, ITG3200_DF_DLPF_CFG_BIT,
+ ITG3200_DF_DLPF_CFG_LENGTH);
+ }
+
+ /**
+ * Set digital low-pass filter bandwidth.
+ *
+ * @param bandwidth
+ * New DLFP bandwidth setting
+ * @see getDLPFBandwidth()
+ * @see ITG3200_DLPF_BW_256
+ * @see ITG3200_RA_DLPF_FS
+ * @see ITG3200_DF_DLPF_CFG_BIT
+ * @see ITG3200_DF_DLPF_CFG_LENGTH
+ */
+ public void setDLPFBandwidth(byte bandwidth) {
+ writeBits(ITG3200_RA_DLPF_FS, ITG3200_DF_DLPF_CFG_BIT,
+ ITG3200_DF_DLPF_CFG_LENGTH, bandwidth);
+ }
+
+ // INT_CFG register
+
+ /**
+ * Get interrupt logic level mode. Will be set 0 for active-high, 1 for
+ * active-low.
+ *
+ * @return Current interrupt mode (0=active-high, 1=active-low)
+ * @see ITG3200_RA_INT_CFG
+ * @see ITG3200_INTCFG_ACTL_BIT
+ */
+ public boolean getInterruptMode() {
+ return readBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_ACTL_BIT);
+ }
+
+ /**
+ * Set interrupt logic level mode.
+ *
+ * @param mode
+ * New interrupt mode (0=active-high, 1=active-low)
+ * @see getInterruptMode()
+ * @see ITG3200_RA_INT_CFG
+ * @see ITG3200_INTCFG_ACTL_BIT
+ */
+ public void setInterruptMode(boolean mode) {
+ writeBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_ACTL_BIT, mode);
+ }
+
+ /**
+ * Get interrupt drive mode. Will be set 0 for push-pull, 1 for open-drain.
+ *
+ * @return Current interrupt drive mode (0=push-pull, 1=open-drain)
+ * @see ITG3200_RA_INT_CFG
+ * @see ITG3200_INTCFG_OPEN_BIT
+ */
+ public boolean getInterruptDrive() {
+ return readBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_OPEN_BIT);
+ }
+
+ /**
+ * Set interrupt drive mode.
+ *
+ * @param drive
+ * New interrupt drive mode (0=push-pull, 1=open-drain)
+ * @see getInterruptDrive()
+ * @see ITG3200_RA_INT_CFG
+ * @see ITG3200_INTCFG_OPEN_BIT
+ */
+ public void setInterruptDrive(boolean drive) {
+ writeBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_OPEN_BIT, drive);
+ }
+
+ /**
+ * Get interrupt latch mode. Will be set 0 for 50us-pulse, 1 for
+ * latch-until-int-cleared.
+ *
+ * @return Current latch mode (0=50us-pulse, 1=latch-until-int-cleared)
+ * @see ITG3200_RA_INT_CFG
+ * @see ITG3200_INTCFG_LATCH_INT_EN_BIT
+ */
+ public boolean getInterruptLatch() {
+ return readBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_LATCH_INT_EN_BIT);
+ }
+
+ /**
+ * Set interrupt latch mode.
+ *
+ * @param latch
+ * New latch mode (0=50us-pulse, 1=latch-until-int-cleared)
+ * @see getInterruptLatch()
+ * @see ITG3200_RA_INT_CFG
+ * @see ITG3200_INTCFG_LATCH_INT_EN_BIT
+ */
+ public void setInterruptLatch(boolean latch) {
+ writeBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_LATCH_INT_EN_BIT, latch);
+ }
+
+ /**
+ * Get interrupt latch clear mode. Will be set 0 for status-read-only, 1 for
+ * any-register-read.
+ *
+ * @return Current latch clear mode (0=status-read-only, 1=any-register-read)
+ * @see ITG3200_RA_INT_CFG
+ * @see ITG3200_INTCFG_INT_ANYRD_2CLEAR_BIT
+ */
+ public boolean getInterruptLatchClear() {
+ return readBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_INT_ANYRD_2CLEAR_BIT);
+ }
+
+ /**
+ * Set interrupt latch clear mode.
+ *
+ * @param clear
+ * New latch clear mode (0=status-read-only, 1=any-register-read)
+ * @see getInterruptLatchClear()
+ * @see ITG3200_RA_INT_CFG
+ * @see ITG3200_INTCFG_INT_ANYRD_2CLEAR_BIT
+ */
+ public void setInterruptLatchClear(boolean clear) {
+ writeBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_INT_ANYRD_2CLEAR_BIT, clear);
+ }
+
+ /**
+ * Get "device ready" interrupt enabled setting. Will be set 0 for disabled, 1
+ * for enabled.
+ *
+ * @return Current interrupt enabled setting
+ * @see ITG3200_RA_INT_CFG
+ * @see ITG3200_INTCFG_ITG_RDY_EN_BIT
+ */
+ public boolean getIntDeviceReadyEnabled() {
+ return readBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_ITG_RDY_EN_BIT);
+ }
+
+ /**
+ * Set "device ready" interrupt enabled setting.
+ *
+ * @param enabled
+ * New interrupt enabled setting
+ * @see getIntDeviceReadyEnabled()
+ * @see ITG3200_RA_INT_CFG
+ * @see ITG3200_INTCFG_ITG_RDY_EN_BIT
+ */
+ public void setIntDeviceReadyEnabled(boolean enabled) {
+ writeBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_ITG_RDY_EN_BIT, enabled);
+ }
+
+ /**
+ * Get "data ready" interrupt enabled setting. Will be set 0 for disabled, 1
+ * for enabled.
+ *
+ * @return Current interrupt enabled setting
+ * @see ITG3200_RA_INT_CFG
+ * @see ITG3200_INTCFG_RAW_RDY_EN_BIT
+ */
+ public boolean getIntDataReadyEnabled() {
+ return readBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_RAW_RDY_EN_BIT);
+ }
+
+ /**
+ * Set "data ready" interrupt enabled setting.
+ *
+ * @param enabled
+ * New interrupt enabled setting
+ * @see getIntDataReadyEnabled()
+ * @see ITG3200_RA_INT_CFG
+ * @see ITG3200_INTCFG_RAW_RDY_EN_BIT
+ */
+ public void setIntDataReadyEnabled(boolean enabled) {
+ writeBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_RAW_RDY_EN_BIT, enabled);
+ }
+
+ // INT_STATUS register
+
+ /**
+ * Get Device Ready interrupt status. The ITG_RDY interrupt indicates that the
+ * PLL is ready and gyroscopic data can be read.
+ *
+ * @return Device Ready interrupt status
+ * @see ITG3200_RA_INT_STATUS
+ * @see ITG3200_INTSTAT_RAW_DATA_READY_BIT
+ */
+ public boolean getIntDeviceReadyStatus() {
+ return readBit(ITG3200_RA_INT_STATUS, ITG3200_INTSTAT_ITG_RDY_BIT);
+ }
+
+ /**
+ * Get Data Ready interrupt status. In normal use, the RAW_DATA_RDY interrupt
+ * is used to determine when new sensor data is available in and of the sensor
+ * registers (27 to 32).
+ *
+ * @return Data Ready interrupt status
+ * @see ITG3200_RA_INT_STATUS
+ * @see ITG3200_INTSTAT_RAW_DATA_READY_BIT
+ */
+ public boolean getIntDataReadyStatus() {
+ return readBit(ITG3200_RA_INT_STATUS, ITG3200_INTSTAT_RAW_DATA_READY_BIT);
+ }
+
+ // TEMP_OUT_* registers
+ /**
+ * Get current internal temperature.
+ *
+ * @return Temperature reading in 16-bit 2's complement format
+ * @see ITG3200_RA_TEMP_OUT_H
+ */
+ public short getTemperature() {
+ byte[] buf = new byte[2];
+ ReadI2CBuffer(ITG3200_RA_TEMP_OUT_H, 2, buf);
+ return (short) (((buf[0]) << 8) | buf[1]);
+ }
+
+ // GYRO_*OUT_* registers
+
+ public static class AllAxes {
+ public short XAxis;
+ public short YAxis;
+ public short ZAxis;
+ }
+
+ /**
+ * Get 3-axis gyroscope readings.
+ *
+ * @param x
+ * 16-bit signed integer container for X-axis rotation
+ * @param y
+ * 16-bit signed integer container for Y-axis rotation
+ * @param z
+ * 16-bit signed integer container for Z-axis rotation
+ * @see ITG3200_RA_GYRO_XOUT_H
+ */
+ public AllAxes getRotation() {
+ AllAxes data = new AllAxes();
+ byte[] buffer = new byte[6];
+ ReadI2CBuffer(ITG3200_RA_GYRO_XOUT_H, 6, buffer);
+ data.XAxis = (short) (((buffer[0]) << 8) | buffer[1]);
+ data.YAxis = (short) (((buffer[2]) << 8) | buffer[3]);
+ data.ZAxis = (short) (((buffer[4]) << 8) | buffer[5]);
+ return data;
+ }
+
+ public void ReadI2CBuffer(int registerAddress, int count, byte[] buffer) {
+ try {
+ m_i2c.read(registerAddress, count, buffer);
+ } catch (Throwable t) {
+ DriverStation.reportError("ERROR Unhandled exception in I2C Read: "
+ + t.toString() + " at " + Arrays.toString(t.getStackTrace()), false);
+ }
+ }
+
+ public short ReadShortFromRegister(byte register, int count) {
+ byte[] buffer = new byte[count];
+ ReadI2CBuffer(register, count, buffer);
+ return (short) (((buffer[0]) << 8) | buffer[1]);
+ }
+
+ /**
+ * Get X-axis gyroscope reading.
+ *
+ * @return X-axis rotation measurement in 16-bit 2's complement format
+ * @see ITG3200_RA_GYRO_XOUT_H
+ */
+ public short getRotationX() {
+ return ReadShortFromRegister(ITG3200_RA_GYRO_XOUT_H, 2);
+ }
+
+ /**
+ * Get Y-axis gyroscope reading.
+ *
+ * @return Y-axis rotation measurement in 16-bit 2's complement format
+ * @see ITG3200_RA_GYRO_YOUT_H
+ */
+ public short getRotationY() {
+ return ReadShortFromRegister(ITG3200_RA_GYRO_YOUT_H, 2);
+ }
+
+ /**
+ * Get Z-axis gyroscope reading.
+ *
+ * @return Z-axis rotation measurement in 16-bit 2's complement format
+ * @see ITG3200_RA_GYRO_ZOUT_H
+ */
+ public short getRotationZ() {
+ return ReadShortFromRegister(ITG3200_RA_GYRO_ZOUT_H, 0);
+ }
+
+ // PWR_MGM register
+
+ /**
+ * Trigger a full device reset. A small delay of ~50ms may be desirable after
+ * triggering a reset.
+ *
+ * @see ITG3200_RA_PWR_MGM
+ * @see ITG3200_PWR_H_RESET_BIT
+ */
+
+ public void reset() {
+ writeBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_H_RESET_BIT, true);
+ }
+
+ /**
+ * Get sleep mode status. Setting the SLEEP bit in the register puts the
+ * device into very low power sleep mode. In this mode, only the serial
+ * interface and internal registers remain active, allowing for a very low
+ * standby current. Clearing this bit puts the device back into normal mode.
+ * To save power, the individual standby selections for each of the gyros
+ * should be used if any gyro axis is not used by the application.
+ *
+ * @return Current sleep mode enabled status
+ * @see ITG3200_RA_PWR_MGM
+ * @see ITG3200_PWR_SLEEP_BIT
+ */
+ public boolean getSleepEnabled() {
+ return readBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_SLEEP_BIT);
+ }
+
+ /**
+ * Set sleep mode status.
+ *
+ * @param enabled
+ * New sleep mode enabled status
+ * @see getSleepEnabled()
+ * @see ITG3200_RA_PWR_MGM
+ * @see ITG3200_PWR_SLEEP_BIT
+ */
+ public void setSleepEnabled(boolean enabled) {
+ writeBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_SLEEP_BIT, enabled);
+ }
+
+ /**
+ * Get X-axis standby enabled status. If enabled, the X-axis will not gather
+ * or report data (or use power).
+ *
+ * @return Current X-axis standby enabled status
+ * @see ITG3200_RA_PWR_MGM
+ * @see ITG3200_PWR_STBY_XG_BIT
+ */
+ public boolean getStandbyXEnabled() {
+ return readBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_STBY_XG_BIT);
+ }
+
+ /**
+ * Set X-axis standby enabled status.
+ *
+ * @param New
+ * X-axis standby enabled status
+ * @see getStandbyXEnabled()
+ * @see ITG3200_RA_PWR_MGM
+ * @see ITG3200_PWR_STBY_XG_BIT
+ */
+ public void setStandbyXEnabled(boolean enabled) {
+ writeBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_STBY_XG_BIT, enabled);
+ }
+
+ /**
+ * Get Y-axis standby enabled status. If enabled, the Y-axis will not gather
+ * or report data (or use power).
+ *
+ * @return Current Y-axis standby enabled status
+ * @see ITG3200_RA_PWR_MGM
+ * @see ITG3200_PWR_STBY_YG_BIT
+ */
+ public boolean getStandbyYEnabled() {
+ return readBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_STBY_YG_BIT);
+ }
+
+ /**
+ * Set Y-axis standby enabled status.
+ *
+ * @param New
+ * Y-axis standby enabled status
+ * @see getStandbyYEnabled()
+ * @see ITG3200_RA_PWR_MGM
+ * @see ITG3200_PWR_STBY_YG_BIT
+ */
+ public void setStandbyYEnabled(boolean enabled) {
+ writeBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_STBY_YG_BIT, enabled);
+ }
+
+ /**
+ * Get Z-axis standby enabled status. If enabled, the Z-axis will not gather
+ * or report data (or use power).
+ *
+ * @return Current Z-axis standby enabled status
+ * @see ITG3200_RA_PWR_MGM
+ * @see ITG3200_PWR_STBY_ZG_BIT
+ */
+ public boolean getStandbyZEnabled() {
+ return readBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_STBY_ZG_BIT);
+ }
+
+ /**
+ * Set Z-axis standby enabled status.
+ *
+ * @param New
+ * Z-axis standby enabled status
+ * @see getStandbyZEnabled()
+ * @see ITG3200_RA_PWR_MGM
+ * @see ITG3200_PWR_STBY_ZG_BIT
+ */
+ public void setStandbyZEnabled(boolean enabled) {
+ writeBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_STBY_ZG_BIT, enabled);
+ }
+
+ /**
+ * Get clock source setting.
+ *
+ * @return Current clock source setting
+ * @see ITG3200_RA_PWR_MGM
+ * @see ITG3200_PWR_CLK_SEL_BIT
+ * @see ITG3200_PWR_CLK_SEL_LENGTH
+ */
+ public byte getClockSource() {
+ byte[] buf = new byte[1];
+ ReadI2CBuffer(ITG3200_RA_PWR_MGM, 1, buf);
+ // I2Cdev::readBits(devAddr, ITG3200_RA_PWR_MGM, ITG3200_PWR_CLK_SEL_BIT,
+ // ITG3200_PWR_CLK_SEL_LENGTH, buffer);
+ return (byte) (buf[0] & ITG3200_PWR_CLK_SEL_BIT);
+ }
+
+ /**
+ * Set clock source setting. On power up, the ITG-3200 defaults to the
+ * internal oscillator. It is highly recommended that the device is configured
+ * to use one of the gyros (or an external clock) as the clock reference, due
+ * to the improved stability.
+ *
+ * The CLK_SEL setting determines the device clock source as follows:
+ *
+ * CLK_SEL | Clock Source --------+-------------------------------------- 0 |
+ * Internal oscillator 1 | PLL with X Gyro reference 2 | PLL with Y Gyro
+ * reference 3 | PLL with Z Gyro reference 4 | PLL with external 32.768kHz
+ * reference 5 | PLL with external 19.2MHz reference 6 | Reserved 7 | Reserved
+ *
+ * @param source
+ * New clock source setting
+ * @see getClockSource()
+ * @see ITG3200_RA_PWR_MGM
+ * @see ITG3200_PWR_CLK_SEL_BIT
+ * @see ITG3200_PWR_CLK_SEL_LENGTH
+ */
+ public void setClockSource(byte source) {
+ writeBits(ITG3200_RA_PWR_MGM, ITG3200_PWR_CLK_SEL_BIT,
+ ITG3200_PWR_CLK_SEL_LENGTH, source);
+ }
+
+ private ITable m_table;
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void initTable(ITable subtable) {
+ m_table = subtable;
+ updateTable();
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public ITable getTable() {
+ return m_table;
+ }
+
+ /**
+ * {@inheritDoc}
+ */
+ @Override
+ public void updateTable() {
+ if (m_table != null) {
+ m_table.putNumber("GyroX", getRotationX());
+ m_table.putNumber("GyroY", getRotationY());
+ m_table.putNumber("GyroZ", getRotationZ());
+ m_table.putNumber("GyroPIDValue", pidGet());
+ }
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see edu.wpi.first.wpilibj.Sendable#getSmartDashboardType()
+ */
+ @Override
+ public String getSmartDashboardType() {
+ return "Gyro";
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see
+ * edu.wpi.first.wpilibj.livewindow.LiveWindowSendable#startLiveWindowMode()
+ */
+ @Override
+ public void startLiveWindowMode() {
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see
+ * edu.wpi.first.wpilibj.livewindow.LiveWindowSendable#stopLiveWindowMode()
+ */
+ @Override
+ public void stopLiveWindowMode() {
+ }
+
+ /*
+ * (non-Javadoc)
+ *
+ * @see edu.wpi.first.wpilibj.PIDSource#pidGet()
+ */
+ @Override
+ public double pidGet() {
+ // TODO We likely want to return one of the axes based on a setup option.
+ AllAxes var = getRotation();
+ double result = Math.cbrt(
+ var.XAxis * var.XAxis + var.YAxis * var.YAxis + var.ZAxis * var.ZAxis);
+ return result;
+ // return 0;
+ }
+
+ public static final byte ITG3200_ADDRESS_AD0_LOW = 0x68; // address pin low
+ // (GND), default for
+ // SparkFun IMU
+ // Digital Combo
+ // board
+ public static final byte ITG3200_ADDRESS_AD0_HIGH = 0x69; // address pin high
+ // (VCC), default
+ // for SparkFun
+ // ITG-3200 Breakout
+ // board
+
+ public static final int ITG3200_SPARKFUN_ADDRES = 0xD2;
+
+ public static final int ITG3200_DEFAULT_ADDRESS = ITG3200_ADDRESS_AD0_LOW; // ITG3200_ADDRESS_AD0_HIGH;
+
+ public static final byte ITG3200_RA_WHO_AM_I = 0x00;
+ public static final byte ITG3200_RA_SMPLRT_DIV = 0x15;
+ public static final byte ITG3200_RA_DLPF_FS = 0x16;
+ public static final byte ITG3200_RA_INT_CFG = 0x17;
+ public static final byte ITG3200_RA_INT_STATUS = 0x1A;
+ public static final byte ITG3200_RA_TEMP_OUT_H = 0x1B;
+ public static final byte ITG3200_RA_TEMP_OUT_L = 0x1C;
+ public static final byte ITG3200_RA_GYRO_XOUT_H = 0x1D;
+ public static final byte ITG3200_RA_GYRO_XOUT_L = 0x1E;
+ public static final byte ITG3200_RA_GYRO_YOUT_H = 0x1F;
+ public static final byte ITG3200_RA_GYRO_YOUT_L = 0x20;
+ public static final byte ITG3200_RA_GYRO_ZOUT_H = 0x21;
+ public static final byte ITG3200_RA_GYRO_ZOUT_L = 0x22;
+ public static final byte ITG3200_RA_PWR_MGM = 0x3E;
+
+ public static final short ITG3200_DEVID_BIT = 6;
+ public static final short ITG3200_DEVID_LENGTH = 6;
+
+ public static final short ITG3200_DF_FS_SEL_BIT = 4;
+ public static final short ITG3200_DF_FS_SEL_LENGTH = 2;
+ public static final short ITG3200_DF_DLPF_CFG_BIT = 2;
+ public static final short ITG3200_DF_DLPF_CFG_LENGTH = 3;
+
+ public static final byte ITG3200_FULLSCALE_2000 = 0x03;
+
+ public static final byte ITG3200_DLPF_BW_256 = 0x00;
+ public static final byte ITG3200_DLPF_BW_188 = 0x01;
+ public static final byte ITG3200_DLPF_BW_98 = 0x02;
+ public static final byte ITG3200_DLPF_BW_42 = 0x03;
+ public static final byte ITG3200_DLPF_BW_20 = 0x04;
+ public static final byte ITG3200_DLPF_BW_10 = 0x05;
+ public static final byte ITG3200_DLPF_BW_5 = 0x06;
+
+ public static final byte ITG3200_INTCFG_ACTL_BIT = 7;
+ public static final byte ITG3200_INTCFG_OPEN_BIT = 6;
+ public static final byte ITG3200_INTCFG_LATCH_INT_EN_BIT = 5;
+ public static final byte ITG3200_INTCFG_INT_ANYRD_2CLEAR_BIT = 4;
+ public static final byte ITG3200_INTCFG_ITG_RDY_EN_BIT = 2;
+ public static final byte ITG3200_INTCFG_RAW_RDY_EN_BIT = 0;
+
+ public static final byte ITG3200_INTMODE_ACTIVEHIGH = 0x00;
+ public static final byte ITG3200_INTMODE_ACTIVELOW = 0x01;
+
+ public static final byte ITG3200_INTDRV_PUSHPULL = 0x00;
+ public static final byte ITG3200_INTDRV_OPENDRAIN = 0x01;
+
+ public static final byte ITG3200_INTLATCH_50USPULSE = 0x00;
+ public static final byte ITG3200_INTLATCH_WAITCLEAR = 0x01;
+
+ public static final byte ITG3200_INTCLEAR_STATUSREAD = 0x00;
+ public static final byte ITG3200_INTCLEAR_ANYREAD = 0x01;
+
+ public static final byte ITG3200_INTSTAT_ITG_RDY_BIT = 2;
+ public static final byte ITG3200_INTSTAT_RAW_DATA_READY_BIT = 0;
+
+ public static final byte ITG3200_PWR_H_RESET_BIT = 7;
+ public static final byte ITG3200_PWR_SLEEP_BIT = 6;
+ public static final byte ITG3200_PWR_STBY_XG_BIT = 5;
+ public static final byte ITG3200_PWR_STBY_YG_BIT = 4;
+ public static final byte ITG3200_PWR_STBY_ZG_BIT = 3;
+ public static final byte ITG3200_PWR_CLK_SEL_BIT = 2;
+ public static final byte ITG3200_PWR_CLK_SEL_LENGTH = 3;
+
+ public static final byte ITG3200_CLOCK_INTERNAL = 0x00;
+ public static final byte ITG3200_CLOCK_PLL_XGYRO = 0x01;
+ public static final byte ITG3200_CLOCK_PLL_YGYRO = 0x02;
+ public static final byte ITG3200_CLOCK_PLL_ZGYRO = 0x03;
+ public static final byte ITG3200_CLOCK_PLL_EXT32K = 0x04;
+ public static final byte ITG3200_CLOCK_PLL_EXT19M = 0x05;
+
+ @Override
+ public void setPIDSourceType(PIDSourceType pidSource) {
+ // TODO Auto-generated method stub
+
+ }
+
+ @Override
+ public PIDSourceType getPIDSourceType() {
+ // TODO Auto-generated method stub
+ return null;
+ }
+
+}
// Gyro stuff
private final static double NANOSECONDS_PER_SECOND = 1000000000;
short rawValue;
- public FirebotGyro gyro;
+ public GyroClass gyro;
double degreesIncreased;
double degrees;
@Override
public void robotInit() {
// driveTrain = new DriveTrain();
- gyro = new FirebotGyro(I2C.Port.kOnboard, (byte) 0x68);
+ gyro = new GyroClass(I2C.Port.kOnboard, (byte) 0x68);
// oi = new OI();
shooter = new Shooter();
projects / 3501 / stronghold-2016 / commitdiff