Kepler's functional spline

MeepMeepTesting/src/main/java/com/example/meepmeeptesting/KeplerTesting.java

@@ -24,23 +24,26 @@ public static class AutoConstants {
         public static Pose2d SAMPLES_START_PUSH = new Pose2d(-60, 20, toRadians(90));
         public static Pose2d SAMPLE_BAR_SCORE = new Pose2d(0, 34, toRadians(270));
         public static Pose2d START_POS = new Pose2d(-34, 58, toRadians(270));
-        public static Pose2d CORNER_OBS = new Pose2d(-30, 30, toRadians(180));
-        public static final Supplier<Trajectory> BASKET_HOLDERS = () ->
+        public static Pose2d CORNER_OBS = new Pose2d(10, 50, toRadians(270));
+        public static Pose2d CORNER_SECOND = new Pose2d(-35, 25.5, toRadians(260));
+        public static Pose2d CORNER_THIRD = new Pose2d(-50, 50, toRadians(180));
+        public static final Supplier<Trajectory> BASKET_HOLDERS_ONE = () ->
             func
                 .apply(START_POS)
                 .splineToSplineHeading(SAMPLE_BAR_SCORE, Math.PI - SAMPLE_BAR_SCORE.getHeading())
                 .splineToSplineHeading(CORNER_OBS, Math.PI - CORNER_OBS.getHeading())
-                .splineToSplineHeading(
-                    SAMPLES_START_PUSH,
-                    Math.PI - SAMPLES_START_PUSH.getHeading()
-                )
-                .splineToSplineHeading(SAMPLE_PUSH_AREA, Math.PI - SAMPLE_PUSH_AREA.getHeading())
-                .splineToSplineHeading(
-                    SAMPLE_COLLECT_AREA,
-                    Math.PI - SAMPLE_COLLECT_AREA.getHeading()
-                )
-                .splineToSplineHeading(SAMPLE_BAR_SCORE, Math.PI - SAMPLE_BAR_SCORE.getHeading())
+                    .splineToSplineHeading(CORNER_SECOND, Math.PI - CORNER_SECOND.getHeading())
+                    .splineToSplineHeading(SAMPLES_START_PUSH, Math.PI - SAMPLES_START_PUSH.getHeading())
+                    .splineToSplineHeading(SAMPLE_PUSH_AREA, Math.PI - SAMPLE_PUSH_AREA.getHeading())
+                    .splineToSplineHeading(CORNER_THIRD, Math.PI - CORNER_THIRD.getHeading())
                 .build();
+        public static final Supplier<Trajectory> BASKET_HOLDERS_TWO = () ->
+                func
+                        .apply(CORNER_THIRD)
+
+                        .splineToSplineHeading(SAMPLE_COLLECT_AREA, Math.PI - SAMPLE_COLLECT_AREA.getHeading())
+                        .splineToSplineHeading(SAMPLE_BAR_SCORE, Math.PI - SAMPLE_BAR_SCORE.getHeading())
+                        .build();
     }
 
     public static void main(String[] args) {
@@ -73,13 +76,15 @@ public static void main(String[] args) {
         RoadRunnerBotEntity myBot = new DefaultBotBuilder(meepMeep)
             .setDimensions(17.5, 16.5)
             .followTrajectorySequence(drive -> getTestTrajectory(drive));
+
         meepMeep.setBackgroundAlpha(0.75f).addEntity(myBot).start();
     }
 
     private static TrajectorySequence getTestTrajectory(DriveShim drive) {
         return drive
             .trajectorySequenceBuilder(AutoConstants.START_POS)
-            .addTrajectory(AutoConstants.BASKET_HOLDERS.get())
+                .addTrajectory(AutoConstants.BASKET_HOLDERS_ONE.get())
+                .addTrajectory(AutoConstants.BASKET_HOLDERS_TWO.get())
             .build();
     }
 }

Twenty403/src/main/java/org/firstinspires/ftc/twenty403/AutoConstants.java

@@ -1,14 +1,18 @@
 package org.firstinspires.ftc.twenty403;
 
+import static java.lang.Math.toRadians;
+
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
 import com.technototes.library.command.Command;
 import com.technototes.path.command.TrajectorySequenceCommand;
 import com.technototes.path.geometry.ConfigurablePoseD;
 import com.technototes.path.trajectorysequence.TrajectorySequence;
 import com.technototes.path.trajectorysequence.TrajectorySequenceBuilder;
 
 import java.util.function.Function;
