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();
}
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();
}
public void moveForward(double pow, int encoderVal, double 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();
opMode.resetStartTime();
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;
if (pow < 0) power = (pow * error) - .2;
else power = (pow * error) + .2;
Range.clip(power, -1, 1);
opMode.telemetry.addData("Power", power);
opMode.telemetry.addData("LeftPower", motorBL.getPower());
opMode.telemetry.addData("RightPower", motorBR.getPower());
opMode.telemetry.addData("error", error);
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(power, power);
opMode.idle();
}
stopMotors();
opMode.telemetry.update();
angleError = sensor.getGyroYaw();
}