| 1 | package org.usfirst.frc.team3501.robot; |
| 2 | /** |
| 3 | * Copyright (c) 2015, www.techhounds.com |
| 4 | * All rights reserved. |
| 5 | * |
| 6 | * <p> |
| 7 | * Redistribution and use in source and binary forms, with or without |
| 8 | * modification, are permitted provided that the following conditions are met: |
| 9 | * </p> |
| 10 | * <ul> |
| 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> |
| 19 | * </ul> |
| 20 | * |
| 21 | * <p> |
| 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. |
| 32 | * </p> |
| 33 | */ |
| 34 | |
| 35 | import java.io.File; |
| 36 | import java.io.IOException; |
| 37 | import java.io.PrintStream; |
| 38 | |
| 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; |
| 43 | |
| 44 | /** |
| 45 | * A Java wrapper around the ITC based ITG-3200 triple axis gryo. |
| 46 | * |
| 47 | * <p> |
| 48 | * Typical usage: |
| 49 | * </p> |
| 50 | * <ul> |
| 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 |
| 57 | * facing).</li> |
| 58 | * </ul> |
| 59 | * <p> |
| 60 | * Be aware of the following: |
| 61 | * </p> |
| 62 | * <ul> |
| 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 |
| 65 | * possible).</li> |
| 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 |
| 76 | * CPU.</li> |
| 77 | * </ul> |
| 78 | * |
| 79 | * <h2>Suggested Usage</h2> |
| 80 | * <p> |
| 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 |
| 83 | * 90 degrees. |
| 84 | * </p> |
| 85 | * <ul> |
| 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. |
| 91 | * </li> |
| 92 | * </ul> |
| 93 | */ |
| 94 | public final class GyroLib { |
| 95 | |
| 96 | /** |
| 97 | * Object used to monitor robot rotation. |
| 98 | * |
| 99 | * <ul> |
| 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 |
| 111 | * </ul> |
| 112 | */ |
| 113 | public final class Rotation implements RotationTracker { |
| 114 | /** Raw axis accumulator on gyro associated with this rotation tracker. */ |
| 115 | private Accumulator m_axis; |
| 116 | /** |
| 117 | * The degrees reported by the accumulator the last time this tracker was |
| 118 | * zeroed. |
| 119 | */ |
| 120 | private double m_zeroDeg; |
| 121 | /** |
| 122 | * The number of readings reported by the accumulator the last time this |
| 123 | * tracker was zeroed. |
| 124 | */ |
| 125 | private int m_zeroCnt; |
| 126 | |
| 127 | /** |
| 128 | * Constructor is protected, instances are created through the |
| 129 | * {@link GyroLib} methods. |
| 130 | * |
| 131 | * @param axis |
| 132 | * An accumulator from the gyro for the axis to be tracked. |
| 133 | */ |
| 134 | private Rotation(Accumulator axis) { |
| 135 | m_axis = axis; |
| 136 | m_zeroDeg = 0; |
| 137 | m_zeroCnt = 0; |
| 138 | } |
| 139 | |
| 140 | /** |
| 141 | * Zero the tracker (sets the current heading/direction as the zero point). |
| 142 | */ |
| 143 | public void zero() { |
| 144 | m_zeroDeg = m_axis.getDegrees(); |
| 145 | m_zeroCnt = m_axis.getReadings(); |
| 146 | } |
| 147 | |
| 148 | /** |
| 149 | * Get the number of degrees rotated since last zeroed. |
| 150 | * |
| 151 | * @return A signed number of degrees [-INF, +INF]. |
| 152 | */ |
| 153 | public double getAngle() { |
| 154 | double angle = m_axis.getDegrees() - m_zeroDeg; |
| 155 | return angle; |
| 156 | } |
| 157 | |
| 158 | /** |
| 159 | * Get the total number of times the raw values from the gyro have been read |
| 160 | * since zeroed. |
| 161 | * |
| 162 | * @return A diagnostic count that can be used to make sure the angle is |
| 163 | * still being updated. |
| 164 | */ |
| 165 | public int getReadings() { |
| 166 | return m_axis.getReadings() - m_zeroCnt; |
| 167 | } |
| 168 | |
| 169 | /** |
| 170 | * Returns the last raw (integer) value read from the gyro for the axis. |
| 171 | * |
| 172 | * @return An integer value from the ITG-3200 for the associated axis. |
| 173 | */ |
| 174 | public int getAngleRateRaw() { |
| 175 | return m_axis.getRaw(); |
| 176 | } |
| 177 | |
| 178 | /** |
| 179 | * Returns the current rotation rate in degrees/second from the last |
| 180 | * reading. |
| 181 | * |
| 182 | * @return How quickly the system is rotating about the axis in |
| 183 | * degrees/second. |
| 184 | */ |
| 185 | public double getAngleRate() { |
| 186 | return getAngleRateRaw() * COUNT_TO_DEGSEC; |
| 187 | } |
| 188 | |
| 189 | /** |
| 190 | * Returns the angle value from {@link #getAngle()} so object can be used as |
| 191 | * a source to a PID controller. |
| 192 | * |
| 193 | * @return See {@link #getAngle()}. |
| 194 | * |
| 195 | * @see edu.wpi.first.wpilibj.PIDSource#pidGet() |
| 196 | */ |
| 197 | public double pidGet() { |
| 198 | return getAngle(); |
| 199 | } |
| 200 | |
| 201 | public void setPIDSourceType(PIDSourceType pidSource) { |
| 202 | // TODO Auto-generated method stub |
| 203 | |
| 204 | } |
| 205 | |
| 206 | public PIDSourceType getPIDSourceType() { |
| 207 | // TODO Auto-generated method stub |
| 208 | return null; |
| 209 | } |
| 210 | } |
| 211 | |
| 212 | // |
| 213 | // List of I2C registers which the ITG-3200 uses from the datasheet |
| 214 | // |
| 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; |
| 228 | |
| 229 | // |
| 230 | // Bit flags used for interrupt operation |
| 231 | // |
| 232 | |
| 233 | /* |
| 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). |
| 236 | */ |
| 237 | // private static final byte INT_CFG_ACTL_LOW = (byte) (1 << 7); |
| 238 | |
| 239 | /* |
| 240 | * Set drive type for interrupt to open drain mode, omit if you want push-pull |
| 241 | * mode (what does this mean?). |
| 242 | */ |
| 243 | // private static final byte INT_CFG_OPEN_DRAIN = 1 << 6; |
| 244 | |
| 245 | /* |
| 246 | * Interrupt latch mode (remains set until you clear it), omit this flag to |
| 247 | * get a 0-50us interrupt pulse. |
| 248 | */ |
| 249 | // private static final byte INT_CFG_LATCH_INT_EN = 1 << 5; |
| 250 | |
| 251 | /* |
| 252 | * Allow any read operation of data to clear the interrupt flag (otherwise it |
| 253 | * is only cleared after reading status register). |
| 254 | */ |
| 255 | // private static final byte INT_CFG_ANYRD_2CLEAR = 1 << 4; |
| 256 | |
| 257 | /* |
| 258 | * Enable interrup when device is ready (PLL ready after changing clock |
| 259 | * source). Hmmm? |
| 260 | */ |
| 261 | // private static final byte INT_CFG_RDY_EN = 1 << 3; |
| 262 | |
| 263 | /* Enable interrupt when new data is available. */ |
| 264 | // private static final byte INT_CFG_RAW_RDY_EN = 1 << 1; |
| 265 | |
| 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); |
| 269 | |
| 270 | // |
| 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. |
| 273 | // |
| 274 | |
| 275 | // |
| 276 | // The low pass filter bandwidth settings |
| 277 | // |
| 278 | |
| 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; |
| 286 | |
| 287 | /** Select range of +/-2000 deg/sec. (only range supported). */ |
| 288 | private static final byte DLPF_FS_SEL_2000 = 3 << 3; |
| 289 | |
| 290 | /** |
| 291 | * The I2C address of the ITG-3200 when AD0 (pin 9) is jumpered to logic high. |
| 292 | */ |
| 293 | private static final byte itgAddressJumper = 0x69; |
| 294 | |
| 295 | /** |
| 296 | * The I2C address of the ITG-3200 when AD0 (pin 9) is jumpered to logic low. |
| 297 | */ |
| 298 | private static final byte itgAddressNoJumper = 0x68; |
| 299 | |
| 300 | /** Multiplier to convert raw integer values returned to degrees/sec. */ |
| 301 | private static final float COUNT_TO_DEGSEC = (float) (1.0 / 14.375); |
| 302 | |
| 303 | /** Set this to true for lots of diagnostic output. */ |
| 304 | private static final boolean DEBUG = false; |
| 305 | |
| 306 | /** |
| 307 | * How many sample readings to make to determine the bias value for each axis. |
| 308 | */ |
| 309 | private static final int MIN_READINGS_TO_SET_BIAS = 50; |
| 310 | |
| 311 | /** I2C Address to use to communicate with the ITG-3200. */ |
| 312 | private byte m_addr; |
| 313 | |
| 314 | /** I2C object used to communicate with Gyro. */ |
| 315 | private I2C m_i2c; |
| 316 | |
| 317 | /** |
| 318 | * Background thread responsible for accumulating angle data from the sensor. |
| 319 | */ |
| 320 | private Thread m_thread; |
| 321 | |
| 322 | /** Flag used to signal background thread that the gyro should be reset. */ |
| 323 | private boolean m_need_reset; |
| 324 | |
| 325 | /** Flag used to signal background thread that it's time to stop. */ |
| 326 | private boolean m_run_thread; |
| 327 | |
| 328 | /** Accumulator for rotation around the x-axis. */ |
| 329 | private Accumulator m_x; |
| 330 | |
| 331 | /** Accumulator for rotation around the y-axis. */ |
| 332 | private Accumulator m_y; |
| 333 | |
| 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; |
| 342 | |
| 343 | /** |
| 344 | * Construct a new instance of the ITG-3200 gryo class. |
| 345 | * |
| 346 | * <p> |
| 347 | * IMPORTANT |
| 348 | * |
| 349 | * @param port |
| 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. |
| 354 | * @param jumper |
| 355 | * This should be true if the ITG-3200 has the AD0 jumpered to logic |
| 356 | * level high and false if not. |
| 357 | */ |
| 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() { |
| 362 | @Override |
| 363 | public void run() { |
| 364 | accumulateData(); |
| 365 | } |
| 366 | }); |
| 367 | if (DEBUG) { |
| 368 | check(); |
| 369 | } |
| 370 | m_x = new Accumulator(); |
| 371 | m_y = new Accumulator(); |
| 372 | m_z = new Accumulator(); |
| 373 | m_need_reset = true; |
| 374 | |
| 375 | start(); |
| 376 | } |
| 377 | |
| 378 | /** |
| 379 | * Construct a {@link Rotation} object used to monitor rotation about the |
| 380 | * Z-axis. |
| 381 | * |
| 382 | * @return A rotation object that very useful for checking the direction your |
| 383 | * robot is facing. |
| 384 | */ |
| 385 | public Rotation getRotationZ() { |
| 386 | return new Rotation(m_z); |
| 387 | } |
| 388 | |
| 389 | /** |
| 390 | * Construct a {@link Rotation} object used to monitor rotation about the |
| 391 | * X-axis. |
| 392 | * |
| 393 | * @return A rotation object that is probably only useful for checking if your |
| 394 | * robot is starting to tip over. |
| 395 | */ |
| 396 | public Rotation getRotationX() { |
| 397 | return new Rotation(m_x); |
| 398 | } |
| 399 | |
| 400 | /** |
| 401 | * Construct a {@link Rotation} object used to monitor rotation about the |
| 402 | * Y-axis. |
| 403 | * |
| 404 | * @return A rotation object that is probably only useful for checking if your |
| 405 | * robot is starting to tip over. |
| 406 | */ |
| 407 | public Rotation getRotationY() { |
| 408 | return new Rotation(m_y); |
| 409 | } |
| 410 | |
| 411 | /** |
| 412 | * Returns string representation of the object for debug purposes. |
| 413 | */ |
| 414 | public String toString() { |
| 415 | return "Gyro[0x" + Integer.toHexString(m_addr & 0xff) + "]"; |
| 416 | } |
| 417 | |
| 418 | /** |
| 419 | * Dumps information about the state of the Gyro to the smart dashboard. |
| 420 | * |
| 421 | * @param tag |
| 422 | * Short name like "Gyro" to prefix each label with on the dashboard. |
| 423 | * @param debug |
| 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. |
| 427 | */ |
| 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()); |
| 432 | |
| 433 | if (debug) { |
| 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()); |
| 437 | |
| 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()); |
| 441 | |
| 442 | SmartDashboard.putString(tag + " I2C Address", |
| 443 | "0x" + Integer.toHexString(m_addr)); |
| 444 | } |
| 445 | } |
| 446 | |
| 447 | /** |
| 448 | * Internal method that runs in the background thread to accumulate data from |
| 449 | * the Gyro. |
| 450 | */ |
| 451 | private void accumulateData() { |
| 452 | m_run_thread = true; |
| 453 | int resetCnt = 0; |
| 454 | |
| 455 | while (m_run_thread) { |
| 456 | if (m_need_reset) { |
| 457 | // Set gyro to the proper mode |
| 458 | performResetSequence(); |
| 459 | |
| 460 | // Reset accumulators and set the number of readings to take to |
| 461 | // compute bias values |
| 462 | resetCnt = MIN_READINGS_TO_SET_BIAS; |
| 463 | m_x.reset(); |
| 464 | m_y.reset(); |
| 465 | m_z.reset(); |
| 466 | m_need_reset = false; |
| 467 | } else { |
| 468 | // Go read raw values from ITG-3200 and update our accumulators |
| 469 | readRawAngleBytes(); |
| 470 | |
| 471 | if (resetCnt > 0) { |
| 472 | // If we were recently reset, and have made enough initial |
| 473 | // readings, |
| 474 | // then go compute and set our new bias (correction) values |
| 475 | // for each accumulator |
| 476 | resetCnt--; |
| 477 | if (resetCnt == 0) { |
| 478 | m_x.setBiasByAccumulatedValues(); |
| 479 | m_y.setBiasByAccumulatedValues(); |
| 480 | m_z.setBiasByAccumulatedValues(); |
| 481 | } |
| 482 | } |
| 483 | } |
| 484 | |
| 485 | // Short delay between each reading |
| 486 | if (m_run_thread) { |
| 487 | Timer.delay(.01); |
| 488 | } |
| 489 | } |
| 490 | } |
| 491 | |
| 492 | /** |
| 493 | * Singles the gyro's background thread that we want to reset the gyro. |
| 494 | * |
| 495 | * <p> |
| 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. |
| 499 | * </p> |
| 500 | */ |
| 501 | public void reset() { |
| 502 | m_need_reset = true; |
| 503 | } |
| 504 | |
| 505 | /** |
| 506 | * Starts up the background thread that accumulates gyro statistics. |
| 507 | * |
| 508 | * <p> |
| 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. |
| 512 | * </p> |
| 513 | */ |
| 514 | public void start() { |
| 515 | if (!m_thread.isAlive()) { |
| 516 | m_thread.start(); |
| 517 | } |
| 518 | } |
| 519 | |
| 520 | /** |
| 521 | * Stops the background thread from accumulating angle information (turns OFF |
| 522 | * gyro!). |
| 523 | * |
| 524 | * <p> |
| 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). |
| 530 | * </p> |
| 531 | */ |
| 532 | public void stop() { |
| 533 | m_run_thread = false; |
| 534 | } |
| 535 | |
| 536 | /** |
| 537 | * Sends commands to configure the ITG-3200 the way we need it to run. |
| 538 | */ |
| 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); |
| 545 | } |
| 546 | |
| 547 | /** |
| 548 | * Enables the buffering of the next "n" data samples (which can then be saved |
| 549 | * for analysis). |
| 550 | * |
| 551 | * @param samples |
| 552 | * Maximum number of samples to read. |
| 553 | */ |
| 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; |
| 561 | m_xBuffer = xBuffer; |
| 562 | m_yBuffer = yBuffer; |
| 563 | m_zBuffer = zBuffer; |
| 564 | m_cntBuffer = 0; |
| 565 | m_sizeBuffer = samples; |
| 566 | } |
| 567 | } |
| 568 | |
| 569 | /** |
| 570 | * Check to see if the buffer is full. |
| 571 | * |
| 572 | * @return true if buffer is at capacity. |
| 573 | */ |
| 574 | public boolean isBufferFull() { |
| 575 | boolean isFull; |
| 576 | synchronized (this) { |
| 577 | isFull = (m_cntBuffer == m_sizeBuffer); |
| 578 | } |
| 579 | return isFull; |
| 580 | } |
| 581 | |
| 582 | /** |
| 583 | * Writes any raw buffered data to the file "/tmp/gyro-data.csv" for |
| 584 | * inspection via Excel. |
| 585 | */ |
| 586 | public void saveBuffer() { |
| 587 | double[] timeBuffer; |
| 588 | int[] xBuffer; |
| 589 | int[] yBuffer; |
| 590 | int[] zBuffer; |
| 591 | int size; |
| 592 | |
| 593 | // Transfer buffer info to local variables and turn off buffering in a |
| 594 | // thread safe way. |
| 595 | synchronized (this) { |
| 596 | timeBuffer = m_timeBuffer; |
| 597 | xBuffer = m_xBuffer; |
| 598 | yBuffer = m_yBuffer; |
| 599 | zBuffer = m_zBuffer; |
| 600 | size = m_cntBuffer; |
| 601 | m_sizeBuffer = 0; |
| 602 | m_cntBuffer = 0; |
| 603 | } |
| 604 | |
| 605 | if (size > 0) { |
| 606 | try { |
| 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] + "," |
| 611 | + zBuffer[i]); |
| 612 | } |
| 613 | out.close(); |
| 614 | SmartDashboard.putBoolean("Gyro Save OK", true); |
| 615 | } catch (IOException ignore) { |
| 616 | SmartDashboard.putBoolean("Gyro Save OK", false); |
| 617 | } |
| 618 | } |
| 619 | } |
| 620 | |
| 621 | /** |
| 622 | * Internal method run in the background thread that reads values from the |
| 623 | * ITG-3200 and updates the accumulators. |
| 624 | */ |
| 625 | private void readRawAngleBytes() { |
| 626 | double now = Timer.getFPGATimestamp(); |
| 627 | |
| 628 | byte[] buffer = new byte[6]; |
| 629 | boolean rc = m_i2c.read(GYRO_XOUT_H, buffer.length, buffer); |
| 630 | |
| 631 | if (rc) { |
| 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); |
| 637 | |
| 638 | m_x.update(x, now); |
| 639 | m_y.update(y, now); |
| 640 | m_z.update(z, now); |
| 641 | |
| 642 | // If buffered enabled, then save values in a thread safe way |
| 643 | if (m_sizeBuffer > 0) { |
| 644 | synchronized (this) { |
| 645 | int i = m_cntBuffer; |
| 646 | if (i < m_sizeBuffer) { |
| 647 | m_timeBuffer[i] = now; |
| 648 | m_xBuffer[i] = x; |
| 649 | m_yBuffer[i] = y; |
| 650 | m_zBuffer[i] = z; |
| 651 | m_cntBuffer++; |
| 652 | } |
| 653 | } |
| 654 | } |
| 655 | } |
| 656 | |
| 657 | if (DEBUG) { |
| 658 | String name = toString(); |
| 659 | String[] labels = { "XOUT_H", "XOUT_L", "YOUT_H", "YOUT_L", "ZOUT_H", |
| 660 | "ZOUT_L" }; |
| 661 | for (int i = 0; i < labels.length; i++) { |
| 662 | SmartDashboard.putString(name + " " + labels[i], |
| 663 | "0x" + Integer.toHexString(0xff & buffer[i])); |
| 664 | } |
| 665 | } |
| 666 | } |
| 667 | |
| 668 | /** |
| 669 | * Helper method to check that we can communicate with the gyro. |
| 670 | */ |
| 671 | private void check() { |
| 672 | byte[] buffer = new byte[1]; |
| 673 | boolean rc = m_i2c.read(WHO_AM_I, buffer.length, buffer); |
| 674 | if (DEBUG) { |
| 675 | String name = toString(); |
| 676 | SmartDashboard.putBoolean(name + " Check OK?", rc); |
| 677 | SmartDashboard.putNumber(name + " WHO_AM_I", buffer[0]); |
| 678 | } |
| 679 | } |
| 680 | |
| 681 | /** |
| 682 | * Private helper class to accumulate values read from the gryo and convert |
| 683 | * degs/sec into degrees. |
| 684 | */ |
| 685 | private class Accumulator { |
| 686 | /** Accumulated degrees since zero. */ |
| 687 | private double m_accumulatedDegs; |
| 688 | /** |
| 689 | * 2 times the computed bias value that is used when getting average of |
| 690 | * readings. |
| 691 | */ |
| 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. */ |
| 698 | private int m_cnt; |
| 699 | /** The sum of all of the raw values read. */ |
| 700 | private long m_sum; |
| 701 | |
| 702 | /** Multipler to covert 2*Count to degrees/sec (optimization). */ |
| 703 | private static final double COUNT2_TO_DEGSEC = (COUNT_TO_DEGSEC / 2.0); |
| 704 | |
| 705 | /** |
| 706 | * Returns the accumulated degrees. |
| 707 | * |
| 708 | * @return Accumulated signed degrees since last zeroed. |
| 709 | */ |
| 710 | public synchronized double getDegrees() { |
| 711 | return m_accumulatedDegs; |
| 712 | } |
| 713 | |
| 714 | /** |
| 715 | * @return The raw integer reading from the ITG-3200 associated with the |
| 716 | * axis. |
| 717 | */ |
| 718 | public int getRaw() { |
| 719 | return m_lastRaw; |
| 720 | } |
| 721 | |
| 722 | /** |
| 723 | * Returns the number or readings that went into the accumulated degrees. |
| 724 | * |
| 725 | * @return Count of readings since last zeroed. |
| 726 | */ |
| 727 | public synchronized int getReadings() { |
| 728 | return m_cnt; |
| 729 | } |
| 730 | |
| 731 | /** |
| 732 | * Constructs a new instance. |
| 733 | */ |
| 734 | private Accumulator() { |
| 735 | m_bias2 = 0; |
| 736 | zero(); |
| 737 | } |
| 738 | |
| 739 | /** |
| 740 | * Zeros out accumulated information. |
| 741 | */ |
| 742 | private void zero() { |
| 743 | m_lastRaw = 0; |
| 744 | m_lastTime = 0; |
| 745 | m_sum = 0; |
| 746 | synchronized (this) { |
| 747 | m_cnt = 0; |
| 748 | m_accumulatedDegs = 0; |
| 749 | } |
| 750 | } |
| 751 | |
| 752 | /** |
| 753 | * Zeros out accumulated information and clears (zeros) the internal bias |
| 754 | * value. |
| 755 | */ |
| 756 | private void reset() { |
| 757 | zero(); |
| 758 | m_bias2 = 0; |
| 759 | } |
| 760 | |
| 761 | /** |
| 762 | * Computes new bias value from accumulated values and then zeros. |
| 763 | */ |
| 764 | private void setBiasByAccumulatedValues() { |
| 765 | m_bias2 = 2.0 * ((double) m_sum) / ((double) m_cnt); |
| 766 | zero(); |
| 767 | } |
| 768 | |
| 769 | /** |
| 770 | * Updates (accumulates) new value read from axis. |
| 771 | * |
| 772 | * @param raw |
| 773 | * Raw signed 16 bit value read from gyro for axis. |
| 774 | * @param time |
| 775 | * The time stamp when the value was read. |
| 776 | */ |
| 777 | private void update(int raw, double time) { |
| 778 | double degs = 0; |
| 779 | |
| 780 | if (m_cnt != 0) { |
| 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; |
| 787 | } |
| 788 | |
| 789 | // Update our thread shared values |
| 790 | synchronized (this) { |
| 791 | m_accumulatedDegs += degs; |
| 792 | m_sum += raw; |
| 793 | m_cnt++; |
| 794 | m_lastRaw = raw; |
| 795 | m_lastTime = time; |
| 796 | } |
| 797 | } |
| 798 | } |
| 799 | } |