Fix two instances of navigation

Big-Green-7245 · Jan 19, 2024 · 3bef0c8 · 3bef0c8
1 parent 1d48ee2
commit 3bef0c8
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 15 deletions.
diff --git 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
@@ -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
@@ -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();