From c668c1b2363ed6a1add988e11e5a4cba648a5d74 Mon Sep 17 00:00:00 2001 From: EvanYap Date: Thu, 11 Feb 2016 21:00:56 -0800 Subject: [PATCH] Use GyroClass and currently using method getDegrees in teleopPeriodic --- .../usfirst/frc/team3501/robot/GyroClass.java | 1043 +++++++++++++++++ src/org/usfirst/frc/team3501/robot/Robot.java | 58 +- 2 files changed, 1088 insertions(+), 13 deletions(-) create mode 100644 src/org/usfirst/frc/team3501/robot/GyroClass.java diff --git a/src/org/usfirst/frc/team3501/robot/GyroClass.java b/src/org/usfirst/frc/team3501/robot/GyroClass.java new file mode 100644 index 00000000..571f5ba9 --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/GyroClass.java @@ -0,0 +1,1043 @@ +// 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 +// Java implementation for First Robotics Competition Team 2521 using WPILibj +// 1/27/2015 by Joe Bussell +// 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) (((short) (buf[0]) << 8) | (short) 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) ((((short) buffer[0]) << 8) | buffer[1]); + data.YAxis = (short) ((((short) buffer[2]) << 8) | buffer[3]); + data.ZAxis = (short) ((((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) ((((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, 2); + } + + // 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; + } +} diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java index 63f83b6b..ce4d31d6 100644 --- a/src/org/usfirst/frc/team3501/robot/Robot.java +++ b/src/org/usfirst/frc/team3501/robot/Robot.java @@ -19,35 +19,36 @@ public class Robot extends IterativeRobot { public static Shooter shooter; public static Scaler scaler; - public static DefenseArm defenseArm; + double then; public static IntakeArm intakeArm; + public static DefenseArm defenseArm; // Sendable Choosers send a drop down menu to the Smart Dashboard. SendableChooser positionChooser; SendableChooser positionOneDefense, positionTwoDefense, positionThreeDefense, - positionFourDefense, positionFiveDefense; + positionFourDefense, positionFiveDefense; // Gyro stuff private final static double NANOSECONDS_PER_SECOND = 1000000000; short rawValue; - public FirebotGyro gyro; + public GyroClass gyro; - double initialSpeedNanoseconds; - double finalSpeedNanoseconds; - double initialSpeedSeconds; - double finalSpeedSeconds; - double deltaSpeed; + double now; + double degreesIncreased; double degrees; + Rotation rotation; + @Override public void robotInit() { - driveTrain = new DriveTrain(); - gyro = new FirebotGyro(I2C.Port.kOnboard, (byte) 0x68); - oi = new OI(); + // driveTrain = new DriveTrain(); + gyro = new GyroClass(I2C.Port.kOnboard, gyro.ITG3200_ADDRESS_AD0_LOW); + // oi = new OI(); + shooter = new Shooter(); scaler = new Scaler(); defenseArm = new DefenseArm(); - intakeArm = new IntakeArm(); + intakeArm = new IntakeArm(); // Sendable Choosers allows the driver to select the position of the robot // and the positions of the defenses from a drop-down menu on the Smart @@ -146,12 +147,43 @@ public class Robot extends IterativeRobot { @Override public void teleopInit() { - Robot.driveTrain.initializeGyro(); } @Override public void teleopPeriodic() { Scheduler.getInstance().run(); + } + + public double getZAxisDegreesPerSeconds() { + double rawValue = gyro.getRotationZ() / 14.375; + return rawValue; + } + public void initializeGyro() { + degrees = 0; + then = System.nanoTime() / 1000000000.0; + gyro.reset(); + gyro.initialize(); + System.out.println("Testing Gyro Init"); } + + public void addZAxisDegrees() { + double degreesRead = getZAxisDegreesPerSeconds(); + now = System.nanoTime(); + now = now / (1000000000.0); + double differenceInTime = now - then; + then = now; + degreesIncreased = differenceInTime * degreesRead; + + // 0.0 = register + // + 1.0 is the formula constant + // + degrees += degreesIncreased; + + } + + public double getDegrees() { + return degrees; + } + } -- 2.30.2