Exemplo n.º 1
0
  public void DriveOnHeadingReverse(
      navXPIDController.PIDResult yawPIDResult, float heading, float distanceInches) {

    // calculate encoder counts for distance
    float wheelDiameter = 4; // inches
    float wheelCirc = wheelDiameter * (float) 3.14159;
    float encoderTicksPerRotation = 1120;
    float ticksPerInch = encoderTicksPerRotation / wheelCirc;
    int ticksToTravel = (int) (distanceInches * ticksPerInch);

    // check motor position
    int startEncCount = robot.leftfrontMotor.getCurrentPosition();
    int DEVICE_TIMEOUT_MS = 500;
    /* Drive straight forward at 1/2 of full drive speed */
    double drive_speed = 0.5;
    yawPIDController.setPID(0.05, YAW_PID_I, YAW_PID_D);
    yawPIDController.setSetpoint(heading);
    DecimalFormat df = new DecimalFormat("#.##");
    try {
      while ((robot.leftfrontMotor.getCurrentPosition() - startEncCount) > -ticksToTravel
          && !Thread.currentThread().isInterrupted()) {
        if (yawPIDController.waitForNewUpdate(yawPIDResult, DEVICE_TIMEOUT_MS)) {
          if (yawPIDResult.isOnTarget()) {
            robot.Drive(-drive_speed, -drive_speed);
            telemetry.addData("PIDOutput", df.format(drive_speed) + ", " + df.format(drive_speed));
          } else {
            double output = yawPIDResult.getOutput();
            if (output < -0.5) {
              output = -0.5;
            }
            if (output > 0.5) {
              output = 0.5;
            }
            robot.Drive(-drive_speed + output, -drive_speed - output);

            telemetry.addData(
                "PIDOutput",
                df.format(limit(-drive_speed - output))
                    + ", "
                    + df.format(limit(-drive_speed + output)));
          }
          telemetry.addData("Yaw", df.format(navx_device.getYaw()));
          telemetry.addData("Output", df.format(yawPIDResult.getOutput()));
          telemetry.addData("RevEnc:", robot.leftfrontMotor.getCurrentPosition());
          telemetry.addData("EncStart:", startEncCount);
        } else {
          /* A timeout occurred */
          Log.w("navXDriveStraightOp", "Yaw PID waitForNewUpdate() TIMEOUT.");
        }
      }
    } catch (InterruptedException ex) {
      Thread.currentThread().interrupt();
    }
  }
  @Override
  public void loop() {
    /* reset the navX-Model device yaw angle so that whatever direction */
    /* it is currently pointing will be zero degrees.                   */
    if (first_iteration) {
      first_iteration = false;
      navx_device.zeroYaw();
      yawPIDResult = new navXPIDController.PIDResult();
    }

    /* Wait for new Yaw PID output values, then update the motors
      with the new PID value with each new output value.
    */

    /* Drive straight forward at 1/2 of full drive speed */
    double drive_speed = 0.5;

    if (yawPIDController.isNewUpdateAvailable(yawPIDResult)) {
      if (yawPIDResult.isOnTarget()) {
        leftMotor.setPower(drive_speed);
        rightMotor.setPower(drive_speed);
      } else {
        double output = yawPIDResult.getOutput();
        leftMotor.setPower(drive_speed + output);
        rightMotor.setPower(drive_speed - output);
      }
    } else {
      /* No sensor update has been received since the last time  */
      /* the loop() function was invoked.  Therefore, there's no */
      /* need to update the motors at this time.                 */
    }
  }
Exemplo n.º 3
0
  public void TurnToHeading(
      navXPIDController.PIDResult yawPIDResult, double heading, double maxTimeSeconds) {
    try {
      long starttime = System.currentTimeMillis();

      TARGET_ANGLE_DEGREES = heading;
      int DEVICE_TIMEOUT_MS = 500;
      yawPIDController.setPID(0.02, YAW_PID_I, YAW_PID_D);
      yawPIDController.setSetpoint(TARGET_ANGLE_DEGREES);
      DecimalFormat df = new DecimalFormat("#.##");

      while (!Thread.currentThread().isInterrupted()
          && (System.currentTimeMillis() - starttime) < (long) (maxTimeSeconds * 1000)) {
        if (yawPIDController.waitForNewUpdate(yawPIDResult, DEVICE_TIMEOUT_MS)) {
          if (yawPIDResult.isOnTarget()) {
            float zero = 0;
            robot.FloatMotors();
            telemetry.addData("PIDOutput", df.format(0.00));
            telemetry.update();
          } else {
            double output = yawPIDResult.getOutput();
            if (output > 0 && output < 0.25) {
              output = 0.25;
            }
            if (output < 0 && output > -0.25) {
              output = -0.25;
            }
            robot.Drive(output, -output);
            telemetry.addData("PIDOutput", df.format(output) + ", " + df.format(-output));
          }
        } else {
          /* A timeout occurred */
          Log.w("navXRotateOp", "Yaw PID waitForNewUpdate() TIMEOUT.");
          telemetry.addData("timeout", "timeout occured");
        }
        telemetry.addData("Yaw aw", df.format(navx_device.getYaw()));
        telemetry.update();
      }
    } catch (InterruptedException ex) {
      Thread.currentThread().interrupt();
    }
  }
  @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;
  }
 @Override
 public void loop() {
   if (!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) {
       navx_device.zeroYaw();
     } else {
       telemetry.addData("navX-Micro", "Startup Calibration in Progress");
     }
   } else {
     /* Wait for new Yaw PID output values, then update the motors
       with the new PID value with each new output value.
     */
     if (yawPIDController.isNewUpdateAvailable(yawPIDResult)) {
       if (yawPIDResult.isOnTarget()) {
         leftMotor.setPowerFloat();
         rightMotor.setPowerFloat();
         telemetry.addData("Motor Output", df.format(0.00));
       } else {
         double output = yawPIDResult.getOutput();
         leftMotor.setPower(output);
         rightMotor.setPower(-output);
         telemetry.addData("Motor Output", df.format(output) + ", " + df.format(-output));
       }
     } else {
       /* No sensor update has been received since the last time  */
       /* the loop() function was invoked.  Therefore, there's no */
       /* need to update the motors at this time.                 */
     }
     telemetry.addData("Yaw", df.format(navx_device.getYaw()));
   }
 }
Exemplo n.º 6
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();
  }