001// Copyright (c) FIRST and other WPILib contributors. 002// Open Source Software; you can modify and/or share it under the terms of 003// the WPILib BSD license file in the root directory of this project. 004 005/*----------------------------------------------------------------------------*/ 006/* Copyright (c) FIRST 2016. All Rights Reserved. */ 007/* Open Source Software - may be modified and shared by FRC teams. The code */ 008/* must be accompanied by the FIRST BSD license file in the root directory of */ 009/* the project. */ 010/*----------------------------------------------------------------------------*/ 011 012package edu.wpi.first.wpilibj; 013 014// import java.lang.FdLibm.Pow; 015import edu.wpi.first.hal.FRCNetComm.tResourceType; 016import edu.wpi.first.hal.HAL; 017import edu.wpi.first.hal.SimBoolean; 018import edu.wpi.first.hal.SimDevice; 019import edu.wpi.first.hal.SimDouble; 020import edu.wpi.first.util.sendable.Sendable; 021import edu.wpi.first.util.sendable.SendableBuilder; 022import java.nio.ByteBuffer; 023import java.nio.ByteOrder; 024 025// CHECKSTYLE.OFF: TypeName 026// CHECKSTYLE.OFF: MemberName 027// CHECKSTYLE.OFF: SummaryJavadoc 028// CHECKSTYLE.OFF: UnnecessaryParentheses 029// CHECKSTYLE.OFF: OverloadMethodsDeclarationOrder 030// CHECKSTYLE.OFF: NonEmptyAtclauseDescription 031// CHECKSTYLE.OFF: MissingOverride 032// CHECKSTYLE.OFF: AtclauseOrder 033// CHECKSTYLE.OFF: LocalVariableName 034// CHECKSTYLE.OFF: RedundantModifier 035// CHECKSTYLE.OFF: AbbreviationAsWordInName 036// CHECKSTYLE.OFF: ParameterName 037// CHECKSTYLE.OFF: EmptyCatchBlock 038// CHECKSTYLE.OFF: MissingJavadocMethod 039// CHECKSTYLE.OFF: MissingSwitchDefault 040// CHECKSTYLE.OFF: VariableDeclarationUsageDistance 041// CHECKSTYLE.OFF: ArrayTypeStyle 042 043/** This class is for the ADIS16470 IMU that connects to the RoboRIO SPI port. */ 044@SuppressWarnings({ 045 "unused", 046 "PMD.RedundantFieldInitializer", 047 "PMD.ImmutableField", 048 "PMD.SingularField", 049 "PMD.CollapsibleIfStatements", 050 "PMD.MissingOverride", 051 "PMD.EmptyIfStmt", 052 "PMD.EmptyStatementNotInLoop" 053}) 054public class ADIS16470_IMU implements AutoCloseable, Sendable { 055 /* ADIS16470 Register Map Declaration */ 056 private static final int FLASH_CNT = 0x00; // Flash memory write count 057 private static final int DIAG_STAT = 0x02; // Diagnostic and operational status 058 private static final int X_GYRO_LOW = 0x04; // X-axis gyroscope output, lower word 059 private static final int X_GYRO_OUT = 0x06; // X-axis gyroscope output, upper word 060 private static final int Y_GYRO_LOW = 0x08; // Y-axis gyroscope output, lower word 061 private static final int Y_GYRO_OUT = 0x0A; // Y-axis gyroscope output, upper word 062 private static final int Z_GYRO_LOW = 0x0C; // Z-axis gyroscope output, lower word 063 private static final int Z_GYRO_OUT = 0x0E; // Z-axis gyroscope output, upper word 064 private static final int X_ACCL_LOW = 0x10; // X-axis accelerometer output, lower word 065 private static final int X_ACCL_OUT = 0x12; // X-axis accelerometer output, upper word 066 private static final int Y_ACCL_LOW = 0x14; // Y-axis accelerometer output, lower word 067 private static final int Y_ACCL_OUT = 0x16; // Y-axis accelerometer output, upper word 068 private static final int Z_ACCL_LOW = 0x18; // Z-axis accelerometer output, lower word 069 private static final int Z_ACCL_OUT = 0x1A; // Z-axis accelerometer output, upper word 070 private static final int TEMP_OUT = 0x1C; // Temperature output (internal, not calibrated) 071 private static final int TIME_STAMP = 0x1E; // PPS mode time stamp 072 private static final int X_DELTANG_LOW = 0x24; // X-axis delta angle output, lower word 073 private static final int X_DELTANG_OUT = 0x26; // X-axis delta angle output, upper word 074 private static final int Y_DELTANG_LOW = 0x28; // Y-axis delta angle output, lower word 075 private static final int Y_DELTANG_OUT = 0x2A; // Y-axis delta angle output, upper word 076 private static final int Z_DELTANG_LOW = 0x2C; // Z-axis delta angle output, lower word 077 private static final int Z_DELTANG_OUT = 0x2E; // Z-axis delta angle output, upper word 078 private static final int X_DELTVEL_LOW = 0x30; // X-axis delta velocity output, lower word 079 private static final int X_DELTVEL_OUT = 0x32; // X-axis delta velocity output, upper word 080 private static final int Y_DELTVEL_LOW = 0x34; // Y-axis delta velocity output, lower word 081 private static final int Y_DELTVEL_OUT = 0x36; // Y-axis delta velocity output, upper word 082 private static final int Z_DELTVEL_LOW = 0x38; // Z-axis delta velocity output, lower word 083 private static final int Z_DELTVEL_OUT = 0x3A; // Z-axis delta velocity output, upper word 084 private static final int XG_BIAS_LOW = 085 0x40; // X-axis gyroscope bias offset correction, lower word 086 private static final int XG_BIAS_HIGH = 087 0x42; // X-axis gyroscope bias offset correction, upper word 088 private static final int YG_BIAS_LOW = 089 0x44; // Y-axis gyroscope bias offset correction, lower word 090 private static final int YG_BIAS_HIGH = 091 0x46; // Y-axis gyroscope bias offset correction, upper word 092 private static final int ZG_BIAS_LOW = 093 0x48; // Z-axis gyroscope bias offset correction, lower word 094 private static final int ZG_BIAS_HIGH = 095 0x4A; // Z-axis gyroscope bias offset correction, upper word 096 private static final int XA_BIAS_LOW = 097 0x4C; // X-axis accelerometer bias offset correction, lower word 098 private static final int XA_BIAS_HIGH = 099 0x4E; // X-axis accelerometer bias offset correction, upper word 100 private static final int YA_BIAS_LOW = 101 0x50; // Y-axis accelerometer bias offset correction, lower word 102 private static final int YA_BIAS_HIGH = 103 0x52; // Y-axis accelerometer bias offset correction, upper word 104 private static final int ZA_BIAS_LOW = 105 0x54; // Z-axis accelerometer bias offset correction, lower word 106 private static final int ZA_BIAS_HIGH = 107 0x56; // Z-axis accelerometer bias offset correction, upper word 108 private static final int FILT_CTRL = 0x5C; // Filter control 109 private static final int MSC_CTRL = 0x60; // Miscellaneous control 110 private static final int UP_SCALE = 0x62; // Clock scale factor, PPS mode 111 private static final int DEC_RATE = 0x64; // Decimation rate control (output data rate) 112 private static final int NULL_CNFG = 0x66; // Auto-null configuration control 113 private static final int GLOB_CMD = 0x68; // Global commands 114 private static final int FIRM_REV = 0x6C; // Firmware revision 115 private static final int FIRM_DM = 0x6E; // Firmware revision date, month and day 116 private static final int FIRM_Y = 0x70; // Firmware revision date, year 117 private static final int PROD_ID = 0x72; // Product identification 118 private static final int SERIAL_NUM = 0x74; // Serial number (relative to assembly lot) 119 private static final int USER_SCR1 = 0x76; // User scratch register 1 120 private static final int USER_SCR2 = 0x78; // User scratch register 2 121 private static final int USER_SCR3 = 0x7A; // User scratch register 3 122 private static final int FLSHCNT_LOW = 0x7C; // Flash update count, lower word 123 private static final int FLSHCNT_HIGH = 0x7E; // Flash update count, upper word 124 125 // Weight between the previous and current gyro angles represents 1 second for the timestamp, 126 // this is the point at which we ignore the previous angle because it is too old to be of use. 127 // The IMU timestamp conversion is 1 = 49.02us, the value 1_000_000 is the number of microseconds 128 // we average over. 129 private static final double AVERAGE_RATE_SCALING_FACTOR = 49.02 / 1_000_000; 130 131 private static final byte[] m_autospi_allAngle_packet = { 132 X_DELTANG_OUT, 133 FLASH_CNT, 134 X_DELTANG_LOW, 135 FLASH_CNT, 136 Y_DELTANG_OUT, 137 FLASH_CNT, 138 Y_DELTANG_LOW, 139 FLASH_CNT, 140 Z_DELTANG_OUT, 141 FLASH_CNT, 142 Z_DELTANG_LOW, 143 FLASH_CNT, 144 X_GYRO_OUT, 145 FLASH_CNT, 146 Y_GYRO_OUT, 147 FLASH_CNT, 148 Z_GYRO_OUT, 149 FLASH_CNT, 150 X_ACCL_OUT, 151 FLASH_CNT, 152 Y_ACCL_OUT, 153 FLASH_CNT, 154 Z_ACCL_OUT, 155 FLASH_CNT 156 }; 157 158 /** ADIS16470 calibration times. */ 159 public enum CalibrationTime { 160 /** 32 ms calibration time */ 161 _32ms(0), 162 /** 64 ms calibration time */ 163 _64ms(1), 164 /** 128 ms calibration time */ 165 _128ms(2), 166 /** 256 ms calibration time */ 167 _256ms(3), 168 /** 512 ms calibration time */ 169 _512ms(4), 170 /** 1 s calibration time */ 171 _1s(5), 172 /** 2 s calibration time */ 173 _2s(6), 174 /** 4 s calibration time */ 175 _4s(7), 176 /** 8 s calibration time */ 177 _8s(8), 178 /** 16 s calibration time */ 179 _16s(9), 180 /** 32 s calibration time */ 181 _32s(10), 182 /** 64 s calibration time */ 183 _64s(11); 184 185 private final int value; 186 187 CalibrationTime(int value) { 188 this.value = value; 189 } 190 } 191 192 /** 193 * IMU axes. 194 * 195 * <p>kX, kY, and kZ refer to the IMU's X, Y, and Z axes respectively. kYaw, kPitch, and kRoll are 196 * configured by the user to refer to an X, Y, or Z axis. 197 */ 198 public enum IMUAxis { 199 /** The IMU's X axis */ 200 kX, 201 /** The IMU's Y axis */ 202 kY, 203 /** The IMU's Z axis */ 204 kZ, 205 /** The user-configured yaw axis */ 206 kYaw, 207 /** The user-configured pitch axis */ 208 kPitch, 209 /** The user-configured roll axis */ 210 kRoll, 211 } 212 213 // Static Constants 214 private static final double delta_angle_sf = 2160.0 / 2147483648.0; /* 2160 / (2^31) */ 215 private static final double rad_to_deg = 57.2957795; 216 private static final double deg_to_rad = 0.0174532; 217 private static final double grav = 9.81; 218 219 // User-specified axes 220 private IMUAxis m_yaw_axis; 221 private IMUAxis m_pitch_axis; 222 private IMUAxis m_roll_axis; 223 224 // Instant raw outputs 225 private double m_gyro_rate_x = 0.0; 226 private double m_gyro_rate_y = 0.0; 227 private double m_gyro_rate_z = 0.0; 228 private double m_average_gyro_rate_x = 0.0; 229 private double m_average_gyro_rate_y = 0.0; 230 private double m_average_gyro_rate_z = 0.0; 231 private double m_accel_x = 0.0; 232 private double m_accel_y = 0.0; 233 private double m_accel_z = 0.0; 234 235 // Integrated gyro angle 236 private double m_integ_angle_x = 0.0; 237 private double m_integ_angle_y = 0.0; 238 private double m_integ_angle_z = 0.0; 239 240 // Complementary filter variables 241 private double m_dt = 0.0; 242 private double m_alpha = 0.0; 243 private double m_tau = 1.0; 244 private double m_compAngleX = 0.0; 245 private double m_compAngleY = 0.0; 246 private double m_accelAngleX = 0.0; 247 private double m_accelAngleY = 0.0; 248 249 // State variables 250 private volatile boolean m_thread_active = false; 251 private int m_calibration_time = 0; 252 private volatile boolean m_first_run = true; 253 private volatile boolean m_thread_idle = false; 254 private boolean m_auto_configured = false; 255 private double m_scaled_sample_rate = 2500.0; 256 private boolean m_needs_flash = false; 257 258 // Resources 259 private SPI m_spi; 260 private SPI.Port m_spi_port; 261 private DigitalInput m_auto_interrupt; 262 private DigitalOutput m_reset_out; 263 private DigitalInput m_reset_in; 264 private DigitalOutput m_status_led; 265 private Thread m_acquire_task; 266 private boolean m_connected; 267 268 private SimDevice m_simDevice; 269 private SimBoolean m_simConnected; 270 private SimDouble m_simGyroAngleX; 271 private SimDouble m_simGyroAngleY; 272 private SimDouble m_simGyroAngleZ; 273 private SimDouble m_simGyroRateX; 274 private SimDouble m_simGyroRateY; 275 private SimDouble m_simGyroRateZ; 276 private SimDouble m_simAccelX; 277 private SimDouble m_simAccelY; 278 private SimDouble m_simAccelZ; 279 280 private static class AcquireTask implements Runnable { 281 private ADIS16470_IMU imu; 282 283 public AcquireTask(ADIS16470_IMU imu) { 284 this.imu = imu; 285 } 286 287 @Override 288 public void run() { 289 imu.acquire(); 290 } 291 } 292 293 /** 294 * Creates a new ADIS16740 IMU object. 295 * 296 * <p>The default setup is the onboard SPI port with a calibration time of 1 second. Yaw, pitch, 297 * and roll are kZ, kX, and kY respectively. 298 */ 299 public ADIS16470_IMU() { 300 this(IMUAxis.kZ, IMUAxis.kX, IMUAxis.kY, SPI.Port.kOnboardCS0, CalibrationTime._1s); 301 } 302 303 /** 304 * Creates a new ADIS16740 IMU object. 305 * 306 * <p>The default setup is the onboard SPI port with a calibration time of 1 second. 307 * 308 * <p><b><i>Input axes limited to kX, kY and kZ. Specifying kYaw, kPitch,or kRoll will result in 309 * an error.</i></b> 310 * 311 * @param yaw_axis The axis that measures the yaw 312 * @param pitch_axis The axis that measures the pitch 313 * @param roll_axis The axis that measures the roll 314 */ 315 public ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis, IMUAxis roll_axis) { 316 this(yaw_axis, pitch_axis, roll_axis, SPI.Port.kOnboardCS0, CalibrationTime._1s); 317 } 318 319 /** 320 * Creates a new ADIS16740 IMU object. 321 * 322 * <p><b><i>Input axes limited to kX, kY and kZ. Specifying kYaw, kPitch,or kRoll will result in 323 * an error.</i></b> 324 * 325 * @param yaw_axis The axis that measures the yaw 326 * @param pitch_axis The axis that measures the pitch 327 * @param roll_axis The axis that measures the roll 328 * @param port The SPI Port the gyro is plugged into 329 * @param cal_time Calibration time 330 */ 331 @SuppressWarnings("this-escape") 332 public ADIS16470_IMU( 333 IMUAxis yaw_axis, 334 IMUAxis pitch_axis, 335 IMUAxis roll_axis, 336 SPI.Port port, 337 CalibrationTime cal_time) { 338 if (yaw_axis == IMUAxis.kYaw 339 || yaw_axis == IMUAxis.kPitch 340 || yaw_axis == IMUAxis.kRoll 341 || pitch_axis == IMUAxis.kYaw 342 || pitch_axis == IMUAxis.kPitch 343 || pitch_axis == IMUAxis.kRoll 344 || roll_axis == IMUAxis.kYaw 345 || roll_axis == IMUAxis.kPitch 346 || roll_axis == IMUAxis.kRoll) { 347 DriverStation.reportError( 348 "ADIS16470 constructor only allows IMUAxis.kX, IMUAxis.kY or IMUAxis.kZ as arguments.", 349 false); 350 DriverStation.reportError( 351 "Constructing ADIS with default axes. (IMUAxis.kZ is defined as Yaw)", false); 352 yaw_axis = IMUAxis.kZ; 353 pitch_axis = IMUAxis.kY; 354 roll_axis = IMUAxis.kX; 355 } 356 357 m_yaw_axis = yaw_axis; 358 m_pitch_axis = pitch_axis; 359 m_roll_axis = roll_axis; 360 361 m_calibration_time = cal_time.value; 362 m_spi_port = port; 363 364 m_acquire_task = new Thread(new AcquireTask(this)); 365 366 m_simDevice = SimDevice.create("Gyro:ADIS16470", port.value); 367 if (m_simDevice != null) { 368 m_simConnected = m_simDevice.createBoolean("connected", SimDevice.Direction.kInput, true); 369 m_simGyroAngleX = m_simDevice.createDouble("gyro_angle_x", SimDevice.Direction.kInput, 0.0); 370 m_simGyroAngleY = m_simDevice.createDouble("gyro_angle_y", SimDevice.Direction.kInput, 0.0); 371 m_simGyroAngleZ = m_simDevice.createDouble("gyro_angle_z", SimDevice.Direction.kInput, 0.0); 372 m_simGyroRateX = m_simDevice.createDouble("gyro_rate_x", SimDevice.Direction.kInput, 0.0); 373 m_simGyroRateY = m_simDevice.createDouble("gyro_rate_y", SimDevice.Direction.kInput, 0.0); 374 m_simGyroRateZ = m_simDevice.createDouble("gyro_rate_z", SimDevice.Direction.kInput, 0.0); 375 m_simAccelX = m_simDevice.createDouble("accel_x", SimDevice.Direction.kInput, 0.0); 376 m_simAccelY = m_simDevice.createDouble("accel_y", SimDevice.Direction.kInput, 0.0); 377 m_simAccelZ = m_simDevice.createDouble("accel_z", SimDevice.Direction.kInput, 0.0); 378 } 379 380 if (m_simDevice == null) { 381 // Force the IMU reset pin to toggle on startup (doesn't require DS enable) 382 // Relies on the RIO hardware by default configuring an output as low 383 // and configuring an input as high Z. The 10k pull-up resistor internal to the 384 // IMU then forces the reset line high for normal operation. 385 m_reset_out = new DigitalOutput(27); // Drive SPI CS2 (IMU RST) low 386 Timer.delay(0.01); // Wait 10ms 387 m_reset_out.close(); 388 m_reset_in = new DigitalInput(27); // Set SPI CS2 (IMU RST) high 389 Timer.delay(0.25); // Wait 250ms for reset to complete 390 391 if (!switchToStandardSPI()) { 392 return; 393 } 394 395 // Set IMU internal decimation to 4 (ODR = 2000 SPS / (4 + 1) = 400Hz), BW = 200Hz 396 if (readRegister(DEC_RATE) != 0x0004) { 397 writeRegister(DEC_RATE, 0x0004); 398 m_needs_flash = true; 399 DriverStation.reportWarning( 400 "ADIS16470: DEC_RATE register configuration inconsistent! Scheduling flash update.", 401 false); 402 } 403 404 // Set data ready polarity (HIGH = Good Data), Disable gSense Compensation and PoP 405 if (readRegister(MSC_CTRL) != 0x0001) { 406 writeRegister(MSC_CTRL, 0x0001); 407 m_needs_flash = true; 408 DriverStation.reportWarning( 409 "ADIS16470: MSC_CTRL register configuration inconsistent! Scheduling flash update.", 410 false); 411 } 412 413 // Disable IMU internal Bartlett filter (200Hz bandwidth is sufficient) 414 if (readRegister(FILT_CTRL) != 0x0000) { 415 writeRegister(FILT_CTRL, 0x0000); 416 m_needs_flash = true; 417 DriverStation.reportWarning( 418 "ADIS16470: FILT_CTRL register configuration inconsistent! Scheduling flash update.", 419 false); 420 } 421 422 // If any registers on the IMU don't match the config, trigger a flash update 423 if (m_needs_flash) { 424 DriverStation.reportWarning( 425 "ADIS16470: Register configuration changed! Starting IMU flash update.", false); 426 writeRegister(GLOB_CMD, 0x0008); 427 // Wait long enough for the flash update to finish (72ms minimum as per the datasheet) 428 Timer.delay(0.3); 429 DriverStation.reportWarning("ADIS16470: Flash update finished!", false); 430 m_needs_flash = false; 431 } else { 432 DriverStation.reportWarning( 433 "ADIS16470: Flash and RAM configuration consistent. No flash update required!", false); 434 } 435 436 // Configure continuous bias calibration time based on user setting 437 writeRegister(NULL_CNFG, (m_calibration_time | 0x0700)); 438 439 // Notify DS that IMU calibration delay is active 440 DriverStation.reportWarning("ADIS16470: Starting initial calibration delay.", false); 441 442 // Wait for samples to accumulate internal to the IMU (110% of user-defined 443 // time) 444 try { 445 Thread.sleep((long) (Math.pow(2.0, m_calibration_time) / 2000 * 64 * 1.1 * 1000)); 446 } catch (InterruptedException e) { 447 } 448 449 // Write offset calibration command to IMU 450 writeRegister(GLOB_CMD, 0x0001); 451 452 // Configure and enable auto SPI 453 if (!switchToAutoSPI()) { 454 return; 455 } 456 457 // Let the user know the IMU was initiallized successfully 458 DriverStation.reportWarning("ADIS16470 IMU Successfully Initialized!", false); 459 460 // Drive "Ready" LED low 461 m_status_led = new DigitalOutput(28); // Set SPI CS3 (IMU Ready LED) low 462 } 463 464 // Report usage and post data to DS 465 HAL.report(tResourceType.kResourceType_ADIS16470, 0); 466 m_connected = true; 467 } 468 469 /** 470 * Checks the connection status of the IMU. 471 * 472 * @return True if the IMU is connected, false otherwise. 473 */ 474 public boolean isConnected() { 475 if (m_simConnected != null) { 476 return m_simConnected.get(); 477 } 478 return m_connected; 479 } 480 481 /** 482 * @param buf 483 * @return 484 */ 485 private static int toUShort(ByteBuffer buf) { 486 return buf.getShort(0) & 0xFFFF; 487 } 488 489 /** 490 * @param sint 491 * @return 492 */ 493 private static long toULong(int sint) { 494 return sint & 0x00000000FFFFFFFFL; 495 } 496 497 /** 498 * @param buf 499 * @return 500 */ 501 private static int toShort(int... buf) { 502 return (short) (((buf[0] & 0xFF) << 8) + (buf[1] & 0xFF)); 503 } 504 505 /** 506 * @param buf 507 * @return 508 */ 509 private static int toInt(int... buf) { 510 return (buf[0] & 0xFF) << 24 | (buf[1] & 0xFF) << 16 | (buf[2] & 0xFF) << 8 | (buf[3] & 0xFF); 511 } 512 513 /** 514 * Switch to standard SPI mode. 515 * 516 * @return True if successful, false otherwise. 517 */ 518 private boolean switchToStandardSPI() { 519 // Check to see whether the acquire thread is active. If so, wait for it to stop 520 // producing data. 521 if (m_thread_active) { 522 m_thread_active = false; 523 while (!m_thread_idle) { 524 try { 525 Thread.sleep(10); 526 } catch (InterruptedException e) { 527 } 528 } 529 System.out.println("Paused the IMU processing thread successfully!"); 530 // Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI. 531 if (m_spi != null && m_auto_configured) { 532 m_spi.stopAuto(); 533 // We need to get rid of all the garbage left in the auto SPI buffer after 534 // stopping it. 535 // Sometimes data magically reappears, so we have to check the buffer size a 536 // couple of times 537 // to be sure we got it all. Yuck. 538 int[] trashBuffer = new int[200]; 539 try { 540 Thread.sleep(100); 541 } catch (InterruptedException e) { 542 } 543 int data_count = m_spi.readAutoReceivedData(trashBuffer, 0, 0); 544 while (data_count > 0) { 545 m_spi.readAutoReceivedData(trashBuffer, Math.min(data_count, 200), 0); 546 data_count = m_spi.readAutoReceivedData(trashBuffer, 0, 0); 547 } 548 System.out.println("Paused auto SPI successfully."); 549 } 550 } 551 // There doesn't seem to be a SPI port active. Let's try to set one up 552 if (m_spi == null) { 553 System.out.println("Setting up a new SPI port."); 554 m_spi = new SPI(m_spi_port); 555 m_spi.setClockRate(2000000); 556 m_spi.setMode(SPI.Mode.kMode3); 557 m_spi.setChipSelectActiveLow(); 558 readRegister(PROD_ID); // Dummy read 559 560 // Validate the product ID 561 if (readRegister(PROD_ID) != 16982) { 562 DriverStation.reportError("Could not find ADIS16470", false); 563 close(); 564 return false; 565 } 566 return true; 567 } else { 568 // Maybe the SPI port is active, but not in auto SPI mode? Try to read the 569 // product ID. 570 readRegister(PROD_ID); // dummy read 571 if (readRegister(PROD_ID) != 16982) { 572 DriverStation.reportError("Could not find an ADIS16470", false); 573 close(); 574 return false; 575 } else { 576 return true; 577 } 578 } 579 } 580 581 /** 582 * Switch to auto SPI mode. 583 * 584 * @return True if successful, false otherwise. 585 */ 586 boolean switchToAutoSPI() { 587 // No SPI port has been set up. Go set one up first. 588 if (m_spi == null) { 589 if (!switchToStandardSPI()) { 590 DriverStation.reportError("Failed to start/restart auto SPI", false); 591 return false; 592 } 593 } 594 // Only set up the interrupt if needed. 595 if (m_auto_interrupt == null) { 596 // Configure interrupt on SPI CS1 597 m_auto_interrupt = new DigitalInput(26); 598 } 599 // The auto SPI controller gets angry if you try to set up two instances on one 600 // bus. 601 if (!m_auto_configured) { 602 m_spi.initAuto(8200); 603 m_auto_configured = true; 604 } 605 // Do we need to change auto SPI settings? 606 m_spi.setAutoTransmitData(m_autospi_allAngle_packet, 2); 607 // Configure auto stall time 608 m_spi.configureAutoStall(5, 1000, 1); 609 // Kick off auto SPI (Note: Device configuration impossible after auto SPI is 610 // activated) 611 // DR High = Data good (data capture should be triggered on the rising edge) 612 m_spi.startAutoTrigger(m_auto_interrupt, true, false); 613 614 // Check to see if the acquire thread is running. If not, kick one off. 615 if (!m_acquire_task.isAlive()) { 616 m_first_run = true; 617 m_thread_active = true; 618 m_acquire_task.start(); 619 System.out.println("Processing thread activated!"); 620 } else { 621 // The thread was running, re-init run variables and start it up again. 622 m_first_run = true; 623 m_thread_active = true; 624 System.out.println("Processing thread activated!"); 625 } 626 // Looks like the thread didn't start for some reason. Abort. 627 if (!m_acquire_task.isAlive()) { 628 DriverStation.reportError("Failed to start/restart the acquire() thread.", false); 629 close(); 630 return false; 631 } 632 return true; 633 } 634 635 /** 636 * Configures calibration time 637 * 638 * @param new_cal_time New calibration time 639 * @return 1 if the new calibration time is the same as the current one else 0 640 */ 641 public int configCalTime(CalibrationTime new_cal_time) { 642 if (m_calibration_time == new_cal_time.value) { 643 return 1; 644 } 645 if (!switchToStandardSPI()) { 646 DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false); 647 return 2; 648 } 649 m_calibration_time = new_cal_time.value; 650 writeRegister(NULL_CNFG, (m_calibration_time | 0x700)); 651 if (!switchToAutoSPI()) { 652 DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false); 653 return 2; 654 } 655 return 0; 656 } 657 658 /** 659 * Configures the decimation rate of the IMU. 660 * 661 * @param decimationRate The new decimation value. 662 * @return 0 if success, 1 if no change, 2 if error. 663 */ 664 public int configDecRate(int decimationRate) { 665 // Switches the active SPI port to standard SPI mode, writes a new value to 666 // the DECIMATE register in the IMU, and re-enables auto SPI. 667 // 668 // This function enters standard SPI mode, writes a new DECIMATE setting to 669 // the IMU, adjusts the sample scale factor, and re-enters auto SPI mode. 670 if (!switchToStandardSPI()) { 671 DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false); 672 return 2; 673 } 674 if (decimationRate > 1999) { 675 DriverStation.reportError("Attempted to write an invalid decimation value.", false); 676 decimationRate = 1999; 677 } 678 m_scaled_sample_rate = (((decimationRate + 1.0) / 2000.0) * 1000000.0); 679 writeRegister(DEC_RATE, decimationRate); 680 System.out.println("Decimation register: " + readRegister(DEC_RATE)); 681 if (!switchToAutoSPI()) { 682 DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false); 683 return 2; 684 } 685 return 0; 686 } 687 688 /** 689 * Calibrate the gyro. It's important to make sure that the robot is not moving while the 690 * calibration is in progress, this is typically done when the robot is first turned on while it's 691 * sitting at rest before the match starts. 692 */ 693 public void calibrate() { 694 if (!switchToStandardSPI()) { 695 DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false); 696 } 697 writeRegister(GLOB_CMD, 0x0001); 698 if (!switchToAutoSPI()) { 699 DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false); 700 } 701 } 702 703 /** 704 * @param reg 705 * @return 706 */ 707 private int readRegister(int reg) { 708 ByteBuffer buf = ByteBuffer.allocateDirect(2); 709 buf.order(ByteOrder.BIG_ENDIAN); 710 buf.put(0, (byte) (reg & 0x7f)); 711 buf.put(1, (byte) 0); 712 713 m_spi.write(buf, 2); 714 m_spi.read(false, buf, 2); 715 716 return toUShort(buf); 717 } 718 719 /** 720 * @param reg 721 * @param val 722 */ 723 private void writeRegister(int reg, int val) { 724 ByteBuffer buf = ByteBuffer.allocateDirect(2); 725 // low byte 726 buf.put(0, (byte) (0x80 | reg)); 727 buf.put(1, (byte) (val & 0xff)); 728 m_spi.write(buf, 2); 729 // high byte 730 buf.put(0, (byte) (0x81 | reg)); 731 buf.put(1, (byte) (val >> 8)); 732 m_spi.write(buf, 2); 733 } 734 735 /** Delete (free) the spi port used for the IMU. */ 736 @Override 737 public void close() { 738 if (m_thread_active) { 739 m_thread_active = false; 740 try { 741 if (m_acquire_task != null) { 742 m_acquire_task.join(); 743 m_acquire_task = null; 744 } 745 } catch (InterruptedException e) { 746 } 747 if (m_spi != null) { 748 if (m_auto_configured) { 749 m_spi.stopAuto(); 750 } 751 m_spi.close(); 752 m_auto_configured = false; 753 if (m_auto_interrupt != null) { 754 m_auto_interrupt.close(); 755 m_auto_interrupt = null; 756 } 757 m_spi = null; 758 } 759 } 760 if (m_simDevice != null) { 761 m_simDevice.close(); 762 m_simDevice = null; 763 } 764 System.out.println("Finished cleaning up after the IMU driver."); 765 } 766 767 /** */ 768 private void acquire() { 769 // Set data packet length 770 final int dataset_len = 27; // 26 data points + timestamp 771 final int BUFFER_SIZE = 4000; 772 773 // Set up buffers and variables 774 int[] buffer = new int[BUFFER_SIZE]; 775 int data_count = 0; 776 int data_remainder = 0; 777 int data_to_read = 0; 778 double previous_timestamp = 0.0; 779 double delta_angle_x = 0.0; 780 double delta_angle_y = 0.0; 781 double delta_angle_z = 0.0; 782 double gyro_rate_x = 0.0; 783 double gyro_rate_y = 0.0; 784 double gyro_rate_z = 0.0; 785 double accel_x = 0.0; 786 double accel_y = 0.0; 787 double accel_z = 0.0; 788 double gyro_rate_x_si = 0.0; 789 double gyro_rate_y_si = 0.0; 790 double gyro_rate_z_si = 0.0; 791 double accel_x_si = 0.0; 792 double accel_y_si = 0.0; 793 double accel_z_si = 0.0; 794 double compAngleX = 0.0; 795 double compAngleY = 0.0; 796 double accelAngleX = 0.0; 797 double accelAngleY = 0.0; 798 799 while (true) { 800 // Sleep loop for 10ms 801 try { 802 Thread.sleep(10); 803 } catch (InterruptedException e) { 804 } 805 806 if (m_thread_active) { 807 m_thread_idle = false; 808 809 data_count = 810 m_spi.readAutoReceivedData( 811 buffer, 0, 0); // Read number of bytes currently stored in the buffer 812 813 data_remainder = 814 data_count % dataset_len; // Check if frame is incomplete. Add 1 because of timestamp 815 data_to_read = data_count - data_remainder; // Remove incomplete data from read count 816 /* Want to cap the data to read in a single read at the buffer size */ 817 if (data_to_read > BUFFER_SIZE) { 818 DriverStation.reportWarning( 819 "ADIS16470 data processing thread overrun has occurred!", false); 820 data_to_read = BUFFER_SIZE - (BUFFER_SIZE % dataset_len); 821 } 822 m_spi.readAutoReceivedData( 823 buffer, data_to_read, 0); // Read data from DMA buffer (only complete sets) 824 825 // Could be multiple data sets in the buffer. Handle each one. 826 for (int i = 0; i < data_to_read; i += dataset_len) { 827 // Timestamp is at buffer[i] 828 m_dt = ((double) buffer[i] - previous_timestamp) / 1000000.0; 829 830 /* 831 * System.out.println(((toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5], 832 * buffer[i + 6]))*delta_angle_sf) / ((10000.0 / (buffer[i] - 833 * previous_timestamp)) / 100.0)); 834 * // DEBUG: Plot Sub-Array Data in Terminal 835 * for (int j = 0; j < data_to_read; j++) { 836 * System.out.print(buffer[j]); 837 * System.out.print(" ,"); 838 * } 839 * System.out.println(" "); 840 * //System.out.println(((toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5], 841 * buffer[i + 6]))*delta_angle_sf) / ((10000.0 / (buffer[i] - 842 * previous_timestamp)) / 100.0) + "," + buffer[3] + "," + buffer[4] + "," + 843 * buffer[5] + "," + buffer[6] 844 * /*toShort(buffer[7], buffer[8]) + "," + 845 * toShort(buffer[9], buffer[10]) + "," + 846 * toShort(buffer[11], buffer[12]) + "," + 847 * toShort(buffer[13], buffer[14]) + "," + 848 * toShort(buffer[15], buffer[16]) + "," 849 * + toShort(buffer[17], buffer[18])); 850 */ 851 852 /* 853 * Get delta angle value for all 3 axes and scale by the elapsed time 854 * (based on timestamp) 855 */ 856 delta_angle_x = 857 (toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5], buffer[i + 6]) * delta_angle_sf) 858 / (m_scaled_sample_rate / (buffer[i] - previous_timestamp)); 859 delta_angle_y = 860 (toInt(buffer[i + 7], buffer[i + 8], buffer[i + 9], buffer[i + 10]) * delta_angle_sf) 861 / (m_scaled_sample_rate / (buffer[i] - previous_timestamp)); 862 delta_angle_z = 863 (toInt(buffer[i + 11], buffer[i + 12], buffer[i + 13], buffer[i + 14]) 864 * delta_angle_sf) 865 / (m_scaled_sample_rate / (buffer[i] - previous_timestamp)); 866 867 gyro_rate_x = (toShort(buffer[i + 15], buffer[i + 16]) / 10.0); 868 gyro_rate_y = (toShort(buffer[i + 17], buffer[i + 18]) / 10.0); 869 gyro_rate_z = (toShort(buffer[i + 19], buffer[i + 20]) / 10.0); 870 871 accel_x = (toShort(buffer[i + 21], buffer[i + 22]) / 800.0); 872 accel_y = (toShort(buffer[i + 23], buffer[i + 24]) / 800.0); 873 accel_z = (toShort(buffer[i + 25], buffer[i + 26]) / 800.0); 874 875 // Convert scaled sensor data to SI units (for tilt calculations) 876 // TODO: Should the unit outputs be selectable? 877 gyro_rate_x_si = gyro_rate_x * deg_to_rad; 878 gyro_rate_y_si = gyro_rate_y * deg_to_rad; 879 gyro_rate_z_si = gyro_rate_z * deg_to_rad; 880 accel_x_si = accel_x * grav; 881 accel_y_si = accel_y * grav; 882 accel_z_si = accel_z * grav; 883 884 // Store timestamp for next iteration 885 previous_timestamp = buffer[i]; 886 887 m_alpha = m_tau / (m_tau + m_dt); 888 889 if (m_first_run) { 890 // Set up inclinometer calculations for first run 891 accelAngleX = 892 Math.atan2( 893 accel_x_si, Math.sqrt((accel_y_si * accel_y_si) + (accel_z_si * accel_z_si))); 894 accelAngleY = 895 Math.atan2( 896 accel_y_si, Math.sqrt((accel_x_si * accel_x_si) + (accel_z_si * accel_z_si))); 897 compAngleX = accelAngleX; 898 compAngleY = accelAngleY; 899 900 m_average_gyro_rate_x = 0.0; 901 m_average_gyro_rate_y = 0.0; 902 m_average_gyro_rate_z = 0.0; 903 } else { 904 // Run inclinometer calculations 905 accelAngleX = 906 Math.atan2( 907 accel_x_si, Math.sqrt((accel_y_si * accel_y_si) + (accel_z_si * accel_z_si))); 908 accelAngleY = 909 Math.atan2( 910 accel_y_si, Math.sqrt((accel_x_si * accel_x_si) + (accel_z_si * accel_z_si))); 911 accelAngleX = formatAccelRange(accelAngleX, accel_z_si); 912 accelAngleY = formatAccelRange(accelAngleY, accel_z_si); 913 compAngleX = compFilterProcess(compAngleX, accelAngleX, -gyro_rate_y_si); 914 compAngleY = compFilterProcess(compAngleY, accelAngleY, gyro_rate_x_si); 915 } 916 917 synchronized (this) { 918 /* Push data to global variables */ 919 if (m_first_run) { 920 /* 921 * Don't accumulate first run. previous_timestamp will be "very" old and the 922 * integration will end up way off 923 */ 924 m_integ_angle_x = 0.0; 925 m_integ_angle_y = 0.0; 926 m_integ_angle_z = 0.0; 927 } else { 928 m_integ_angle_x += delta_angle_x; 929 m_integ_angle_y += delta_angle_y; 930 m_integ_angle_z += delta_angle_z; 931 } 932 m_gyro_rate_x = gyro_rate_x; 933 m_gyro_rate_y = gyro_rate_y; 934 m_gyro_rate_z = gyro_rate_z; 935 m_accel_x = accel_x; 936 m_accel_y = accel_y; 937 m_accel_z = accel_z; 938 m_compAngleX = compAngleX * rad_to_deg; 939 m_compAngleY = compAngleY * rad_to_deg; 940 m_accelAngleX = accelAngleX * rad_to_deg; 941 m_accelAngleY = accelAngleY * rad_to_deg; 942 m_average_gyro_rate_x += gyro_rate_x; 943 m_average_gyro_rate_y += gyro_rate_y; 944 m_average_gyro_rate_z += gyro_rate_z; 945 } 946 m_first_run = false; 947 } 948 949 // The inverse of data to read divided by dataset length, his is the number of iterations 950 // of the for loop inverted (so multiplication can be used instead of division) 951 double invTotalIterations = (double) dataset_len / data_to_read; 952 m_average_gyro_rate_x *= invTotalIterations; 953 m_average_gyro_rate_y *= invTotalIterations; 954 m_average_gyro_rate_z *= invTotalIterations; 955 } else { 956 m_thread_idle = true; 957 data_count = 0; 958 data_remainder = 0; 959 data_to_read = 0; 960 previous_timestamp = 0.0; 961 delta_angle_x = 0.0; 962 delta_angle_y = 0.0; 963 delta_angle_z = 0.0; 964 gyro_rate_x = 0.0; 965 gyro_rate_y = 0.0; 966 gyro_rate_z = 0.0; 967 accel_x = 0.0; 968 accel_y = 0.0; 969 accel_z = 0.0; 970 gyro_rate_x_si = 0.0; 971 gyro_rate_y_si = 0.0; 972 gyro_rate_z_si = 0.0; 973 accel_x_si = 0.0; 974 accel_y_si = 0.0; 975 accel_z_si = 0.0; 976 compAngleX = 0.0; 977 compAngleY = 0.0; 978 accelAngleX = 0.0; 979 accelAngleY = 0.0; 980 } 981 } 982 } 983 984 /** 985 * @param compAngle 986 * @param accAngle 987 * @return 988 */ 989 private double formatFastConverge(double compAngle, double accAngle) { 990 if (compAngle > accAngle + Math.PI) { 991 compAngle = compAngle - 2.0 * Math.PI; 992 } else if (accAngle > compAngle + Math.PI) { 993 compAngle = compAngle + 2.0 * Math.PI; 994 } 995 return compAngle; 996 } 997 998 /** 999 * @param compAngle 1000 * @return 1001 */ 1002 private double formatRange0to2PI(double compAngle) { 1003 while (compAngle >= 2 * Math.PI) { 1004 compAngle = compAngle - 2.0 * Math.PI; 1005 } 1006 while (compAngle < 0.0) { 1007 compAngle = compAngle + 2.0 * Math.PI; 1008 } 1009 return compAngle; 1010 } 1011 1012 /** 1013 * @param accelAngle 1014 * @param accelZ 1015 * @return 1016 */ 1017 private double formatAccelRange(double accelAngle, double accelZ) { 1018 if (accelZ < 0.0) { 1019 accelAngle = Math.PI - accelAngle; 1020 } else if (accelZ > 0.0 && accelAngle < 0.0) { 1021 accelAngle = 2.0 * Math.PI + accelAngle; 1022 } 1023 return accelAngle; 1024 } 1025 1026 /** 1027 * @param compAngle 1028 * @param accelAngle 1029 * @param omega 1030 * @return 1031 */ 1032 private double compFilterProcess(double compAngle, double accelAngle, double omega) { 1033 compAngle = formatFastConverge(compAngle, accelAngle); 1034 compAngle = m_alpha * (compAngle + omega * m_dt) + (1.0 - m_alpha) * accelAngle; 1035 compAngle = formatRange0to2PI(compAngle); 1036 if (compAngle > Math.PI) { 1037 compAngle = compAngle - 2.0 * Math.PI; 1038 } 1039 return compAngle; 1040 } 1041 1042 /** 1043 * Reset the gyro. 1044 * 1045 * <p>Resets the gyro accumulations to a heading of zero. This can be used if there is significant 1046 * drift in the gyro and it needs to be recalibrated after running. 1047 */ 1048 public void reset() { 1049 synchronized (this) { 1050 m_integ_angle_x = 0.0; 1051 m_integ_angle_y = 0.0; 1052 m_integ_angle_z = 0.0; 1053 } 1054 } 1055 1056 /** 1057 * Allow the designated gyro angle to be set to a given value. This may happen with unread values 1058 * in the buffer, it is suggested that the IMU is not moving when this method is run. 1059 * 1060 * @param axis IMUAxis that will be changed 1061 * @param angle A double in degrees (CCW positive) 1062 */ 1063 public void setGyroAngle(IMUAxis axis, double angle) { 1064 switch (axis) { 1065 case kYaw: 1066 axis = m_yaw_axis; 1067 break; 1068 case kPitch: 1069 axis = m_pitch_axis; 1070 break; 1071 case kRoll: 1072 axis = m_roll_axis; 1073 break; 1074 default: 1075 } 1076 1077 switch (axis) { 1078 case kX: 1079 this.setGyroAngleX(angle); 1080 break; 1081 case kY: 1082 this.setGyroAngleY(angle); 1083 break; 1084 case kZ: 1085 this.setGyroAngleZ(angle); 1086 break; 1087 default: 1088 } 1089 } 1090 1091 /** 1092 * Allow the gyro angle X to be set to a given value. This may happen with unread values in the 1093 * buffer, it is suggested that the IMU is not moving when this method is run. 1094 * 1095 * @param angle A double in degrees (CCW positive) 1096 */ 1097 public void setGyroAngleX(double angle) { 1098 synchronized (this) { 1099 m_integ_angle_x = angle; 1100 } 1101 } 1102 1103 /** 1104 * Allow the gyro angle Y to be set to a given value. This may happen with unread values in the 1105 * buffer, it is suggested that the IMU is not moving when this method is run. 1106 * 1107 * @param angle A double in degrees (CCW positive) 1108 */ 1109 public void setGyroAngleY(double angle) { 1110 synchronized (this) { 1111 m_integ_angle_y = angle; 1112 } 1113 } 1114 1115 /** 1116 * Allow the gyro angle Z to be set to a given value. This may happen with unread values in the 1117 * buffer, it is suggested that the IMU is not moving when this method is run. 1118 * 1119 * @param angle A double in degrees (CCW positive) 1120 */ 1121 public void setGyroAngleZ(double angle) { 1122 synchronized (this) { 1123 m_integ_angle_z = angle; 1124 } 1125 } 1126 1127 /** 1128 * Returns the axis angle in degrees (CCW positive). 1129 * 1130 * @param axis The IMUAxis whose angle to return. 1131 * @return The axis angle in degrees (CCW positive). 1132 */ 1133 public synchronized double getAngle(IMUAxis axis) { 1134 switch (axis) { 1135 case kYaw: 1136 axis = m_yaw_axis; 1137 break; 1138 case kPitch: 1139 axis = m_pitch_axis; 1140 break; 1141 case kRoll: 1142 axis = m_roll_axis; 1143 break; 1144 default: 1145 } 1146 1147 switch (axis) { 1148 case kX: 1149 if (m_simGyroAngleX != null) { 1150 return m_simGyroAngleX.get(); 1151 } 1152 return m_integ_angle_x; 1153 case kY: 1154 if (m_simGyroAngleY != null) { 1155 return m_simGyroAngleY.get(); 1156 } 1157 return m_integ_angle_y; 1158 case kZ: 1159 if (m_simGyroAngleZ != null) { 1160 return m_simGyroAngleZ.get(); 1161 } 1162 return m_integ_angle_z; 1163 default: 1164 } 1165 1166 return 0.0; 1167 } 1168 1169 /** 1170 * Returns the Yaw axis angle in degrees (CCW positive). 1171 * 1172 * @return The Yaw axis angle in degrees (CCW positive). 1173 */ 1174 public synchronized double getAngle() { 1175 switch (m_yaw_axis) { 1176 case kX: 1177 if (m_simGyroAngleX != null) { 1178 return m_simGyroAngleX.get(); 1179 } 1180 return m_integ_angle_x; 1181 case kY: 1182 if (m_simGyroAngleY != null) { 1183 return m_simGyroAngleY.get(); 1184 } 1185 return m_integ_angle_y; 1186 case kZ: 1187 if (m_simGyroAngleZ != null) { 1188 return m_simGyroAngleZ.get(); 1189 } 1190 return m_integ_angle_z; 1191 default: 1192 } 1193 return 0.0; 1194 } 1195 1196 /** 1197 * Returns the axis angular rate in degrees per second (CCW positive). 1198 * 1199 * @param axis The IMUAxis whose rate to return. 1200 * @return Axis angular rate in degrees per second (CCW positive). 1201 */ 1202 public synchronized double getRate(IMUAxis axis) { 1203 switch (axis) { 1204 case kYaw: 1205 axis = m_yaw_axis; 1206 break; 1207 case kPitch: 1208 axis = m_pitch_axis; 1209 break; 1210 case kRoll: 1211 axis = m_roll_axis; 1212 break; 1213 default: 1214 } 1215 1216 switch (axis) { 1217 case kX: 1218 if (m_simGyroRateX != null) { 1219 return m_simGyroRateX.get(); 1220 } 1221 return m_gyro_rate_x; 1222 case kY: 1223 if (m_simGyroRateY != null) { 1224 return m_simGyroRateY.get(); 1225 } 1226 return m_gyro_rate_y; 1227 case kZ: 1228 if (m_simGyroRateZ != null) { 1229 return m_simGyroRateZ.get(); 1230 } 1231 return m_gyro_rate_z; 1232 default: 1233 } 1234 return 0.0; 1235 } 1236 1237 /** 1238 * Returns the Yaw axis angular rate in degrees per second (CCW positive). 1239 * 1240 * @return Yaw axis angular rate in degrees per second (CCW positive). 1241 */ 1242 public synchronized double getRate() { 1243 switch (m_yaw_axis) { 1244 case kX: 1245 if (m_simGyroRateX != null) { 1246 return m_simGyroRateX.get(); 1247 } 1248 return m_gyro_rate_x; 1249 case kY: 1250 if (m_simGyroRateY != null) { 1251 return m_simGyroRateY.get(); 1252 } 1253 return m_gyro_rate_y; 1254 case kZ: 1255 if (m_simGyroRateZ != null) { 1256 return m_simGyroRateZ.get(); 1257 } 1258 return m_gyro_rate_z; 1259 default: 1260 } 1261 return 0.0; 1262 } 1263 1264 /** 1265 * Returns which axis, kX, kY, or kZ, is set to the yaw axis. 1266 * 1267 * @return IMUAxis Yaw Axis 1268 */ 1269 public IMUAxis getYawAxis() { 1270 return m_yaw_axis; 1271 } 1272 1273 /** 1274 * Returns which axis, kX, kY, or kZ, is set to the pitch axis. 1275 * 1276 * @return IMUAxis Pitch Axis 1277 */ 1278 public IMUAxis getPitchAxis() { 1279 return m_pitch_axis; 1280 } 1281 1282 /** 1283 * Returns which axis, kX, kY, or kZ, is set to the roll axis. 1284 * 1285 * @return IMUAxis Roll Axis 1286 */ 1287 public IMUAxis getRollAxis() { 1288 return m_roll_axis; 1289 } 1290 1291 /** 1292 * Returns the acceleration in the X axis in meters per second squared. 1293 * 1294 * @return The acceleration in the X axis in meters per second squared. 1295 */ 1296 public synchronized double getAccelX() { 1297 return m_accel_x * 9.81; 1298 } 1299 1300 /** 1301 * Returns the acceleration in the Y axis in meters per second squared. 1302 * 1303 * @return The acceleration in the Y axis in meters per second squared. 1304 */ 1305 public synchronized double getAccelY() { 1306 return m_accel_y * 9.81; 1307 } 1308 1309 /** 1310 * Returns the acceleration in the Z axis in meters per second squared. 1311 * 1312 * @return The acceleration in the Z axis in meters per second squared. 1313 */ 1314 public synchronized double getAccelZ() { 1315 return m_accel_z * 9.81; 1316 } 1317 1318 /** 1319 * Returns the complementary angle around the X axis computed from accelerometer and gyro rate 1320 * measurements. 1321 * 1322 * @return The X-axis complementary angle in degrees. 1323 */ 1324 public synchronized double getXComplementaryAngle() { 1325 return m_compAngleX; 1326 } 1327 1328 /** 1329 * Returns the complementary angle around the Y axis computed from accelerometer and gyro rate 1330 * measurements. 1331 * 1332 * @return The Y-axis complementary angle in degrees. 1333 */ 1334 public synchronized double getYComplementaryAngle() { 1335 return m_compAngleY; 1336 } 1337 1338 /** 1339 * Returns the X-axis filtered acceleration angle in degrees. 1340 * 1341 * @return The X-axis filtered acceleration angle in degrees. 1342 */ 1343 public synchronized double getXFilteredAccelAngle() { 1344 return m_accelAngleX; 1345 } 1346 1347 /** 1348 * Returns the Y-axis filtered acceleration angle in degrees. 1349 * 1350 * @return The Y-axis filtered acceleration angle in degrees. 1351 */ 1352 public synchronized double getYFilteredAccelAngle() { 1353 return m_accelAngleY; 1354 } 1355 1356 /** 1357 * Gets the SPI port number. 1358 * 1359 * @return The SPI port number. 1360 */ 1361 public int getPort() { 1362 return m_spi_port.value; 1363 } 1364 1365 @Override 1366 public void initSendable(SendableBuilder builder) { 1367 builder.setSmartDashboardType("Gyro"); 1368 builder.addDoubleProperty("Value", () -> getAngle(m_yaw_axis), null); 1369 } 1370}