From: Kevin Zhang Date: Wed, 17 Feb 2016 18:44:18 +0000 (-0800) Subject: Put all sensors in sensor package and update the import paths X-Git-Url: http://challenge-bot.com/repos/?p=3501%2Fstronghold-2016;a=commitdiff_plain;h=7670b3f40e062d762b0d329f4ec36472f496ccbd Put all sensors in sensor package and update the import paths --- diff --git a/src/org/usfirst/frc/team3501/robot/GyroLib.java b/src/org/usfirst/frc/team3501/robot/GyroLib.java deleted file mode 100644 index 36d177c3..00000000 --- a/src/org/usfirst/frc/team3501/robot/GyroLib.java +++ /dev/null @@ -1,799 +0,0 @@ -package org.usfirst.frc.team3501.robot; -/** - * 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: - *

- * - * - *

- * 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. - *

- */ - -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. - * - *

- * Typical usage: - *

- * - *

- * Be aware of the following: - *

- * - * - *

Suggested Usage

- *

- * 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. - *

- * - */ -public final class GyroLib { - - /** - * Object used to monitor robot rotation. - * - * - */ - 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. - * - *

- * 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. - * - *

- * 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. - *

- */ - public void reset() { - m_need_reset = true; - } - - /** - * Starts up the background thread that accumulates gyro statistics. - * - *

- * 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. - *

- */ - public void start() { - if (!m_thread.isAlive()) { - m_thread.start(); - } - } - - /** - * Stops the background thread from accumulating angle information (turns OFF - * gyro!). - * - *

- * 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). - *

- */ - 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; - } - } - } -} diff --git a/src/org/usfirst/frc/team3501/robot/Lidar.java b/src/org/usfirst/frc/team3501/robot/Lidar.java deleted file mode 100644 index 2e6aee64..00000000 --- a/src/org/usfirst/frc/team3501/robot/Lidar.java +++ /dev/null @@ -1,85 +0,0 @@ -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; - } -} diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java index 9e2ec446..90584412 100644 --- a/src/org/usfirst/frc/team3501/robot/Robot.java +++ b/src/org/usfirst/frc/team3501/robot/Robot.java @@ -1,10 +1,11 @@ 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; diff --git a/src/org/usfirst/frc/team3501/robot/RotationTracker.java b/src/org/usfirst/frc/team3501/robot/RotationTracker.java deleted file mode 100644 index f0757ff1..00000000 --- a/src/org/usfirst/frc/team3501/robot/RotationTracker.java +++ /dev/null @@ -1,74 +0,0 @@ -/** - * 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: - *

- * - * - *

- * 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(); - -} diff --git a/src/org/usfirst/frc/team3501/robot/sensors/GyroLib.java b/src/org/usfirst/frc/team3501/robot/sensors/GyroLib.java new file mode 100644 index 00000000..c683b516 --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/sensors/GyroLib.java @@ -0,0 +1,799 @@ +package org.usfirst.frc.team3501.robot.sensors; +/** + * 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: + *

+ * + * + *

+ * 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. + *

+ */ + +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. + * + *

+ * Typical usage: + *

+ * + *

+ * Be aware of the following: + *

+ * + * + *

Suggested Usage

+ *

+ * 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. + *

+ * + */ +public final class GyroLib { + + /** + * Object used to monitor robot rotation. + * + * + */ + 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. + * + *

+ * 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. + * + *

+ * 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. + *

+ */ + public void reset() { + m_need_reset = true; + } + + /** + * Starts up the background thread that accumulates gyro statistics. + * + *

+ * 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. + *

+ */ + public void start() { + if (!m_thread.isAlive()) { + m_thread.start(); + } + } + + /** + * Stops the background thread from accumulating angle information (turns OFF + * gyro!). + * + *

+ * 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). + *

+ */ + 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; + } + } + } +} diff --git a/src/org/usfirst/frc/team3501/robot/sensors/Lidar.java b/src/org/usfirst/frc/team3501/robot/sensors/Lidar.java new file mode 100644 index 00000000..8ba641e3 --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/sensors/Lidar.java @@ -0,0 +1,85 @@ +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; + } +} diff --git a/src/org/usfirst/frc/team3501/robot/sensors/Photogate.java b/src/org/usfirst/frc/team3501/robot/sensors/Photogate.java new file mode 100644 index 00000000..ddb52472 --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/sensors/Photogate.java @@ -0,0 +1,52 @@ +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; + } +} diff --git a/src/org/usfirst/frc/team3501/robot/sensors/RotationTracker.java b/src/org/usfirst/frc/team3501/robot/sensors/RotationTracker.java new file mode 100644 index 00000000..4c0d6df8 --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/sensors/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: + *

+ * + * + *

+ * 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.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. + * + *

+ * 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(); + +} diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java index bbca72fc..2b11491a 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java @@ -1,10 +1,10 @@ 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; diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/Photogate.java b/src/org/usfirst/frc/team3501/robot/subsystems/Photogate.java deleted file mode 100644 index 33f7b151..00000000 --- a/src/org/usfirst/frc/team3501/robot/subsystems/Photogate.java +++ /dev/null @@ -1,52 +0,0 @@ -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; - } -} diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java b/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java index a9e8d6e8..bcce7bcd 100755 --- a/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java @@ -1,7 +1,7 @@ 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;