10/5/24 - Harshini

added a mini line to see if i can change headings in a spline, not working but ill fix it later
technototes · Oct 9, 2024 · 2c53000 · 2c53000
1 parent 2243c6c
commit 2c53000
Showing 1 changed file with 8 additions and 4 deletions.
diff --git a/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/HarshiniTesting.java b/MeepMeepTesting/src/main/java/com/example/meepmeeptesting/HarshiniTesting.java
@@ -25,7 +25,9 @@ public static class AutoConstants {
         public static Pose2d OBSERVATION_ZONE = new Pose2d(-57, 55, toRadians(130));
         public static Pose2d SUBMARINE2 = new Pose2d(0, 36, toRadians(-90));
         public static Pose2d SUBMARINE3 = new Pose2d(5, 36, toRadians(-90));
-        public static Pose2d PUSH_HALF = new Pose2d(-16, 36, toRadians(0));
+        public static Pose2d MINI_LINE = new Pose2d(3, 36, toRadians(90));
+
+        public static Pose2d PUSH_HALF = new Pose2d(-16, 36, toRadians(90));
 
         public static Pose2d PUSH_1 = new Pose2d(-34, 45, toRadians(90));
         public static Pose2d PUSH_2 = new Pose2d(-34, 10, toRadians(90));
@@ -65,10 +67,12 @@ public static class AutoConstants {
 
         public static final Supplier<Trajectory> OBSERVATION_TEST3 = () ->
                 func
-                        .apply(PUSH_1)
+                        .apply(SUBMARINE)
+                        .lineToLinearHeading(MINI_LINE)
+                        .splineToConstantHeading(PUSH_HALF.vec(), Math.PI - PUSH_HALF.getHeading())
                         .splineToConstantHeading(PUSH_1.vec(), Math.PI - PUSH_1.getHeading())
                         .splineToConstantHeading(PUSH_2.vec(), Math.PI - PUSH_2.getHeading())
-                        .splineToConstantHeading(PUSH_3.vec(), PUSH_3.getHeading())
+                        .splineToConstantHeading(PUSH_3.vec(), Math.PI - PUSH_3.getHeading())
                         .splineToConstantHeading(PUSH_3_AND_A_HALF.vec(), PUSH_3_AND_A_HALF.getHeading())
                         .splineToConstantHeading(SAMPLE_1.vec(), Math.PI - SAMPLE_1.getHeading())
                         .splineToConstantHeading(OBSERVATION_PUSH_HALF.vec(), Math.PI - OBSERVATION_PUSH_HALF.getHeading())
@@ -132,7 +136,7 @@ private static TrajectorySequence getTestTrajectory(DriveShim drive) {
         return drive
             .trajectorySequenceBuilder(AutoConstants.OBSERVATION_START)
             .addTrajectory(AutoConstants.OBSERVATION_TEST1.get())
-            .addTrajectory(AutoConstants.OBSERVATION_TEST1QUARTER.get())
+            //.addTrajectory(AutoConstants.OBSERVATION_TEST1QUARTER.get())
             //.addTrajectory(AutoConstants.OBSERVATION_TEST2.get())
             .addTrajectory(AutoConstants.OBSERVATION_TEST3.get())
             .addTrajectory(AutoConstants.OBSERVATION_TEST4.get())