1 package org
.usfirst
.frc
.team3501
.robot
;
3 * Copyright (c) 2015, www.techhounds.com
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions are met:
11 * <li>Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.</li>
13 * <li>Redistributions in binary form must reproduce the above copyright
14 * notice, this list of conditions and the following disclaimer in the
15 * documentation and/or other materials provided with the distribution.</li>
16 * <li>Neither the name of the www.techhounds.com nor the
17 * names of its contributors may be used to endorse or promote products
18 * derived from this software without specific prior written permission.</li>
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
23 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
24 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
25 * DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
26 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
27 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
29 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
30 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
31 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
36 import java
.io
.IOException
;
37 import java
.io
.PrintStream
;
39 import edu
.wpi
.first
.wpilibj
.I2C
;
40 import edu
.wpi
.first
.wpilibj
.PIDSourceType
;
41 import edu
.wpi
.first
.wpilibj
.Timer
;
42 import edu
.wpi
.first
.wpilibj
.smartdashboard
.SmartDashboard
;
45 * A Java wrapper around the ITC based ITG-3200 triple axis gryo.
51 * <li>Make sure your ITG-3200 is connected to the I2C bus on the roboRIO and
52 * mounted flat (so Z axis is used to track direction robot is facing).</li>
53 * <li>Construct a single instance of the {@link GyroLib} class to be
54 * shared throughout your Robot code.</li>
55 * <li>Use the {@link #getRotationZ()} method create "trackers" that allow you
56 * to keep track of how much your robot has rotated (direction your robot is
60 * Be aware of the following:
63 * <li>Angles are in signed degrees (both positive and negative values are
64 * possible) and not necessarily normalized (large values like -1203 degrees are
66 * <li>The {@link #reset} method is called once initially at construction.
67 * Reseting the gyro should only be done when your robot is stationary and it
68 * may take up to one second. You should not need to do this and should avoid
69 * doing this during the autonomous or teleop periods (unless you know your
70 * robot won't be moving).</li>
71 * <li>There is a background thread that is automatically started that keeps
72 * reading and accumulating values from the ITG-3200. You should not need to use
73 * the {@link #start()} or {@link #stop()} methods during normal matches.
74 * However, if you only use the gyro during the autonomous period, you can use
75 * the {@link #stop()} method at the end of the autonomous period to save some
79 * <h2>Suggested Usage</h2>
81 * You should be able to use this class to aid your robot in making relative
82 * turns. For example, if you want to create a command which rotates your robot
86 * <li>In your command's initialize method, use the {@link #getRotationZ()} and
87 * the {@link Rotation#zero()} method on the returned {@link Rotation} object to
88 * track how much you have turned.</li>
89 * <li>Use the {@link Rotation} object as a PID source and/or check the current
90 * angle reported by the {@link Rotation} object in your isFinished() method.
94 public final class GyroLib
{
97 * Object used to monitor robot rotation.
100 * <li>Use this class to track how much your robot has rotated.</li>
101 * <li>Use the {@link GyroLib#getRotationZ()} to create an instance
102 * to track how much your robot has rotated around the z-axis (direction robot
103 * is facing - useful for making turns)..</li>
104 * <li>Use the {@link GyroLib#getRotationX()} to create an instance
105 * to track how much your robot has rotated around the x-axis (hopefully not
106 * much unless you are tipping).</li>
107 * <li>Use the {@link GyroLib#getRotationY()} to create an instance
108 * to track how much your robot has rotated around the y-axis (hopefully not
109 * much unless you are tipping).</li>
110 * <li>Use the {@link GyroLib#getRo
113 public final class Rotation
implements RotationTracker
{
114 /** Raw axis accumulator on gyro associated with this rotation tracker. */
115 private Accumulator m_axis
;
117 * The degrees reported by the accumulator the last time this tracker was
120 private double m_zeroDeg
;
122 * The number of readings reported by the accumulator the last time this
123 * tracker was zeroed.
125 private int m_zeroCnt
;
128 * Constructor is protected, instances are created through the
129 * {@link GyroLib} methods.
132 * An accumulator from the gyro for the axis to be tracked.
134 private Rotation(Accumulator axis
) {
141 * Zero the tracker (sets the current heading/direction as the zero point).
144 m_zeroDeg
= m_axis
.getDegrees();
145 m_zeroCnt
= m_axis
.getReadings();
149 * Get the number of degrees rotated since last zeroed.
151 * @return A signed number of degrees [-INF, +INF].
153 public double getAngle() {
154 double angle
= m_axis
.getDegrees() - m_zeroDeg
;
159 * Get the total number of times the raw values from the gyro have been read
162 * @return A diagnostic count that can be used to make sure the angle is
163 * still being updated.
165 public int getReadings() {
166 return m_axis
.getReadings() - m_zeroCnt
;
170 * Returns the last raw (integer) value read from the gyro for the axis.
172 * @return An integer value from the ITG-3200 for the associated axis.
174 public int getAngleRateRaw() {
175 return m_axis
.getRaw();
179 * Returns the current rotation rate in degrees/second from the last
182 * @return How quickly the system is rotating about the axis in
185 public double getAngleRate() {
186 return getAngleRateRaw() * COUNT_TO_DEGSEC
;
190 * Returns the angle value from {@link #getAngle()} so object can be used as
191 * a source to a PID controller.
193 * @return See {@link #getAngle()}.
195 * @see edu.wpi.first.wpilibj.PIDSource#pidGet()
197 public double pidGet() {
201 public void setPIDSourceType(PIDSourceType pidSource
) {
202 // TODO Auto-generated method stub
206 public PIDSourceType
getPIDSourceType() {
207 // TODO Auto-generated method stub
213 // List of I2C registers which the ITG-3200 uses from the datasheet
215 private static final byte WHO_AM_I
= 0x00;
216 private static final byte SMPLRT_DIV
= 0x15;
217 private static final byte DLPF_FS
= 0x16;
218 // private static final byte INT_CFG = 0x17;
219 // private static final byte INT_STATUS = 0x1A;
220 // private static final byte TEMP_OUT_H = 0x1B;
221 // private static final byte TEMP_OUT_L = 0x1C;
222 private static final byte GYRO_XOUT_H
= 0x1D;
223 // private static final byte GYRO_XOUT_L = 0x1E;
224 // private static final byte GYRO_YOUT_H = 0x1F;
225 // private static final byte GYRO_YOUT_L = 0x20;
226 // private static final byte GYRO_ZOUT_H = 0x21;
227 // private static final byte GYRO_ZOUT_L = 0x22;
230 // Bit flags used for interrupt operation
234 * Set this bit in INT_CFG register for logic level of INT output pin to be
235 * low when interrupt is active (leave 0 if you want it high).
237 // private static final byte INT_CFG_ACTL_LOW = (byte) (1 << 7);
240 * Set drive type for interrupt to open drain mode, omit if you want push-pull
241 * mode (what does this mean?).
243 // private static final byte INT_CFG_OPEN_DRAIN = 1 << 6;
246 * Interrupt latch mode (remains set until you clear it), omit this flag to
247 * get a 0-50us interrupt pulse.
249 // private static final byte INT_CFG_LATCH_INT_EN = 1 << 5;
252 * Allow any read operation of data to clear the interrupt flag (otherwise it
253 * is only cleared after reading status register).
255 // private static final byte INT_CFG_ANYRD_2CLEAR = 1 << 4;
258 * Enable interrup when device is ready (PLL ready after changing clock
261 // private static final byte INT_CFG_RDY_EN = 1 << 3;
263 /* Enable interrupt when new data is available. */
264 // private static final byte INT_CFG_RAW_RDY_EN = 1 << 1;
266 /* Guess at mode to use if we want to try enabling interrupts. */
267 // private static final byte INT_CFG_SETTING = (INT_CFG_LATCH_INT_EN |
268 // INT_CFG_ANYRD_2CLEAR | INT_CFG_RAW_RDY_EN);
271 // The bit flags that can be set in the DLPF register on the ITG-3200
272 // as specified in the ITG-3200 data sheet.
276 // The low pass filter bandwidth settings
279 // private static final byte DLPF_LOWPASS_256HZ = 0 << 0;
280 private static final byte DLPF_LOWPASS_188HZ
= 1 << 0;
281 // private static final byte DLPF_LOWPASS_98HZ = 2 << 0;
282 // private static final byte DLPF_LOWPASS_42HZ = 3 << 0;
283 // private static final byte DLPF_LOWPASS_20HZ = 4 << 0;
284 // private static final byte DLPF_LOWPASS_10HZ = 5 << 0;
285 // private static final byte DLPF_LOWPASS_5HZ = 6 << 0;
287 /** Select range of +/-2000 deg/sec. (only range supported). */
288 private static final byte DLPF_FS_SEL_2000
= 3 << 3;
291 * The I2C address of the ITG-3200 when AD0 (pin 9) is jumpered to logic high.
293 private static final byte itgAddressJumper
= 0x69;
296 * The I2C address of the ITG-3200 when AD0 (pin 9) is jumpered to logic low.
298 private static final byte itgAddressNoJumper
= 0x68;
300 /** Multiplier to convert raw integer values returned to degrees/sec. */
301 private static final float COUNT_TO_DEGSEC
= (float) (1.0 / 14.375);
303 /** Set this to true for lots of diagnostic output. */
304 private static final boolean DEBUG
= false;
307 * How many sample readings to make to determine the bias value for each axis.
309 private static final int MIN_READINGS_TO_SET_BIAS
= 50;
311 /** I2C Address to use to communicate with the ITG-3200. */
314 /** I2C object used to communicate with Gyro. */
318 * Background thread responsible for accumulating angle data from the sensor.
320 private Thread m_thread
;
322 /** Flag used to signal background thread that the gyro should be reset. */
323 private boolean m_need_reset
;
325 /** Flag used to signal background thread that it's time to stop. */
326 private boolean m_run_thread
;
328 /** Accumulator for rotation around the x-axis. */
329 private Accumulator m_x
;
331 /** Accumulator for rotation around the y-axis. */
332 private Accumulator m_y
;
334 /** Accumulator for rotation around the z-axis. */
335 private Accumulator m_z
;
336 private int[] m_xBuffer
;
337 private int[] m_yBuffer
;
338 private int[] m_zBuffer
;
339 private int m_cntBuffer
;
340 private int m_sizeBuffer
;
341 private double[] m_timeBuffer
;
344 * Construct a new instance of the ITG-3200 gryo class.
350 * This should be {@link I2C.Port#kOnboard} if the ITG-3200 is
351 * connected to the main I2C bus on the roboRIO. This should be
352 * {@link I2C.Port#kMXP} if it is connected to the I2C bus on the MXP
353 * port on the roboRIO.
355 * This should be true if the ITG-3200 has the AD0 jumpered to logic
356 * level high and false if not.
358 public GyroLib(I2C
.Port port
, boolean jumper
) {
359 m_addr
= (jumper ? itgAddressJumper
: itgAddressNoJumper
);
360 m_i2c
= new I2C(port
, m_addr
);
361 m_thread
= new Thread(new Runnable() {
370 m_x
= new Accumulator();
371 m_y
= new Accumulator();
372 m_z
= new Accumulator();
379 * Construct a {@link Rotation} object used to monitor rotation about the
382 * @return A rotation object that very useful for checking the direction your
385 public Rotation
getRotationZ() {
386 return new Rotation(m_z
);
390 * Construct a {@link Rotation} object used to monitor rotation about the
393 * @return A rotation object that is probably only useful for checking if your
394 * robot is starting to tip over.
396 public Rotation
getRotationX() {
397 return new Rotation(m_x
);
401 * Construct a {@link Rotation} object used to monitor rotation about the
404 * @return A rotation object that is probably only useful for checking if your
405 * robot is starting to tip over.
407 public Rotation
getRotationY() {
408 return new Rotation(m_y
);
412 * Returns string representation of the object for debug purposes.
414 public String
toString() {
415 return "Gyro[0x" + Integer
.toHexString(m_addr
& 0xff) + "]";
419 * Dumps information about the state of the Gyro to the smart dashboard.
422 * Short name like "Gyro" to prefix each label with on the dashboard.
424 * Pass true if you want a whole lot of details dumped onto the
425 * dashboard, pass false if you just want the direction of each axis
426 * and the temperature.
428 public void updateDashboard(String tag
, boolean debug
) {
429 SmartDashboard
.putNumber(tag
+ " x-axis degrees", m_x
.getDegrees());
430 SmartDashboard
.putNumber(tag
+ " y-axis degrees", m_y
.getDegrees());
431 SmartDashboard
.putNumber(tag
+ " z-axis degrees", m_z
.getDegrees());
434 SmartDashboard
.putNumber(tag
+ " x-axis raw", m_x
.getRaw());
435 SmartDashboard
.putNumber(tag
+ " y-axis raw", m_y
.getRaw());
436 SmartDashboard
.putNumber(tag
+ " z-axis raw", m_z
.getRaw());
438 SmartDashboard
.putNumber(tag
+ " x-axis count", m_x
.getReadings());
439 SmartDashboard
.putNumber(tag
+ " y-axis count", m_y
.getReadings());
440 SmartDashboard
.putNumber(tag
+ " z-axis count", m_z
.getReadings());
442 SmartDashboard
.putString(tag
+ " I2C Address",
443 "0x" + Integer
.toHexString(m_addr
));
448 * Internal method that runs in the background thread to accumulate data from
451 private void accumulateData() {
455 while (m_run_thread
) {
457 // Set gyro to the proper mode
458 performResetSequence();
460 // Reset accumulators and set the number of readings to take to
461 // compute bias values
462 resetCnt
= MIN_READINGS_TO_SET_BIAS
;
466 m_need_reset
= false;
468 // Go read raw values from ITG-3200 and update our accumulators
472 // If we were recently reset, and have made enough initial
474 // then go compute and set our new bias (correction) values
475 // for each accumulator
478 m_x
.setBiasByAccumulatedValues();
479 m_y
.setBiasByAccumulatedValues();
480 m_z
.setBiasByAccumulatedValues();
485 // Short delay between each reading
493 * Singles the gyro's background thread that we want to reset the gyro.
496 * You don't typically need to call this during a match. If you do call it,
497 * you should only do so when the robot is stationary and will remain
498 * stationary for a short time.
501 public void reset() {
506 * Starts up the background thread that accumulates gyro statistics.
509 * You never need to call this method unless you have stopped the gyro and now
510 * want to start it up again. If you do call this method, you should probably
511 * also call the {@link #reset} method.
514 public void start() {
515 if (!m_thread
.isAlive()) {
521 * Stops the background thread from accumulating angle information (turns OFF
525 * This method is not typically called as it stops the gyro from accumulating
526 * statistics essentially turning it off. The only time you might want to do
527 * this is if you are done using the gyro for the rest of the match and want
528 * to save some CPU cyles (for example, if you only needed the gyro during the
529 * autonomous period).
533 m_run_thread
= false;
537 * Sends commands to configure the ITG-3200 the way we need it to run.
539 private void performResetSequence() {
540 // Configure the gyroscope
541 // Set the gyroscope scale for the outputs to +/-2000 degrees per second
542 m_i2c
.write(DLPF_FS
, (DLPF_FS_SEL_2000
| DLPF_LOWPASS_188HZ
));
543 // Set the sample rate to 100 hz
544 m_i2c
.write(SMPLRT_DIV
, 9);
548 * Enables the buffering of the next "n" data samples (which can then be saved
552 * Maximum number of samples to read.
554 public void enableBuffer(int samples
) {
555 double[] timeBuffer
= new double[samples
];
556 int[] xBuffer
= new int[samples
];
557 int[] yBuffer
= new int[samples
];
558 int[] zBuffer
= new int[samples
];
559 synchronized (this) {
560 m_timeBuffer
= timeBuffer
;
565 m_sizeBuffer
= samples
;
570 * Check to see if the buffer is full.
572 * @return true if buffer is at capacity.
574 public boolean isBufferFull() {
576 synchronized (this) {
577 isFull
= (m_cntBuffer
== m_sizeBuffer
);
583 * Writes any raw buffered data to the file "/tmp/gyro-data.csv" for
584 * inspection via Excel.
586 public void saveBuffer() {
593 // Transfer buffer info to local variables and turn off buffering in a
595 synchronized (this) {
596 timeBuffer
= m_timeBuffer
;
607 PrintStream out
= new PrintStream(new File("/tmp/gryo-data.csv"));
608 out
.println("\"FPGA Time\",\"x-axis\",\"y-axis\",\"z-axis\"");
609 for (int i
= 0; i
< size
; i
++) {
610 out
.println(timeBuffer
[i
] + "," + xBuffer
[i
] + "," + yBuffer
[i
] + ","
614 SmartDashboard
.putBoolean("Gyro Save OK", true);
615 } catch (IOException ignore
) {
616 SmartDashboard
.putBoolean("Gyro Save OK", false);
622 * Internal method run in the background thread that reads values from the
623 * ITG-3200 and updates the accumulators.
625 private void readRawAngleBytes() {
626 double now
= Timer
.getFPGATimestamp();
628 byte[] buffer
= new byte[6];
629 boolean rc
= m_i2c
.read(GYRO_XOUT_H
, buffer
.length
, buffer
);
632 // Got a good read, get 16 bit integer values for each axis and
633 // update accumulated values
634 int x
= (buffer
[0] << 8) | (buffer
[1] & 0xff);
635 int y
= (buffer
[2] << 8) | (buffer
[3] & 0xff);
636 int z
= (buffer
[4] << 8) | (buffer
[5] & 0xff);
642 // If buffered enabled, then save values in a thread safe way
643 if (m_sizeBuffer
> 0) {
644 synchronized (this) {
646 if (i
< m_sizeBuffer
) {
647 m_timeBuffer
[i
] = now
;
658 String name
= toString();
659 String
[] labels
= { "XOUT_H", "XOUT_L", "YOUT_H", "YOUT_L", "ZOUT_H",
661 for (int i
= 0; i
< labels
.length
; i
++) {
662 SmartDashboard
.putString(name
+ " " + labels
[i
],
663 "0x" + Integer
.toHexString(0xff & buffer
[i
]));
669 * Helper method to check that we can communicate with the gyro.
671 private void check() {
672 byte[] buffer
= new byte[1];
673 boolean rc
= m_i2c
.read(WHO_AM_I
, buffer
.length
, buffer
);
675 String name
= toString();
676 SmartDashboard
.putBoolean(name
+ " Check OK?", rc
);
677 SmartDashboard
.putNumber(name
+ " WHO_AM_I", buffer
[0]);
682 * Private helper class to accumulate values read from the gryo and convert
683 * degs/sec into degrees.
685 private class Accumulator
{
686 /** Accumulated degrees since zero. */
687 private double m_accumulatedDegs
;
689 * 2 times the computed bias value that is used when getting average of
692 private double m_bias2
;
693 /** The prior raw value read from the gyro. */
694 private int m_lastRaw
;
695 /** The prior time stamp the last raw value was read. */
696 private double m_lastTime
;
697 /** The total count of time the gyro value has been read. */
699 /** The sum of all of the raw values read. */
702 /** Multipler to covert 2*Count to degrees/sec (optimization). */
703 private static final double COUNT2_TO_DEGSEC
= (COUNT_TO_DEGSEC
/ 2.0);
706 * Returns the accumulated degrees.
708 * @return Accumulated signed degrees since last zeroed.
710 public synchronized double getDegrees() {
711 return m_accumulatedDegs
;
715 * @return The raw integer reading from the ITG-3200 associated with the
718 public int getRaw() {
723 * Returns the number or readings that went into the accumulated degrees.
725 * @return Count of readings since last zeroed.
727 public synchronized int getReadings() {
732 * Constructs a new instance.
734 private Accumulator() {
740 * Zeros out accumulated information.
742 private void zero() {
746 synchronized (this) {
748 m_accumulatedDegs
= 0;
753 * Zeros out accumulated information and clears (zeros) the internal bias
756 private void reset() {
762 * Computes new bias value from accumulated values and then zeros.
764 private void setBiasByAccumulatedValues() {
765 m_bias2
= 2.0 * ((double) m_sum
) / ((double) m_cnt
);
770 * Updates (accumulates) new value read from axis.
773 * Raw signed 16 bit value read from gyro for axis.
775 * The time stamp when the value was read.
777 private void update(int raw
, double time
) {
781 // Get average of degrees per second over the time span
782 double degPerSec
= (m_lastRaw
+ raw
- m_bias2
) * COUNT2_TO_DEGSEC
;
783 // Get time span this rate occurred for
784 double secs
= (m_lastTime
- time
);
785 // Get number of degrees rotated for time period
786 degs
= degPerSec
* secs
;
789 // Update our thread shared values
790 synchronized (this) {
791 m_accumulatedDegs
+= degs
;