From 1e039ebdf71f746dd2fb21f7bcf811d4d80ac3fa 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 --- .../frc/team3501/robot/AnotherGyroClass.java | 27 +----- .../usfirst/frc/team3501/robot/GyroClass.java | 97 +++++++++---------- src/org/usfirst/frc/team3501/robot/Robot.java | 41 +++++++- .../frc/team3501/robot/RotationTracker.java | 74 ++++++++++++++ 4 files changed, 162 insertions(+), 77 deletions(-) create mode 100644 src/org/usfirst/frc/team3501/robot/RotationTracker.java diff --git a/src/org/usfirst/frc/team3501/robot/AnotherGyroClass.java b/src/org/usfirst/frc/team3501/robot/AnotherGyroClass.java index a6e4380e..c358b8bf 100644 --- a/src/org/usfirst/frc/team3501/robot/AnotherGyroClass.java +++ b/src/org/usfirst/frc/team3501/robot/AnotherGyroClass.java @@ -39,7 +39,6 @@ import java.io.PrintStream; import edu.wpi.first.wpilibj.I2C; import edu.wpi.first.wpilibj.PIDSourceType; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.interfaces.Gyro; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** @@ -111,7 +110,7 @@ public final class AnotherGyroClass { *
  • Use the {@link AnotherGyroClass#getRo * */ - public final class Rotation implements Gyro { + public final class Rotation implements RotationTracker { /** Raw axis accumulator on gyro associated with this rotation tracker. */ private Accumulator m_axis; /** @@ -208,30 +207,6 @@ public final class AnotherGyroClass { // TODO Auto-generated method stub return null; } - - @Override - public void calibrate() { - // TODO Auto-generated method stub - - } - - @Override - public void reset() { - // TODO Auto-generated method stub - - } - - @Override - public double getRate() { - // TODO Auto-generated method stub - return 0; - } - - @Override - public void free() { - // TODO Auto-generated method stub - - } } // diff --git a/src/org/usfirst/frc/team3501/robot/GyroClass.java b/src/org/usfirst/frc/team3501/robot/GyroClass.java index 70edc98c..571f5ba9 100644 --- a/src/org/usfirst/frc/team3501/robot/GyroClass.java +++ b/src/org/usfirst/frc/team3501/robot/GyroClass.java @@ -1,7 +1,7 @@ // 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 +// 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 // @@ -60,7 +60,7 @@ public class GyroClass extends SensorBase /** * Default constructor, uses default I2C address. - * + * * @see ITG3200_DEFAULT_ADDRESS */ public GyroClass(I2C.Port port) { @@ -76,7 +76,7 @@ public class GyroClass extends SensorBase /** * Specific address constructor. - * + * * @param address * I2C address * @see ITG3200_DEFAULT_ADDRESS @@ -117,7 +117,7 @@ public class GyroClass extends SensorBase /** * 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() { @@ -308,7 +308,7 @@ public class GyroClass extends SensorBase /** * 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 @@ -322,7 +322,7 @@ public class GyroClass extends SensorBase /** * 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() @@ -361,7 +361,7 @@ public class GyroClass extends SensorBase /** * Set sample rate. - * + * * @param rate * New sample rate * @see getRate() @@ -393,7 +393,7 @@ public class GyroClass extends SensorBase /** * Set full-scale range setting. - * + * * @param range * New full-scale range value * @see getFullScaleRange() @@ -429,7 +429,7 @@ public class GyroClass extends SensorBase /** * Set digital low-pass filter bandwidth. - * + * * @param bandwidth * New DLFP bandwidth setting * @see getDLPFBandwidth() @@ -448,7 +448,7 @@ public class GyroClass extends SensorBase /** * 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 @@ -459,7 +459,7 @@ public class GyroClass extends SensorBase /** * Set interrupt logic level mode. - * + * * @param mode * New interrupt mode (0=active-high, 1=active-low) * @see getInterruptMode() @@ -472,7 +472,7 @@ public class GyroClass extends SensorBase /** * 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 @@ -483,7 +483,7 @@ public class GyroClass extends SensorBase /** * Set interrupt drive mode. - * + * * @param drive * New interrupt drive mode (0=push-pull, 1=open-drain) * @see getInterruptDrive() @@ -497,7 +497,7 @@ public class GyroClass extends SensorBase /** * 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 @@ -508,7 +508,7 @@ public class GyroClass extends SensorBase /** * Set interrupt latch mode. - * + * * @param latch * New latch mode (0=50us-pulse, 1=latch-until-int-cleared) * @see getInterruptLatch() @@ -522,7 +522,7 @@ public class GyroClass extends SensorBase /** * 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 @@ -533,7 +533,7 @@ public class GyroClass extends SensorBase /** * Set interrupt latch clear mode. - * + * * @param clear * New latch clear mode (0=status-read-only, 1=any-register-read) * @see getInterruptLatchClear() @@ -547,7 +547,7 @@ public class GyroClass extends SensorBase /** * 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 @@ -558,7 +558,7 @@ public class GyroClass extends SensorBase /** * Set "device ready" interrupt enabled setting. - * + * * @param enabled * New interrupt enabled setting * @see getIntDeviceReadyEnabled() @@ -572,7 +572,7 @@ public class GyroClass extends SensorBase /** * 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 @@ -583,7 +583,7 @@ public class GyroClass extends SensorBase /** * Set "data ready" interrupt enabled setting. - * + * * @param enabled * New interrupt enabled setting * @see getIntDataReadyEnabled() @@ -599,7 +599,7 @@ public class GyroClass extends SensorBase /** * 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 @@ -612,7 +612,7 @@ public class GyroClass extends SensorBase * 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 @@ -624,14 +624,14 @@ public class GyroClass extends SensorBase // TEMP_OUT_* registers /** * Get current internal temperature. - * + * * @return Temperature reading in 16-bit 2's complement format * @see ITG3200_RA_TEMP_OUT_H */ public short getTemperature() { byte[] buf = new byte[2]; ReadI2CBuffer(ITG3200_RA_TEMP_OUT_H, 2, buf); - return (short) (((buf[0]) << 8) | buf[1]); + return (short) (((short) (buf[0]) << 8) | (short) buf[1]); } // GYRO_*OUT_* registers @@ -644,7 +644,7 @@ public class GyroClass extends SensorBase /** * Get 3-axis gyroscope readings. - * + * * @param x * 16-bit signed integer container for X-axis rotation * @param y @@ -657,9 +657,9 @@ public class GyroClass extends SensorBase AllAxes data = new AllAxes(); byte[] buffer = new byte[6]; ReadI2CBuffer(ITG3200_RA_GYRO_XOUT_H, 6, buffer); - data.XAxis = (short) (((buffer[0]) << 8) | buffer[1]); - data.YAxis = (short) (((buffer[2]) << 8) | buffer[3]); - data.ZAxis = (short) (((buffer[4]) << 8) | buffer[5]); + 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; } @@ -675,12 +675,12 @@ public class GyroClass extends SensorBase public short ReadShortFromRegister(byte register, int count) { byte[] buffer = new byte[count]; ReadI2CBuffer(register, count, buffer); - return (short) (((buffer[0]) << 8) | buffer[1]); + 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 */ @@ -690,7 +690,7 @@ public class GyroClass extends SensorBase /** * Get Y-axis gyroscope reading. - * + * * @return Y-axis rotation measurement in 16-bit 2's complement format * @see ITG3200_RA_GYRO_YOUT_H */ @@ -700,12 +700,12 @@ public class GyroClass extends SensorBase /** * Get Z-axis gyroscope reading. - * + * * @return Z-axis rotation measurement in 16-bit 2's complement format * @see ITG3200_RA_GYRO_ZOUT_H */ public short getRotationZ() { - return ReadShortFromRegister(ITG3200_RA_GYRO_ZOUT_H, 0); + return ReadShortFromRegister(ITG3200_RA_GYRO_ZOUT_H, 2); } // PWR_MGM register @@ -713,7 +713,7 @@ public class GyroClass extends SensorBase /** * 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 */ @@ -729,7 +729,7 @@ public class GyroClass extends SensorBase * 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 @@ -740,7 +740,7 @@ public class GyroClass extends SensorBase /** * Set sleep mode status. - * + * * @param enabled * New sleep mode enabled status * @see getSleepEnabled() @@ -754,7 +754,7 @@ public class GyroClass extends SensorBase /** * 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 @@ -765,7 +765,7 @@ public class GyroClass extends SensorBase /** * Set X-axis standby enabled status. - * + * * @param New * X-axis standby enabled status * @see getStandbyXEnabled() @@ -779,7 +779,7 @@ public class GyroClass extends SensorBase /** * 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 @@ -790,7 +790,7 @@ public class GyroClass extends SensorBase /** * Set Y-axis standby enabled status. - * + * * @param New * Y-axis standby enabled status * @see getStandbyYEnabled() @@ -804,7 +804,7 @@ public class GyroClass extends SensorBase /** * 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 @@ -815,7 +815,7 @@ public class GyroClass extends SensorBase /** * Set Z-axis standby enabled status. - * + * * @param New * Z-axis standby enabled status * @see getStandbyZEnabled() @@ -828,7 +828,7 @@ public class GyroClass extends SensorBase /** * Get clock source setting. - * + * * @return Current clock source setting * @see ITG3200_RA_PWR_MGM * @see ITG3200_PWR_CLK_SEL_BIT @@ -901,7 +901,7 @@ public class GyroClass extends SensorBase /* * (non-Javadoc) - * + * * @see edu.wpi.first.wpilibj.Sendable#getSmartDashboardType() */ @Override @@ -911,7 +911,7 @@ public class GyroClass extends SensorBase /* * (non-Javadoc) - * + * * @see * edu.wpi.first.wpilibj.livewindow.LiveWindowSendable#startLiveWindowMode() */ @@ -921,7 +921,7 @@ public class GyroClass extends SensorBase /* * (non-Javadoc) - * + * * @see * edu.wpi.first.wpilibj.livewindow.LiveWindowSendable#stopLiveWindowMode() */ @@ -931,7 +931,7 @@ public class GyroClass extends SensorBase /* * (non-Javadoc) - * + * * @see edu.wpi.first.wpilibj.PIDSource#pidGet() */ @Override @@ -1040,5 +1040,4 @@ public class GyroClass extends SensorBase // 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 8d69370e..425e6704 100644 --- a/src/org/usfirst/frc/team3501/robot/Robot.java +++ b/src/org/usfirst/frc/team3501/robot/Robot.java @@ -1,5 +1,6 @@ package org.usfirst.frc.team3501.robot; +import org.usfirst.frc.team3501.robot.AnotherGyroClass.Rotation; import org.usfirst.frc.team3501.robot.Constants.Defense; import org.usfirst.frc.team3501.robot.subsystems.DefenseArm; import org.usfirst.frc.team3501.robot.subsystems.DriveTrain; @@ -19,6 +20,7 @@ public class Robot extends IterativeRobot { public static Shooter shooter; public static Scaler scaler; + double then; public static IntakeArm intakeArm; public static DefenseArm defenseArm; @@ -29,17 +31,19 @@ public class Robot extends IterativeRobot { positionFourDefense, positionFiveDefense; // Gyro stuff - private final static double NANOSECONDS_PER_SECOND = 1000000000; short rawValue; public GyroClass gyro; + double now; double degreesIncreased; double degrees; + Rotation rotation; + @Override public void robotInit() { // driveTrain = new DriveTrain(); - gyro = new GyroClass(I2C.Port.kOnboard, (byte) 0x68); + gyro = new GyroClass(I2C.Port.kOnboard, gyro.ITG3200_ADDRESS_AD0_LOW); // oi = new OI(); shooter = new Shooter(); @@ -151,6 +155,39 @@ public class Robot extends IterativeRobot { @Override public void teleopPeriodic() { Scheduler.getInstance().run(); + + } + + public double getZAxisDegreesPerSeconds() { + double rawValue = (double) 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; } } diff --git a/src/org/usfirst/frc/team3501/robot/RotationTracker.java b/src/org/usfirst/frc/team3501/robot/RotationTracker.java new file mode 100644 index 00000000..f0757ff1 --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/RotationTracker.java @@ -0,0 +1,74 @@ +/** + * Copyright (c) 2015, www.techhounds.com + * All rights reserved. + * + *

    + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + *

    + *
      + *
    • Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer.
      • + *
    • Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution.
      • + *
    • Neither the name of the www.techhounds.com nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission.
      • + *
    + * + *

    + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *

    + */ + +package org.usfirst.frc.team3501.robot; + +import edu.wpi.first.wpilibj.PIDSource; + +/** + * A rotation tracker allows you to keep track of how far the robot has rotated + * since the object was constructed or zeroed. + * + *

    + * This object is most useful when you want to turn your robot a particular + * number of degrees, or when you want to detect whether your robot is tipping. + *

    + * + *

    + * You will typically use the {@link #getRotationTracker} method associated with + * the gyro class associated with your hardware. For example + * {@link GyroItg3200.getRotationTrackerZ()}. + *

    + */ +public interface RotationTracker extends PIDSource { + + /** + * Returns the angle in signed decimal degrees since construction or zeroing. + * + *

    + * This value is used as the PID sensor value. + *

    + * + * @return Number of degrees the robot has rotated in the range of [-INF, + * +INF]. Positive values indicate clockwise rotation (720 means it + * has spun around twice and is facing the same direction). + */ + public double getAngle(); + + /** + * Zeros the rotation tracker so the current direction we are pointing now + * becomes the zero point. + */ + public void zero(); + +} -- 2.30.2