@Override
  public void init() {
    leftMotor = hardwareMap.dcMotor.get("left_drive");
    rightMotor = hardwareMap.dcMotor.get("right_drive");

    navx_device =
        AHRS.getInstance(
            hardwareMap.deviceInterfaceModule.get("dim"),
            NAVX_DIM_I2C_PORT,
            AHRS.DeviceDataType.kProcessedData,
            NAVX_DEVICE_UPDATE_RATE_HZ);

    rightMotor.setDirection(DcMotor.Direction.REVERSE);

    /* If possible, use encoders when driving, as it results in more */
    /* predictable drive system response.                           */
    leftMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
    rightMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS);

    /* Create a PID Controller which uses the Yaw Angle as input. */
    yawPIDController =
        new navXPIDController(navx_device, navXPIDController.navXTimestampedDataSource.YAW);

    /* Configure the PID controller */
    yawPIDController.setSetpoint(TARGET_ANGLE_DEGREES);
    yawPIDController.setContinuous(true);
    yawPIDController.setOutputRange(MIN_MOTOR_OUTPUT_VALUE, MAX_MOTOR_OUTPUT_VALUE);
    yawPIDController.setTolerance(navXPIDController.ToleranceType.ABSOLUTE, TOLERANCE_DEGREES);
    yawPIDController.setPID(YAW_PID_P, YAW_PID_I, YAW_PID_D);
    yawPIDController.enable(true);

    first_iteration = true;
  }