+import java.util.function.Supplier;
 
 @Config
 public class AutoConstants {
@@ -31,6 +35,15 @@ public class AutoConstants {
         public static ConfigurablePoseD MID_SPLINE_CLEAR = new ConfigurablePoseD(35,34,-180);
         public static ConfigurablePoseD START_STAGE = new ConfigurablePoseD(35,58,0);
         public static ConfigurablePoseD HEAD_TO_STAGE = new ConfigurablePoseD(0,58,0);
+        //Kepler stuff
+        public static ConfigurablePoseD SAMPLE_PUSH_AREA = new ConfigurablePoseD(-60, 54, 90);
+        public static ConfigurablePoseD SAMPLE_COLLECT_AREA = new ConfigurablePoseD(-40, 57,120);
+        public static ConfigurablePoseD SAMPLES_START_PUSH = new ConfigurablePoseD(-60, 20, 90);
+        public static ConfigurablePoseD SAMPLE_BAR_SCORE = new ConfigurablePoseD(0, 34, 270);
+        public static ConfigurablePoseD START_POS = new ConfigurablePoseD(-34, 58, 270);
+        public static ConfigurablePoseD CORNER_OBS = new ConfigurablePoseD(10, 50, 270);
+        public static ConfigurablePoseD CORNER_SECOND = new ConfigurablePoseD(-35, 25.5, 260);
+        public static ConfigurablePoseD CORNER_THIRD = new ConfigurablePoseD(-50, 50, 180);
         // These are 'trajectory pieces' which should be named like this:
         // {STARTING_POSITION}_TO_{ENDING_POSITION}
         // testing trajectories
@@ -65,4 +78,15 @@ public class AutoConstants {
                 b.apply(SPLINETEST1.toPose())
                         .splineToConstantHeading(SPLINETEST2.toPose().vec(),SPLINETEST2.getHeading())
                         .build();
+        //kepler stuff paths
+        public static final Function<Function<Pose2d, TrajectorySequenceBuilder>, TrajectorySequence>
+                OBS_START_TO_SCORING = b ->
+                b.apply(START_POS.toPose())
+                        .splineToSplineHeading(SAMPLE_BAR_SCORE.toPose(), Math.PI - SAMPLE_BAR_SCORE.getHeading())
+                        .splineToSplineHeading(CORNER_OBS.toPose(), Math.PI - CORNER_OBS.getHeading())
+                        .splineToSplineHeading(CORNER_SECOND.toPose(), Math.PI - CORNER_SECOND.getHeading())
+                        .splineToSplineHeading(SAMPLES_START_PUSH.toPose(), Math.PI - SAMPLES_START_PUSH.getHeading())
+                        .splineToSplineHeading(SAMPLE_PUSH_AREA.toPose(), Math.PI - SAMPLE_PUSH_AREA.getHeading())
+                        .splineToSplineHeading(CORNER_THIRD.toPose(), Math.PI - CORNER_THIRD.getHeading())
+                        .build();
     }

Twenty403/src/main/java/org/firstinspires/ftc/twenty403/commands/auto/ForwardBackwardCommand.java

@@ -11,10 +11,8 @@ public ForwardBackwardCommand(Robot r) {
         super(
             new TrajectorySequenceCommand(
                 r.drivebaseSubsystem,
-                AutoConstants.BACKWARD_TO_FORWARD
-            ).andThen(
-                new TrajectorySequenceCommand(r.drivebaseSubsystem, AutoConstants.FORWARD_TO_BACKWARD)
-            )
+                AutoConstants.OBS_START_TO_SCORING )
+
         );
         //        super(
         //                new TrajectorySequenceCommand(r.drivebaseSubsystem, WingRed.SIDE_LEFT_TO_SIDE_RIGHT)

Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmodes/auto/ForwardBackward.java

@@ -18,7 +18,7 @@
 import org.firstinspires.ftc.twenty403.controls.SafetyTestController;
 import org.firstinspires.ftc.twenty403.helpers.StartingPosition;
 
-@Autonomous(name = "Forward_Backward")
+@Autonomous(name = "Kepler1")
 @SuppressWarnings("unused")
 public class ForwardBackward extends CommandOpMode {