diff --git a/Autonomous.java b/Autonomous.java index 4b3dd3f..4c41b33 100644 --- a/Autonomous.java +++ b/Autonomous.java @@ -58,13 +58,12 @@ public void runOpMode() throws InterruptedException { // loc = detectTape(randomizationWebcam); // } // randomizationWebcam.stop(); - navigation = new Navigation(new double[]{12, 66}, 0, this, hardwareMap); + navigation = new Navigation(new double[]{12, 66}, 270, this, hardwareMap); while (opModeInInit()) { TelemetryWrapper.setLine(7, "x: " + navigation.getCurrentPos()[0] + " y: " + navigation.getCurrentPos()[1]); TelemetryWrapper.setLine(8, "Gyro bearing: " + navigation.getGyroBearing()); } - navigation = new Navigation(new double[]{12, 66}, 270, this, hardwareMap); navigation.MoveToPosDirect(new double[]{12, 33}); navigation.setBearing(0); while (!navigation.tagCam.isDetecting) { diff --git a/TeleOp.java b/TeleOp.java index 4911f82..b1dc179 100644 --- a/TeleOp.java +++ b/TeleOp.java @@ -78,15 +78,15 @@ public void runOpMode() { while (opModeIsActive()) { - TelemetryWrapper.setLine(2, "Output Box Lock State " + currentLockState); - TelemetryWrapper.setLine(3, "LeftSlidePos" + outputSlide.getCurrentPosition()[0]); - TelemetryWrapper.setLine(4, "RightSlidePos" + outputSlide.getCurrentPosition()[1]); - TelemetryWrapper.setLine(5, "LeftSlideTargetPos" + outputSlide.getTargetPosition()[0]); - TelemetryWrapper.setLine(6, "RightSlideTargetPos" + outputSlide.getTargetPosition()[1]); - TelemetryWrapper.setLine(7, "LeftSlideButton" + outputSlide.isElevatorBtnPressed()[0]); - TelemetryWrapper.setLine(8, "RightSlideButton" + outputSlide.isElevatorBtnPressed()[1]); - TelemetryWrapper.setLine(9, "PivotPos" + pivot.getCurrentPosition()); - TelemetryWrapper.setLine(10, "PivotTargetPos" + pivot.getTargetPosition()); + TelemetryWrapper.setLineNoRender(2, "Output Box Lock State " + currentLockState); + TelemetryWrapper.setLineNoRender(3, "LeftSlidePos" + outputSlide.getCurrentPosition()[0]); + TelemetryWrapper.setLineNoRender(4, "RightSlidePos" + outputSlide.getCurrentPosition()[1]); + TelemetryWrapper.setLineNoRender(5, "LeftSlideTargetPos" + outputSlide.getTargetPosition()[0]); + TelemetryWrapper.setLineNoRender(6, "RightSlideTargetPos" + outputSlide.getTargetPosition()[1]); + TelemetryWrapper.setLineNoRender(7, "LeftSlideButton" + outputSlide.isElevatorBtnPressed()[0]); + TelemetryWrapper.setLineNoRender(8, "RightSlideButton" + outputSlide.isElevatorBtnPressed()[1]); + TelemetryWrapper.setLineNoRender(9, "PivotPos" + pivot.getCurrentPosition()); + TelemetryWrapper.setLineNoRender(10, "PivotTargetPos" + pivot.getTargetPosition()); TelemetryWrapper.setLine(11, "PivotButton" + pivot.isPressed()); // Update ButtonHelper gp1.update(); diff --git a/modules/Navigation.java b/modules/Navigation.java index 5eb625d..0f29c31 100644 --- a/modules/Navigation.java +++ b/modules/Navigation.java @@ -91,10 +91,10 @@ public void setBearing(double targetBearing) { targetBearing = ((targetBearing % 360) + 360) % 360; double dir; double dAngle; - while ((dAngle = (targetBearing - currentBearing + 180) % 360 - 180) > 1) { + while (opMode.opModeIsActive() && (dAngle = (targetBearing - currentBearing + 180) % 360 - 180) > 1) { TelemetryWrapper.setLine(6, "Target Bearing: " + targetBearing + " | " + "Current Bearing: " + currentBearing); currentBearing = getGyroBearing(); - driveTrain.move(0, 0, dAngle / 180, 1); + driveTrain.move(0, 0, Math.min(dAngle / 2, 1), 1); // TODO turning broken } driveTrain.stopStayInPlace(); } @@ -107,7 +107,7 @@ public void MoveToPosDirect(double[] targetPos) { double[] displacement = new double[]{targetPos[0] - currentPos[0], targetPos[1] - currentPos[1]}; long prevTime = System.currentTimeMillis(); // TODO debug remove driveTrain.move(0, 0.7, 0, 1); - while (magnitude(displacement) > 5) { + while (opMode.opModeIsActive() && magnitude(displacement) > 5) { long curTime = System.currentTimeMillis(); // TODO debug remove System.out.println("Navigation movement time for tick: " + (curTime - prevTime)); // TODO debug remove prevTime = curTime; // TODO debug remove @@ -177,7 +177,7 @@ public void MoveToPosAtAngle(double[] targetPos, double angle) { double[] lastEncoderPos = driveTrain.getEncPos(); double[] displacement = new double[]{targetPos[0] - currentPos[0], targetPos[1] - currentPos[1]}; - while (magnitude(displacement) > 4) { + while (opMode.opModeIsActive() && magnitude(displacement) > 4) { TelemetryWrapper.setLine(5, "x: " + currentPos[0] + "| y: " + currentPos[1] + "| theta: " + currentBearing); double[] currentEncPos = driveTrain.getEncPos(); currentBearing = getGyroBearing();