public void startIMU() {
/*
* The IMU datasheet lists the following actions as necessary to initialize and set up the IMU,
* asssuming that all operations of the Constructor completed successfully:
* 1. Register the callback method which will keep the reading of IMU registers going
* 2. Enable I2C read mode and start the self-perpetuating sequence of register readings
*
*/
i2cIMU.registerForI2cPortReadyCallback(this);
offsetsInitialized = false;
i2cIMU.enableI2cReadMode(baseI2Caddress, registerStartAddress, numberOfRegisters);
maxReadInterval = 0.0;
avgReadInterval = 0.0;
totalI2Creads = 0L;
i2cIMU.setI2cPortActionFlag(); // Set this flag to do the next read
i2cIMU.writeI2cCacheToController();
readStartTime = System.nanoTime();
}
private boolean autoCalibrationOK(int timeOutSeconds) {
boolean readingEnabled = false, calibrationDone = false;
long calibrationStart = System.nanoTime(),
rightNow = System.nanoTime(),
loopStart = System.nanoTime();
;
while ((System.nanoTime() - calibrationStart)
<= 60000000000L) { // Set a 1-minute overall timeout
try {
loopStart = System.nanoTime();
while ((!i2cIMU.isI2cPortReady())
&& (((rightNow = System.nanoTime()) - loopStart) < (timeOutSeconds * 1000000000L))) {
Thread.sleep(
250); // "Snooze" right here, until the port is ready (a good thing) OR n billion
// nanoseconds pass with the port "stuck busy" (a VERY bad thing)
}
} catch (InterruptedException e) {
Log.i(
"FtcRobotController", "Unexpected interrupt while \"sleeping\" in autoCalibrationOK.");
return false;
}
if ((rightNow - loopStart) >= (timeOutSeconds * 1000000000L)) {
Log.i(
"FtcRobotController",
"IMU I2C port \"stuck busy\" for " + (rightNow - loopStart) + " ns.");
return false; // Signals the "stuck busy" condition
}
if (!readingEnabled) { // Start a stream of reads of the calibration status byte
i2cIMU.enableI2cReadMode(baseI2Caddress, BNO055_CALIB_STAT_ADDR, 2);
// i2cIMU.enableI2cReadMode(baseI2Caddress, BNO055_CHIP_ID_ADDR, 1); //FOR TESTING ONLY!
// i2cIMU.enableI2cReadMode(baseI2Caddress, BNO055_PAGE_ID_ADDR, 1); //FOR TESTING ONLY!
i2cIMU.setI2cPortActionFlag(); // Set this flag to do the next read
i2cIMU.writeI2cCacheToController();
snooze(250); // Give the data time to go from the Interface Module to the IMU hardware
readingEnabled = true;
} else { // Check the Calibration Status and Self-Test bytes in the Read Cache. IMU datasheet
// Sec. 3.10, p.47. Also, see p. 70
i2cIMU.readI2cCacheFromController(); // Read in the most recent data from the device
snooze(500); // Give the data time to come into the Interface Module from the IMU hardware
try {
i2cReadCacheLock.lock();
if ((i2cReadCache[I2cController.I2C_BUFFER_START_ADDRESS + 1] == (byte) 0X0F)
&& (i2cReadCache[I2cController.I2C_BUFFER_START_ADDRESS] >= (byte) 0X30)
// (i2cReadCache[I2cController.I2C_BUFFER_START_ADDRESS] == (byte)BNO055_ID)//FOR TESTING
// ONLY!
// (i2cReadCache[I2cController.I2C_BUFFER_START_ADDRESS] == (byte)0X00)//FOR TESTING ONLY!
) {
// See IMU datasheet p.67. As an example, 0X30 checks whether at least the gyros are
// calinbrated.
// Also on that page: Self-Test byte value of 0X0F means everything passed self-test.
calibrationDone = true; // Auto calibration finished successfully
}
} finally {
i2cReadCacheLock.unlock();
}
if (calibrationDone) {
Log.i(
"FtcRobotController",
"Autocalibration OK! Cal status byte = "
+ String.format("0X%02X", i2cReadCache[I2cController.I2C_BUFFER_START_ADDRESS])
+ ". Self Test byte = "
+ String.format(
"0X%02X", i2cReadCache[I2cController.I2C_BUFFER_START_ADDRESS + 1])
+ ".");
return true;
}
i2cIMU.setI2cPortActionFlag(); // Set this flag to do the next read
i2cIMU.writeI2cPortFlagOnlyToController();
// At this point, the port becomes busy (not ready) doing the next read
snooze(250); // Give the data time to go from the Interface Module to the IMU hardware
}
}
Log.i(
"FtcRobotController",
"Autocalibration timed out! Cal status byte = "
+ String.format("0X%02X", i2cReadCache[I2cController.I2C_BUFFER_START_ADDRESS])
+ ". Self Test byte = "
+ String.format("0X%02X", i2cReadCache[I2cController.I2C_BUFFER_START_ADDRESS + 1])
+ ".");
return false;
}