Пример #1
0
  public void rotatePB(double pow, int deg) throws InterruptedException {

    double power = pow;
    double angleTo = deg;
    double error;
    double inte = 0;
    double inteNoE = 0;
    double der;

    double currentAngle = sensor.getGyroYaw();
    double previousError = angleTo - currentAngle;

    opMode.telemetry.addData("Current Angle", currentAngle + "");
    opMode.telemetry.addData("Angle To", angleTo + "");
    opMode.telemetry.update();

    opMode.resetStartTime();

    currentAngle = 0;

    while (Math.abs(currentAngle) < Math.abs(angleTo) - 2) {
      currentAngle = sensor.getGyroYaw();
      error = Math.abs(angleTo) - Math.abs(currentAngle);
      opMode.telemetry.addData("error", error);
      power = (pow * (error) * .005) + .1; // update p values
      inte = ((opMode.getRuntime()) * error * .0025); // update inte value
      inteNoE = ((opMode.getRuntime()) * .05);
      der = (error - previousError) / opMode.getRuntime() * 0; // update der value

      power = power + inteNoE + der;

      if (angleTo > 0) power *= -1;

      Range.clip(power, -1, 1);
      startMotors(-power, power);
      opMode.telemetry.addData("PID", power);
      //            opMode.telemetry.addData("integral", inte);
      opMode.telemetry.addData("integral without error", inteNoE);
      opMode.telemetry.addData("angle", currentAngle);

      opMode.telemetry.update();
      previousError = error;
      opMode.idle();
    }

    stopMotors();
    opMode.telemetry.addData("finished", "done");
    opMode.telemetry.update();
  }
Пример #2
0
 @Override
 public void waitOneFullHardwareCycle() throws InterruptedException {
   if (isAllianceColorSet())
     telemetry.addData("alliance color", getAllianceColor().toString().toLowerCase());
   robotController.loop();
   super.waitOneFullHardwareCycle();
 }
Пример #3
0
  public void moveForwardToWall(double pow, int timeout) throws InterruptedException {
    double angle = Math.abs(sensor.getGyroYaw());
    double startAngle = angle;

    double power = pow;

    ElapsedTime time = new ElapsedTime();
    time.startTime();
    while (Math.abs(angle - startAngle) < 10 && time.milliseconds() < timeout) {
      angle = Math.abs(sensor.getGyroYaw());
      opMode.telemetry.addData("LeftPower", motorBL.getPower());
      opMode.telemetry.addData("RightPower", motorBR.getPower());
      opMode.telemetry.update();

      if (angle < startAngle - 1) {
        startMotors((power * .75), power);
      } else if (angle > startAngle + 1) {
        startMotors(power, (power * .75));
      } else {
        startMotors(power, power);
      }

      opMode.idle();
    }

    stopMotors();

    angleError = sensor.getGyroYaw();
    opMode.telemetry.update();
  }
Пример #4
0
  public void moveFowardToLine(double ri, double le, int timeout) throws InterruptedException {
    double angle;
    double startAngle = Math.abs(sensor.getGyroYaw());
    opMode.telemetry.update();

    setNullValue();
    ElapsedTime time = new ElapsedTime();
    time.startTime();

    while (!sensor.isLeftLine() && time.milliseconds() < timeout) {
      angle = Math.abs(sensor.getGyroYaw());

      opMode.telemetry.addData("LeftPower", motorBL.getPower());
      opMode.telemetry.addData("RightPower", motorBR.getPower());
      opMode.telemetry.update();

      //            if(angle < startAngle - 2) {
      //                startMotors((power * .75), power);
      //            } else if(angle > startAngle + 2) {
      //                startMotors(power, (power * .75));
      //            } else {
      //                startMotors(power, power);
      //            }

      startMotors(ri, le);
      opMode.idle();
    }
    stopMotors();
    opMode.telemetry.update();
    angleError = sensor.getGyroYaw();
  }
Пример #5
0
  public void rotatePZeroRev(double pow) throws InterruptedException {

    double power = pow;
    double angleTo = 0;
    double error;
    double inte = 0;
    double inteNoE = 0;
    double der;

    double currentAngle = sensor.getGyroYaw();
    double previousError = angleTo - currentAngle;

    opMode.telemetry.addData("Current Angle", currentAngle + "");
    opMode.telemetry.addData("Angle To", angleTo + "");
    opMode.telemetry.update();

    opMode.resetStartTime();

    while (Math.abs(currentAngle) > 2) {
      currentAngle = sensor.getGyroYaw();
      error = Math.abs(Math.abs(angleTo) - Math.abs(currentAngle));
      opMode.telemetry.addData("error", error);
      power = (pow * (error) * .02) + .125; // update p values
      inte = ((opMode.getRuntime()) * error * .0020); // update inte value
      inteNoE = ((opMode.getRuntime()) * .075);
      der = (error - previousError) / opMode.getRuntime() * 0; // update der value

      power = power + inteNoE - der;

      power *= 1; // -1 is right

      Range.clip(power, -1, 1);
      startMotors(-power, power);
      opMode.telemetry.addData("PID", power);
      //            opMode.telemetry.addData("integral", inte);
      opMode.telemetry.addData("integral without error", inteNoE);
      opMode.telemetry.addData("angle", currentAngle);

      opMode.telemetry.update();
      previousError = error;
      opMode.idle();
    }

    opMode.telemetry.update();
    stopMotors();
  }
