Esempio n. 1
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
 }
Esempio n. 2
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;
  }