private boolean i2cWriteImmediately(byte[] outboundBytes, int byteCount, int registerAddress) {
long rightNow = System.nanoTime(), startTime = System.nanoTime(), timeOut = 3000000000L;
int indx;
try {
while ((!i2cIMU.isI2cPortReady())
&& (((rightNow = System.nanoTime()) - startTime) < timeOut)) {
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 i2cWriteImmediately.");
return false;
}
if ((rightNow - startTime) >= timeOut) return false; // Signals the "stuck busy" condition
try {
i2cWriteCacheLock.lock();
for (indx = 0; indx < byteCount; indx++) {
i2cWriteCache[I2cController.I2C_BUFFER_START_ADDRESS + indx] = outboundBytes[indx];
// Both the read and write caches start with 5 bytes of prefix data.
}
} finally {
i2cWriteCacheLock.unlock();
}
i2cIMU.enableI2cWriteMode(baseI2Caddress, registerAddress, byteCount);
i2cIMU.setI2cPortActionFlag(); // Set the "go do it" flag
i2cIMU.writeI2cCacheToController(); // Then write it and the cache out
snooze(250); // Give the data time to go from the Interface Module to the IMU hardware
return true;
}
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;
}