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
9 // 2011-07-31 - initial release
10 // 2015-01-30 - Java FRC revision
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
31 ===============================================
34 package org
.usfirst
.frc
.team3501
.robot
;
36 import java
.util
.Arrays
;
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
;
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
52 public class FirebotGyro
extends SensorBase
53 implements PIDSource
, LiveWindowSendable
{
55 byte buffer
[] = new byte[7];
57 public static final boolean DEBUG
= true;
62 * Default constructor, uses default I2C address.
64 * @see ITG3200_DEFAULT_ADDRESS
66 public FirebotGyro(I2C
.Port port
) {
67 devAddr
= ITG3200_DEFAULT_ADDRESS
;
69 m_i2c
= new I2C(port
, devAddr
);
71 // TODO: This report is incorrect. Need to create instance for I2C ITG3200
73 // UsageReporting.report( tResourceType.kResourceType_I2C, tInstances.?? );
74 LiveWindow
.addSensor("ITG3200_Gyro_I2C", port
.getValue(), this);
78 * Specific address constructor.
82 * @see ITG3200_DEFAULT_ADDRESS
83 * @see ITG3200_ADDRESS_AD0_LOW
84 * @see ITG3200_ADDRESS_AD0_HIGH
86 public FirebotGyro(I2C
.Port port
, byte address
) {
89 m_i2c
= new I2C(port
, address
);
91 // TODO: This report is incorrect. Need to create instance for I2C ITG3200
93 // UsageReporting.report( tResourceType.kResourceType_I2C, tInstances.?? );
94 LiveWindow
.addSensor("ITG3200_Gyro_I2C", port
.getValue(), this);
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.
107 public void initialize() {
108 if (!testConnection()) {
109 DriverStation
.reportError("Test connection failed!", false);
111 setFullScaleRange(ITG3200_FULLSCALE_2000
);
112 setClockSource(ITG3200_CLOCK_PLL_XGYRO
);
113 setIntDeviceReadyEnabled(true);
114 setIntDataReadyEnabled(true);
118 * Verify the I2C connection. Make sure the device is connected and responds
121 * @return True if connection is valid, false otherwise
123 public boolean testConnection() {
124 return getDeviceID() == 0b110100
;
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
);
135 ReadI2CBuffer(register
, 1, buf
);
136 if (newValue
!= buf
[0]) {
137 System
.out
.println("Expected " + newValue
+ " seeing " + buf
[0]);
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
,
147 throw new IllegalArgumentException(
148 "This routine is intended to use 8-bit bytes. \n Value: "
149 + GetBinaryString(value
) + "\n Number bits: " + numBits
);
152 throw new IllegalArgumentException(
153 "This routine is intended to use 8-bit bytes. \n Value: "
154 + GetBinaryString(value
) + "\n Bit: " + bit
);
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: "
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
);
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: "
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);
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) );
193 public static String
GetBinaryString(byte value
) {
194 return String
.format("%8s", Integer
.toBinaryString(value
& 0xFF))
198 public boolean writeI2CBuffer(int registerAddress
, int data
) {
199 boolean retVal
= false;
201 retVal
= m_i2c
.write(registerAddress
, data
);
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);
210 } catch (Throwable t
) {
211 DriverStation
.reportError("ERROR Unhandled exception: " + t
.toString()
212 + " at " + Arrays
.toString(t
.getStackTrace()), false);
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
) {
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);
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;
238 // Get n bits from the byte to form a byte slice
239 private static byte getBits(byte bitField
, int bit
, int numBits
) {
242 throw new IllegalArgumentException(
243 "This routine is intended to use 8-bit bytes." + "\n Number bits: "
247 throw new IllegalArgumentException(
248 "This routine is intended to use 8-bit bytes. " + "\n Bit: " + bit
);
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
);
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
);
264 byte mask
= (byte) (~
getMask(bit
, numBits
) & 0xFF);
265 byte maskedInput
= (byte) ((bitField
& mask
) & 0xFF);
266 result
= (byte) ((maskedInput
>>> (1 + bit
- numBits
)) & 0xFF);
269 * // Debug code System.out.println( "mask = " +
270 * GetBinaryString(mask) ); System.out.println( "maskedInput = " +
271 * GetBinaryString(maskedInput) ); System.out.println( "result = " +
272 * GetBinaryString(result) );
278 // Gets the bit mask for the given bit and number of bits
279 private static byte getMask(int bit
, int numBits
) {
281 for (int i
= 0; i
<= 7; i
++) {
282 if (i
> bit
|| i
<= bit
- numBits
) {
284 newMask
= (int) (newMask
+ Math
.pow(2, i
));
287 byte mask
= (byte) (newMask
& 0xFF);
291 private byte getRegisterByte(int register
) {
292 byte[] buf
= new byte[1];
293 ReadI2CBuffer(register
, 1, buf
);
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
302 private byte getRegisterBits(int register
, int bit
, int numBits
) {
303 byte containingByte
= getRegisterByte(register
);
304 return getBits(containingByte
, bit
, numBits
);
309 * Get Device ID. This register is used to verify the identity of the device
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
317 public byte getDeviceID() {
318 return getRegisterBits(ITG3200_RA_WHO_AM_I
, ITG3200_DEVID_BIT
,
319 ITG3200_DEVID_LENGTH
);
323 * Set Device ID. Write a new ID into the WHO_AM_I register (no idea why this
324 * should ever be necessary though).
327 * New device ID to set.
329 * @see ITG3200_RA_WHO_AM_I
330 * @see ITG3200_RA_DEVID_BIT
331 * @see ITG3200_RA_DEVID_LENGTH
333 public void setDeviceID(byte id
) {
334 writeBits(ITG3200_RA_WHO_AM_I
, ITG3200_DEVID_BIT
, ITG3200_DEVID_LENGTH
, id
);
337 // SMPLRT_DIV register
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
346 * F_sample = F_internal / (divider+1), where F_internal is either 1kHz or
349 * As an example, if the internal sampling is at 1kHz, then setting this
350 * register to 7 would give the following:
352 * F_sample = 1kHz / (7 + 1) = 125Hz, or 8ms per sample
354 * @return Current sample rate
355 * @see setDLPFBandwidth()
356 * @see ITG3200_RA_SMPLRT_DIV
358 public byte getRate() {
359 return getRegisterByte(ITG3200_RA_SMPLRT_DIV
);
368 * @see setDLPFBandwidth()
369 * @see ITG3200_RA_SMPLRT_DIV
371 public void setRate(byte rate
) {
372 writeI2CBuffer(ITG3200_RA_SMPLRT_DIV
, rate
);
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.
381 * 0 = Reserved 1 = Reserved 2 = Reserved 3 = +/- 2000 degrees/sec
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
389 public byte getFullScaleRange() {
390 return getRegisterBits(ITG3200_RA_DLPF_FS
, ITG3200_DF_FS_SEL_BIT
,
391 ITG3200_DF_FS_SEL_LENGTH
);
395 * Set full-scale range setting.
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
405 public void setFullScaleRange(byte range
) {
406 writeBits(ITG3200_RA_DLPF_FS
, ITG3200_DF_FS_SEL_BIT
,
407 ITG3200_DF_FS_SEL_LENGTH
, range
);
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.
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
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
425 public byte getDLPFBandwidth() {
426 return getRegisterBits(ITG3200_RA_DLPF_FS
, ITG3200_DF_DLPF_CFG_BIT
,
427 ITG3200_DF_DLPF_CFG_LENGTH
);
431 * Set digital low-pass filter 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
441 public void setDLPFBandwidth(byte bandwidth
) {
442 writeBits(ITG3200_RA_DLPF_FS
, ITG3200_DF_DLPF_CFG_BIT
,
443 ITG3200_DF_DLPF_CFG_LENGTH
, bandwidth
);
449 * Get interrupt logic level mode. Will be set 0 for active-high, 1 for
452 * @return Current interrupt mode (0=active-high, 1=active-low)
453 * @see ITG3200_RA_INT_CFG
454 * @see ITG3200_INTCFG_ACTL_BIT
456 public boolean getInterruptMode() {
457 return readBit(ITG3200_RA_INT_CFG
, ITG3200_INTCFG_ACTL_BIT
);
461 * Set interrupt logic level 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
469 public void setInterruptMode(boolean mode
) {
470 writeBit(ITG3200_RA_INT_CFG
, ITG3200_INTCFG_ACTL_BIT
, mode
);
474 * Get interrupt drive mode. Will be set 0 for push-pull, 1 for open-drain.
476 * @return Current interrupt drive mode (0=push-pull, 1=open-drain)
477 * @see ITG3200_RA_INT_CFG
478 * @see ITG3200_INTCFG_OPEN_BIT
480 public boolean getInterruptDrive() {
481 return readBit(ITG3200_RA_INT_CFG
, ITG3200_INTCFG_OPEN_BIT
);
485 * Set interrupt drive mode.
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
493 public void setInterruptDrive(boolean drive
) {
494 writeBit(ITG3200_RA_INT_CFG
, ITG3200_INTCFG_OPEN_BIT
, drive
);
498 * Get interrupt latch mode. Will be set 0 for 50us-pulse, 1 for
499 * latch-until-int-cleared.
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
505 public boolean getInterruptLatch() {
506 return readBit(ITG3200_RA_INT_CFG
, ITG3200_INTCFG_LATCH_INT_EN_BIT
);
510 * Set interrupt latch mode.
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
518 public void setInterruptLatch(boolean latch
) {
519 writeBit(ITG3200_RA_INT_CFG
, ITG3200_INTCFG_LATCH_INT_EN_BIT
, latch
);
523 * Get interrupt latch clear mode. Will be set 0 for status-read-only, 1 for
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
530 public boolean getInterruptLatchClear() {
531 return readBit(ITG3200_RA_INT_CFG
, ITG3200_INTCFG_INT_ANYRD_2CLEAR_BIT
);
535 * Set interrupt latch clear mode.
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
543 public void setInterruptLatchClear(boolean clear
) {
544 writeBit(ITG3200_RA_INT_CFG
, ITG3200_INTCFG_INT_ANYRD_2CLEAR_BIT
, clear
);
548 * Get "device ready" interrupt enabled setting. Will be set 0 for disabled, 1
551 * @return Current interrupt enabled setting
552 * @see ITG3200_RA_INT_CFG
553 * @see ITG3200_INTCFG_ITG_RDY_EN_BIT
555 public boolean getIntDeviceReadyEnabled() {
556 return readBit(ITG3200_RA_INT_CFG
, ITG3200_INTCFG_ITG_RDY_EN_BIT
);
560 * Set "device ready" interrupt enabled setting.
563 * New interrupt enabled setting
564 * @see getIntDeviceReadyEnabled()
565 * @see ITG3200_RA_INT_CFG
566 * @see ITG3200_INTCFG_ITG_RDY_EN_BIT
568 public void setIntDeviceReadyEnabled(boolean enabled
) {
569 writeBit(ITG3200_RA_INT_CFG
, ITG3200_INTCFG_ITG_RDY_EN_BIT
, enabled
);
573 * Get "data ready" interrupt enabled setting. Will be set 0 for disabled, 1
576 * @return Current interrupt enabled setting
577 * @see ITG3200_RA_INT_CFG
578 * @see ITG3200_INTCFG_RAW_RDY_EN_BIT
580 public boolean getIntDataReadyEnabled() {
581 return readBit(ITG3200_RA_INT_CFG
, ITG3200_INTCFG_RAW_RDY_EN_BIT
);
585 * Set "data ready" interrupt enabled setting.
588 * New interrupt enabled setting
589 * @see getIntDataReadyEnabled()
590 * @see ITG3200_RA_INT_CFG
591 * @see ITG3200_INTCFG_RAW_RDY_EN_BIT
593 public void setIntDataReadyEnabled(boolean enabled
) {
594 writeBit(ITG3200_RA_INT_CFG
, ITG3200_INTCFG_RAW_RDY_EN_BIT
, enabled
);
597 // INT_STATUS register
600 * Get Device Ready interrupt status. The ITG_RDY interrupt indicates that the
601 * PLL is ready and gyroscopic data can be read.
603 * @return Device Ready interrupt status
604 * @see ITG3200_RA_INT_STATUS
605 * @see ITG3200_INTSTAT_RAW_DATA_READY_BIT
607 public boolean getIntDeviceReadyStatus() {
608 return readBit(ITG3200_RA_INT_STATUS
, ITG3200_INTSTAT_ITG_RDY_BIT
);
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).
616 * @return Data Ready interrupt status
617 * @see ITG3200_RA_INT_STATUS
618 * @see ITG3200_INTSTAT_RAW_DATA_READY_BIT
620 public boolean getIntDataReadyStatus() {
621 return readBit(ITG3200_RA_INT_STATUS
, ITG3200_INTSTAT_RAW_DATA_READY_BIT
);
624 // TEMP_OUT_* registers
626 * Get current internal temperature.
628 * @return Temperature reading in 16-bit 2's complement format
629 * @see ITG3200_RA_TEMP_OUT_H
631 public short getTemperature() {
632 byte[] buf
= new byte[2];
633 ReadI2CBuffer(ITG3200_RA_TEMP_OUT_H
, 2, buf
);
634 return (short) (((buf
[0]) << 8) | buf
[1]);
637 // GYRO_*OUT_* registers
639 public static class AllAxes
{
646 * Get 3-axis gyroscope readings.
649 * 16-bit signed integer container for X-axis rotation
651 * 16-bit signed integer container for Y-axis rotation
653 * 16-bit signed integer container for Z-axis rotation
654 * @see ITG3200_RA_GYRO_XOUT_H
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) (((buffer
[0]) << 8) | buffer
[1]);
661 data
.YAxis
= (short) (((buffer
[2]) << 8) | buffer
[3]);
662 data
.ZAxis
= (short) (((buffer
[4]) << 8) | buffer
[5]);
666 public void ReadI2CBuffer(int registerAddress
, int count
, byte[] buffer
) {
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);
675 public short ReadShortFromRegister(byte register
, int count
) {
676 byte[] buffer
= new byte[count
];
677 ReadI2CBuffer(register
, count
, buffer
);
678 return (short) (((buffer
[0]) << 8) | buffer
[1]);
682 * Get X-axis gyroscope reading.
684 * @return X-axis rotation measurement in 16-bit 2's complement format
685 * @see ITG3200_RA_GYRO_XOUT_H
687 public short getRotationX() {
688 return ReadShortFromRegister(ITG3200_RA_GYRO_XOUT_H
, 2);
692 * Get Y-axis gyroscope reading.
694 * @return Y-axis rotation measurement in 16-bit 2's complement format
695 * @see ITG3200_RA_GYRO_YOUT_H
697 public short getRotationY() {
698 return ReadShortFromRegister(ITG3200_RA_GYRO_YOUT_H
, 2);
702 * Get Z-axis gyroscope reading.
704 * @return Z-axis rotation measurement in 16-bit 2's complement format
705 * @see ITG3200_RA_GYRO_ZOUT_H
707 public short getRotationZ() {
708 return ReadShortFromRegister(ITG3200_RA_GYRO_ZOUT_H
, 2);
714 * Trigger a full device reset. A small delay of ~50ms may be desirable after
715 * triggering a reset.
717 * @see ITG3200_RA_PWR_MGM
718 * @see ITG3200_PWR_H_RESET_BIT
721 public void reset() {
722 writeBit(ITG3200_RA_PWR_MGM
, ITG3200_PWR_H_RESET_BIT
, true);
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.
733 * @return Current sleep mode enabled status
734 * @see ITG3200_RA_PWR_MGM
735 * @see ITG3200_PWR_SLEEP_BIT
737 public boolean getSleepEnabled() {
738 return readBit(ITG3200_RA_PWR_MGM
, ITG3200_PWR_SLEEP_BIT
);
742 * Set sleep mode status.
745 * New sleep mode enabled status
746 * @see getSleepEnabled()
747 * @see ITG3200_RA_PWR_MGM
748 * @see ITG3200_PWR_SLEEP_BIT
750 public void setSleepEnabled(boolean enabled
) {
751 writeBit(ITG3200_RA_PWR_MGM
, ITG3200_PWR_SLEEP_BIT
, enabled
);
755 * Get X-axis standby enabled status. If enabled, the X-axis will not gather
756 * or report data (or use power).
758 * @return Current X-axis standby enabled status
759 * @see ITG3200_RA_PWR_MGM
760 * @see ITG3200_PWR_STBY_XG_BIT
762 public boolean getStandbyXEnabled() {
763 return readBit(ITG3200_RA_PWR_MGM
, ITG3200_PWR_STBY_XG_BIT
);
767 * Set X-axis standby enabled status.
770 * X-axis standby enabled status
771 * @see getStandbyXEnabled()
772 * @see ITG3200_RA_PWR_MGM
773 * @see ITG3200_PWR_STBY_XG_BIT
775 public void setStandbyXEnabled(boolean enabled
) {
776 writeBit(ITG3200_RA_PWR_MGM
, ITG3200_PWR_STBY_XG_BIT
, enabled
);
780 * Get Y-axis standby enabled status. If enabled, the Y-axis will not gather
781 * or report data (or use power).
783 * @return Current Y-axis standby enabled status
784 * @see ITG3200_RA_PWR_MGM
785 * @see ITG3200_PWR_STBY_YG_BIT
787 public boolean getStandbyYEnabled() {
788 return readBit(ITG3200_RA_PWR_MGM
, ITG3200_PWR_STBY_YG_BIT
);
792 * Set Y-axis standby enabled status.
795 * Y-axis standby enabled status
796 * @see getStandbyYEnabled()
797 * @see ITG3200_RA_PWR_MGM
798 * @see ITG3200_PWR_STBY_YG_BIT
800 public void setStandbyYEnabled(boolean enabled
) {
801 writeBit(ITG3200_RA_PWR_MGM
, ITG3200_PWR_STBY_YG_BIT
, enabled
);
805 * Get Z-axis standby enabled status. If enabled, the Z-axis will not gather
806 * or report data (or use power).
808 * @return Current Z-axis standby enabled status
809 * @see ITG3200_RA_PWR_MGM
810 * @see ITG3200_PWR_STBY_ZG_BIT
812 public boolean getStandbyZEnabled() {
813 return readBit(ITG3200_RA_PWR_MGM
, ITG3200_PWR_STBY_ZG_BIT
);
817 * Set Z-axis standby enabled status.
820 * Z-axis standby enabled status
821 * @see getStandbyZEnabled()
822 * @see ITG3200_RA_PWR_MGM
823 * @see ITG3200_PWR_STBY_ZG_BIT
825 public void setStandbyZEnabled(boolean enabled
) {
826 writeBit(ITG3200_RA_PWR_MGM
, ITG3200_PWR_STBY_ZG_BIT
, enabled
);
830 * Get clock source setting.
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
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
);
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.
851 * The CLK_SEL setting determines the device clock source as follows:
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
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
865 public void setClockSource(byte source
) {
866 writeBits(ITG3200_RA_PWR_MGM
, ITG3200_PWR_CLK_SEL_BIT
,
867 ITG3200_PWR_CLK_SEL_LENGTH
, source
);
870 private ITable m_table
;
876 public void initTable(ITable subtable
) {
885 public ITable
getTable() {
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());
905 * @see edu.wpi.first.wpilibj.Sendable#getSmartDashboardType()
908 public String
getSmartDashboardType() {
916 * edu.wpi.first.wpilibj.livewindow.LiveWindowSendable#startLiveWindowMode()
919 public void startLiveWindowMode() {
926 * edu.wpi.first.wpilibj.livewindow.LiveWindowSendable#stopLiveWindowMode()
929 public void stopLiveWindowMode() {
935 * @see edu.wpi.first.wpilibj.PIDSource#pidGet()
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
);
947 public static final byte ITG3200_ADDRESS_AD0_LOW
= 0x68; // address pin low
948 // (GND), default for
952 public static final byte ITG3200_ADDRESS_AD0_HIGH
= 0x69; // address pin high
958 public static final int ITG3200_SPARKFUN_ADDRES
= 0xD2;
960 public static final int ITG3200_DEFAULT_ADDRESS
= ITG3200_ADDRESS_AD0_LOW
; // ITG3200_ADDRESS_AD0_HIGH;
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;
977 public static final short ITG3200_DEVID_BIT
= 6;
978 public static final short ITG3200_DEVID_LENGTH
= 6;
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;
985 public static final byte ITG3200_FULLSCALE_2000
= 0x03;
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;
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;
1002 public static final byte ITG3200_INTMODE_ACTIVEHIGH
= 0x00;
1003 public static final byte ITG3200_INTMODE_ACTIVELOW
= 0x01;
1005 public static final byte ITG3200_INTDRV_PUSHPULL
= 0x00;
1006 public static final byte ITG3200_INTDRV_OPENDRAIN
= 0x01;
1008 public static final byte ITG3200_INTLATCH_50USPULSE
= 0x00;
1009 public static final byte ITG3200_INTLATCH_WAITCLEAR
= 0x01;
1011 public static final byte ITG3200_INTCLEAR_STATUSREAD
= 0x00;
1012 public static final byte ITG3200_INTCLEAR_ANYREAD
= 0x01;
1014 public static final byte ITG3200_INTSTAT_ITG_RDY_BIT
= 2;
1015 public static final byte ITG3200_INTSTAT_RAW_DATA_READY_BIT
= 0;
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;
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;
1033 public void setPIDSourceType(PIDSourceType pidSource
) {
1034 // TODO Auto-generated method stub
1039 public PIDSourceType
getPIDSourceType() {
1040 // TODO Auto-generated method stub