challenge-bot.com Git - 3501/stronghold-2016/blob - src/org/usfirst/frc/team3501/robot/GyroClass.java

   1 // GyroITG3200 I2C device class file
   2 // Based on InvenSense ITG-3200 datasheet rev. 1.4, 3/30/2010 (PS-ITG-3200A-00-01.4)
   3 // Original work by 7/31/2011 by Jeff Rowberg <jeff@rowberg.net>
   4 // Java implementation for First Robotics Competition Team 2521 using WPILibj
   5 // 1/27/2015 by Joe Bussell <joe dot bussell at gmail dot com>
   6 // Updates should (hopefully) always be available at https://github.com/bussell
   7 //
   8 // Changelog:
   9 //     2011-07-31 - initial release
  10 //     2015-01-30 - Java FRC revision
  11
  12 /* ============================================
  13 GyroITG3200 device library code is placed under the MIT license
  14 Copyright (c) 2011 by Jeff Rowberg
  15 Copyright (c) 2015 Joe Bussell
  16 Permission is hereby granted, free of charge, to any person obtaining a copy
  17 of this software and associated documentation files (the "Software"), to deal
  18 in the Software without restriction, including without limitation the rights
  19 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  20 copies of the Software, and to permit persons to whom the Software is
  21 furnished to do so, subject to the following conditions:
  22 The above copyright notice and this permission notice shall be included in
  23 all copies or substantial portions of the Software.
  24 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  25 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  26 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  27 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  28 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  29 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  30 THE SOFTWARE.
  31 ===============================================
  32 */
  33
  34 package org.usfirst.frc.team3501.robot;
  35
  36 import java.util.Arrays;
  37
  38 import edu.wpi.first.wpilibj.DriverStation;
  39 import edu.wpi.first.wpilibj.I2C;
  40 import edu.wpi.first.wpilibj.PIDSource;
  41 import edu.wpi.first.wpilibj.PIDSourceType;
  42 import edu.wpi.first.wpilibj.SensorBase;
  43 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
  44 import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
  45 import edu.wpi.first.wpilibj.tables.ITable;
  46
  47 /**
  48  * @author Joe Bussell Team 2521 Mentor With thanks to the c++ version authors
  49  *         at: https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/ITG3200
  50  *
  51  */
  52 public class GyroClass extends SensorBase
  53     implements PIDSource, LiveWindowSendable {
  54   int devAddr;
  55   byte buffer[] = new byte[7];
  56
  57   public static final boolean DEBUG = true;
  58
  59   I2C m_i2c;
  60
  61   /**
  62    * Default constructor, uses default I2C address.
  63    *
  64    * @see ITG3200_DEFAULT_ADDRESS
  65    */
  66   public GyroClass(I2C.Port port) {
  67     devAddr = ITG3200_DEFAULT_ADDRESS;
  68
  69     m_i2c = new I2C(port, devAddr);
  70
  71     // TODO: This report is incorrect. Need to create instance for I2C ITG3200
  72     // Gyro
  73     // UsageReporting.report( tResourceType.kResourceType_I2C, tInstances.?? );
  74     LiveWindow.addSensor("ITG3200_Gyro_I2C", port.getValue(), this);
  75   }
  76
  77   /**
  78    * Specific address constructor.
  79    *
  80    * @param address
  81    *          I2C address
  82    * @see ITG3200_DEFAULT_ADDRESS
  83    * @see ITG3200_ADDRESS_AD0_LOW
  84    * @see ITG3200_ADDRESS_AD0_HIGH
  85    */
  86   public GyroClass(I2C.Port port, byte address) {
  87     devAddr = address;
  88
  89     m_i2c = new I2C(port, address);
  90
  91     // TODO: This report is incorrect. Need to create instance for I2C ITG3200
  92     // Gyro
  93     // UsageReporting.report( tResourceType.kResourceType_I2C, tInstances.?? );
  94     LiveWindow.addSensor("ITG3200_Gyro_I2C", port.getValue(), this);
  95   }
  96
  97   /**
  98    * Power on and prepare for general usage. This will activate the gyroscope,
  99    * so be sure to adjust the power settings after you call this method if you
 100    * want it to enter standby mode, or another less demanding mode of operation.
 101    * This also sets the gyroscope to use the X-axis gyro for a clock source.
 102    * Note that it doesn't have any delays in the routine, which means you might
 103    * want to add ~50ms to be safe if you happen to need to read gyro data
 104    * immediately after initialization. The data will flow in either case, but
 105    * the first reports may have higher error offsets.
 106    */
 107   public void initialize() {
 108     if (!testConnection()) {
 109       DriverStation.reportError("Test connection failed!", false);
 110     }
 111     setFullScaleRange(ITG3200_FULLSCALE_2000);
 112     setClockSource(ITG3200_CLOCK_PLL_XGYRO);
 113     setIntDeviceReadyEnabled(true);
 114     setIntDataReadyEnabled(true);
 115   }
 116
 117   /**
 118    * Verify the I2C connection. Make sure the device is connected and responds
 119    * as expected.
 120    *
 121    * @return True if connection is valid, false otherwise
 122    */
 123   public boolean testConnection() {
 124     return getDeviceID() == 0b110100;
 125   }
 126
 127   private void writeBit(int register, byte bit, boolean value) {
 128     byte[] buf = new byte[1];
 129     ReadI2CBuffer(register, 1, buf);
 130     byte newValue = (byte) (value ? (buf[0] | (1 << bit))
 131         : (buf[0] & ~(1 << bit)));
 132     writeI2CBuffer(register, newValue);
 133
 134     if (DEBUG) {
 135       ReadI2CBuffer(register, 1, buf);
 136       if (newValue != buf[0]) {
 137         System.out.println("Expected " + newValue + " seeing " + buf[0]);
 138       }
 139     }
 140   }
 141
 142   // this routine should update the original byte with the new data properly
 143   // shifted to the correct bit location
 144   public static byte updateByte(byte original, int bit, int numBits,
 145       byte value) {
 146     if (numBits > 7) {
 147       throw new IllegalArgumentException(
 148           "This routine is intended to use 8-bit bytes. \n Value: "
 149               + GetBinaryString(value) + "\n Number bits: " + numBits);
 150     }
 151     if (bit > 7) {
 152       throw new IllegalArgumentException(
 153           "This routine is intended to use 8-bit bytes. \n Value: "
 154               + GetBinaryString(value) + "\n Bit: " + bit);
 155     }
 156     if (bit < numBits - 1) {
 157       throw new IllegalArgumentException(
 158           "This routine is intended to use 8-bit bytes. \n Value: "
 159               + GetBinaryString(value) + "\n Bit: " + bit + "\n Number bits: "
 160               + numBits);
 161     }
 162     if (value > Math.pow(2, numBits)) {
 163       throw new IllegalArgumentException(
 164           "Cannot encode a number this big using the number of bits requested \n Value: "
 165               + GetBinaryString(value) + "\n Number bits: " + numBits);
 166     }
 167     if (bit < 0 || numBits < 0 || value < 0) {
 168       throw new IllegalArgumentException(
 169           "This routine is intended to use 8-bit bytes. "
 170               + "\n All inputs should be greater than 0. " + "\n Value: "
 171               + GetBinaryString(value) + "\n Bit: " + bit + "\n Number bits: "
 172               + numBits);
 173
 174     }
 175     byte mask = getMask(bit, numBits);
 176     byte maskedOriginal = (byte) ((original & mask) & 0xFF);
 177     byte shiftedValue = (byte) ((value << (1 + bit - numBits)) & 0xFF);
 178     byte result = (byte) ((shiftedValue | maskedOriginal) & 0xFF);
 179     /*
 180      * // Debug code System.out.println( "bit            = " + bit );
 181      * System.out.println( "num bits       = " + numBits ); System.out.println(
 182      * "original       = " + GetBinaryString(original) ); System.out.println(
 183      * "        Value  = " + GetBinaryString(value) ); System.out.println( "" );
 184      * System.out.println( "mask           = " + GetBinaryString(mask) );
 185      * System.out.println( "maskedOriginal = " + GetBinaryString(maskedOriginal)
 186      * ); System.out.println( "shifted Value  = " +
 187      * GetBinaryString(shiftedValue) ); System.out.println( "" );
 188      * System.out.println( "result         = " + GetBinaryString(result) );
 189      */
 190     return result;
 191   }
 192
 193   public static String GetBinaryString(byte value) {
 194     return String.format("%8s", Integer.toBinaryString(value & 0xFF))
 195         .replace(' ', '0');
 196   }
 197
 198   public boolean writeI2CBuffer(int registerAddress, int data) {
 199     boolean retVal = false;
 200     try {
 201       retVal = m_i2c.write(registerAddress, data);
 202       if (DEBUG) {
 203         byte[] buf = new byte[1];
 204         ReadI2CBuffer(registerAddress, 1, buf);
 205         if (data != buf[0]) {
 206           DriverStation.reportError(
 207               "Expected " + data + "\nseeing " + buf[0] + "\n", false);
 208         }
 209       }
 210     } catch (Throwable t) {
 211       DriverStation.reportError("ERROR Unhandled exception: " + t.toString()
 212           + " at " + Arrays.toString(t.getStackTrace()), false);
 213     }
 214     return retVal;
 215   }
 216
 217   //
 218   // I2Cdev::writeBits(devAddr, ITG3200_RA_WHO_AM_I, ITG3200_DEVID_BIT,
 219   // ITG3200_DEVID_LENGTH, id);
 220   private void writeBits(int register, int bit, int numBits, byte value) {
 221     try {
 222       byte[] rawData = new byte[1];
 223       ReadI2CBuffer(register, 1, rawData);
 224       byte newValue = updateByte(rawData[0], bit, numBits, value);
 225       writeI2CBuffer(register, newValue);
 226     } catch (Throwable t) {
 227       DriverStation.reportError("ERROR Unhandled exception: " + t.toString()
 228           + " at " + Arrays.toString(t.getStackTrace()), false);
 229     }
 230   }
 231
 232   private boolean readBit(int register, byte bit) {
 233     byte buf[] = new byte[1];
 234     ReadI2CBuffer(register, 1, buf);
 235     return (buf[0] & bit) != 0;
 236   }
 237
 238   // Get n bits from the byte to form a byte slice
 239   private static byte getBits(byte bitField, int bit, int numBits) {
 240
 241     if (numBits > 7) {
 242       throw new IllegalArgumentException(
 243           "This routine is intended to use 8-bit bytes." + "\n Number bits: "
 244               + numBits);
 245     }
 246     if (bit > 7) {
 247       throw new IllegalArgumentException(
 248           "This routine is intended to use 8-bit bytes. " + "\n Bit: " + bit);
 249     }
 250     if (bit < numBits - 1) {
 251       throw new IllegalArgumentException(
 252           "This routine is intended to use 8-bit bytes. " + "\n Bit: " + bit
 253               + "\n Number bits: " + numBits);
 254     }
 255     if (bit < 0 || numBits < 0) {
 256       throw new IllegalArgumentException(
 257           "This routine is intended to use 8-bit bytes. "
 258               + "\n All inputs should be greater than 0. " + "\n Bit: " + bit
 259               + "\n Number bits: " + numBits);
 260
 261     }
 262     byte result = 0;
 263
 264     byte mask = (byte) (~getMask(bit, numBits) & 0xFF);
 265     byte maskedInput = (byte) ((bitField & mask) & 0xFF);
 266     result = (byte) ((maskedInput >>> (1 + bit - numBits)) & 0xFF);
 267
 268     /*
 269      * // Debug code System.out.println( "mask           = " +
 270      * GetBinaryString(mask) ); System.out.println( "maskedInput    = " +
 271      * GetBinaryString(maskedInput) ); System.out.println( "result         = " +
 272      * GetBinaryString(result) );
 273      */
 274
 275     return result;
 276   }
 277
 278   // Gets the bit mask for the given bit and number of bits
 279   private static byte getMask(int bit, int numBits) {
 280     int newMask = 0;
 281     for (int i = 0; i <= 7; i++) {
 282       if (i > bit || i <= bit - numBits) {
 283         // set the mask bit
 284         newMask = (int) (newMask + Math.pow(2, i));
 285       }
 286     }
 287     byte mask = (byte) (newMask & 0xFF);
 288     return mask;
 289   }
 290
 291   private byte getRegisterByte(int register) {
 292     byte[] buf = new byte[1];
 293     ReadI2CBuffer(register, 1, buf);
 294     return buf[0];
 295   }
 296
 297   /**
 298    * Get specified bits from the specified register. Form a new value from a
 299    * byte (0b10110100) get the 3rd bit request 6 bits and you should get a new
 300    * byte (0b00110100).
 301    */
 302   private byte getRegisterBits(int register, int bit, int numBits) {
 303     byte containingByte = getRegisterByte(register);
 304     return getBits(containingByte, bit, numBits);
 305   }
 306
 307   // WHO_AM_I register
 308   /**
 309    * Get Device ID. This register is used to verify the identity of the device
 310    * (0b110100).
 311    *
 312    * @return Device ID (should be 0x34, 52 dec, 64 oct)
 313    * @see ITG3200_RA_WHO_AM_I
 314    * @see ITG3200_RA_DEVID_BIT
 315    * @see ITG3200_RA_DEVID_LENGTH
 316    */
 317   public byte getDeviceID() {
 318     return getRegisterBits(ITG3200_RA_WHO_AM_I, ITG3200_DEVID_BIT,
 319         ITG3200_DEVID_LENGTH);
 320   }
 321
 322   /**
 323    * Set Device ID. Write a new ID into the WHO_AM_I register (no idea why this
 324    * should ever be necessary though).
 325    *
 326    * @param id
 327    *          New device ID to set.
 328    * @see getDeviceID()
 329    * @see ITG3200_RA_WHO_AM_I
 330    * @see ITG3200_RA_DEVID_BIT
 331    * @see ITG3200_RA_DEVID_LENGTH
 332    */
 333   public void setDeviceID(byte id) {
 334     writeBits(ITG3200_RA_WHO_AM_I, ITG3200_DEVID_BIT, ITG3200_DEVID_LENGTH, id);
 335   }
 336
 337   // SMPLRT_DIV register
 338   /**
 339    * Get sample rate. This register determines the sample rate of the ITG-3200
 340    * gyros. The gyros' outputs are sampled internally at either 1kHz or 8kHz,
 341    * determined by the DLPF_CFG setting (see register 22). This sampling is then
 342    * filtered digitally and delivered into the sensor registers after the number
 343    * of cycles determined by this register. The sample rate is given by the
 344    * following formula:
 345    *
 346    * F_sample = F_internal / (divider+1), where F_internal is either 1kHz or
 347    * 8kHz
 348    *
 349    * As an example, if the internal sampling is at 1kHz, then setting this
 350    * register to 7 would give the following:
 351    *
 352    * F_sample = 1kHz / (7 + 1) = 125Hz, or 8ms per sample
 353    *
 354    * @return Current sample rate
 355    * @see setDLPFBandwidth()
 356    * @see ITG3200_RA_SMPLRT_DIV
 357    */
 358   public byte getRate() {
 359     return getRegisterByte(ITG3200_RA_SMPLRT_DIV);
 360   }
 361
 362   /**
 363    * Set sample rate.
 364    *
 365    * @param rate
 366    *          New sample rate
 367    * @see getRate()
 368    * @see setDLPFBandwidth()
 369    * @see ITG3200_RA_SMPLRT_DIV
 370    */
 371   public void setRate(byte rate) {
 372     writeI2CBuffer(ITG3200_RA_SMPLRT_DIV, rate);
 373   }
 374
 375   // DLPF_FS register
 376   /**
 377    * Full-scale range. The FS_SEL parameter allows setting the full-scale range
 378    * of the gyro sensors, as described in the table below. The power-on-reset
 379    * value of FS_SEL is 00h. Set to 03h for proper operation.
 380    *
 381    * 0 = Reserved 1 = Reserved 2 = Reserved 3 = +/- 2000 degrees/sec
 382    *
 383    * @return Current full-scale range setting
 384    * @see ITG3200_FULLSCALE_2000
 385    * @see ITG3200_RA_DLPF_FS
 386    * @see ITG3200_DF_FS_SEL_BIT
 387    * @see ITG3200_DF_FS_SEL_LENGTH
 388    */
 389   public byte getFullScaleRange() {
 390     return getRegisterBits(ITG3200_RA_DLPF_FS, ITG3200_DF_FS_SEL_BIT,
 391         ITG3200_DF_FS_SEL_LENGTH);
 392   }
 393
 394   /**
 395    * Set full-scale range setting.
 396    *
 397    * @param range
 398    *          New full-scale range value
 399    * @see getFullScaleRange()
 400    * @see ITG3200_FULLSCALE_2000
 401    * @see ITG3200_RA_DLPF_FS
 402    * @see ITG3200_DF_FS_SEL_BIT
 403    * @see ITG3200_DF_FS_SEL_LENGTH
 404    */
 405   public void setFullScaleRange(byte range) {
 406     writeBits(ITG3200_RA_DLPF_FS, ITG3200_DF_FS_SEL_BIT,
 407         ITG3200_DF_FS_SEL_LENGTH, range);
 408   }
 409
 410   /**
 411    * Get digital low-pass filter bandwidth. The DLPF_CFG parameter sets the
 412    * digital low pass filter configuration. It also determines the internal
 413    * sampling rate used by the device as shown in the table below.
 414    *
 415    * DLPF_CFG | Low-Pass Filter Bandwidth | Internal Sample Rate
 416    * ---------+---------------------------+--------------------- 0 | 256Hz |
 417    * 8kHz 1 | 188Hz | 1kHz 2 | 98Hz | 1kHz 3 | 42Hz | 1kHz 4 | 20Hz | 1kHz 5 |
 418    * 10Hz | 1kHz 6 | 5Hz | 1kHz 7 | Reserved | Reserved
 419    *
 420    * @return DLFP bandwidth setting
 421    * @see ITG3200_RA_DLPF_FS
 422    * @see ITG3200_DF_DLPF_CFG_BIT
 423    * @see ITG3200_DF_DLPF_CFG_LENGTH
 424    */
 425   public byte getDLPFBandwidth() {
 426     return getRegisterBits(ITG3200_RA_DLPF_FS, ITG3200_DF_DLPF_CFG_BIT,
 427         ITG3200_DF_DLPF_CFG_LENGTH);
 428   }
 429
 430   /**
 431    * Set digital low-pass filter bandwidth.
 432    *
 433    * @param bandwidth
 434    *          New DLFP bandwidth setting
 435    * @see getDLPFBandwidth()
 436    * @see ITG3200_DLPF_BW_256
 437    * @see ITG3200_RA_DLPF_FS
 438    * @see ITG3200_DF_DLPF_CFG_BIT
 439    * @see ITG3200_DF_DLPF_CFG_LENGTH
 440    */
 441   public void setDLPFBandwidth(byte bandwidth) {
 442     writeBits(ITG3200_RA_DLPF_FS, ITG3200_DF_DLPF_CFG_BIT,
 443         ITG3200_DF_DLPF_CFG_LENGTH, bandwidth);
 444   }
 445
 446   // INT_CFG register
 447
 448   /**
 449    * Get interrupt logic level mode. Will be set 0 for active-high, 1 for
 450    * active-low.
 451    *
 452    * @return Current interrupt mode (0=active-high, 1=active-low)
 453    * @see ITG3200_RA_INT_CFG
 454    * @see ITG3200_INTCFG_ACTL_BIT
 455    */
 456   public boolean getInterruptMode() {
 457     return readBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_ACTL_BIT);
 458   }
 459
 460   /**
 461    * Set interrupt logic level mode.
 462    *
 463    * @param mode
 464    *          New interrupt mode (0=active-high, 1=active-low)
 465    * @see getInterruptMode()
 466    * @see ITG3200_RA_INT_CFG
 467    * @see ITG3200_INTCFG_ACTL_BIT
 468    */
 469   public void setInterruptMode(boolean mode) {
 470     writeBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_ACTL_BIT, mode);
 471   }
 472
 473   /**
 474    * Get interrupt drive mode. Will be set 0 for push-pull, 1 for open-drain.
 475    *
 476    * @return Current interrupt drive mode (0=push-pull, 1=open-drain)
 477    * @see ITG3200_RA_INT_CFG
 478    * @see ITG3200_INTCFG_OPEN_BIT
 479    */
 480   public boolean getInterruptDrive() {
 481     return readBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_OPEN_BIT);
 482   }
 483
 484   /**
 485    * Set interrupt drive mode.
 486    *
 487    * @param drive
 488    *          New interrupt drive mode (0=push-pull, 1=open-drain)
 489    * @see getInterruptDrive()
 490    * @see ITG3200_RA_INT_CFG
 491    * @see ITG3200_INTCFG_OPEN_BIT
 492    */
 493   public void setInterruptDrive(boolean drive) {
 494     writeBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_OPEN_BIT, drive);
 495   }
 496
 497   /**
 498    * Get interrupt latch mode. Will be set 0 for 50us-pulse, 1 for
 499    * latch-until-int-cleared.
 500    *
 501    * @return Current latch mode (0=50us-pulse, 1=latch-until-int-cleared)
 502    * @see ITG3200_RA_INT_CFG
 503    * @see ITG3200_INTCFG_LATCH_INT_EN_BIT
 504    */
 505   public boolean getInterruptLatch() {
 506     return readBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_LATCH_INT_EN_BIT);
 507   }
 508
 509   /**
 510    * Set interrupt latch mode.
 511    *
 512    * @param latch
 513    *          New latch mode (0=50us-pulse, 1=latch-until-int-cleared)
 514    * @see getInterruptLatch()
 515    * @see ITG3200_RA_INT_CFG
 516    * @see ITG3200_INTCFG_LATCH_INT_EN_BIT
 517    */
 518   public void setInterruptLatch(boolean latch) {
 519     writeBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_LATCH_INT_EN_BIT, latch);
 520   }
 521
 522   /**
 523    * Get interrupt latch clear mode. Will be set 0 for status-read-only, 1 for
 524    * any-register-read.
 525    *
 526    * @return Current latch clear mode (0=status-read-only, 1=any-register-read)
 527    * @see ITG3200_RA_INT_CFG
 528    * @see ITG3200_INTCFG_INT_ANYRD_2CLEAR_BIT
 529    */
 530   public boolean getInterruptLatchClear() {
 531     return readBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_INT_ANYRD_2CLEAR_BIT);
 532   }
 533
 534   /**
 535    * Set interrupt latch clear mode.
 536    *
 537    * @param clear
 538    *          New latch clear mode (0=status-read-only, 1=any-register-read)
 539    * @see getInterruptLatchClear()
 540    * @see ITG3200_RA_INT_CFG
 541    * @see ITG3200_INTCFG_INT_ANYRD_2CLEAR_BIT
 542    */
 543   public void setInterruptLatchClear(boolean clear) {
 544     writeBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_INT_ANYRD_2CLEAR_BIT, clear);
 545   }
 546
 547   /**
 548    * Get "device ready" interrupt enabled setting. Will be set 0 for disabled, 1
 549    * for enabled.
 550    *
 551    * @return Current interrupt enabled setting
 552    * @see ITG3200_RA_INT_CFG
 553    * @see ITG3200_INTCFG_ITG_RDY_EN_BIT
 554    */
 555   public boolean getIntDeviceReadyEnabled() {
 556     return readBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_ITG_RDY_EN_BIT);
 557   }
 558
 559   /**
 560    * Set "device ready" interrupt enabled setting.
 561    *
 562    * @param enabled
 563    *          New interrupt enabled setting
 564    * @see getIntDeviceReadyEnabled()
 565    * @see ITG3200_RA_INT_CFG
 566    * @see ITG3200_INTCFG_ITG_RDY_EN_BIT
 567    */
 568   public void setIntDeviceReadyEnabled(boolean enabled) {
 569     writeBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_ITG_RDY_EN_BIT, enabled);
 570   }
 571
 572   /**
 573    * Get "data ready" interrupt enabled setting. Will be set 0 for disabled, 1
 574    * for enabled.
 575    *
 576    * @return Current interrupt enabled setting
 577    * @see ITG3200_RA_INT_CFG
 578    * @see ITG3200_INTCFG_RAW_RDY_EN_BIT
 579    */
 580   public boolean getIntDataReadyEnabled() {
 581     return readBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_RAW_RDY_EN_BIT);
 582   }
 583
 584   /**
 585    * Set "data ready" interrupt enabled setting.
 586    *
 587    * @param enabled
 588    *          New interrupt enabled setting
 589    * @see getIntDataReadyEnabled()
 590    * @see ITG3200_RA_INT_CFG
 591    * @see ITG3200_INTCFG_RAW_RDY_EN_BIT
 592    */
 593   public void setIntDataReadyEnabled(boolean enabled) {
 594     writeBit(ITG3200_RA_INT_CFG, ITG3200_INTCFG_RAW_RDY_EN_BIT, enabled);
 595   }
 596
 597   // INT_STATUS register
 598
 599   /**
 600    * Get Device Ready interrupt status. The ITG_RDY interrupt indicates that the
 601    * PLL is ready and gyroscopic data can be read.
 602    *
 603    * @return Device Ready interrupt status
 604    * @see ITG3200_RA_INT_STATUS
 605    * @see ITG3200_INTSTAT_RAW_DATA_READY_BIT
 606    */
 607   public boolean getIntDeviceReadyStatus() {
 608     return readBit(ITG3200_RA_INT_STATUS, ITG3200_INTSTAT_ITG_RDY_BIT);
 609   }
 610
 611   /**
 612    * Get Data Ready interrupt status. In normal use, the RAW_DATA_RDY interrupt
 613    * is used to determine when new sensor data is available in and of the sensor
 614    * registers (27 to 32).
 615    *
 616    * @return Data Ready interrupt status
 617    * @see ITG3200_RA_INT_STATUS
 618    * @see ITG3200_INTSTAT_RAW_DATA_READY_BIT
 619    */
 620   public boolean getIntDataReadyStatus() {
 621     return readBit(ITG3200_RA_INT_STATUS, ITG3200_INTSTAT_RAW_DATA_READY_BIT);
 622   }
 623
 624   // TEMP_OUT_* registers
 625   /**
 626    * Get current internal temperature.
 627    *
 628    * @return Temperature reading in 16-bit 2's complement format
 629    * @see ITG3200_RA_TEMP_OUT_H
 630    */
 631   public short getTemperature() {
 632     byte[] buf = new byte[2];
 633     ReadI2CBuffer(ITG3200_RA_TEMP_OUT_H, 2, buf);
 634     return (short) (((short) (buf[0]) << 8) | (short) buf[1]);
 635   }
 636
 637   // GYRO_*OUT_* registers
 638
 639   public static class AllAxes {
 640     public short XAxis;
 641     public short YAxis;
 642     public short ZAxis;
 643   }
 644
 645   /**
 646    * Get 3-axis gyroscope readings.
 647    *
 648    * @param x
 649    *          16-bit signed integer container for X-axis rotation
 650    * @param y
 651    *          16-bit signed integer container for Y-axis rotation
 652    * @param z
 653    *          16-bit signed integer container for Z-axis rotation
 654    * @see ITG3200_RA_GYRO_XOUT_H
 655    */
 656   public AllAxes getRotation() {
 657     AllAxes data = new AllAxes();
 658     byte[] buffer = new byte[6];
 659     ReadI2CBuffer(ITG3200_RA_GYRO_XOUT_H, 6, buffer);
 660     data.XAxis = (short) ((((short) buffer[0]) << 8) | buffer[1]);
 661     data.YAxis = (short) ((((short) buffer[2]) << 8) | buffer[3]);
 662     data.ZAxis = (short) ((((short) buffer[4]) << 8) | buffer[5]);
 663     return data;
 664   }
 665
 666   public void ReadI2CBuffer(int registerAddress, int count, byte[] buffer) {
 667     try {
 668       m_i2c.read(registerAddress, count, buffer);
 669     } catch (Throwable t) {
 670       DriverStation.reportError("ERROR Unhandled exception in I2C Read: "
 671           + t.toString() + " at " + Arrays.toString(t.getStackTrace()), false);
 672     }
 673   }
 674
 675   public short ReadShortFromRegister(byte register, int count) {
 676     byte[] buffer = new byte[count];
 677     ReadI2CBuffer(register, count, buffer);
 678     return (short) ((((short) buffer[0]) << 8) | buffer[1]);
 679   }
 680
 681   /**
 682    * Get X-axis gyroscope reading.
 683    *
 684    * @return X-axis rotation measurement in 16-bit 2's complement format
 685    * @see ITG3200_RA_GYRO_XOUT_H
 686    */
 687   public short getRotationX() {
 688     return ReadShortFromRegister(ITG3200_RA_GYRO_XOUT_H, 2);
 689   }
 690
 691   /**
 692    * Get Y-axis gyroscope reading.
 693    *
 694    * @return Y-axis rotation measurement in 16-bit 2's complement format
 695    * @see ITG3200_RA_GYRO_YOUT_H
 696    */
 697   public short getRotationY() {
 698     return ReadShortFromRegister(ITG3200_RA_GYRO_YOUT_H, 2);
 699   }
 700
 701   /**
 702    * Get Z-axis gyroscope reading.
 703    *
 704    * @return Z-axis rotation measurement in 16-bit 2's complement format
 705    * @see ITG3200_RA_GYRO_ZOUT_H
 706    */
 707   public short getRotationZ() {
 708     return ReadShortFromRegister(ITG3200_RA_GYRO_ZOUT_H, 2);
 709   }
 710
 711   // PWR_MGM register
 712
 713   /**
 714    * Trigger a full device reset. A small delay of ~50ms may be desirable after
 715    * triggering a reset.
 716    *
 717    * @see ITG3200_RA_PWR_MGM
 718    * @see ITG3200_PWR_H_RESET_BIT
 719    */
 720
 721   public void reset() {
 722     writeBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_H_RESET_BIT, true);
 723   }
 724
 725   /**
 726    * Get sleep mode status. Setting the SLEEP bit in the register puts the
 727    * device into very low power sleep mode. In this mode, only the serial
 728    * interface and internal registers remain active, allowing for a very low
 729    * standby current. Clearing this bit puts the device back into normal mode.
 730    * To save power, the individual standby selections for each of the gyros
 731    * should be used if any gyro axis is not used by the application.
 732    *
 733    * @return Current sleep mode enabled status
 734    * @see ITG3200_RA_PWR_MGM
 735    * @see ITG3200_PWR_SLEEP_BIT
 736    */
 737   public boolean getSleepEnabled() {
 738     return readBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_SLEEP_BIT);
 739   }
 740
 741   /**
 742    * Set sleep mode status.
 743    *
 744    * @param enabled
 745    *          New sleep mode enabled status
 746    * @see getSleepEnabled()
 747    * @see ITG3200_RA_PWR_MGM
 748    * @see ITG3200_PWR_SLEEP_BIT
 749    */
 750   public void setSleepEnabled(boolean enabled) {
 751     writeBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_SLEEP_BIT, enabled);
 752   }
 753
 754   /**
 755    * Get X-axis standby enabled status. If enabled, the X-axis will not gather
 756    * or report data (or use power).
 757    *
 758    * @return Current X-axis standby enabled status
 759    * @see ITG3200_RA_PWR_MGM
 760    * @see ITG3200_PWR_STBY_XG_BIT
 761    */
 762   public boolean getStandbyXEnabled() {
 763     return readBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_STBY_XG_BIT);
 764   }
 765
 766   /**
 767    * Set X-axis standby enabled status.
 768    *
 769    * @param New
 770    *          X-axis standby enabled status
 771    * @see getStandbyXEnabled()
 772    * @see ITG3200_RA_PWR_MGM
 773    * @see ITG3200_PWR_STBY_XG_BIT
 774    */
 775   public void setStandbyXEnabled(boolean enabled) {
 776     writeBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_STBY_XG_BIT, enabled);
 777   }
 778
 779   /**
 780    * Get Y-axis standby enabled status. If enabled, the Y-axis will not gather
 781    * or report data (or use power).
 782    *
 783    * @return Current Y-axis standby enabled status
 784    * @see ITG3200_RA_PWR_MGM
 785    * @see ITG3200_PWR_STBY_YG_BIT
 786    */
 787   public boolean getStandbyYEnabled() {
 788     return readBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_STBY_YG_BIT);
 789   }
 790
 791   /**
 792    * Set Y-axis standby enabled status.
 793    *
 794    * @param New
 795    *          Y-axis standby enabled status
 796    * @see getStandbyYEnabled()
 797    * @see ITG3200_RA_PWR_MGM
 798    * @see ITG3200_PWR_STBY_YG_BIT
 799    */
 800   public void setStandbyYEnabled(boolean enabled) {
 801     writeBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_STBY_YG_BIT, enabled);
 802   }
 803
 804   /**
 805    * Get Z-axis standby enabled status. If enabled, the Z-axis will not gather
 806    * or report data (or use power).
 807    *
 808    * @return Current Z-axis standby enabled status
 809    * @see ITG3200_RA_PWR_MGM
 810    * @see ITG3200_PWR_STBY_ZG_BIT
 811    */
 812   public boolean getStandbyZEnabled() {
 813     return readBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_STBY_ZG_BIT);
 814   }
 815
 816   /**
 817    * Set Z-axis standby enabled status.
 818    *
 819    * @param New
 820    *          Z-axis standby enabled status
 821    * @see getStandbyZEnabled()
 822    * @see ITG3200_RA_PWR_MGM
 823    * @see ITG3200_PWR_STBY_ZG_BIT
 824    */
 825   public void setStandbyZEnabled(boolean enabled) {
 826     writeBit(ITG3200_RA_PWR_MGM, ITG3200_PWR_STBY_ZG_BIT, enabled);
 827   }
 828
 829   /**
 830    * Get clock source setting.
 831    *
 832    * @return Current clock source setting
 833    * @see ITG3200_RA_PWR_MGM
 834    * @see ITG3200_PWR_CLK_SEL_BIT
 835    * @see ITG3200_PWR_CLK_SEL_LENGTH
 836    */
 837   public byte getClockSource() {
 838     byte[] buf = new byte[1];
 839     ReadI2CBuffer(ITG3200_RA_PWR_MGM, 1, buf);
 840     // I2Cdev::readBits(devAddr, ITG3200_RA_PWR_MGM, ITG3200_PWR_CLK_SEL_BIT,
 841     // ITG3200_PWR_CLK_SEL_LENGTH, buffer);
 842     return (byte) (buf[0] & ITG3200_PWR_CLK_SEL_BIT);
 843   }
 844
 845   /**
 846    * Set clock source setting. On power up, the ITG-3200 defaults to the
 847    * internal oscillator. It is highly recommended that the device is configured
 848    * to use one of the gyros (or an external clock) as the clock reference, due
 849    * to the improved stability.
 850    *
 851    * The CLK_SEL setting determines the device clock source as follows:
 852    *
 853    * CLK_SEL | Clock Source --------+-------------------------------------- 0 |
 854    * Internal oscillator 1 | PLL with X Gyro reference 2 | PLL with Y Gyro
 855    * reference 3 | PLL with Z Gyro reference 4 | PLL with external 32.768kHz
 856    * reference 5 | PLL with external 19.2MHz reference 6 | Reserved 7 | Reserved
 857    *
 858    * @param source
 859    *          New clock source setting
 860    * @see getClockSource()
 861    * @see ITG3200_RA_PWR_MGM
 862    * @see ITG3200_PWR_CLK_SEL_BIT
 863    * @see ITG3200_PWR_CLK_SEL_LENGTH
 864    */
 865   public void setClockSource(byte source) {
 866     writeBits(ITG3200_RA_PWR_MGM, ITG3200_PWR_CLK_SEL_BIT,
 867         ITG3200_PWR_CLK_SEL_LENGTH, source);
 868   }
 869
 870   private ITable m_table;
 871
 872   /**
 873    * {@inheritDoc}
 874    */
 875   @Override
 876   public void initTable(ITable subtable) {
 877     m_table = subtable;
 878     updateTable();
 879   }
 880
 881   /**
 882    * {@inheritDoc}
 883    */
 884   @Override
 885   public ITable getTable() {
 886     return m_table;
 887   }
 888
 889   /**
 890    * {@inheritDoc}
 891    */
 892   @Override
 893   public void updateTable() {
 894     if (m_table != null) {
 895       m_table.putNumber("GyroX", getRotationX());
 896       m_table.putNumber("GyroY", getRotationY());
 897       m_table.putNumber("GyroZ", getRotationZ());
 898       m_table.putNumber("GyroPIDValue", pidGet());
 899     }
 900   }
 901
 902   /*
 903    * (non-Javadoc)
 904    *
 905    * @see edu.wpi.first.wpilibj.Sendable#getSmartDashboardType()
 906    */
 907   @Override
 908   public String getSmartDashboardType() {
 909     return "Gyro";
 910   }
 911
 912   /*
 913    * (non-Javadoc)
 914    *
 915    * @see
 916    * edu.wpi.first.wpilibj.livewindow.LiveWindowSendable#startLiveWindowMode()
 917    */
 918   @Override
 919   public void startLiveWindowMode() {
 920   }
 921
 922   /*
 923    * (non-Javadoc)
 924    *
 925    * @see
 926    * edu.wpi.first.wpilibj.livewindow.LiveWindowSendable#stopLiveWindowMode()
 927    */
 928   @Override
 929   public void stopLiveWindowMode() {
 930   }
 931
 932   /*
 933    * (non-Javadoc)
 934    *
 935    * @see edu.wpi.first.wpilibj.PIDSource#pidGet()
 936    */
 937   @Override
 938   public double pidGet() {
 939     // TODO We likely want to return one of the axes based on a setup option.
 940     AllAxes var = getRotation();
 941     double result = Math.cbrt(
 942         var.XAxis * var.XAxis + var.YAxis * var.YAxis + var.ZAxis * var.ZAxis);
 943     return result;
 944     // return 0;
 945   }
 946
 947   public static final byte ITG3200_ADDRESS_AD0_LOW = 0x68; // address pin low
 948                                                            // (GND), default for
 949                                                            // SparkFun IMU
 950                                                            // Digital Combo
 951                                                            // board
 952   public static final byte ITG3200_ADDRESS_AD0_HIGH = 0x69; // address pin high
 953                                                             // (VCC), default
 954                                                             // for SparkFun
 955                                                             // ITG-3200 Breakout
 956                                                             // board
 957
 958   public static final int ITG3200_SPARKFUN_ADDRES = 0xD2;
 959
 960   public static final int ITG3200_DEFAULT_ADDRESS = ITG3200_ADDRESS_AD0_LOW; // ITG3200_ADDRESS_AD0_HIGH;
 961
 962   public static final byte ITG3200_RA_WHO_AM_I = 0x00;
 963   public static final byte ITG3200_RA_SMPLRT_DIV = 0x15;
 964   public static final byte ITG3200_RA_DLPF_FS = 0x16;
 965   public static final byte ITG3200_RA_INT_CFG = 0x17;
 966   public static final byte ITG3200_RA_INT_STATUS = 0x1A;
 967   public static final byte ITG3200_RA_TEMP_OUT_H = 0x1B;
 968   public static final byte ITG3200_RA_TEMP_OUT_L = 0x1C;
 969   public static final byte ITG3200_RA_GYRO_XOUT_H = 0x1D;
 970   public static final byte ITG3200_RA_GYRO_XOUT_L = 0x1E;
 971   public static final byte ITG3200_RA_GYRO_YOUT_H = 0x1F;
 972   public static final byte ITG3200_RA_GYRO_YOUT_L = 0x20;
 973   public static final byte ITG3200_RA_GYRO_ZOUT_H = 0x21;
 974   public static final byte ITG3200_RA_GYRO_ZOUT_L = 0x22;
 975   public static final byte ITG3200_RA_PWR_MGM = 0x3E;
 976
 977   public static final short ITG3200_DEVID_BIT = 6;
 978   public static final short ITG3200_DEVID_LENGTH = 6;
 979
 980   public static final short ITG3200_DF_FS_SEL_BIT = 4;
 981   public static final short ITG3200_DF_FS_SEL_LENGTH = 2;
 982   public static final short ITG3200_DF_DLPF_CFG_BIT = 2;
 983   public static final short ITG3200_DF_DLPF_CFG_LENGTH = 3;
 984
 985   public static final byte ITG3200_FULLSCALE_2000 = 0x03;
 986
 987   public static final byte ITG3200_DLPF_BW_256 = 0x00;
 988   public static final byte ITG3200_DLPF_BW_188 = 0x01;
 989   public static final byte ITG3200_DLPF_BW_98 = 0x02;
 990   public static final byte ITG3200_DLPF_BW_42 = 0x03;
 991   public static final byte ITG3200_DLPF_BW_20 = 0x04;
 992   public static final byte ITG3200_DLPF_BW_10 = 0x05;
 993   public static final byte ITG3200_DLPF_BW_5 = 0x06;
 994
 995   public static final byte ITG3200_INTCFG_ACTL_BIT = 7;
 996   public static final byte ITG3200_INTCFG_OPEN_BIT = 6;
 997   public static final byte ITG3200_INTCFG_LATCH_INT_EN_BIT = 5;
 998   public static final byte ITG3200_INTCFG_INT_ANYRD_2CLEAR_BIT = 4;
 999   public static final byte ITG3200_INTCFG_ITG_RDY_EN_BIT = 2;
