public void shutdownCoreInterfaceDeviceModules() {
   for (Entry entry : this.f17h.deviceInterfaceModule.entrySet()) {
     String str = (String) entry.getKey();
     DeviceInterfaceModule deviceInterfaceModule = (DeviceInterfaceModule) entry.getValue();
     DbgLog.msg("Stopping Core Interface Device Module " + str);
     deviceInterfaceModule.close();
   }
 }
 public ModernRoboticsI2cGyro(
     final DeviceInterfaceModule deviceInterfaceModule, final int physicalPort) {
   this.a = 32;
   this.b = deviceInterfaceModule;
   this.g = physicalPort;
   this.c = deviceInterfaceModule.getI2cReadCache(physicalPort);
   this.d = deviceInterfaceModule.getI2cReadCacheLock(physicalPort);
   this.e = deviceInterfaceModule.getI2cWriteCache(physicalPort);
   this.f = deviceInterfaceModule.getI2cWriteCacheLock(physicalPort);
   this.h = HeadingMode.HEADING_CARDINAL;
   deviceInterfaceModule.enableI2cReadMode(physicalPort, 32, 0, 18);
   deviceInterfaceModule.setI2cPortActionFlag(physicalPort);
   deviceInterfaceModule.writeI2cCacheToController(physicalPort);
   deviceInterfaceModule.registerForI2cPortReadyCallback(
       (I2cController.I2cPortReadyCallback) this, physicalPort);
   this.transactionQueue = new ConcurrentLinkedQueue<GyroI2cTransaction>();
   this.j = new a();
   this.i = MeasurementMode.GYRO_NORMAL;
 }
  @Override
  public void runOpMode() {
    elapsedTime = new ElapsedTime();
    dim = hardwareMap.deviceInterfaceModule.get("dim");
    dim.setDigitalChannelMode(GYROSCOPE_SPOT, DigitalChannelController.Mode.OUTPUT);
    gyro = hardwareMap.gyroSensor.get("gyro");
    dim.setDigitalChannelState(GYROSCOPE_SPOT, true);
    motorBL = hardwareMap.dcMotor.get("motorBL");
    motorBR = hardwareMap.dcMotor.get("motorBR");
    motorFL = hardwareMap.dcMotor.get("motorFL");
    elapsedTime.startTime();
    motorFR = hardwareMap.dcMotor.get("motorFR");
    //        color = hardwareMap.colorSensor.get("color");
    int distance1 = 5550;
    int distance3 = 9700;
    double currentAngle = 0.0;
    while (motorBL.getCurrentPosition() < distance1) {
      startMotors(-1, 1, 1, -1);
      getEncoderValues();
    }
    while (currentAngle < 90.0) {
      startMotors(-1, -1, -1, -1);
      getEncoderValues();
      currentAngle = gyro.getRotation();
    }
    while (motorBL.getCurrentPosition() < distance3) {
      startMotors(-1, 1, 1, -1);
      getEncoderValues();
    }
    getTime();
    elapsedTime.reset();
    double currentTime = 0.0;
    while (currentTime < 5.0) {
      getEncoderValues();
      getTime();
      currentTime = elapsedTime.time();
    }
    while (motorBL.getCurrentPosition() > 9000) { // 9034
      startMotors(1, -1, -1, 1);
      getEncoderValues();
    }
    while (currentAngle > 45.0) {
      startMotors(-1, -1, -1, -1);
      getEncoderValues();
      currentAngle = gyro.getRotation();
    }

    elapsedTime.reset();
    currentTime = 0.0;
    while (currentTime < 5.0) {
      startMotors(-1, 1, 1, -1);
      getEncoderValues();
      getTime();
      currentTime = elapsedTime.time();
    }
    stopMotors();
    motorBL.close();
    motorFL.close();
    motorBR.close();
    motorFR.close();
  }
  @Override
  public void runOpMode() throws InterruptedException {

    // hsvValues is an array that will hold the hue, saturation, and value information.
    float hsvValues[] = {0F, 0F, 0F};

    // values is a reference to the hsvValues array.
    final float values[] = hsvValues;

    // get a reference to the RelativeLayout so we can change the background
    // color of the Robot Controller app to match the hue detected by the RGB sensor.
    final View relativeLayout =
        ((Activity) hardwareMap.appContext).findViewById(R.id.RelativeLayout);

    // bPrevState and bCurrState represent the previous and current state of the button.
    boolean bPrevState = false;
    boolean bCurrState = false;

    // bLedOn represents the state of the LED.
    boolean bLedOn = true;

    // get a reference to our DeviceInterfaceModule object.
    cdim = hardwareMap.deviceInterfaceModule.get("dim");

    // set the digital channel to output mode.
    // remember, the Adafruit sensor is actually two devices.
    // It's an I2C sensor and it's also an LED that can be turned on or off.
    cdim.setDigitalChannelMode(LED_CHANNEL, DigitalChannelController.Mode.OUTPUT);

    // get a reference to our ColorSensor object.
    sensorRGB = hardwareMap.colorSensor.get("color");

    // turn the LED on in the beginning, just so user will know that the sensor is active.
    cdim.setDigitalChannelState(LED_CHANNEL, bLedOn);

    // wait for the start button to be pressed.
    waitForStart();

    // loop and read the RGB data.
    // Note we use opModeIsActive() as our loop condition because it is an interruptible method.
    while (opModeIsActive()) {

      // check the status of the x button on gamepad.
      bCurrState = gamepad1.x;

      // check for button-press state transitions.
      if ((bCurrState == true) && (bCurrState != bPrevState)) {

        // button is transitioning to a pressed state. Toggle the LED.
        bLedOn = !bLedOn;
        cdim.setDigitalChannelState(LED_CHANNEL, bLedOn);
      }

      // update previous state variable.
      bPrevState = bCurrState;

      // convert the RGB values to HSV values.
      Color.RGBToHSV(
          (sensorRGB.red() * 255) / 800,
          (sensorRGB.green() * 255) / 800,
          (sensorRGB.blue() * 255) / 800,
          hsvValues);

      // send the info back to driver station using telemetry function.
      telemetry.addData("LED", bLedOn ? "On" : "Off");
      telemetry.addData("Clear", sensorRGB.alpha());
      telemetry.addData("Red  ", sensorRGB.red());
      telemetry.addData("Green", sensorRGB.green());
      telemetry.addData("Blue ", sensorRGB.blue());
      telemetry.addData("Hue", hsvValues[0]);

      // change the background color to match the color detected by the RGB sensor.
      // pass a reference to the hue, saturation, and value array as an argument
      // to the HSVToColor method.
      relativeLayout.post(
          new Runnable() {
            public void run() {
              relativeLayout.setBackgroundColor(Color.HSVToColor(0xff, values));
            }
          });

      telemetry.update();
      idle(); // Always call idle() at the bottom of your while(opModeIsActive()) loop
    }
  }
  @Override
  public void main() throws InterruptedException {

    // write some device information (connection info, name and type)
    // to the log file.
    hardwareMap.logDevices();

    // get a reference to our DeviceInterfaceModule object.
    cdim = hardwareMap.deviceInterfaceModule.get("dim");

    // set the digital channel to output mode.
    // remember, the Adafruit sensor is actually two devices.
    // It's an I2C sensor and it's also an LED that can be turned on or off.
    cdim.setDigitalChannelMode(LED_CHANNEL, DigitalChannelController.Mode.OUTPUT);

    // get a reference to our ColorSensor object.
    sensorRGB = hardwareMap.colorSensor.get("color");

    // bEnabled represents the state of the LED.
    boolean bEnabled = true;

    // turn the LED on in the beginning, just so user will know that the sensor is active.
    cdim.setDigitalChannelState(LED_CHANNEL, bEnabled);

    // Set up our dashboard computations
    composeDashboard();

    // wait for the start button to be pressed.
    waitForStart();

    // hsvValues is an array that will hold the hue, saturation, and value information.
    float hsvValues[] = {0F, 0F, 0F};

    // values is a reference to the hsvValues array.
    final float values[] = hsvValues;

    // get a reference to the RelativeLayout so we can change the background
    // color of the Robot Controller app to match the hue detected by the RGB sensor.
    final View relativeLayout =
        ((Activity) hardwareMap.appContext).findViewById(R.id.RelativeLayout);

    // bPrevState and bCurrState represent the previous and current state of the button.
    boolean bPrevState = false;
    boolean bCurrState = false;

    // while the op mode is active, loop and read the RGB data.
    // Note we use opModeIsActive() as our loop condition because it is an interruptible method.
    while (opModeIsActive()) {
      // check the status of the x button on either gamepad.
      bCurrState = gamepad1.x || gamepad2.x;

      // check for button state transitions.
      if (bCurrState == true && bCurrState != bPrevState) {
        // button is transitioning to a pressed state.

        // print a debug statement.
        DbgLog.msg("MY_DEBUG - x button was pressed!");

        // update previous state variable.
        bPrevState = bCurrState;

        // on button press, enable the LED.
        bEnabled = true;

        // turn on the LED.
        cdim.setDigitalChannelState(LED_CHANNEL, bEnabled);

      } else if (bCurrState == false && bCurrState != bPrevState) {
        // button is transitioning to a released state.

        // print a debug statement.
        DbgLog.msg("MY_DEBUG - x button was released!");

        // update previous state variable.
        bPrevState = bCurrState;

        // on button press, enable the LED.
        bEnabled = false;

        // turn off the LED.
        cdim.setDigitalChannelState(LED_CHANNEL, bEnabled);
      }

      // convert the RGB values to HSV values.
      Color.RGBToHSV(
          (sensorRGB.red() * 255) / 800,
          (sensorRGB.green() * 255) / 800,
          (sensorRGB.blue() * 255) / 800,
          hsvValues);

      this.hue = hsvValues[0];

      // change the background color to match the color detected by the RGB sensor.
      // pass a reference to the hue, saturation, and value array as an argument
      // to the HSVToColor method.
      relativeLayout.post(
          new Runnable() {
            public void run() {
              relativeLayout.setBackgroundColor(Color.HSVToColor(0xff, values));
            }
          });

      this.telemetry.update();
      this.idle();
    }
  }