示例#1
0
  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;
  }
示例#2
0
 /*
  * 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
 }
示例#3
0
 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();
 }
示例#4
0
  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;
  }
示例#5
0
  // 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!");
    }
  }