1000   public static final byte ITG3200_INTCFG_RAW_RDY_EN_BIT = 0;
1001
1002   public static final byte ITG3200_INTMODE_ACTIVEHIGH = 0x00;
1003   public static final byte ITG3200_INTMODE_ACTIVELOW = 0x01;
1004
1005   public static final byte ITG3200_INTDRV_PUSHPULL = 0x00;
1006   public static final byte ITG3200_INTDRV_OPENDRAIN = 0x01;
1007
1008   public static final byte ITG3200_INTLATCH_50USPULSE = 0x00;
1009   public static final byte ITG3200_INTLATCH_WAITCLEAR = 0x01;
1010
1011   public static final byte ITG3200_INTCLEAR_STATUSREAD = 0x00;
1012   public static final byte ITG3200_INTCLEAR_ANYREAD = 0x01;
1013
1014   public static final byte ITG3200_INTSTAT_ITG_RDY_BIT = 2;
1015   public static final byte ITG3200_INTSTAT_RAW_DATA_READY_BIT = 0;
1016
1017   public static final byte ITG3200_PWR_H_RESET_BIT = 7;
1018   public static final byte ITG3200_PWR_SLEEP_BIT = 6;
1019   public static final byte ITG3200_PWR_STBY_XG_BIT = 5;
1020   public static final byte ITG3200_PWR_STBY_YG_BIT = 4;
1021   public static final byte ITG3200_PWR_STBY_ZG_BIT = 3;
1022   public static final byte ITG3200_PWR_CLK_SEL_BIT = 2;
1023   public static final byte ITG3200_PWR_CLK_SEL_LENGTH = 3;
1024
1025   public static final byte ITG3200_CLOCK_INTERNAL = 0x00;
1026   public static final byte ITG3200_CLOCK_PLL_XGYRO = 0x01;
1027   public static final byte ITG3200_CLOCK_PLL_YGYRO = 0x02;
1028   public static final byte ITG3200_CLOCK_PLL_ZGYRO = 0x03;
1029   public static final byte ITG3200_CLOCK_PLL_EXT32K = 0x04;
1030   public static final byte ITG3200_CLOCK_PLL_EXT19M = 0x05;
1031
1032   @Override
1033   public void setPIDSourceType(PIDSourceType pidSource) {
1034     // TODO Auto-generated method stub
1035
1036   }
1037
1038   @Override
1039   public PIDSourceType getPIDSourceType() {
1040     // TODO Auto-generated method stub
1041     return null;
1042   }
1043 }