continuing the compartment stuff

svhsrobotics · Jan 8, 2024 · 6fcec15 · 6fcec15
1 parent 057ee35
commit 6fcec15
Showing 1 changed file with 6 additions and 12 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/ParkingIn.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/components/ParkingIn.java
@@ -25,20 +25,14 @@ public void drive() {
         TrajectorySequenceBuilder trajB = getRobot().getDrive().trajectorySequenceBuilder(currentPose);
 
         if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_AUDIENCE) {
-            trajB = trajB.lineTo(new Vector2d(currentPose.getX(), -6))
-                    .lineTo(new Vector2d(currentPose.getX(), -5 * 12 + 3))
-                    .turn(Math.toRadians(90))
-                    .lineTo(new Vector2d(3 * 12 + 7, -6));
+            trajB = trajB.lineTo(new Vector2d(currentPose.getX(), -12))
+                    .lineTo(new Vector2d(6 * 12 + 14, -12));
         } else if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.RED_BOARD) {
-            trajB = trajB.lineTo(new Vector2d(currentPose.getX(), -6))
-                    .lineTo(new Vector2d(currentPose.getX(), -5 * 12 + 3))
-                    .turn(Math.toRadians(90))
-                    .lineTo(new Vector2d(3 * 12 + 7, -6));
+            trajB = trajB.lineTo(new Vector2d(currentPose.getX(), -12))
+                    .lineTo(new Vector2d(6 * 12 + 14, -12));
         } else if (getRobot().getDrive().currentQuadrant() == TrajectoryDrive.Quadrant.BLUE_BOARD) {
-            trajB = trajB.lineTo(new Vector2d(currentPose.getX(), 6))
-                    .lineTo(new Vector2d(currentPose.getX(), 5 * 12 + 3))
-                    .turn(Math.toRadians(90))
-                    .lineTo(new Vector2d(3 * 12 + 7, 6));
+            trajB = trajB.lineTo(new Vector2d(currentPose.getX(), 12))
+                    .lineTo(new Vector2d(6 * 12 + 14, 12));
 
         }