Пример #6
0
  public void moveForwardUntilZero(double pow, double timeout) throws InterruptedException {

    double angle = sensor.getGyroYaw();

    ElapsedTime time = new ElapsedTime();
    time.startTime();
    while (Math.abs(angle) > 2 && time.milliseconds() < timeout) {
      startMotors(-.3, -.5);

      opMode.telemetry.addData("current", "zeroout");
      opMode.telemetry.update();

      opMode.idle();
    }

    stopMotors();
  }
Пример #7
0
  public void moveForwardToWallEnc(double pow, int timeout) throws InterruptedException {
    double angle = Math.abs(sensor.getGyroYaw());
    double startAngle = angle;

    double power = pow;

    int tick = 1;

    resetEncoders();

    int startEncoder = Math.abs(motorFL.getCurrentPosition());

    int endEncoder;

    ElapsedTime time = new ElapsedTime();
    time.startTime();
    while (Math.abs(angle - startAngle) < 10 && time.milliseconds() < timeout) {
      angle = Math.abs(sensor.getGyroYaw());
      opMode.telemetry.addData("LeftPower", motorBL.getPower());
      opMode.telemetry.addData("RightPower", motorBR.getPower());
      opMode.telemetry.update();

      if (time.milliseconds() > 250 * tick) {
        endEncoder = Math.abs(motorFL.getCurrentPosition());
        if (startEncoder + 250 < endEncoder) {
          break;
        }
      }

      if (angle < startAngle - 1) {
        startMotors((power * .75), power);
      } else if (angle > startAngle + 1) {
        startMotors(power, (power * .75));
      } else {
        startMotors(power, power);
      }

      opMode.idle();
    }

    stopMotors();

    angleError = sensor.getGyroYaw();
    opMode.telemetry.update();
  }
Пример #8
0
  public void resetEncoders() throws InterruptedException {

    motorBL.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
    opMode.idle();
    motorBR.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
    opMode.idle();
    motorFR.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
    opMode.idle();
    motorFL.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
    opMode.idle();

    motorBL.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
    opMode.idle();
    motorBR.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
    opMode.idle();
    motorFR.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
    opMode.idle();
    motorFL.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
    opMode.idle();
  }
Пример #9
0
 @Override
 public void waitForStart() throws InterruptedException {
   super.waitForStart();
   this.loop();
 }
Пример #10
0
  public void moveBackward(double pow, int encoderVal, int timeout) throws InterruptedException {
    //        sensor.resetGyro();
    double angle;
    double startAngle = Math.abs(sensor.getGyroYaw());
    opMode.telemetry.update();

    double error;
    double power;

    motorBL.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
    opMode.idle();
    motorBR.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
    opMode.idle();
    motorFR.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
    opMode.idle();
    motorFL.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
    opMode.idle();

    motorBL.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
    opMode.idle();
    motorBR.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
    opMode.idle();
    motorFR.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
    opMode.idle();
    motorFL.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
    opMode.idle();

    setNullValue();

    nullValue = 0;

    ElapsedTime time = new ElapsedTime();
    time.startTime();

    int currentEncoder = getEncoderAvg() - nullValue;
    while (encoderVal > currentEncoder && time.milliseconds() < timeout) {
      opMode.telemetry.update();
      angle = Math.abs(sensor.getGyroYaw());

      currentEncoder = getEncoderAvg() - nullValue;

      error = (double) (encoderVal - currentEncoder) / encoderVal;

      power = (pow * error) - .25;

      Range.clip(power, -1, 1);

      opMode.telemetry.addData("Power", power);
      opMode.telemetry.addData("LeftPower", motorBL.getPower());
      opMode.telemetry.addData("RightPower", motorBR.getPower());
      opMode.telemetry.update();

      if (angle < startAngle - 2) {
        startMotors((power), (power * .75));
      } else if (angle > startAngle + 2) {
        startMotors((power * .75), (power));
      } else {
        startMotors(power, power);
      }

      startMotors(power, power);
      opMode.idle();
    }
    stopMotors();
    opMode.telemetry.update();
    angleError = sensor.getGyroYaw();
  }