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 257 // Resources 258 private SPI m_spi; 259 private SPI.Port m_spi_port; 260 private DigitalInput m_auto_interrupt; 261 private DigitalOutput m_reset_out; 262 private DigitalInput m_reset_in; 263 private DigitalOutput m_status_led; 264 private Thread m_acquire_task; 265 private boolean m_connected; 266 267 private SimDevice m_simDevice; 268 private SimBoolean m_simConnected; 269 private SimDouble m_simGyroAngleX; 270 private SimDouble m_simGyroAngleY; 271 private SimDouble m_simGyroAngleZ; 272 private SimDouble m_simGyroRateX; 273 private SimDouble m_simGyroRateY; 274 private SimDouble m_simGyroRateZ; 275 private SimDouble m_simAccelX; 276 private SimDouble m_simAccelY; 277 private SimDouble m_simAccelZ; 278 279 private static class AcquireTask implements Runnable { 280 private ADIS16470_IMU imu; 281 282 public AcquireTask(ADIS16470_IMU imu) { 283 this.imu = imu; 284 } 285 286 @Override 287 public void run() { 288 imu.acquire(); 289 } 290 } 291 292 /** 293 * Creates a new ADIS16740 IMU object. 294 * 295 * <p>The default setup is the onboard SPI port with a calibration time of 4 seconds. Yaw, pitch, 296 * and roll are kZ, kX, and kY respectively. 297 */ 298 public ADIS16470_IMU() { 299 this(IMUAxis.kZ, IMUAxis.kX, IMUAxis.kY, SPI.Port.kOnboardCS0, CalibrationTime._4s); 300 } 301 302 /** 303 * Creates a new ADIS16740 IMU object. 304 * 305 * <p>The default setup is the onboard SPI port with a calibration time of 4 seconds. 306 * 307 * <p><b><i>Input axes limited to kX, kY and kZ. Specifying kYaw, kPitch,or kRoll will result in 308 * an error.</i></b> 309 * 310 * @param yaw_axis The axis that measures the yaw 311 * @param pitch_axis The axis that measures the pitch 312 * @param roll_axis The axis that measures the roll 313 */ 314 public ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis, IMUAxis roll_axis) { 315 this(yaw_axis, pitch_axis, roll_axis, SPI.Port.kOnboardCS0, CalibrationTime._4s); 316 } 317 318 /** 319 * Creates a new ADIS16740 IMU object. 320 * 321 * <p><b><i>Input axes limited to kX, kY and kZ. Specifying kYaw, kPitch,or kRoll will result in 322 * an error.</i></b> 323 * 324 * @param yaw_axis The axis that measures the yaw 325 * @param pitch_axis The axis that measures the pitch 326 * @param roll_axis The axis that measures the roll 327 * @param port The SPI Port the gyro is plugged into 328 * @param cal_time Calibration time 329 */ 330 @SuppressWarnings("this-escape") 331 public ADIS16470_IMU( 332 IMUAxis yaw_axis, 333 IMUAxis pitch_axis, 334 IMUAxis roll_axis, 335 SPI.Port port, 336 CalibrationTime cal_time) { 337 if (yaw_axis == IMUAxis.kYaw 338 || yaw_axis == IMUAxis.kPitch 339 || yaw_axis == IMUAxis.kRoll 340 || pitch_axis == IMUAxis.kYaw 341 || pitch_axis == IMUAxis.kPitch 342 || pitch_axis == IMUAxis.kRoll 343 || roll_axis == IMUAxis.kYaw 344 || roll_axis == IMUAxis.kPitch 345 || roll_axis == IMUAxis.kRoll) { 346 DriverStation.reportError( 347 "ADIS16470 constructor only allows IMUAxis.kX, IMUAxis.kY or IMUAxis.kZ as arguments.", 348 false); 349 DriverStation.reportError( 350 "Constructing ADIS with default axes. (IMUAxis.kZ is defined as Yaw)", false); 351 yaw_axis = IMUAxis.kZ; 352 pitch_axis = IMUAxis.kY; 353 roll_axis = IMUAxis.kX; 354 } 355 356 m_yaw_axis = yaw_axis; 357 m_pitch_axis = pitch_axis; 358 m_roll_axis = roll_axis; 359 360 m_calibration_time = cal_time.value; 361 m_spi_port = port; 362 363 m_acquire_task = new Thread(new AcquireTask(this)); 364 365 m_simDevice = SimDevice.create("Gyro:ADIS16470", port.value); 366 if (m_simDevice != null) { 367 m_simConnected = m_simDevice.createBoolean("connected", SimDevice.Direction.kInput, true); 368 m_simGyroAngleX = m_simDevice.createDouble("gyro_angle_x", SimDevice.Direction.kInput, 0.0); 369 m_simGyroAngleY = m_simDevice.createDouble("gyro_angle_y", SimDevice.Direction.kInput, 0.0); 370 m_simGyroAngleZ = m_simDevice.createDouble("gyro_angle_z", SimDevice.Direction.kInput, 0.0); 371 m_simGyroRateX = m_simDevice.createDouble("gyro_rate_x", SimDevice.Direction.kInput, 0.0); 372 m_simGyroRateY = m_simDevice.createDouble("gyro_rate_y", SimDevice.Direction.kInput, 0.0); 373 m_simGyroRateZ = m_simDevice.createDouble("gyro_rate_z", SimDevice.Direction.kInput, 0.0); 374 m_simAccelX = m_simDevice.createDouble("accel_x", SimDevice.Direction.kInput, 0.0); 375 m_simAccelY = m_simDevice.createDouble("accel_y", SimDevice.Direction.kInput, 0.0); 376 m_simAccelZ = m_simDevice.createDouble("accel_z", SimDevice.Direction.kInput, 0.0); 377 } 378 379 if (m_simDevice == null) { 380 // Force the IMU reset pin to toggle on startup (doesn't require DS enable) 381 // Relies on the RIO hardware by default configuring an output as low 382 // and configuring an input as high Z. The 10k pull-up resistor internal to the 383 // IMU then forces the reset line high for normal operation. 384 m_reset_out = new DigitalOutput(27); // Drive SPI CS2 (IMU RST) low 385 Timer.delay(0.01); // Wait 10ms 386 m_reset_out.close(); 387 m_reset_in = new DigitalInput(27); // Set SPI CS2 (IMU RST) high 388 Timer.delay(0.25); // Wait 250ms for reset to complete 389 390 if (!switchToStandardSPI()) { 391 return; 392 } 393 394 // Set IMU internal decimation to 4 (output data rate of 2000 SPS / (4 + 1) = 395 // 400Hz) 396 writeRegister(DEC_RATE, 4); 397 398 // Set data ready polarity (HIGH = Good Data), Disable gSense Compensation and 399 // PoP 400 writeRegister(MSC_CTRL, 1); 401 402 // Configure IMU internal Bartlett filter 403 writeRegister(FILT_CTRL, 0); 404 405 // Configure continuous bias calibration time based on user setting 406 writeRegister(NULL_CNFG, (m_calibration_time | 0x0700)); 407 408 // Notify DS that IMU calibration delay is active 409 DriverStation.reportWarning( 410 "ADIS16470 IMU Detected. Starting initial calibration delay.", false); 411 412 // Wait for samples to accumulate internal to the IMU (110% of user-defined 413 // time) 414 try { 415 Thread.sleep((long) (Math.pow(2.0, m_calibration_time) / 2000 * 64 * 1.1 * 1000)); 416 } catch (InterruptedException e) { 417 } 418 419 // Write offset calibration command to IMU 420 writeRegister(GLOB_CMD, 0x0001); 421 422 // Configure and enable auto SPI 423 if (!switchToAutoSPI()) { 424 return; 425 } 426 427 // Let the user know the IMU was initiallized successfully 428 DriverStation.reportWarning("ADIS16470 IMU Successfully Initialized!", false); 429 430 // Drive "Ready" LED low 431 m_status_led = new DigitalOutput(28); // Set SPI CS3 (IMU Ready LED) low 432 } 433 434 // Report usage and post data to DS 435 HAL.report(tResourceType.kResourceType_ADIS16470, 0); 436 m_connected = true; 437 } 438 439 /** 440 * Checks the connection status of the IMU. 441 * 442 * @return True if the IMU is connected, false otherwise. 443 */ 444 public boolean isConnected() { 445 if (m_simConnected != null) { 446 return m_simConnected.get(); 447 } 448 return m_connected; 449 } 450 451 /** 452 * @param buf 453 * @return 454 */ 455 private static int toUShort(ByteBuffer buf) { 456 return buf.getShort(0) & 0xFFFF; 457 } 458 459 /** 460 * @param sint 461 * @return 462 */ 463 private static long toULong(int sint) { 464 return sint & 0x00000000FFFFFFFFL; 465 } 466 467 /** 468 * @param buf 469 * @return 470 */ 471 private static int toShort(int... buf) { 472 return (short) (((buf[0] & 0xFF) << 8) + (buf[1] & 0xFF)); 473 } 474 475 /** 476 * @param buf 477 * @return 478 */ 479 private static int toInt(int... buf) { 480 return (buf[0] & 0xFF) << 24 | (buf[1] & 0xFF) << 16 | (buf[2] & 0xFF) << 8 | (buf[3] & 0xFF); 481 } 482 483 /** 484 * Switch to standard SPI mode. 485 * 486 * @return True if successful, false otherwise. 487 */ 488 private boolean switchToStandardSPI() { 489 // Check to see whether the acquire thread is active. If so, wait for it to stop 490 // producing data. 491 if (m_thread_active) { 492 m_thread_active = false; 493 while (!m_thread_idle) { 494 try { 495 Thread.sleep(10); 496 } catch (InterruptedException e) { 497 } 498 } 499 System.out.println("Paused the IMU processing thread successfully!"); 500 // Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI. 501 if (m_spi != null && m_auto_configured) { 502 m_spi.stopAuto(); 503 // We need to get rid of all the garbage left in the auto SPI buffer after 504 // stopping it. 505 // Sometimes data magically reappears, so we have to check the buffer size a 506 // couple of times 507 // to be sure we got it all. Yuck. 508 int[] trashBuffer = new int[200]; 509 try { 510 Thread.sleep(100); 511 } catch (InterruptedException e) { 512 } 513 int data_count = m_spi.readAutoReceivedData(trashBuffer, 0, 0); 514 while (data_count > 0) { 515 m_spi.readAutoReceivedData(trashBuffer, Math.min(data_count, 200), 0); 516 data_count = m_spi.readAutoReceivedData(trashBuffer, 0, 0); 517 } 518 System.out.println("Paused auto SPI successfully."); 519 } 520 } 521 // There doesn't seem to be a SPI port active. Let's try to set one up 522 if (m_spi == null) { 523 System.out.println("Setting up a new SPI port."); 524 m_spi = new SPI(m_spi_port); 525 m_spi.setClockRate(2000000); 526 m_spi.setMode(SPI.Mode.kMode3); 527 m_spi.setChipSelectActiveLow(); 528 readRegister(PROD_ID); // Dummy read 529 530 // Validate the product ID 531 if (readRegister(PROD_ID) != 16982) { 532 DriverStation.reportError("Could not find ADIS16470", false); 533 close(); 534 return false; 535 } 536 return true; 537 } else { 538 // Maybe the SPI port is active, but not in auto SPI mode? Try to read the 539 // product ID. 540 readRegister(PROD_ID); // dummy read 541 if (readRegister(PROD_ID) != 16982) { 542 DriverStation.reportError("Could not find an ADIS16470", false); 543 close(); 544 return false; 545 } else { 546 return true; 547 } 548 } 549 } 550 551 /** 552 * Switch to auto SPI mode. 553 * 554 * @return True if successful, false otherwise. 555 */ 556 boolean switchToAutoSPI() { 557 // No SPI port has been set up. Go set one up first. 558 if (m_spi == null) { 559 if (!switchToStandardSPI()) { 560 DriverStation.reportError("Failed to start/restart auto SPI", false); 561 return false; 562 } 563 } 564 // Only set up the interrupt if needed. 565 if (m_auto_interrupt == null) { 566 // Configure interrupt on SPI CS1 567 m_auto_interrupt = new DigitalInput(26); 568 } 569 // The auto SPI controller gets angry if you try to set up two instances on one 570 // bus. 571 if (!m_auto_configured) { 572 m_spi.initAuto(8200); 573 m_auto_configured = true; 574 } 575 // Do we need to change auto SPI settings? 576 m_spi.setAutoTransmitData(m_autospi_allAngle_packet, 2); 577 // Configure auto stall time 578 m_spi.configureAutoStall(5, 1000, 1); 579 // Kick off auto SPI (Note: Device configuration impossible after auto SPI is 580 // activated) 581 // DR High = Data good (data capture should be triggered on the rising edge) 582 m_spi.startAutoTrigger(m_auto_interrupt, true, false); 583 584 // Check to see if the acquire thread is running. If not, kick one off. 585 if (!m_acquire_task.isAlive()) { 586 m_first_run = true; 587 m_thread_active = true; 588 m_acquire_task.start(); 589 System.out.println("Processing thread activated!"); 590 } else { 591 // The thread was running, re-init run variables and start it up again. 592 m_first_run = true; 593 m_thread_active = true; 594 System.out.println("Processing thread activated!"); 595 } 596 // Looks like the thread didn't start for some reason. Abort. 597 if (!m_acquire_task.isAlive()) { 598 DriverStation.reportError("Failed to start/restart the acquire() thread.", false); 599 close(); 600 return false; 601 } 602 return true; 603 } 604 605 /** 606 * Configures calibration time 607 * 608 * @param new_cal_time New calibration time 609 * @return 1 if the new calibration time is the same as the current one else 0 610 */ 611 public int configCalTime(CalibrationTime new_cal_time) { 612 if (m_calibration_time == new_cal_time.value) { 613 return 1; 614 } 615 if (!switchToStandardSPI()) { 616 DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false); 617 return 2; 618 } 619 m_calibration_time = new_cal_time.value; 620 writeRegister(NULL_CNFG, (m_calibration_time | 0x700)); 621 if (!switchToAutoSPI()) { 622 DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false); 623 return 2; 624 } 625 return 0; 626 } 627 628 /** 629 * Configures the decimation rate of the IMU. 630 * 631 * @param decimationRate The new decimation value. 632 * @return 0 if success, 1 if no change, 2 if error. 633 */ 634 public int configDecRate(int decimationRate) { 635 // Switches the active SPI port to standard SPI mode, writes a new value to 636 // the DECIMATE register in the IMU, and re-enables auto SPI. 637 // 638 // This function enters standard SPI mode, writes a new DECIMATE setting to 639 // the IMU, adjusts the sample scale factor, and re-enters auto SPI mode. 640 if (!switchToStandardSPI()) { 641 DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false); 642 return 2; 643 } 644 if (decimationRate > 1999) { 645 DriverStation.reportError("Attempted to write an invalid decimation value.", false); 646 decimationRate = 1999; 647 } 648 m_scaled_sample_rate = (((decimationRate + 1.0) / 2000.0) * 1000000.0); 649 writeRegister(DEC_RATE, decimationRate); 650 System.out.println("Decimation register: " + readRegister(DEC_RATE)); 651 if (!switchToAutoSPI()) { 652 DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false); 653 return 2; 654 } 655 return 0; 656 } 657 658 /** 659 * Calibrate the gyro. It's important to make sure that the robot is not moving while the 660 * calibration is in progress, this is typically done when the robot is first turned on while it's 661 * sitting at rest before the match starts. 662 */ 663 public void calibrate() { 664 if (!switchToStandardSPI()) { 665 DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false); 666 } 667 writeRegister(GLOB_CMD, 0x0001); 668 if (!switchToAutoSPI()) { 669 DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false); 670 } 671 } 672 673 /** 674 * @param reg 675 * @return 676 */ 677 private int readRegister(int reg) { 678 ByteBuffer buf = ByteBuffer.allocateDirect(2); 679 buf.order(ByteOrder.BIG_ENDIAN); 680 buf.put(0, (byte) (reg & 0x7f)); 681 buf.put(1, (byte) 0); 682 683 m_spi.write(buf, 2); 684 m_spi.read(false, buf, 2); 685 686 return toUShort(buf); 687 } 688 689 /** 690 * @param reg 691 * @param val 692 */ 693 private void writeRegister(int reg, int val) { 694 ByteBuffer buf = ByteBuffer.allocateDirect(2); 695 // low byte 696 buf.put(0, (byte) (0x80 | reg)); 697 buf.put(1, (byte) (val & 0xff)); 698 m_spi.write(buf, 2); 699 // high byte 700 buf.put(0, (byte) (0x81 | reg)); 701 buf.put(1, (byte) (val >> 8)); 702 m_spi.write(buf, 2); 703 } 704 705 /** Delete (free) the spi port used for the IMU. */ 706 @Override 707 public void close() { 708 if (m_thread_active) { 709 m_thread_active = false; 710 try { 711 if (m_acquire_task != null) { 712 m_acquire_task.join(); 713 m_acquire_task = null; 714 } 715 } catch (InterruptedException e) { 716 } 717 if (m_spi != null) { 718 if (m_auto_configured) { 719 m_spi.stopAuto(); 720 } 721 m_spi.close(); 722 m_auto_configured = false; 723 if (m_auto_interrupt != null) { 724 m_auto_interrupt.close(); 725 m_auto_interrupt = null; 726 } 727 m_spi = null; 728 } 729 } 730 if (m_simDevice != null) { 731 m_simDevice.close(); 732 m_simDevice = null; 733 } 734 System.out.println("Finished cleaning up after the IMU driver."); 735 } 736 737 /** */ 738 private void acquire() { 739 // Set data packet length 740 final int dataset_len = 27; // 26 data points + timestamp 741 final int BUFFER_SIZE = 4000; 742 743 // Set up buffers and variables 744 int[] buffer = new int[BUFFER_SIZE]; 745 int data_count = 0; 746 int data_remainder = 0; 747 int data_to_read = 0; 748 double previous_timestamp = 0.0; 749 double delta_angle_x = 0.0; 750 double delta_angle_y = 0.0; 751 double delta_angle_z = 0.0; 752 double gyro_rate_x = 0.0; 753 double gyro_rate_y = 0.0; 754 double gyro_rate_z = 0.0; 755 double accel_x = 0.0; 756 double accel_y = 0.0; 757 double accel_z = 0.0; 758 double gyro_rate_x_si = 0.0; 759 double gyro_rate_y_si = 0.0; 760 double gyro_rate_z_si = 0.0; 761 double accel_x_si = 0.0; 762 double accel_y_si = 0.0; 763 double accel_z_si = 0.0; 764 double compAngleX = 0.0; 765 double compAngleY = 0.0; 766 double accelAngleX = 0.0; 767 double accelAngleY = 0.0; 768 769 while (true) { 770 // Sleep loop for 10ms 771 try { 772 Thread.sleep(10); 773 } catch (InterruptedException e) { 774 } 775 776 if (m_thread_active) { 777 m_thread_idle = false; 778 779 data_count = 780 m_spi.readAutoReceivedData( 781 buffer, 0, 0); // Read number of bytes currently stored in the buffer 782 783 data_remainder = 784 data_count % dataset_len; // Check if frame is incomplete. Add 1 because of timestamp 785 data_to_read = data_count - data_remainder; // Remove incomplete data from read count 786 /* Want to cap the data to read in a single read at the buffer size */ 787 if (data_to_read > BUFFER_SIZE) { 788 DriverStation.reportWarning( 789 "ADIS16470 data processing thread overrun has occurred!", false); 790 data_to_read = BUFFER_SIZE - (BUFFER_SIZE % dataset_len); 791 } 792 m_spi.readAutoReceivedData( 793 buffer, data_to_read, 0); // Read data from DMA buffer (only complete sets) 794 795 // Could be multiple data sets in the buffer. Handle each one. 796 for (int i = 0; i < data_to_read; i += dataset_len) { 797 // Timestamp is at buffer[i] 798 m_dt = ((double) buffer[i] - previous_timestamp) / 1000000.0; 799 800 /* 801 * System.out.println(((toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5], 802 * buffer[i + 6]))*delta_angle_sf) / ((10000.0 / (buffer[i] - 803 * previous_timestamp)) / 100.0)); 804 * // DEBUG: Plot Sub-Array Data in Terminal 805 * for (int j = 0; j < data_to_read; j++) { 806 * System.out.print(buffer[j]); 807 * System.out.print(" ,"); 808 * } 809 * System.out.println(" "); 810 * //System.out.println(((toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5], 811 * buffer[i + 6]))*delta_angle_sf) / ((10000.0 / (buffer[i] - 812 * previous_timestamp)) / 100.0) + "," + buffer[3] + "," + buffer[4] + "," + 813 * buffer[5] + "," + buffer[6] 814 * /*toShort(buffer[7], buffer[8]) + "," + 815 * toShort(buffer[9], buffer[10]) + "," + 816 * toShort(buffer[11], buffer[12]) + "," + 817 * toShort(buffer[13], buffer[14]) + "," + 818 * toShort(buffer[15], buffer[16]) + "," 819 * + toShort(buffer[17], buffer[18])); 820 */ 821 822 /* 823 * Get delta angle value for all 3 axes and scale by the elapsed time 824 * (based on timestamp) 825 */ 826 delta_angle_x = 827 (toInt(buffer[i + 3], buffer[i + 4], buffer[i + 5], buffer[i + 6]) * delta_angle_sf) 828 / (m_scaled_sample_rate / (buffer[i] - previous_timestamp)); 829 delta_angle_y = 830 (toInt(buffer[i + 7], buffer[i + 8], buffer[i + 9], buffer[i + 10]) * delta_angle_sf) 831 / (m_scaled_sample_rate / (buffer[i] - previous_timestamp)); 832 delta_angle_z = 833 (toInt(buffer[i + 11], buffer[i + 12], buffer[i + 13], buffer[i + 14]) 834 * delta_angle_sf) 835 / (m_scaled_sample_rate / (buffer[i] - previous_timestamp)); 836 837 gyro_rate_x = (toShort(buffer[i + 15], buffer[i + 16]) / 10.0); 838 gyro_rate_y = (toShort(buffer[i + 17], buffer[i + 18]) / 10.0); 839 gyro_rate_z = (toShort(buffer[i + 19], buffer[i + 20]) / 10.0); 840 841 accel_x = (toShort(buffer[i + 21], buffer[i + 22]) / 800.0); 842 accel_y = (toShort(buffer[i + 23], buffer[i + 24]) / 800.0); 843 accel_z = (toShort(buffer[i + 25], buffer[i + 26]) / 800.0); 844 845 // Convert scaled sensor data to SI units (for tilt calculations) 846 // TODO: Should the unit outputs be selectable? 847 gyro_rate_x_si = gyro_rate_x * deg_to_rad; 848 gyro_rate_y_si = gyro_rate_y * deg_to_rad; 849 gyro_rate_z_si = gyro_rate_z * deg_to_rad; 850 accel_x_si = accel_x * grav; 851 accel_y_si = accel_y * grav; 852 accel_z_si = accel_z * grav; 853 854 // Store timestamp for next iteration 855 previous_timestamp = buffer[i]; 856 857 m_alpha = m_tau / (m_tau + m_dt); 858 859 if (m_first_run) { 860 // Set up inclinometer calculations for first run 861 accelAngleX = 862 Math.atan2( 863 accel_x_si, Math.sqrt((accel_y_si * accel_y_si) + (accel_z_si * accel_z_si))); 864 accelAngleY = 865 Math.atan2( 866 accel_y_si, Math.sqrt((accel_x_si * accel_x_si) + (accel_z_si * accel_z_si))); 867 compAngleX = accelAngleX; 868 compAngleY = accelAngleY; 869 870 m_average_gyro_rate_x = 0.0; 871 m_average_gyro_rate_y = 0.0; 872 m_average_gyro_rate_z = 0.0; 873 } else { 874 // Run inclinometer calculations 875 accelAngleX = 876 Math.atan2( 877 accel_x_si, Math.sqrt((accel_y_si * accel_y_si) + (accel_z_si * accel_z_si))); 878 accelAngleY = 879 Math.atan2( 880 accel_y_si, Math.sqrt((accel_x_si * accel_x_si) + (accel_z_si * accel_z_si))); 881 accelAngleX = formatAccelRange(accelAngleX, accel_z_si); 882 accelAngleY = formatAccelRange(accelAngleY, accel_z_si); 883 compAngleX = compFilterProcess(compAngleX, accelAngleX, -gyro_rate_y_si); 884 compAngleY = compFilterProcess(compAngleY, accelAngleY, gyro_rate_x_si); 885 } 886 887 synchronized (this) { 888 /* Push data to global variables */ 889 if (m_first_run) { 890 /* 891 * Don't accumulate first run. previous_timestamp will be "very" old and the 892 * integration will end up way off 893 */ 894 m_integ_angle_x = 0.0; 895 m_integ_angle_y = 0.0; 896 m_integ_angle_z = 0.0; 897 } else { 898 m_integ_angle_x += delta_angle_x; 899 m_integ_angle_y += delta_angle_y; 900 m_integ_angle_z += delta_angle_z; 901 } 902 m_gyro_rate_x = gyro_rate_x; 903 m_gyro_rate_y = gyro_rate_y; 904 m_gyro_rate_z = gyro_rate_z; 905 m_accel_x = accel_x; 906 m_accel_y = accel_y; 907 m_accel_z = accel_z; 908 m_compAngleX = compAngleX * rad_to_deg; 909 m_compAngleY = compAngleY * rad_to_deg; 910 m_accelAngleX = accelAngleX * rad_to_deg; 911 m_accelAngleY = accelAngleY * rad_to_deg; 912 m_average_gyro_rate_x += gyro_rate_x; 913 m_average_gyro_rate_y += gyro_rate_y; 914 m_average_gyro_rate_z += gyro_rate_z; 915 } 916 m_first_run = false; 917 } 918 919 // The inverse of data to read divided by dataset length, his is the number of iterations 920 // of the for loop inverted (so multiplication can be used instead of division) 921 double invTotalIterations = (double) dataset_len / data_to_read; 922 m_average_gyro_rate_x *= invTotalIterations; 923 m_average_gyro_rate_y *= invTotalIterations; 924 m_average_gyro_rate_z *= invTotalIterations; 925 } else { 926 m_thread_idle = true; 927 data_count = 0; 928 data_remainder = 0; 929 data_to_read = 0; 930 previous_timestamp = 0.0; 931 delta_angle_x = 0.0; 932 delta_angle_y = 0.0; 933 delta_angle_z = 0.0; 934 gyro_rate_x = 0.0; 935 gyro_rate_y = 0.0; 936 gyro_rate_z = 0.0; 937 accel_x = 0.0; 938 accel_y = 0.0; 939 accel_z = 0.0; 940 gyro_rate_x_si = 0.0; 941 gyro_rate_y_si = 0.0; 942 gyro_rate_z_si = 0.0; 943 accel_x_si = 0.0; 944 accel_y_si = 0.0; 945 accel_z_si = 0.0; 946 compAngleX = 0.0; 947 compAngleY = 0.0; 948 accelAngleX = 0.0; 949 accelAngleY = 0.0; 950 } 951 } 952 } 953 954 /** 955 * @param compAngle 956 * @param accAngle 957 * @return 958 */ 959 private double formatFastConverge(double compAngle, double accAngle) { 960 if (compAngle > accAngle + Math.PI) { 961 compAngle = compAngle - 2.0 * Math.PI; 962 } else if (accAngle > compAngle + Math.PI) { 963 compAngle = compAngle + 2.0 * Math.PI; 964 } 965 return compAngle; 966 } 967 968 /** 969 * @param compAngle 970 * @return 971 */ 972 private double formatRange0to2PI(double compAngle) { 973 while (compAngle >= 2 * Math.PI) { 974 compAngle = compAngle - 2.0 * Math.PI; 975 } 976 while (compAngle < 0.0) { 977 compAngle = compAngle + 2.0 * Math.PI; 978 } 979 return compAngle; 980 } 981 982 /** 983 * @param accelAngle 984 * @param accelZ 985 * @return 986 */ 987 private double formatAccelRange(double accelAngle, double accelZ) { 988 if (accelZ < 0.0) { 989 accelAngle = Math.PI - accelAngle; 990 } else if (accelZ > 0.0 && accelAngle < 0.0) { 991 accelAngle = 2.0 * Math.PI + accelAngle; 992 } 993 return accelAngle; 994 } 995 996 /** 997 * @param compAngle 998 * @param accelAngle 999 * @param omega 1000 * @return 1001 */ 1002 private double compFilterProcess(double compAngle, double accelAngle, double omega) { 1003 compAngle = formatFastConverge(compAngle, accelAngle); 1004 compAngle = m_alpha * (compAngle + omega * m_dt) + (1.0 - m_alpha) * accelAngle; 1005 compAngle = formatRange0to2PI(compAngle); 1006 if (compAngle > Math.PI) { 1007 compAngle = compAngle - 2.0 * Math.PI; 1008 } 1009 return compAngle; 1010 } 1011 1012 /** 1013 * Reset the gyro. 1014 * 1015 * <p>Resets the gyro accumulations to a heading of zero. This can be used if there is significant 1016 * drift in the gyro and it needs to be recalibrated after running. 1017 */ 1018 public void reset() { 1019 synchronized (this) { 1020 m_integ_angle_x = 0.0; 1021 m_integ_angle_y = 0.0; 1022 m_integ_angle_z = 0.0; 1023 } 1024 } 1025 1026 /** 1027 * Allow the designated gyro angle to be set to a given value. This may happen with unread values 1028 * in the buffer, it is suggested that the IMU is not moving when this method is run. 1029 * 1030 * @param axis IMUAxis that will be changed 1031 * @param angle A double in degrees (CCW positive) 1032 */ 1033 public void setGyroAngle(IMUAxis axis, double angle) { 1034 switch (axis) { 1035 case kYaw: 1036 axis = m_yaw_axis; 1037 break; 1038 case kPitch: 1039 axis = m_pitch_axis; 1040 break; 1041 case kRoll: 1042 axis = m_roll_axis; 1043 break; 1044 default: 1045 } 1046 1047 switch (axis) { 1048 case kX: 1049 this.setGyroAngleX(angle); 1050 break; 1051 case kY: 1052 this.setGyroAngleY(angle); 1053 break; 1054 case kZ: 1055 this.setGyroAngleZ(angle); 1056 break; 1057 default: 1058 } 1059 } 1060 1061 /** 1062 * Allow the gyro angle X to be set to a given value. This may happen with unread values in the 1063 * buffer, it is suggested that the IMU is not moving when this method is run. 1064 * 1065 * @param angle A double in degrees (CCW positive) 1066 */ 1067 public void setGyroAngleX(double angle) { 1068 synchronized (this) { 1069 m_integ_angle_x = angle; 1070 } 1071 } 1072 1073 /** 1074 * Allow the gyro angle Y to be set to a given value. This may happen with unread values in the 1075 * buffer, it is suggested that the IMU is not moving when this method is run. 1076 * 1077 * @param angle A double in degrees (CCW positive) 1078 */ 1079 public void setGyroAngleY(double angle) { 1080 synchronized (this) { 1081 m_integ_angle_y = angle; 1082 } 1083 } 1084 1085 /** 1086 * Allow the gyro angle Z to be set to a given value. This may happen with unread values in the 1087 * buffer, it is suggested that the IMU is not moving when this method is run. 1088 * 1089 * @param angle A double in degrees (CCW positive) 1090 */ 1091 public void setGyroAngleZ(double angle) { 1092 synchronized (this) { 1093 m_integ_angle_z = angle; 1094 } 1095 } 1096 1097 /** 1098 * Returns the axis angle in degrees (CCW positive). 1099 * 1100 * @param axis The IMUAxis whose angle to return. 1101 * @return The axis angle in degrees (CCW positive). 1102 */ 1103 public synchronized double getAngle(IMUAxis axis) { 1104 switch (axis) { 1105 case kYaw: 1106 axis = m_yaw_axis; 1107 break; 1108 case kPitch: 1109 axis = m_pitch_axis; 1110 break; 1111 case kRoll: 1112 axis = m_roll_axis; 1113 break; 1114 default: 1115 } 1116 1117 switch (axis) { 1118 case kX: 1119 if (m_simGyroAngleX != null) { 1120 return m_simGyroAngleX.get(); 1121 } 1122 return m_integ_angle_x; 1123 case kY: 1124 if (m_simGyroAngleY != null) { 1125 return m_simGyroAngleY.get(); 1126 } 1127 return m_integ_angle_y; 1128 case kZ: 1129 if (m_simGyroAngleZ != null) { 1130 return m_simGyroAngleZ.get(); 1131 } 1132 return m_integ_angle_z; 1133 default: 1134 } 1135 1136 return 0.0; 1137 } 1138 1139 /** 1140 * Returns the Yaw axis angle in degrees (CCW positive). 1141 * 1142 * @return The Yaw axis angle in degrees (CCW positive). 1143 */ 1144 public synchronized double getAngle() { 1145 switch (m_yaw_axis) { 1146 case kX: 1147 if (m_simGyroAngleX != null) { 1148 return m_simGyroAngleX.get(); 1149 } 1150 return m_integ_angle_x; 1151 case kY: 1152 if (m_simGyroAngleY != null) { 1153 return m_simGyroAngleY.get(); 1154 } 1155 return m_integ_angle_y; 1156 case kZ: 1157 if (m_simGyroAngleZ != null) { 1158 return m_simGyroAngleZ.get(); 1159 } 1160 return m_integ_angle_z; 1161 default: 1162 } 1163 return 0.0; 1164 } 1165 1166 /** 1167 * Returns the axis angular rate in degrees per second (CCW positive). 1168 * 1169 * @param axis The IMUAxis whose rate to return. 1170 * @return Axis angular rate in degrees per second (CCW positive). 1171 */ 1172 public synchronized double getRate(IMUAxis axis) { 1173 switch (axis) { 1174 case kYaw: 1175 axis = m_yaw_axis; 1176 break; 1177 case kPitch: 1178 axis = m_pitch_axis; 1179 break; 1180 case kRoll: 1181 axis = m_roll_axis; 1182 break; 1183 default: 1184 } 1185 1186 switch (axis) { 1187 case kX: 1188 if (m_simGyroRateX != null) { 1189 return m_simGyroRateX.get(); 1190 } 1191 return m_gyro_rate_x; 1192 case kY: 1193 if (m_simGyroRateY != null) { 1194 return m_simGyroRateY.get(); 1195 } 1196 return m_gyro_rate_y; 1197 case kZ: 1198 if (m_simGyroRateZ != null) { 1199 return m_simGyroRateZ.get(); 1200 } 1201 return m_gyro_rate_z; 1202 default: 1203 } 1204 return 0.0; 1205 } 1206 1207 /** 1208 * Returns the Yaw axis angular rate in degrees per second (CCW positive). 1209 * 1210 * @return Yaw axis angular rate in degrees per second (CCW positive). 1211 */ 1212 public synchronized double getRate() { 1213 switch (m_yaw_axis) { 1214 case kX: 1215 if (m_simGyroRateX != null) { 1216 return m_simGyroRateX.get(); 1217 } 1218 return m_gyro_rate_x; 1219 case kY: 1220 if (m_simGyroRateY != null) { 1221 return m_simGyroRateY.get(); 1222 } 1223 return m_gyro_rate_y; 1224 case kZ: 1225 if (m_simGyroRateZ != null) { 1226 return m_simGyroRateZ.get(); 1227 } 1228 return m_gyro_rate_z; 1229 default: 1230 } 1231 return 0.0; 1232 } 1233 1234 /** 1235 * Returns which axis, kX, kY, or kZ, is set to the yaw axis. 1236 * 1237 * @return IMUAxis Yaw Axis 1238 */ 1239 public IMUAxis getYawAxis() { 1240 return m_yaw_axis; 1241 } 1242 1243 /** 1244 * Returns which axis, kX, kY, or kZ, is set to the pitch axis. 1245 * 1246 * @return IMUAxis Pitch Axis 1247 */ 1248 public IMUAxis getPitchAxis() { 1249 return m_pitch_axis; 1250 } 1251 1252 /** 1253 * Returns which axis, kX, kY, or kZ, is set to the roll axis. 1254 * 1255 * @return IMUAxis Roll Axis 1256 */ 1257 public IMUAxis getRollAxis() { 1258 return m_roll_axis; 1259 } 1260 1261 /** 1262 * Returns the acceleration in the X axis in meters per second squared. 1263 * 1264 * @return The acceleration in the X axis in meters per second squared. 1265 */ 1266 public synchronized double getAccelX() { 1267 return m_accel_x * 9.81; 1268 } 1269 1270 /** 1271 * Returns the acceleration in the Y axis in meters per second squared. 1272 * 1273 * @return The acceleration in the Y axis in meters per second squared. 1274 */ 1275 public synchronized double getAccelY() { 1276 return m_accel_y * 9.81; 1277 } 1278 1279 /** 1280 * Returns the acceleration in the Z axis in meters per second squared. 1281 * 1282 * @return The acceleration in the Z axis in meters per second squared. 1283 */ 1284 public synchronized double getAccelZ() { 1285 return m_accel_z * 9.81; 1286 } 1287 1288 /** 1289 * Returns the complementary angle around the X axis computed from accelerometer and gyro rate 1290 * measurements. 1291 * 1292 * @return The X-axis complementary angle in degrees. 1293 */ 1294 public synchronized double getXComplementaryAngle() { 1295 return m_compAngleX; 1296 } 1297 1298 /** 1299 * Returns the complementary angle around the Y axis computed from accelerometer and gyro rate 1300 * measurements. 1301 * 1302 * @return The Y-axis complementary angle in degrees. 1303 */ 1304 public synchronized double getYComplementaryAngle() { 1305 return m_compAngleY; 1306 } 1307 1308 /** 1309 * Returns the X-axis filtered acceleration angle in degrees. 1310 * 1311 * @return The X-axis filtered acceleration angle in degrees. 1312 */ 1313 public synchronized double getXFilteredAccelAngle() { 1314 return m_accelAngleX; 1315 } 1316 1317 /** 1318 * Returns the Y-axis filtered acceleration angle in degrees. 1319 * 1320 * @return The Y-axis filtered acceleration angle in degrees. 1321 */ 1322 public synchronized double getYFilteredAccelAngle() { 1323 return m_accelAngleY; 1324 } 1325 1326 /** 1327 * Gets the SPI port number. 1328 * 1329 * @return The SPI port number. 1330 */ 1331 public int getPort() { 1332 return m_spi_port.value; 1333 } 1334 1335 @Override 1336 public void initSendable(SendableBuilder builder) { 1337 builder.setSmartDashboardType("Gyro"); 1338 builder.addDoubleProperty("Value", () -> getAngle(m_yaw_axis), null); 1339 } 1340}