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;
}
/*
* Use of the following callback assumes that I2C reading has been enabled for a particular I2C
* register address (as the starting address) and a particular byte count. Registration of this
* callback should only take place when that reading enablement is done.
*/
public void portIsReady(int port) { // Implementation of I2cController.I2cPortReadyCallback
double latestInterval;
if ((latestInterval = ((System.nanoTime() - readStartTime) / 1000000.0)) > maxReadInterval)
maxReadInterval = latestInterval;
avgReadInterval = ((avgReadInterval * 511.0) + latestInterval) / 512.0;
i2cIMU.readI2cCacheFromController(); // Read in the most recent data from the device
totalI2Creads++;
i2cIMU.setI2cPortActionFlag(); // Set this flag to do the next read
i2cIMU.writeI2cPortFlagOnlyToController();
readStartTime = System.nanoTime();
totalI2Creads++;
// At this point, the port becomes busy (not ready) doing the next read
}
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;
}
// The Constructor for the AdafruitIMU class
public AdafruitIMU(
HardwareMap currentHWmap,
String configuredIMUname,
// String configuredInterfaceName, int configuredPort,
byte baseAddress,
byte operMode)
throws RobotCoreException {
boolean okSoFar = true;
long calibrationStartTime = 0L;
byte[] outboundBytes = new byte[i2cBufferSize];
i2cIMU = currentHWmap.i2cDevice.get(configuredIMUname);
// The following code was required when the definition of the "I2cDevice" class was incomplete.
// deviceInterface = currentHWmap.deviceInterfaceModule.get(configuredInterfaceName);
// Log.i("FtcRobotController", "Core Device Interface Module info: "
// + deviceInterface.getConnectionInfo());
// configuredI2CPort = configuredPort;
// i2cIMU = new I2cDevice(deviceInterface, configuredI2CPort); //Identify the IMU with the port
// to
// which it is connected on the Modern Robotics Core Device Interface Module
baseI2Caddress = (int) baseAddress & 0XFF;
operationalMode = (int) operMode & 0XFF;
i2cReadCache = i2cIMU.getI2cReadCache();
i2cReadCacheLock = i2cIMU.getI2cReadCacheLock();
i2cWriteCache = i2cIMU.getI2cWriteCache();
i2cWriteCacheLock = i2cIMU.getI2cWriteCacheLock();
// Log.i("FtcRobotController", String.format("Read Cache length: %d, Write Cache length: %d."
// , i2cReadCache.length, i2cWriteCache.length));
/*
* According to the IMU datasheet, the defaults set up at power-on reset:
* PWR_MODE register = normal (p.19, table 3-1)
* OPR_MODE register = CONFIGMODE (p. 21, table 3-5)
* Default configuration for sensors as listed in table 3-7 on p.26
* Page 0 of the register map is automatically selected (p.50)
*
* Even if the Robot Controller software is restarted without power being cycled, the following
* actions should be done each time the AdafruitIMU class is reconstructed:
* 1. Set the PAGE_ID register to 0, so that Register Map 0 will make the SYS_TRIGGER register
* visible. (Datasheet p.50)
* 2. Reset the IMU, which causes output values to be reset to zero, and forces accelerometers,
* gyros, and magnetic compasses to autocalibrate. (See IMU data sheet p. 18, Table 4-2, p.51
* , and Section 3.10, p.47. Also, set the bit that commands self-test.
* 3. OPR_MODE register = the user-selected operationalMode (passed in as operMode)
*/
Log.i("FtcRobotController", "Preparing to reset IMU to its power-on state......");
// Set the register map PAGE_ID back to 0, to make the SYS_TRIGGER register visible
outboundBytes[0] = 0x00; // Sets the PAGE_ID bit for page 0 (Table 4-2)
okSoFar &= i2cWriteImmediately(outboundBytes, 1, BNO055_PAGE_ID_ADDR);
if (!okSoFar) {
throw new RobotCoreException("IMU PAGE_ID setting interrupted or I2C bus \"stuck busy\".");
}
if (okSoFar) {
// Set several bits bit in the SYS_TRIGGER register, to make the IMU reset
outboundBytes[0] =
(byte) 0XE1; // The "E" sets the RST_SYS and RST_INT bits, and sets the CLK_SEL bit
// , to select the external IMU clock mounted on the Adafruit board (Table 4-2, and p.70). In
// the lower 4 bits, a "1" sets the commanded Self Test bit, which causes self-test to run (p.
// 46)
Log.i(
"FtcRobotController",
String.format("SYS_TRIGGER = 0X%02X. Resetting IMU........", outboundBytes[0]));
okSoFar &= i2cWriteImmediately(outboundBytes, 1, BNO055_SYS_TRIGGER_ADDR);
snooze(
500); // Wait a decent interval until the IMU finishes its reset operations. IMU requires
// a total of 650 milliseconds after a reset before it can respond to further
// commands. Therefore, the wait time in "i2cWriteImmediately" is insufficient.
if (!okSoFar) {
throw new RobotCoreException("IMU reset interrupted or I2C bus \"stuck busy\".");
}
}
if (okSoFar) {
// Set the IMU's operational mode to the selected mode
outboundBytes[0] = (byte) operationalMode;
Log.i(
"FtcRobotController",
String.format("Setting operational mode = 0X%02X........", outboundBytes[0]));
okSoFar &= i2cWriteImmediately(outboundBytes, 1, BNO055_OPR_MODE_ADDR);
if (!okSoFar) {
throw new RobotCoreException(
"Operational mode setting interrupted or I2C bus" + " \"stuck busy\".");
}
}
if (okSoFar) {
Log.i("FtcRobotController", "Now waiting for autocalibration............");
calibrationStartTime = System.nanoTime();
okSoFar &=
autoCalibrationOK(10); // Check auto calibration with a timeout (seconds) on the wait
// for the I2C port to become ready
if (!okSoFar) {
throw new RobotCoreException(
"Auto calibration interrupted or I2C bus \"stuck busy\", "
+ "OR it timed out after "
+ String.format(
"%3.3f", (double) ((System.nanoTime() - calibrationStartTime) / 1000000000L))
+ " sec.");
}
}
if (okSoFar) {
Log.i(
"FtcRobotController",
"Auto calibration completed OK after "
+ String.format(
"%3.3f", (double) ((System.nanoTime() - calibrationStartTime) / 1000000000L))
+ " sec.");
Log.i("FtcRobotController", "IMU fully operational!");
}
}