示例#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
  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();
  }