From: EvanYap Date: Thu, 11 Feb 2016 05:30:05 +0000 (-0800) Subject: add another team's gyro code in case decision to use either one would proceed for... X-Git-Url: http://challenge-bot.com/repos/?a=commitdiff_plain;h=191c43e3ebd037ac152f1470c79c52f7b7627a89;p=3501%2Fstronghold-2016 add another team's gyro code in case decision to use either one would proceed for the better judgement for the robot --- diff --git a/src/org/usfirst/frc/team3501/robot/AnotherGyroClass.java b/src/org/usfirst/frc/team3501/robot/AnotherGyroClass.java new file mode 100644 index 00000000..a6e4380e --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/AnotherGyroClass.java @@ -0,0 +1,824 @@ +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.interfaces.Gyro; +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 AnotherGyroClass { + + /** + * Object used to monitor robot rotation. + * + * + */ + public final class Rotation implements Gyro { + /** 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 AnotherGyroClass} 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; + } + + @Override + public void calibrate() { + // TODO Auto-generated method stub + + } + + @Override + public void reset() { + // TODO Auto-generated method stub + + } + + @Override + public double getRate() { + // TODO Auto-generated method stub + return 0; + } + + @Override + public void free() { + // TODO Auto-generated method stub + + } + } + + // + // 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 AnotherGyroClass(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; + } + } + } +}