Permalink
Find file
Fetching contributors…
Cannot retrieve contributors at this time
794 lines (711 sloc) 23.8 KB
/**
* 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 com.techhounds.gyro;
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 GyroItg3200} 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 GyroItg3200 {
/**
* Object used to monitor robot rotation.
*
* <ul>
* <li>Use this class to track how much your robot has rotated.</li>
* <li>Use the {@link GyroItg3200#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 GyroItg3200#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 GyroItg3200#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 GyroItg3200#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 GyroItg3200} 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()
*/
@Override
public double pidGet() {
return getAngle();
}
@Override
public void setPIDSourceType(PIDSourceType pidSource) {
// TODO Auto-generated method stub
}
@Override
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 GyroItg3200(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;
}
}
}
}