+++ /dev/null
-package org.usfirst.frc.team3501.robot;
-/**
- * Copyright (c) 2015, www.techhounds.com
- * All rights reserved.
- *
- * <p>
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- * </p>
- * <ul>
- * <li>Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.</li>
- * <li>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.</li>
- * <li>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.</li>
- * </ul>
- *
- * <p>
- * 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 <COPYRIGHT HOLDER> 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.
- * </p>
- */
-
-import java.io.File;
-import java.io.IOException;
-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.smartdashboard.SmartDashboard;
-
-/**
- * A Java wrapper around the ITC based ITG-3200 triple axis gryo.
- *
- * <p>
- * Typical usage:
- * </p>
- * <ul>
- * <li>Make sure your ITG-3200 is connected to the I2C bus on the roboRIO and
- * mounted flat (so Z axis is used to track direction robot is facing).</li>
- * <li>Construct a single instance of the {@link GyroLib} class to be
- * shared throughout your Robot code.</li>
- * <li>Use the {@link #getRotationZ()} method create "trackers" that allow you
- * to keep track of how much your robot has rotated (direction your robot is
- * facing).</li>
- * </ul>
- * <p>
- * Be aware of the following:
- * </p>
- * <ul>
- * <li>Angles are in signed degrees (both positive and negative values are
- * possible) and not necessarily normalized (large values like -1203 degrees are
- * possible).</li>
- * <li>The {@link #reset} method is called once initially at construction.
- * Reseting the gyro should only be done when your robot is stationary and it
- * may take up to one second. You should not need to do this and should avoid
- * doing this during the autonomous or teleop periods (unless you know your
- * robot won't be moving).</li>
- * <li>There is a background thread that is automatically started that keeps
- * reading and accumulating values from the ITG-3200. You should not need to use
- * the {@link #start()} or {@link #stop()} methods during normal matches.
- * However, if you only use the gyro during the autonomous period, you can use
- * the {@link #stop()} method at the end of the autonomous period to save some
- * CPU.</li>
- * </ul>
- *
- * <h2>Suggested Usage</h2>
- * <p>
- * You should be able to use this class to aid your robot in making relative
- * turns. For example, if you want to create a command which rotates your robot
- * 90 degrees.
- * </p>
- * <ul>
- * <li>In your command's initialize method, use the {@link #getRotationZ()} and
- * the {@link Rotation#zero()} method on the returned {@link Rotation} object to
- * track how much you have turned.</li>
- * <li>Use the {@link Rotation} object as a PID source and/or check the current
- * angle reported by the {@link Rotation} object in your isFinished() method.
- * </li>
- * </ul>
- */
-public final class GyroLib {
-
- /**
- * Object used to monitor robot rotation.
- *
- * <ul>
- * <li>Use this class to track how much your robot has rotated.</li>
- * <li>Use the {@link GyroLib#getRotationZ()} to create an instance
- * to track how much your robot has rotated around the z-axis (direction robot
- * is facing - useful for making turns)..</li>
- * <li>Use the {@link GyroLib#getRotationX()} to create an instance
- * to track how much your robot has rotated around the x-axis (hopefully not
- * much unless you are tipping).</li>
- * <li>Use the {@link GyroLib#getRotationY()} to create an instance
- * to track how much your robot has rotated around the y-axis (hopefully not
- * much unless you are tipping).</li>
- * <li>Use the {@link GyroLib#getRo
- * </ul>
- */
- public final class Rotation implements RotationTracker {
- /** Raw axis accumulator on gyro associated with this rotation tracker. */
- private Accumulator m_axis;
- /**
- * The degrees reported by the accumulator the last time this tracker was
- * zeroed.
- */
- private double m_zeroDeg;
- /**
- * The number of readings reported by the accumulator the last time this
- * tracker was zeroed.
- */
- private int m_zeroCnt;
-
- /**
- * Constructor is protected, instances are created through the
- * {@link GyroLib} methods.
- *
- * @param axis
- * An accumulator from the gyro for the axis to be tracked.
- */
- private Rotation(Accumulator axis) {
- m_axis = axis;
- m_zeroDeg = 0;
- m_zeroCnt = 0;
- }
-
- /**
- * Zero the tracker (sets the current heading/direction as the zero point).
- */
- public void zero() {
- m_zeroDeg = m_axis.getDegrees();
- m_zeroCnt = m_axis.getReadings();
- }
-
- /**
- * Get the number of degrees rotated since last zeroed.
- *
- * @return A signed number of degrees [-INF, +INF].
- */
- public double getAngle() {
- double angle = m_axis.getDegrees() - m_zeroDeg;
- return angle;
- }
-
- /**
- * Get the total number of times the raw values from the gyro have been read
- * since zeroed.
- *
- * @return A diagnostic count that can be used to make sure the angle is
- * still being updated.
- */
- public int getReadings() {
- return m_axis.getReadings() - m_zeroCnt;
- }
-
- /**
- * Returns the last raw (integer) value read from the gyro for the axis.
- *
- * @return An integer value from the ITG-3200 for the associated axis.
- */
- public int getAngleRateRaw() {
- return m_axis.getRaw();
- }
-
- /**
- * Returns the current rotation rate in degrees/second from the last
- * reading.
- *
- * @return How quickly the system is rotating about the axis in
- * degrees/second.
- */
- public double getAngleRate() {
- return getAngleRateRaw() * COUNT_TO_DEGSEC;
- }
-
- /**
- * Returns the angle value from {@link #getAngle()} so object can be used as
- * a source to a PID controller.
- *
- * @return See {@link #getAngle()}.
- *
- * @see edu.wpi.first.wpilibj.PIDSource#pidGet()
- */
- public double pidGet() {
- return getAngle();
- }
-
- public void setPIDSourceType(PIDSourceType pidSource) {
- // TODO Auto-generated method stub
-
- }
-
- public PIDSourceType getPIDSourceType() {
- // TODO Auto-generated method stub
- return null;
- }
- }
-
- //
- // List of I2C registers which the ITG-3200 uses from the datasheet
- //
- private static final byte WHO_AM_I = 0x00;
- private static final byte SMPLRT_DIV = 0x15;
- private static final byte DLPF_FS = 0x16;
- // private static final byte INT_CFG = 0x17;
- // private static final byte INT_STATUS = 0x1A;
- // private static final byte TEMP_OUT_H = 0x1B;
- // private static final byte TEMP_OUT_L = 0x1C;
- private static final byte GYRO_XOUT_H = 0x1D;
- // private static final byte GYRO_XOUT_L = 0x1E;
- // private static final byte GYRO_YOUT_H = 0x1F;
- // private static final byte GYRO_YOUT_L = 0x20;
- // private static final byte GYRO_ZOUT_H = 0x21;
- // private static final byte GYRO_ZOUT_L = 0x22;
-
- //
- // Bit flags used for interrupt operation
- //
-
- /*
- * Set this bit in INT_CFG register for logic level of INT output pin to be
- * low when interrupt is active (leave 0 if you want it high).
- */
- // private static final byte INT_CFG_ACTL_LOW = (byte) (1 << 7);
-
- /*
- * Set drive type for interrupt to open drain mode, omit if you want push-pull
- * mode (what does this mean?).
- */
- // private static final byte INT_CFG_OPEN_DRAIN = 1 << 6;
-
- /*
- * Interrupt latch mode (remains set until you clear it), omit this flag to
- * get a 0-50us interrupt pulse.
- */
- // private static final byte INT_CFG_LATCH_INT_EN = 1 << 5;
-
- /*
- * Allow any read operation of data to clear the interrupt flag (otherwise it
- * is only cleared after reading status register).
- */
- // private static final byte INT_CFG_ANYRD_2CLEAR = 1 << 4;
-
- /*
- * Enable interrup when device is ready (PLL ready after changing clock
- * source). Hmmm?
- */
- // private static final byte INT_CFG_RDY_EN = 1 << 3;
-
- /* Enable interrupt when new data is available. */
- // private static final byte INT_CFG_RAW_RDY_EN = 1 << 1;
-
- /* Guess at mode to use if we want to try enabling interrupts. */
- // private static final byte INT_CFG_SETTING = (INT_CFG_LATCH_INT_EN |
- // INT_CFG_ANYRD_2CLEAR | INT_CFG_RAW_RDY_EN);
-
- //
- // The bit flags that can be set in the DLPF register on the ITG-3200
- // as specified in the ITG-3200 data sheet.
- //
-
- //
- // The low pass filter bandwidth settings
- //
-
- // private static final byte DLPF_LOWPASS_256HZ = 0 << 0;
- private static final byte DLPF_LOWPASS_188HZ = 1 << 0;
- // private static final byte DLPF_LOWPASS_98HZ = 2 << 0;
- // private static final byte DLPF_LOWPASS_42HZ = 3 << 0;
- // private static final byte DLPF_LOWPASS_20HZ = 4 << 0;
- // private static final byte DLPF_LOWPASS_10HZ = 5 << 0;
- // private static final byte DLPF_LOWPASS_5HZ = 6 << 0;
-
- /** Select range of +/-2000 deg/sec. (only range supported). */
- private static final byte DLPF_FS_SEL_2000 = 3 << 3;
-
- /**
- * The I2C address of the ITG-3200 when AD0 (pin 9) is jumpered to logic high.
- */
- private static final byte itgAddressJumper = 0x69;
-
- /**
- * The I2C address of the ITG-3200 when AD0 (pin 9) is jumpered to logic low.
- */
- private static final byte itgAddressNoJumper = 0x68;
-
- /** Multiplier to convert raw integer values returned to degrees/sec. */
- private static final float COUNT_TO_DEGSEC = (float) (1.0 / 14.375);
-
- /** Set this to true for lots of diagnostic output. */
- private static final boolean DEBUG = false;
-
- /**
- * How many sample readings to make to determine the bias value for each axis.
- */
- private static final int MIN_READINGS_TO_SET_BIAS = 50;
-
- /** I2C Address to use to communicate with the ITG-3200. */
- private byte m_addr;
-
- /** I2C object used to communicate with Gyro. */
- private I2C m_i2c;
-
- /**
- * Background thread responsible for accumulating angle data from the sensor.
- */
- private Thread m_thread;
-
- /** Flag used to signal background thread that the gyro should be reset. */
- private boolean m_need_reset;
-
- /** Flag used to signal background thread that it's time to stop. */
- private boolean m_run_thread;
-
- /** Accumulator for rotation around the x-axis. */
- private Accumulator m_x;
-
- /** Accumulator for rotation around the y-axis. */
- private Accumulator m_y;
-
- /** Accumulator for rotation around the z-axis. */
- private Accumulator m_z;
- private int[] m_xBuffer;
- private int[] m_yBuffer;
- private int[] m_zBuffer;
- private int m_cntBuffer;
- private int m_sizeBuffer;
- private double[] m_timeBuffer;
-
- /**
- * Construct a new instance of the ITG-3200 gryo class.
- *
- * <p>
- * IMPORTANT
- *
- * @param port
- * This should be {@link I2C.Port#kOnboard} if the ITG-3200 is
- * connected to the main I2C bus on the roboRIO. This should be
- * {@link I2C.Port#kMXP} if it is connected to the I2C bus on the MXP
- * port on the roboRIO.
- * @param jumper
- * This should be true if the ITG-3200 has the AD0 jumpered to logic
- * level high and false if not.
- */
- public GyroLib(I2C.Port port, boolean jumper) {
- m_addr = (jumper ? itgAddressJumper : itgAddressNoJumper);
- m_i2c = new I2C(port, m_addr);
- m_thread = new Thread(new Runnable() {
- @Override
- public void run() {
- accumulateData();
- }
- });
- if (DEBUG) {
- check();
- }
- m_x = new Accumulator();
- m_y = new Accumulator();
- m_z = new Accumulator();
- m_need_reset = true;
-
- start();
- }
-
- /**
- * Construct a {@link Rotation} object used to monitor rotation about the
- * Z-axis.
- *
- * @return A rotation object that very useful for checking the direction your
- * robot is facing.
- */
- public Rotation getRotationZ() {
- return new Rotation(m_z);
- }
-
- /**
- * Construct a {@link Rotation} object used to monitor rotation about the
- * X-axis.
- *
- * @return A rotation object that is probably only useful for checking if your
- * robot is starting to tip over.
- */
- public Rotation getRotationX() {
- return new Rotation(m_x);
- }
-
- /**
- * Construct a {@link Rotation} object used to monitor rotation about the
- * Y-axis.
- *
- * @return A rotation object that is probably only useful for checking if your
- * robot is starting to tip over.
- */
- public Rotation getRotationY() {
- return new Rotation(m_y);
- }
-
- /**
- * Returns string representation of the object for debug purposes.
- */
- public String toString() {
- return "Gyro[0x" + Integer.toHexString(m_addr & 0xff) + "]";
- }
-
- /**
- * Dumps information about the state of the Gyro to the smart dashboard.
- *
- * @param tag
- * Short name like "Gyro" to prefix each label with on the dashboard.
- * @param debug
- * Pass true if you want a whole lot of details dumped onto the
- * dashboard, pass false if you just want the direction of each axis
- * and the temperature.
- */
- public void updateDashboard(String tag, boolean debug) {
- SmartDashboard.putNumber(tag + " x-axis degrees", m_x.getDegrees());
- SmartDashboard.putNumber(tag + " y-axis degrees", m_y.getDegrees());
- SmartDashboard.putNumber(tag + " z-axis degrees", m_z.getDegrees());
-
- if (debug) {
- SmartDashboard.putNumber(tag + " x-axis raw", m_x.getRaw());
- SmartDashboard.putNumber(tag + " y-axis raw", m_y.getRaw());
- SmartDashboard.putNumber(tag + " z-axis raw", m_z.getRaw());
-
- SmartDashboard.putNumber(tag + " x-axis count", m_x.getReadings());
- SmartDashboard.putNumber(tag + " y-axis count", m_y.getReadings());
- SmartDashboard.putNumber(tag + " z-axis count", m_z.getReadings());
-
- SmartDashboard.putString(tag + " I2C Address",
- "0x" + Integer.toHexString(m_addr));
- }
- }
-
- /**
- * Internal method that runs in the background thread to accumulate data from
- * the Gyro.
- */
- private void accumulateData() {
- m_run_thread = true;
- int resetCnt = 0;
-
- while (m_run_thread) {
- if (m_need_reset) {
- // Set gyro to the proper mode
- performResetSequence();
-
- // Reset accumulators and set the number of readings to take to
- // compute bias values
- resetCnt = MIN_READINGS_TO_SET_BIAS;
- m_x.reset();
- m_y.reset();
- m_z.reset();
- m_need_reset = false;
- } else {
- // Go read raw values from ITG-3200 and update our accumulators
- readRawAngleBytes();
-
- if (resetCnt > 0) {
- // If we were recently reset, and have made enough initial
- // readings,
- // then go compute and set our new bias (correction) values
- // for each accumulator
- resetCnt--;
- if (resetCnt == 0) {
- m_x.setBiasByAccumulatedValues();
- m_y.setBiasByAccumulatedValues();
- m_z.setBiasByAccumulatedValues();
- }
- }
- }
-
- // Short delay between each reading
- if (m_run_thread) {
- Timer.delay(.01);
- }
- }
- }
-
- /**
- * Singles the gyro's background thread that we want to reset the gyro.
- *
- * <p>
- * You don't typically need to call this during a match. If you do call it,
- * you should only do so when the robot is stationary and will remain
- * stationary for a short time.
- * </p>
- */
- public void reset() {
- m_need_reset = true;
- }
-
- /**
- * Starts up the background thread that accumulates gyro statistics.
- *
- * <p>
- * You never need to call this method unless you have stopped the gyro and now
- * want to start it up again. If you do call this method, you should probably
- * also call the {@link #reset} method.
- * </p>
- */
- public void start() {
- if (!m_thread.isAlive()) {
- m_thread.start();
- }
- }
-
- /**
- * Stops the background thread from accumulating angle information (turns OFF
- * gyro!).
- *
- * <p>
- * This method is not typically called as it stops the gyro from accumulating
- * statistics essentially turning it off. The only time you might want to do
- * this is if you are done using the gyro for the rest of the match and want
- * to save some CPU cyles (for example, if you only needed the gyro during the
- * autonomous period).
- * </p>
- */
- public void stop() {
- m_run_thread = false;
- }
-
- /**
- * Sends commands to configure the ITG-3200 the way we need it to run.
- */
- private void performResetSequence() {
- // Configure the gyroscope
- // Set the gyroscope scale for the outputs to +/-2000 degrees per second
- m_i2c.write(DLPF_FS, (DLPF_FS_SEL_2000 | DLPF_LOWPASS_188HZ));
- // Set the sample rate to 100 hz
- m_i2c.write(SMPLRT_DIV, 9);
- }
-
- /**
- * Enables the buffering of the next "n" data samples (which can then be saved
- * for analysis).
- *
- * @param samples
- * Maximum number of samples to read.
- */
- public void enableBuffer(int samples) {
- double[] timeBuffer = new double[samples];
- int[] xBuffer = new int[samples];
- int[] yBuffer = new int[samples];
- int[] zBuffer = new int[samples];
- synchronized (this) {
- m_timeBuffer = timeBuffer;
- m_xBuffer = xBuffer;
- m_yBuffer = yBuffer;
- m_zBuffer = zBuffer;
- m_cntBuffer = 0;
- m_sizeBuffer = samples;
- }
- }
-
- /**
- * Check to see if the buffer is full.
- *
- * @return true if buffer is at capacity.
- */
- public boolean isBufferFull() {
- boolean isFull;
- synchronized (this) {
- isFull = (m_cntBuffer == m_sizeBuffer);
- }
- return isFull;
- }
-
- /**
- * Writes any raw buffered data to the file "/tmp/gyro-data.csv" for
- * inspection via Excel.
- */
- public void saveBuffer() {
- double[] timeBuffer;
- int[] xBuffer;
- int[] yBuffer;
- int[] zBuffer;
- int size;
-
- // Transfer buffer info to local variables and turn off buffering in a
- // thread safe way.
- synchronized (this) {
- timeBuffer = m_timeBuffer;
- xBuffer = m_xBuffer;
- yBuffer = m_yBuffer;
- zBuffer = m_zBuffer;
- size = m_cntBuffer;
- m_sizeBuffer = 0;
- m_cntBuffer = 0;
- }
-
- if (size > 0) {
- try {
- PrintStream out = new PrintStream(new File("/tmp/gryo-data.csv"));
- out.println("\"FPGA Time\",\"x-axis\",\"y-axis\",\"z-axis\"");
- for (int i = 0; i < size; i++) {
- out.println(timeBuffer[i] + "," + xBuffer[i] + "," + yBuffer[i] + ","
- + zBuffer[i]);
- }
- out.close();
- SmartDashboard.putBoolean("Gyro Save OK", true);
- } catch (IOException ignore) {
- SmartDashboard.putBoolean("Gyro Save OK", false);
- }
- }
- }
-
- /**
- * Internal method run in the background thread that reads values from the
- * ITG-3200 and updates the accumulators.
- */
- private void readRawAngleBytes() {
- double now = Timer.getFPGATimestamp();
-
- byte[] buffer = new byte[6];
- boolean rc = m_i2c.read(GYRO_XOUT_H, buffer.length, buffer);
-
- if (rc) {
- // Got a good read, get 16 bit integer values for each axis and
- // update accumulated values
- int x = (buffer[0] << 8) | (buffer[1] & 0xff);
- int y = (buffer[2] << 8) | (buffer[3] & 0xff);
- int z = (buffer[4] << 8) | (buffer[5] & 0xff);
-
- m_x.update(x, now);
- m_y.update(y, now);
- m_z.update(z, now);
-
- // If buffered enabled, then save values in a thread safe way
- if (m_sizeBuffer > 0) {
- synchronized (this) {
- int i = m_cntBuffer;
- if (i < m_sizeBuffer) {
- m_timeBuffer[i] = now;
- m_xBuffer[i] = x;
- m_yBuffer[i] = y;
- m_zBuffer[i] = z;
- m_cntBuffer++;
- }
- }
- }
- }
-
- if (DEBUG) {
- String name = toString();
- String[] labels = { "XOUT_H", "XOUT_L", "YOUT_H", "YOUT_L", "ZOUT_H",
- "ZOUT_L" };
- for (int i = 0; i < labels.length; i++) {
- SmartDashboard.putString(name + " " + labels[i],
- "0x" + Integer.toHexString(0xff & buffer[i]));
- }
- }
- }
-
- /**
- * Helper method to check that we can communicate with the gyro.
- */
- private void check() {
- byte[] buffer = new byte[1];
- boolean rc = m_i2c.read(WHO_AM_I, buffer.length, buffer);
- if (DEBUG) {
- String name = toString();
- SmartDashboard.putBoolean(name + " Check OK?", rc);
- SmartDashboard.putNumber(name + " WHO_AM_I", buffer[0]);
- }
- }
-
- /**
- * Private helper class to accumulate values read from the gryo and convert
- * degs/sec into degrees.
- */
- private class Accumulator {
- /** Accumulated degrees since zero. */
- private double m_accumulatedDegs;
- /**
- * 2 times the computed bias value that is used when getting average of
- * readings.
- */
- private double m_bias2;
- /** The prior raw value read from the gyro. */
- private int m_lastRaw;
- /** The prior time stamp the last raw value was read. */
- private double m_lastTime;
- /** The total count of time the gyro value has been read. */
- private int m_cnt;
- /** The sum of all of the raw values read. */
- private long m_sum;
-
- /** Multipler to covert 2*Count to degrees/sec (optimization). */
- private static final double COUNT2_TO_DEGSEC = (COUNT_TO_DEGSEC / 2.0);
-
- /**
- * Returns the accumulated degrees.
- *
- * @return Accumulated signed degrees since last zeroed.
- */
- public synchronized double getDegrees() {
- return m_accumulatedDegs;
- }
-
- /**
- * @return The raw integer reading from the ITG-3200 associated with the
- * axis.
- */
- public int getRaw() {
- return m_lastRaw;
- }
-
- /**
- * Returns the number or readings that went into the accumulated degrees.
- *
- * @return Count of readings since last zeroed.
- */
- public synchronized int getReadings() {
- return m_cnt;
- }
-
- /**
- * Constructs a new instance.
- */
- private Accumulator() {
- m_bias2 = 0;
- zero();
- }
-
- /**
- * Zeros out accumulated information.
- */
- private void zero() {
- m_lastRaw = 0;
- m_lastTime = 0;
- m_sum = 0;
- synchronized (this) {
- m_cnt = 0;
- m_accumulatedDegs = 0;
- }
- }
-
- /**
- * Zeros out accumulated information and clears (zeros) the internal bias
- * value.
- */
- private void reset() {
- zero();
- m_bias2 = 0;
- }
-
- /**
- * Computes new bias value from accumulated values and then zeros.
- */
- private void setBiasByAccumulatedValues() {
- m_bias2 = 2.0 * ((double) m_sum) / ((double) m_cnt);
- zero();
- }
-
- /**
- * Updates (accumulates) new value read from axis.
- *
- * @param raw
- * Raw signed 16 bit value read from gyro for axis.
- * @param time
- * The time stamp when the value was read.
- */
- private void update(int raw, double time) {
- double degs = 0;
-
- if (m_cnt != 0) {
- // Get average of degrees per second over the time span
- double degPerSec = (m_lastRaw + raw - m_bias2) * COUNT2_TO_DEGSEC;
- // Get time span this rate occurred for
- double secs = (m_lastTime - time);
- // Get number of degrees rotated for time period
- degs = degPerSec * secs;
- }
-
- // Update our thread shared values
- synchronized (this) {
- m_accumulatedDegs += degs;
- m_sum += raw;
- m_cnt++;
- m_lastRaw = raw;
- m_lastTime = time;
- }
- }
- }
-}
+++ /dev/null
-package org.usfirst.frc.team3501.robot;
-
-import java.util.TimerTask;
-
-import edu.wpi.first.wpilibj.I2C;
-import edu.wpi.first.wpilibj.I2C.Port;
-import edu.wpi.first.wpilibj.PIDSource;
-import edu.wpi.first.wpilibj.PIDSourceType;
-import edu.wpi.first.wpilibj.Timer;
-
-public class Lidar implements PIDSource {
- private I2C i2c;
- private byte[] distance;
- private java.util.Timer updater;
-
- private final int LIDAR_ADDR = 0x62;
- private final int LIDAR_CONFIG_REGISTER = 0x00;
- private final int LIDAR_DISTANCE_REGISTER = 0x8f;
-
- public Lidar(Port port) {
- i2c = new I2C(port, LIDAR_ADDR);
-
- distance = new byte[2];
-
- updater = new java.util.Timer();
- }
-
- // Distance in cm
- public int getDistance() {
- return (int) Integer.toUnsignedLong(distance[0] << 8)
- + Byte.toUnsignedInt(distance[1]);
- }
-
- @Override
- public double pidGet() {
- return getDistance();
- }
-
- // Start 10Hz polling
- public void start() {
- updater.scheduleAtFixedRate(new LIDARUpdater(), 0, 100);
- }
-
- // Start polling for period in milliseconds
- public void start(int period) {
- updater.scheduleAtFixedRate(new LIDARUpdater(), 0, period);
- }
-
- public void stop() {
- updater.cancel();
- updater = new java.util.Timer();
- }
-
- // Update distance variable
- public void update() {
- i2c.write(LIDAR_CONFIG_REGISTER, 0x04); // Initiate measurement
- Timer.delay(0.04); // Delay for measurement to be taken
- i2c.read(LIDAR_DISTANCE_REGISTER, 2, distance); // Read in measurement
- Timer.delay(0.005); // Delay to prevent over polling
- }
-
- // Timer task to keep distance updated
- private class LIDARUpdater extends TimerTask {
- @Override
- public void run() {
- while (true) {
- update();
- try {
- Thread.sleep(10);
- } catch (InterruptedException e) {
- e.printStackTrace();
- }
- }
- }
- }
-
- @Override
- public void setPIDSourceType(PIDSourceType pidSource) {
- }
-
- @Override
- public PIDSourceType getPIDSourceType() {
- return null;
- }
-}
package org.usfirst.frc.team3501.robot;
import org.usfirst.frc.team3501.robot.Constants.Defense;
+import org.usfirst.frc.team3501.robot.sensors.GyroLib;
+import org.usfirst.frc.team3501.robot.sensors.Photogate;
import org.usfirst.frc.team3501.robot.subsystems.DefenseArm;
import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
import org.usfirst.frc.team3501.robot.subsystems.IntakeArm;
-import org.usfirst.frc.team3501.robot.subsystems.Photogate;
import org.usfirst.frc.team3501.robot.subsystems.Scaler;
import org.usfirst.frc.team3501.robot.subsystems.Shooter;
+++ /dev/null
-/**
- * Copyright (c) 2015, www.techhounds.com
- * All rights reserved.
- *
- * <p>
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- * </p>
- * <ul>
- * <li>Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.</li>
- * <li>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.</li>
- * <li>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.</li>
- * </ul>
- *
- * <p>
- * 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 <COPYRIGHT HOLDER> 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.
- * </p>
- */
-
-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.
- *
- * <p>
- * 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.
- * </p>
- *
- * <p>
- * You will typically use the {@link #getRotationTracker} method associated with
- * the gyro class associated with your hardware. For example
- * {@link GyroItg3200.getRotationTrackerZ()}.
- * </p>
- */
-public interface RotationTracker extends PIDSource {
-
- /**
- * Returns the angle in signed decimal degrees since construction or zeroing.
- *
- * <p>
- * This value is used as the PID sensor value.
- * </p>
- *
- * @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();
-
-}
--- /dev/null
+package org.usfirst.frc.team3501.robot.sensors;
+/**
+ * Copyright (c) 2015, www.techhounds.com
+ * All rights reserved.
+ *
+ * <p>
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * </p>
+ * <ul>
+ * <li>Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.</li>
+ * <li>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.</li>
+ * <li>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.</li>
+ * </ul>
+ *
+ * <p>
+ * 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 <COPYRIGHT HOLDER> 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.
+ * </p>
+ */
+
+import java.io.File;
+import java.io.IOException;
+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.smartdashboard.SmartDashboard;
+
+/**
+ * A Java wrapper around the ITC based ITG-3200 triple axis gryo.
+ *
+ * <p>
+ * Typical usage:
+ * </p>
+ * <ul>
+ * <li>Make sure your ITG-3200 is connected to the I2C bus on the roboRIO and
+ * mounted flat (so Z axis is used to track direction robot is facing).</li>
+ * <li>Construct a single instance of the {@link GyroLib} class to be
+ * shared throughout your Robot code.</li>
+ * <li>Use the {@link #getRotationZ()} method create "trackers" that allow you
+ * to keep track of how much your robot has rotated (direction your robot is
+ * facing).</li>
+ * </ul>
+ * <p>
+ * Be aware of the following:
+ * </p>
+ * <ul>
+ * <li>Angles are in signed degrees (both positive and negative values are
+ * possible) and not necessarily normalized (large values like -1203 degrees are
+ * possible).</li>
+ * <li>The {@link #reset} method is called once initially at construction.
+ * Reseting the gyro should only be done when your robot is stationary and it
+ * may take up to one second. You should not need to do this and should avoid
+ * doing this during the autonomous or teleop periods (unless you know your
+ * robot won't be moving).</li>
+ * <li>There is a background thread that is automatically started that keeps
+ * reading and accumulating values from the ITG-3200. You should not need to use
+ * the {@link #start()} or {@link #stop()} methods during normal matches.
+ * However, if you only use the gyro during the autonomous period, you can use
+ * the {@link #stop()} method at the end of the autonomous period to save some
+ * CPU.</li>
+ * </ul>
+ *
+ * <h2>Suggested Usage</h2>
+ * <p>
+ * You should be able to use this class to aid your robot in making relative
+ * turns. For example, if you want to create a command which rotates your robot
+ * 90 degrees.
+ * </p>
+ * <ul>
+ * <li>In your command's initialize method, use the {@link #getRotationZ()} and
+ * the {@link Rotation#zero()} method on the returned {@link Rotation} object to
+ * track how much you have turned.</li>
+ * <li>Use the {@link Rotation} object as a PID source and/or check the current
+ * angle reported by the {@link Rotation} object in your isFinished() method.
+ * </li>
+ * </ul>
+ */
+public final class GyroLib {
+
+ /**
+ * Object used to monitor robot rotation.
+ *
+ * <ul>
+ * <li>Use this class to track how much your robot has rotated.</li>
+ * <li>Use the {@link GyroLib#getRotationZ()} to create an instance
+ * to track how much your robot has rotated around the z-axis (direction robot
+ * is facing - useful for making turns)..</li>
+ * <li>Use the {@link GyroLib#getRotationX()} to create an instance
+ * to track how much your robot has rotated around the x-axis (hopefully not
+ * much unless you are tipping).</li>
+ * <li>Use the {@link GyroLib#getRotationY()} to create an instance
+ * to track how much your robot has rotated around the y-axis (hopefully not
+ * much unless you are tipping).</li>
+ * <li>Use the {@link GyroLib#getRo
+ * </ul>
+ */
+ public final class Rotation implements RotationTracker {
+ /** Raw axis accumulator on gyro associated with this rotation tracker. */
+ private Accumulator m_axis;
+ /**
+ * The degrees reported by the accumulator the last time this tracker was
+ * zeroed.
+ */
+ private double m_zeroDeg;
+ /**
+ * The number of readings reported by the accumulator the last time this
+ * tracker was zeroed.
+ */
+ private int m_zeroCnt;
+
+ /**
+ * Constructor is protected, instances are created through the
+ * {@link GyroLib} methods.
+ *
+ * @param axis
+ * An accumulator from the gyro for the axis to be tracked.
+ */
+ private Rotation(Accumulator axis) {
+ m_axis = axis;
+ m_zeroDeg = 0;
+ m_zeroCnt = 0;
+ }
+
+ /**
+ * Zero the tracker (sets the current heading/direction as the zero point).
+ */
+ public void zero() {
+ m_zeroDeg = m_axis.getDegrees();
+ m_zeroCnt = m_axis.getReadings();
+ }
+
+ /**
+ * Get the number of degrees rotated since last zeroed.
+ *
+ * @return A signed number of degrees [-INF, +INF].
+ */
+ public double getAngle() {
+ double angle = m_axis.getDegrees() - m_zeroDeg;
+ return angle;
+ }
+
+ /**
+ * Get the total number of times the raw values from the gyro have been read
+ * since zeroed.
+ *
+ * @return A diagnostic count that can be used to make sure the angle is
+ * still being updated.
+ */
+ public int getReadings() {
+ return m_axis.getReadings() - m_zeroCnt;
+ }
+
+ /**
+ * Returns the last raw (integer) value read from the gyro for the axis.
+ *
+ * @return An integer value from the ITG-3200 for the associated axis.
+ */
+ public int getAngleRateRaw() {
+ return m_axis.getRaw();
+ }
+
+ /**
+ * Returns the current rotation rate in degrees/second from the last
+ * reading.
+ *
+ * @return How quickly the system is rotating about the axis in
+ * degrees/second.
+ */
+ public double getAngleRate() {
+ return getAngleRateRaw() * COUNT_TO_DEGSEC;
+ }
+
+ /**
+ * Returns the angle value from {@link #getAngle()} so object can be used as
+ * a source to a PID controller.
+ *
+ * @return See {@link #getAngle()}.
+ *
+ * @see edu.wpi.first.wpilibj.PIDSource#pidGet()
+ */
+ public double pidGet() {
+ return getAngle();
+ }
+
+ public void setPIDSourceType(PIDSourceType pidSource) {
+ // TODO Auto-generated method stub
+
+ }
+
+ public PIDSourceType getPIDSourceType() {
+ // TODO Auto-generated method stub
+ return null;
+ }
+ }
+
+ //
+ // List of I2C registers which the ITG-3200 uses from the datasheet
+ //
+ private static final byte WHO_AM_I = 0x00;
+ private static final byte SMPLRT_DIV = 0x15;
+ private static final byte DLPF_FS = 0x16;
+ // private static final byte INT_CFG = 0x17;
+ // private static final byte INT_STATUS = 0x1A;
+ // private static final byte TEMP_OUT_H = 0x1B;
+ // private static final byte TEMP_OUT_L = 0x1C;
+ private static final byte GYRO_XOUT_H = 0x1D;
+ // private static final byte GYRO_XOUT_L = 0x1E;
+ // private static final byte GYRO_YOUT_H = 0x1F;
+ // private static final byte GYRO_YOUT_L = 0x20;
+ // private static final byte GYRO_ZOUT_H = 0x21;
+ // private static final byte GYRO_ZOUT_L = 0x22;
+
+ //
+ // Bit flags used for interrupt operation
+ //
+
+ /*
+ * Set this bit in INT_CFG register for logic level of INT output pin to be
+ * low when interrupt is active (leave 0 if you want it high).
+ */
+ // private static final byte INT_CFG_ACTL_LOW = (byte) (1 << 7);
+
+ /*
+ * Set drive type for interrupt to open drain mode, omit if you want push-pull
+ * mode (what does this mean?).
+ */
+ // private static final byte INT_CFG_OPEN_DRAIN = 1 << 6;
+
+ /*
+ * Interrupt latch mode (remains set until you clear it), omit this flag to
+ * get a 0-50us interrupt pulse.
+ */
+ // private static final byte INT_CFG_LATCH_INT_EN = 1 << 5;
+
+ /*
+ * Allow any read operation of data to clear the interrupt flag (otherwise it
+ * is only cleared after reading status register).
+ */
+ // private static final byte INT_CFG_ANYRD_2CLEAR = 1 << 4;
+
+ /*
+ * Enable interrup when device is ready (PLL ready after changing clock
+ * source). Hmmm?
+ */
+ // private static final byte INT_CFG_RDY_EN = 1 << 3;
+
+ /* Enable interrupt when new data is available. */
+ // private static final byte INT_CFG_RAW_RDY_EN = 1 << 1;
+
+ /* Guess at mode to use if we want to try enabling interrupts. */
+ // private static final byte INT_CFG_SETTING = (INT_CFG_LATCH_INT_EN |
+ // INT_CFG_ANYRD_2CLEAR | INT_CFG_RAW_RDY_EN);
+
+ //
+ // The bit flags that can be set in the DLPF register on the ITG-3200
+ // as specified in the ITG-3200 data sheet.
+ //
+
+ //
+ // The low pass filter bandwidth settings
+ //
+
+ // private static final byte DLPF_LOWPASS_256HZ = 0 << 0;
+ private static final byte DLPF_LOWPASS_188HZ = 1 << 0;
+ // private static final byte DLPF_LOWPASS_98HZ = 2 << 0;
+ // private static final byte DLPF_LOWPASS_42HZ = 3 << 0;
+ // private static final byte DLPF_LOWPASS_20HZ = 4 << 0;
+ // private static final byte DLPF_LOWPASS_10HZ = 5 << 0;
+ // private static final byte DLPF_LOWPASS_5HZ = 6 << 0;
+
+ /** Select range of +/-2000 deg/sec. (only range supported). */
+ private static final byte DLPF_FS_SEL_2000 = 3 << 3;
+
+ /**
+ * The I2C address of the ITG-3200 when AD0 (pin 9) is jumpered to logic high.
+ */
+ private static final byte itgAddressJumper = 0x69;
+
+ /**
+ * The I2C address of the ITG-3200 when AD0 (pin 9) is jumpered to logic low.
+ */
+ private static final byte itgAddressNoJumper = 0x68;
+
+ /** Multiplier to convert raw integer values returned to degrees/sec. */
+ private static final float COUNT_TO_DEGSEC = (float) (1.0 / 14.375);
+
+ /** Set this to true for lots of diagnostic output. */
+ private static final boolean DEBUG = false;
+
+ /**
+ * How many sample readings to make to determine the bias value for each axis.
+ */
+ private static final int MIN_READINGS_TO_SET_BIAS = 50;
+
+ /** I2C Address to use to communicate with the ITG-3200. */
+ private byte m_addr;
+
+ /** I2C object used to communicate with Gyro. */
+ private I2C m_i2c;
+
+ /**
+ * Background thread responsible for accumulating angle data from the sensor.
+ */
+ private Thread m_thread;
+
+ /** Flag used to signal background thread that the gyro should be reset. */
+ private boolean m_need_reset;
+
+ /** Flag used to signal background thread that it's time to stop. */
+ private boolean m_run_thread;
+
+ /** Accumulator for rotation around the x-axis. */
+ private Accumulator m_x;
+
+ /** Accumulator for rotation around the y-axis. */
+ private Accumulator m_y;
+
+ /** Accumulator for rotation around the z-axis. */
+ private Accumulator m_z;
+ private int[] m_xBuffer;
+ private int[] m_yBuffer;
+ private int[] m_zBuffer;
+ private int m_cntBuffer;
+ private int m_sizeBuffer;
+ private double[] m_timeBuffer;
+
+ /**
+ * Construct a new instance of the ITG-3200 gryo class.
+ *
+ * <p>
+ * IMPORTANT
+ *
+ * @param port
+ * This should be {@link I2C.Port#kOnboard} if the ITG-3200 is
+ * connected to the main I2C bus on the roboRIO. This should be
+ * {@link I2C.Port#kMXP} if it is connected to the I2C bus on the MXP
+ * port on the roboRIO.
+ * @param jumper
+ * This should be true if the ITG-3200 has the AD0 jumpered to logic
+ * level high and false if not.
+ */
+ public GyroLib(I2C.Port port, boolean jumper) {
+ m_addr = (jumper ? itgAddressJumper : itgAddressNoJumper);
+ m_i2c = new I2C(port, m_addr);
+ m_thread = new Thread(new Runnable() {
+ @Override
+ public void run() {
+ accumulateData();
+ }
+ });
+ if (DEBUG) {
+ check();
+ }
+ m_x = new Accumulator();
+ m_y = new Accumulator();
+ m_z = new Accumulator();
+ m_need_reset = true;
+
+ start();
+ }
+
+ /**
+ * Construct a {@link Rotation} object used to monitor rotation about the
+ * Z-axis.
+ *
+ * @return A rotation object that very useful for checking the direction your
+ * robot is facing.
+ */
+ public Rotation getRotationZ() {
+ return new Rotation(m_z);
+ }
+
+ /**
+ * Construct a {@link Rotation} object used to monitor rotation about the
+ * X-axis.
+ *
+ * @return A rotation object that is probably only useful for checking if your
+ * robot is starting to tip over.
+ */
+ public Rotation getRotationX() {
+ return new Rotation(m_x);
+ }
+
+ /**
+ * Construct a {@link Rotation} object used to monitor rotation about the
+ * Y-axis.
+ *
+ * @return A rotation object that is probably only useful for checking if your
+ * robot is starting to tip over.
+ */
+ public Rotation getRotationY() {
+ return new Rotation(m_y);
+ }
+
+ /**
+ * Returns string representation of the object for debug purposes.
+ */
+ public String toString() {
+ return "Gyro[0x" + Integer.toHexString(m_addr & 0xff) + "]";
+ }
+
+ /**
+ * Dumps information about the state of the Gyro to the smart dashboard.
+ *
+ * @param tag
+ * Short name like "Gyro" to prefix each label with on the dashboard.
+ * @param debug
+ * Pass true if you want a whole lot of details dumped onto the
+ * dashboard, pass false if you just want the direction of each axis
+ * and the temperature.
+ */
+ public void updateDashboard(String tag, boolean debug) {
+ SmartDashboard.putNumber(tag + " x-axis degrees", m_x.getDegrees());
+ SmartDashboard.putNumber(tag + " y-axis degrees", m_y.getDegrees());
+ SmartDashboard.putNumber(tag + " z-axis degrees", m_z.getDegrees());
+
+ if (debug) {
+ SmartDashboard.putNumber(tag + " x-axis raw", m_x.getRaw());
+ SmartDashboard.putNumber(tag + " y-axis raw", m_y.getRaw());
+ SmartDashboard.putNumber(tag + " z-axis raw", m_z.getRaw());
+
+ SmartDashboard.putNumber(tag + " x-axis count", m_x.getReadings());
+ SmartDashboard.putNumber(tag + " y-axis count", m_y.getReadings());
+ SmartDashboard.putNumber(tag + " z-axis count", m_z.getReadings());
+
+ SmartDashboard.putString(tag + " I2C Address",
+ "0x" + Integer.toHexString(m_addr));
+ }
+ }
+
+ /**
+ * Internal method that runs in the background thread to accumulate data from
+ * the Gyro.
+ */
+ private void accumulateData() {
+ m_run_thread = true;
+ int resetCnt = 0;
+
+ while (m_run_thread) {
+ if (m_need_reset) {
+ // Set gyro to the proper mode
+ performResetSequence();
+
+ // Reset accumulators and set the number of readings to take to
+ // compute bias values
+ resetCnt = MIN_READINGS_TO_SET_BIAS;
+ m_x.reset();
+ m_y.reset();
+ m_z.reset();
+ m_need_reset = false;
+ } else {
+ // Go read raw values from ITG-3200 and update our accumulators
+ readRawAngleBytes();
+
+ if (resetCnt > 0) {
+ // If we were recently reset, and have made enough initial
+ // readings,
+ // then go compute and set our new bias (correction) values
+ // for each accumulator
+ resetCnt--;
+ if (resetCnt == 0) {
+ m_x.setBiasByAccumulatedValues();
+ m_y.setBiasByAccumulatedValues();
+ m_z.setBiasByAccumulatedValues();
+ }
+ }
+ }
+
+ // Short delay between each reading
+ if (m_run_thread) {
+ Timer.delay(.01);
+ }
+ }
+ }
+
+ /**
+ * Singles the gyro's background thread that we want to reset the gyro.
+ *
+ * <p>
+ * You don't typically need to call this during a match. If you do call it,
+ * you should only do so when the robot is stationary and will remain
+ * stationary for a short time.
+ * </p>
+ */
+ public void reset() {
+ m_need_reset = true;
+ }
+
+ /**
+ * Starts up the background thread that accumulates gyro statistics.
+ *
+ * <p>
+ * You never need to call this method unless you have stopped the gyro and now
+ * want to start it up again. If you do call this method, you should probably
+ * also call the {@link #reset} method.
+ * </p>
+ */
+ public void start() {
+ if (!m_thread.isAlive()) {
+ m_thread.start();
+ }
+ }
+
+ /**
+ * Stops the background thread from accumulating angle information (turns OFF
+ * gyro!).
+ *
+ * <p>
+ * This method is not typically called as it stops the gyro from accumulating
+ * statistics essentially turning it off. The only time you might want to do
+ * this is if you are done using the gyro for the rest of the match and want
+ * to save some CPU cyles (for example, if you only needed the gyro during the
+ * autonomous period).
+ * </p>
+ */
+ public void stop() {
+ m_run_thread = false;
+ }
+
+ /**
+ * Sends commands to configure the ITG-3200 the way we need it to run.
+ */
+ private void performResetSequence() {
+ // Configure the gyroscope
+ // Set the gyroscope scale for the outputs to +/-2000 degrees per second
+ m_i2c.write(DLPF_FS, (DLPF_FS_SEL_2000 | DLPF_LOWPASS_188HZ));
+ // Set the sample rate to 100 hz
+ m_i2c.write(SMPLRT_DIV, 9);
+ }
+
+ /**
+ * Enables the buffering of the next "n" data samples (which can then be saved
+ * for analysis).
+ *
+ * @param samples
+ * Maximum number of samples to read.
+ */
+ public void enableBuffer(int samples) {
+ double[] timeBuffer = new double[samples];
+ int[] xBuffer = new int[samples];
+ int[] yBuffer = new int[samples];
+ int[] zBuffer = new int[samples];
+ synchronized (this) {
+ m_timeBuffer = timeBuffer;
+ m_xBuffer = xBuffer;
+ m_yBuffer = yBuffer;
+ m_zBuffer = zBuffer;
+ m_cntBuffer = 0;
+ m_sizeBuffer = samples;
+ }
+ }
+
+ /**
+ * Check to see if the buffer is full.
+ *
+ * @return true if buffer is at capacity.
+ */
+ public boolean isBufferFull() {
+ boolean isFull;
+ synchronized (this) {
+ isFull = (m_cntBuffer == m_sizeBuffer);
+ }
+ return isFull;
+ }
+
+ /**
+ * Writes any raw buffered data to the file "/tmp/gyro-data.csv" for
+ * inspection via Excel.
+ */
+ public void saveBuffer() {
+ double[] timeBuffer;
+ int[] xBuffer;
+ int[] yBuffer;
+ int[] zBuffer;
+ int size;
+
+ // Transfer buffer info to local variables and turn off buffering in a
+ // thread safe way.
+ synchronized (this) {
+ timeBuffer = m_timeBuffer;
+ xBuffer = m_xBuffer;
+ yBuffer = m_yBuffer;
+ zBuffer = m_zBuffer;
+ size = m_cntBuffer;
+ m_sizeBuffer = 0;
+ m_cntBuffer = 0;
+ }
+
+ if (size > 0) {
+ try {
+ PrintStream out = new PrintStream(new File("/tmp/gryo-data.csv"));
+ out.println("\"FPGA Time\",\"x-axis\",\"y-axis\",\"z-axis\"");
+ for (int i = 0; i < size; i++) {
+ out.println(timeBuffer[i] + "," + xBuffer[i] + "," + yBuffer[i] + ","
+ + zBuffer[i]);
+ }
+ out.close();
+ SmartDashboard.putBoolean("Gyro Save OK", true);
+ } catch (IOException ignore) {
+ SmartDashboard.putBoolean("Gyro Save OK", false);
+ }
+ }
+ }
+
+ /**
+ * Internal method run in the background thread that reads values from the
+ * ITG-3200 and updates the accumulators.
+ */
+ private void readRawAngleBytes() {
+ double now = Timer.getFPGATimestamp();
+
+ byte[] buffer = new byte[6];
+ boolean rc = m_i2c.read(GYRO_XOUT_H, buffer.length, buffer);
+
+ if (rc) {
+ // Got a good read, get 16 bit integer values for each axis and
+ // update accumulated values
+ int x = (buffer[0] << 8) | (buffer[1] & 0xff);
+ int y = (buffer[2] << 8) | (buffer[3] & 0xff);
+ int z = (buffer[4] << 8) | (buffer[5] & 0xff);
+
+ m_x.update(x, now);
+ m_y.update(y, now);
+ m_z.update(z, now);
+
+ // If buffered enabled, then save values in a thread safe way
+ if (m_sizeBuffer > 0) {
+ synchronized (this) {
+ int i = m_cntBuffer;
+ if (i < m_sizeBuffer) {
+ m_timeBuffer[i] = now;
+ m_xBuffer[i] = x;
+ m_yBuffer[i] = y;
+ m_zBuffer[i] = z;
+ m_cntBuffer++;
+ }
+ }
+ }
+ }
+
+ if (DEBUG) {
+ String name = toString();
+ String[] labels = { "XOUT_H", "XOUT_L", "YOUT_H", "YOUT_L", "ZOUT_H",
+ "ZOUT_L" };
+ for (int i = 0; i < labels.length; i++) {
+ SmartDashboard.putString(name + " " + labels[i],
+ "0x" + Integer.toHexString(0xff & buffer[i]));
+ }
+ }
+ }
+
+ /**
+ * Helper method to check that we can communicate with the gyro.
+ */
+ private void check() {
+ byte[] buffer = new byte[1];
+ boolean rc = m_i2c.read(WHO_AM_I, buffer.length, buffer);
+ if (DEBUG) {
+ String name = toString();
+ SmartDashboard.putBoolean(name + " Check OK?", rc);
+ SmartDashboard.putNumber(name + " WHO_AM_I", buffer[0]);
+ }
+ }
+
+ /**
+ * Private helper class to accumulate values read from the gryo and convert
+ * degs/sec into degrees.
+ */
+ private class Accumulator {
+ /** Accumulated degrees since zero. */
+ private double m_accumulatedDegs;
+ /**
+ * 2 times the computed bias value that is used when getting average of
+ * readings.
+ */
+ private double m_bias2;
+ /** The prior raw value read from the gyro. */
+ private int m_lastRaw;
+ /** The prior time stamp the last raw value was read. */
+ private double m_lastTime;
+ /** The total count of time the gyro value has been read. */
+ private int m_cnt;
+ /** The sum of all of the raw values read. */
+ private long m_sum;
+
+ /** Multipler to covert 2*Count to degrees/sec (optimization). */
+ private static final double COUNT2_TO_DEGSEC = (COUNT_TO_DEGSEC / 2.0);
+
+ /**
+ * Returns the accumulated degrees.
+ *
+ * @return Accumulated signed degrees since last zeroed.
+ */
+ public synchronized double getDegrees() {
+ return m_accumulatedDegs;
+ }
+
+ /**
+ * @return The raw integer reading from the ITG-3200 associated with the
+ * axis.
+ */
+ public int getRaw() {
+ return m_lastRaw;
+ }
+
+ /**
+ * Returns the number or readings that went into the accumulated degrees.
+ *
+ * @return Count of readings since last zeroed.
+ */
+ public synchronized int getReadings() {
+ return m_cnt;
+ }
+
+ /**
+ * Constructs a new instance.
+ */
+ private Accumulator() {
+ m_bias2 = 0;
+ zero();
+ }
+
+ /**
+ * Zeros out accumulated information.
+ */
+ private void zero() {
+ m_lastRaw = 0;
+ m_lastTime = 0;
+ m_sum = 0;
+ synchronized (this) {
+ m_cnt = 0;
+ m_accumulatedDegs = 0;
+ }
+ }
+
+ /**
+ * Zeros out accumulated information and clears (zeros) the internal bias
+ * value.
+ */
+ private void reset() {
+ zero();
+ m_bias2 = 0;
+ }
+
+ /**
+ * Computes new bias value from accumulated values and then zeros.
+ */
+ private void setBiasByAccumulatedValues() {
+ m_bias2 = 2.0 * ((double) m_sum) / ((double) m_cnt);
+ zero();
+ }
+
+ /**
+ * Updates (accumulates) new value read from axis.
+ *
+ * @param raw
+ * Raw signed 16 bit value read from gyro for axis.
+ * @param time
+ * The time stamp when the value was read.
+ */
+ private void update(int raw, double time) {
+ double degs = 0;
+
+ if (m_cnt != 0) {
+ // Get average of degrees per second over the time span
+ double degPerSec = (m_lastRaw + raw - m_bias2) * COUNT2_TO_DEGSEC;
+ // Get time span this rate occurred for
+ double secs = (m_lastTime - time);
+ // Get number of degrees rotated for time period
+ degs = degPerSec * secs;
+ }
+
+ // Update our thread shared values
+ synchronized (this) {
+ m_accumulatedDegs += degs;
+ m_sum += raw;
+ m_cnt++;
+ m_lastRaw = raw;
+ m_lastTime = time;
+ }
+ }
+ }
+}
--- /dev/null
+package org.usfirst.frc.team3501.robot.sensors;
+
+import java.util.TimerTask;
+
+import edu.wpi.first.wpilibj.I2C;
+import edu.wpi.first.wpilibj.I2C.Port;
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.PIDSourceType;
+import edu.wpi.first.wpilibj.Timer;
+
+public class Lidar implements PIDSource {
+ private I2C i2c;
+ private byte[] distance;
+ private java.util.Timer updater;
+
+ private final int LIDAR_ADDR = 0x62;
+ private final int LIDAR_CONFIG_REGISTER = 0x00;
+ private final int LIDAR_DISTANCE_REGISTER = 0x8f;
+
+ public Lidar(Port port) {
+ i2c = new I2C(port, LIDAR_ADDR);
+
+ distance = new byte[2];
+
+ updater = new java.util.Timer();
+ }
+
+ // Distance in cm
+ public int getDistance() {
+ return (int) Integer.toUnsignedLong(distance[0] << 8)
+ + Byte.toUnsignedInt(distance[1]);
+ }
+
+ @Override
+ public double pidGet() {
+ return getDistance();
+ }
+
+ // Start 10Hz polling
+ public void start() {
+ updater.scheduleAtFixedRate(new LIDARUpdater(), 0, 100);
+ }
+
+ // Start polling for period in milliseconds
+ public void start(int period) {
+ updater.scheduleAtFixedRate(new LIDARUpdater(), 0, period);
+ }
+
+ public void stop() {
+ updater.cancel();
+ updater = new java.util.Timer();
+ }
+
+ // Update distance variable
+ public void update() {
+ i2c.write(LIDAR_CONFIG_REGISTER, 0x04); // Initiate measurement
+ Timer.delay(0.04); // Delay for measurement to be taken
+ i2c.read(LIDAR_DISTANCE_REGISTER, 2, distance); // Read in measurement
+ Timer.delay(0.005); // Delay to prevent over polling
+ }
+
+ // Timer task to keep distance updated
+ private class LIDARUpdater extends TimerTask {
+ @Override
+ public void run() {
+ while (true) {
+ update();
+ try {
+ Thread.sleep(10);
+ } catch (InterruptedException e) {
+ e.printStackTrace();
+ }
+ }
+ }
+ }
+
+ @Override
+ public void setPIDSourceType(PIDSourceType pidSource) {
+ }
+
+ @Override
+ public PIDSourceType getPIDSourceType() {
+ return null;
+ }
+}
--- /dev/null
+package org.usfirst.frc.team3501.robot.sensors;
+
+import edu.wpi.first.wpilibj.AnalogInput;
+
+// TODO: Change certain reactions based on testing, ie what value(s) return true
+// and what value(s) return false.
+
+/***
+ * The photogate is a pair of IR LED and phototransistor sensor that uses a
+ * reflective method to sense the presence of the boulder within the robot's
+ * shooting chamber. This class specifically checks for the ball's presence
+ * using a threshold of voltages outputted from the phototransistor.
+ *
+ * @author niyatisriram
+ */
+public class Photogate extends AnalogInput {
+
+ private double threshold;
+
+ /***
+ * The constructor inputs the channel of the transistor and the threshold
+ * value.
+ * The threshold is a specific value, representing the outputted voltage of
+ * the phototransistor. This value will be somewhere within the range [0,
+ * 4095] Find the value by testing and finding an average value for which the
+ * ball is present when the output is greater, and absent when the output is
+ * less.
+ */
+ public Photogate(int channel, int threshold) {
+ super(channel);
+ this.threshold = threshold;
+ }
+
+ /***
+ * @return whether the ball is present or not
+ */
+ public boolean isBallPresent() {
+ if (this.getValue() > threshold)
+ return true;
+ else
+ return false;
+
+ }
+
+ /***
+ * @param threshold
+ * (range [0, 4095]
+ */
+ public void setThreshold(int threshold) {
+ this.threshold = threshold;
+ }
+}
--- /dev/null
+/**
+ * Copyright (c) 2015, www.techhounds.com
+ * All rights reserved.
+ *
+ * <p>
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * </p>
+ * <ul>
+ * <li>Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.</li>
+ * <li>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.</li>
+ * <li>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.</li>
+ * </ul>
+ *
+ * <p>
+ * 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 <COPYRIGHT HOLDER> 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.
+ * </p>
+ */
+
+package org.usfirst.frc.team3501.robot.sensors;
+
+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.
+ *
+ * <p>
+ * 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.
+ * </p>
+ *
+ * <p>
+ * You will typically use the {@link #getRotationTracker} method associated with
+ * the gyro class associated with your hardware. For example
+ * {@link GyroItg3200.getRotationTrackerZ()}.
+ * </p>
+ */
+public interface RotationTracker extends PIDSource {
+
+ /**
+ * Returns the angle in signed decimal degrees since construction or zeroing.
+ *
+ * <p>
+ * This value is used as the PID sensor value.
+ * </p>
+ *
+ * @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();
+
+}
package org.usfirst.frc.team3501.robot.subsystems;
import org.usfirst.frc.team3501.robot.Constants;
-import org.usfirst.frc.team3501.robot.GyroLib;
-import org.usfirst.frc.team3501.robot.Lidar;
import org.usfirst.frc.team3501.robot.MathLib;
import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive;
+import org.usfirst.frc.team3501.robot.sensors.GyroLib;
+import org.usfirst.frc.team3501.robot.sensors.Lidar;
import edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.CounterBase.EncodingType;
+++ /dev/null
-package org.usfirst.frc.team3501.robot.subsystems;
-
-import edu.wpi.first.wpilibj.AnalogInput;
-
-// TODO: Change certain reactions based on testing, ie what value(s) return true
-// and what value(s) return false.
-
-/***
- * The photogate is a pair of IR LED and phototransistor sensor that uses a
- * reflective method to sense the presence of the boulder within the robot's
- * shooting chamber. This class specifically checks for the ball's presence
- * using a threshold of voltages outputted from the phototransistor.
- *
- * @author niyatisriram
- */
-public class Photogate extends AnalogInput {
-
- private double threshold;
-
- /***
- * The constructor inputs the channel of the transistor and the threshold
- * value.
- * The threshold is a specific value, representing the outputted voltage of
- * the phototransistor. This value will be somewhere within the range [0,
- * 4095] Find the value by testing and finding an average value for which the
- * ball is present when the output is greater, and absent when the output is
- * less.
- */
- public Photogate(int channel, int threshold) {
- super(channel);
- this.threshold = threshold;
- }
-
- /***
- * @return whether the ball is present or not
- */
- public boolean isBallPresent() {
- if (this.getValue() > threshold)
- return true;
- else
- return false;
-
- }
-
- /***
- * @param threshold
- * (range [0, 4095]
- */
- public void setThreshold(int threshold) {
- this.threshold = threshold;
- }
-}
package org.usfirst.frc.team3501.robot.subsystems;
import org.usfirst.frc.team3501.robot.Constants;
-import org.usfirst.frc.team3501.robot.Lidar;
+import org.usfirst.frc.team3501.robot.sensors.Lidar;
import edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.CounterBase.EncodingType;
projects / 3501 / stronghold-2016 / commitdiff