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();
}
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();
}
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();
}
/**
* *
*
* <p>waitForTick implements a periodic delay. However, this acts like a metronome with a regular
* periodic tick. This is used to compensate for varying processing times for each cycle. The
* function looks at the elapsed cycle time, and sleeps for the remaining time interval.
*
* @param periodMs Length of wait cycle in mSec.
* @throws InterruptedException
*/
public void waitForTick(long periodMs) throws InterruptedException {
long remaining = periodMs - (long) period.milliseconds();
// sleep for the remaining portion of the regular cycle period.
if (remaining > 0) Thread.sleep(remaining);
// Reset the cycle clock for the next pass.
period.reset();
}
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();
}
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();
}