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