Пример #2
0
  @Override
  public void runOpMode() throws InterruptedException {

    // use the Hardware4962 stuff that decribes the robot.
    robot.init(hardwareMap);

    // hardware and initialize the sensors
    // odsSensor = hardwareMap.opticalDistanceSensor.get("ods");
    colorC = hardwareMap.i2cDevice.get("color sensor");
    colorCreader = new I2cDeviceSynchImpl(colorC, new I2cAddr(0x1e), false);
    colorCreader.engage();

    // navX setup

    navx_device =
        AHRS.getInstance(
            hardwareMap.deviceInterfaceModule.get("dim"),
            NAVX_DIM_I2C_PORT,
            AHRS.DeviceDataType.kProcessedData,
            NAVX_DEVICE_UPDATE_RATE_HZ);

    /* Create a PID Controller which uses the Yaw Angle as input. */
    yawPIDController =
        new navXPIDController(navx_device, navXPIDController.navXTimestampedDataSource.YAW);

    /* Configure the PID controller */
    yawPIDController.setSetpoint(TARGET_ANGLE_DEGREES);
    yawPIDController.setContinuous(true);
    yawPIDController.setOutputRange(MIN_MOTOR_OUTPUT_VALUE, MAX_MOTOR_OUTPUT_VALUE);
    yawPIDController.setTolerance(navXPIDController.ToleranceType.ABSOLUTE, TOLERANCE_DEGREES);
    yawPIDController.setPID(YAW_PID_P, YAW_PID_I, YAW_PID_D);
    yawPIDController.enable(true);

    // color sensor initilization

    colorCreader.write8(3, 1); // Set the mode of the color sensor to passive (0=active)
    // passive = turn off the LED

    // intake not running.

    double intakePower = 0.0;

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

    // Make sure the navX is ready to be used.

    while (!calibration_complete) {
      /* navX-Micro Calibration completes automatically ~15 seconds after it is
      powered on, as long as the device is still.  To handle the case where the
      navX-Micro has not been able to calibrate successfully, hold off using
      the navX-Micro Yaw value until calibration is complete.
       */
      calibration_complete = !navx_device.isCalibrating();
      if (!calibration_complete) {
        telemetry.addData("navX-Micro", "Startup Calibration in Progress");
        telemetry.update();
      }
    }
    // start at a heading of zero.

    navx_device.zeroYaw();
    int DEVICE_TIMEOUT_MS = 500;
    navXPIDController.PIDResult yawPIDResult = new navXPIDController.PIDResult();

    DecimalFormat df = new DecimalFormat("#.##");

    /** ********************************************************************************** */
    /** ********************************************************************************** */
    /*
    /*  start of code that does stuff
     */

    // DriveOnHeadingReverse(yawPIDResult,0,36);
    // robot.StopDriving();
    //		sleep(10000);

    // Drive parallel to the ramp and then turn parallel to the wall at the first beacon.

    DriveOnHeading(yawPIDResult, 0, 65);
    robot.StopDriving();
  }
  public void gyroTurn(double degrees, boolean buttFirst) {
    // degrees=degrees*-1;

    yawPIDController =
        new navXPIDController(navx_device, navXPIDController.navXTimestampedDataSource.YAW);
    yawPIDController.setSetpoint(degrees);
    yawPIDController.setContinuous(true);
    yawPIDController.setOutputRange(Keys.MAX_SPEED * -1, Keys.MAX_SPEED);
    yawPIDController.setTolerance(navXPIDController.ToleranceType.ABSOLUTE, Keys.TOLERANCE_LEVEL_2);

    yawPIDController.enable(true);
    int DEVICE_TIMEOUT_MS = 500;
    navXPIDController.PIDResult yawPIDResult = new navXPIDController.PIDResult();

    telemetry.addData("Yaw", navx_device.getYaw());
    double degreesNow = navx_device.getYaw();
    double degreesToGo = degreesNow + degrees;
    // telemetry.addData("if state",navx_device.getYaw());
    // telemetry.addData("other if",degreesToGo);
    telemetry.addData("boolean", navx_device.getYaw() > degreesToGo);
    // telemetry.addData("boolean",navx_device.getYaw()<degreesToGo);
    if (navx_device.getYaw() > degreesToGo) {
      telemetry.addData("if", "getYaw>degrees");
      telemetry.addData(
          "more boolean",
          !(degreesToGo - Keys.TOLERANCE_LEVEL_1 < navx_device.getYaw()
              && navx_device.getYaw() < degreesToGo + Keys.TOLERANCE_LEVEL_1));
      while (!(degreesToGo - Keys.TOLERANCE_LEVEL_1 < navx_device.getYaw()
          && navx_device.getYaw() < degreesToGo + Keys.TOLERANCE_LEVEL_1)) {
        collector.setPower(Keys.COLLECTOR_POWER * -1);
        telemetry.addData("while", "turningLeft1");
        double turnPower = .8;
        if (buttFirst) {
          turnPower = -.8;
          turnRight(turnPower);
        } else {
          turnLeft(turnPower);
        }
        telemetry.addData("if", ".yaw" + navx_device.getYaw() + "toGo" + degreesToGo);
      }
      telemetry.addData(
          "more boolean2", navx_device.getYaw() > degreesToGo + Keys.TOLERANCE_LEVEL_2);
      while (navx_device.getYaw() > degreesToGo + Keys.TOLERANCE_LEVEL_2) {
        collector.setPower(Keys.COLLECTOR_POWER * -1);
        telemetry.addData("while", "turningLeft2");
        double turnPower = .75;
        if (buttFirst) {
          turnPower = -1 * turnPower;
          turnRight(turnPower);
        } else {
          turnLeft(turnPower);
        }
        telemetry.addData("if", ".yaw" + navx_device.getYaw() + "toGo" + degreesToGo);
      }
      while (!(degreesToGo - Keys.TOLERANCE_LEVEL_3 < navx_device.getYaw()
          && navx_device.getYaw() < degreesToGo + Keys.TOLERANCE_LEVEL_3)) {
        collector.setPower(Keys.COLLECTOR_POWER * -1);
        telemetry.addData("while", "turningLeft3");
        double turnPower = .7;
        if (buttFirst) {
          turnPower = -1 * turnPower;
          turnRight(turnPower);
        } else {
          turnLeft(turnPower);
        }
        telemetry.addData("if", ".yaw" + navx_device.getYaw() + "toGo" + degreesToGo);
      }
      telemetry.addData("while", "done");
    } else if (navx_device.getYaw() < degreesToGo) {
      telemetry.addData("if", "getYaw<degrees");
      while (!(degreesToGo - Keys.TOLERANCE_LEVEL_1 < navx_device.getYaw()
          && navx_device.getYaw() < degreesToGo + Keys.TOLERANCE_LEVEL_1)) {
        collector.setPower(Keys.COLLECTOR_POWER * -1);
        double turnPower = .8;
        if (buttFirst) {
          turnPower = -1 * turnPower;
          turnLeft(turnPower);
        } else {
          turnRight(turnPower);
        }
        telemetry.addData("if", ".yaw" + navx_device.getYaw() + "toGo" + degreesToGo);
        telemetry.addData("while", "turningRight");
      }
      while (!(degreesToGo - Keys.TOLERANCE_LEVEL_2 < navx_device.getYaw()
          && navx_device.getYaw() < degreesToGo + Keys.TOLERANCE_LEVEL_2)) {
        collector.setPower(Keys.COLLECTOR_POWER * -1);
        double turnPower = .75;
        if (buttFirst) {
          turnPower = -1 * turnPower;
          turnLeft(turnPower);
        } else {
          turnRight(turnPower);
        }
        telemetry.addData("if", ".yaw" + navx_device.getYaw() + "toGo" + degreesToGo);
        telemetry.addData("while", "turningRight");
      }
      while (!(degreesToGo - Keys.TOLERANCE_LEVEL_3 < navx_device.getYaw()
          && navx_device.getYaw() < degreesToGo + Keys.TOLERANCE_LEVEL_3)) {
        collector.setPower(Keys.COLLECTOR_POWER * -1);
        double turnPower = .7;
        if (buttFirst) {
          turnPower = -1 * turnPower;
          turnLeft(turnPower);
        } else {
          turnRight(turnPower);
        }

        telemetry.addData("if", ".yaw" + navx_device.getYaw() + "toGo" + degreesToGo);
        telemetry.addData("while", "turningRight");
      }

      telemetry.addData("whileD", "done");
    }
    telemetry.addData("ifD", "done");
    rest();
  }