16750 Tested Drivebase Constants, and untested Auto Paths - Maggie me…

…rge to main :) (#76)

Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/AutoConstants.java

@@ -22,7 +22,26 @@ public class AutoConstants {
     public static ConfigurablePoseD OBS_START = new ConfigurablePoseD(22, -65, 90);
     public static ConfigurablePoseD OBS_PARK = new ConfigurablePoseD(62, -6, 90);
     public static ConfigurablePoseD OBS_PUSH1 = new ConfigurablePoseD(62, -6, 90);
+    public static ConfigurablePoseD OBSERVATION_START = new ConfigurablePoseD(0, 60, -90);
+    public static ConfigurablePoseD SUBMARINE = new ConfigurablePoseD(-5, 36, (90));
+    public static ConfigurablePoseD OBSERVATION_ZONE = new ConfigurablePoseD(-46, 55, (90));
+    public static ConfigurablePoseD SUBMARINE2 = new ConfigurablePoseD(0, 36, (90));
+    public static ConfigurablePoseD SUBMARINE3 = new ConfigurablePoseD(5, 36, (-90));
+    public static ConfigurablePoseD MINI_LINE = new ConfigurablePoseD(-3, 36, (90));
 
+    public static ConfigurablePoseD PUSH_HALF = new ConfigurablePoseD(-16, 36, (90));
+
+    public static ConfigurablePoseD PUSH_1 = new ConfigurablePoseD(-34, 45, (90));
+    public static ConfigurablePoseD PUSH_2 = new ConfigurablePoseD(-34, 10, (90));
+    public static ConfigurablePoseD PUSH_3_AND_A_HALF = new ConfigurablePoseD(-46, 11, (90));
+    public static ConfigurablePoseD PUSH_3 = new ConfigurablePoseD(-35, 10, (90));
+    public static ConfigurablePoseD PUSH_4 = new ConfigurablePoseD(-56, 10, (90));
+    public static ConfigurablePoseD OBSERVATION_ZONE_2 = new ConfigurablePoseD(-56, 55, (90));
+    public static ConfigurablePoseD PUSH_5 = new ConfigurablePoseD(-61, 10, (90));
+    public static ConfigurablePoseD OBSERVATION_ZONE_3 = new ConfigurablePoseD(-61, 55, (90));
+
+    public static ConfigurablePoseD SAMPLE_1 = new ConfigurablePoseD(-48, 26, (90));
+    public static ConfigurablePoseD OBSERVATION_PUSH_HALF = new ConfigurablePoseD(-47, 40, (90));
 
 
     public static ConfigurablePoseD NETSCORING = new ConfigurablePoseD(55, 55, 45);
@@ -32,7 +51,7 @@ public class AutoConstants {
     public static ConfigurablePoseD INTAKE2 = new ConfigurablePoseD(60, 35, 90);
     public static ConfigurablePoseD INTAKE3 = new ConfigurablePoseD(64, 37, 90);
     public static ConfigurablePoseD ASCENT = new ConfigurablePoseD(23, 12, -0);
-
+    public static ConfigurablePoseD ASCENT_CLEAR = new ConfigurablePoseD(59, 10, 90);
 
     //These are testing constants for last year's game
     public static ConfigurablePoseD START1 = new ConfigurablePoseD(35, 60, -90);
@@ -50,6 +69,63 @@ public class AutoConstants {
 
     // These are 'trajectory pieces' which should be named like this:
     // {STARTING_POSITION}_TO_{ENDING_POSITION}
+    public static final Function<Function<Pose2d, TrajectorySequenceBuilder>, TrajectorySequence> PUSH_BOT_OBSERVATION_SIDE_AUTO1 = func ->
+            func
+                    .apply(OBSERVATION_START.toPose())
+                    .lineToLinearHeading(PUSH_1.toPose())
+                    .build();
+    public static final Function<Function<Pose2d, TrajectorySequenceBuilder>, TrajectorySequence> PUSH_BOT_OBSERVATION_SIDE_AUTO2 = func ->
+            func
+                    .apply(PUSH_1.toPose())
+                    .lineToLinearHeading(PUSH_2.toPose())
+                    .build();
+    public static final Function<Function<Pose2d, TrajectorySequenceBuilder>, TrajectorySequence> PUSH_BOT_OBSERVATION_SIDE_AUTO4 = func ->
+            func
+                    .apply(PUSH_2.toPose())
+                    .lineToLinearHeading(PUSH_3_AND_A_HALF.toPose())
+                    .build();
+
+    public static final Function<Function<Pose2d, TrajectorySequenceBuilder>, TrajectorySequence> PUSH_BOT_OBSERVATION_SIDE_AUTO4HALF = func ->
+            func
+                    .apply(PUSH_3_AND_A_HALF.toPose())
+                    .lineToLinearHeading(OBSERVATION_ZONE.toPose())
+                    .build();
+
+
+    public static final Function<Function<Pose2d, TrajectorySequenceBuilder>, TrajectorySequence> PUSH_BOT_OBSERVATION_SIDE_AUTO5 = func ->
+            func
+                    .apply(OBSERVATION_ZONE.toPose())
+                    .lineToLinearHeading(PUSH_3_AND_A_HALF.toPose())
+                    .build();
+    public static final Function<Function<Pose2d, TrajectorySequenceBuilder>, TrajectorySequence> PUSH_BOT_OBSERVATION_SIDE_AUTO6 = func ->
+            func
+                    .apply(PUSH_3_AND_A_HALF.toPose())
+                    .lineToLinearHeading(PUSH_4.toPose())
+                    .build();
+
+    public static final Function<Function<Pose2d, TrajectorySequenceBuilder>, TrajectorySequence> PUSH_BOT_OBSERVATION_SIDE_AUTO7 = func ->
+            func
+                    .apply(PUSH_4.toPose())
+                    .lineToLinearHeading(OBSERVATION_ZONE_2.toPose())
+                    .build();
+
+    public static final Function<Function<Pose2d, TrajectorySequenceBuilder>, TrajectorySequence> PUSH_BOT_OBSERVATION_SIDE_AUTO8 = func ->
+            func
+                    .apply(OBSERVATION_ZONE_2.toPose())
+                    .lineToLinearHeading(PUSH_4.toPose())
+                    .build();
+    public static final Function<Function<Pose2d, TrajectorySequenceBuilder>, TrajectorySequence> PUSH_BOT_OBSERVATION_SIDE_AUTO9 = func ->
+            func
+                    .apply(PUSH_4.toPose())
+                    .lineToLinearHeading(PUSH_5.toPose())
+                    .build();
+
+    public static final Function<Function<Pose2d, TrajectorySequenceBuilder>, TrajectorySequence> PUSH_BOT_OBSERVATION_SIDE_AUTO10 = func ->
+            func
+                    .apply(PUSH_5.toPose())
+                    .lineToLinearHeading(OBSERVATION_ZONE_3.toPose())
+                    .build();
+
     public static final Function<Function<Pose2d, TrajectorySequenceBuilder>, TrajectorySequence>
             OBS_START_TO_OBS_PARK = b ->
             b.apply(OBS_START.toPose()).lineToLinearHeading(OBS_PARK.toPose()).build();
@@ -74,6 +150,15 @@ public class AutoConstants {
     public static final Function<Function<Pose2d, TrajectorySequenceBuilder>, TrajectorySequence>
             INTAKE3_TO_NETSCORING = b ->
             b.apply(INTAKE3.toPose()).lineToLinearHeading(NETSCORING.toPose()).build();
+    public static final Function<Function<Pose2d, TrajectorySequenceBuilder>, TrajectorySequence>
+            NETSCORING_TO_ASCENT_CLEAR = b ->
+            b.apply(NETSCORING.toPose()).lineToLinearHeading(ASCENT_CLEAR.toPose()).build();
+    public static final Function<Function<Pose2d, TrajectorySequenceBuilder>, TrajectorySequence>
+            ASCENT_CLEAR_TO_ASCENT = b ->
+            b.apply(ASCENT_CLEAR.toPose()).lineToLinearHeading(ASCENT.toPose()).build();
+    public static final Function<Function<Pose2d, TrajectorySequenceBuilder>, TrajectorySequence>
+            NET_START_TO_ASCENT_CLEAR = b ->
+            b.apply(NET_START.toPose()).lineToLinearHeading(ASCENT_CLEAR.toPose()).build();
     public static final Function<Function<Pose2d, TrajectorySequenceBuilder>, TrajectorySequence>
             NETSCORING_TO_ASCENT = b ->
             b.apply(NETSCORING_TEST.toPose())

Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/auto/Paths.java

@@ -1,9 +1,15 @@
 package org.firstinspires.ftc.sixteen750.commands.auto;
 
 import com.technototes.library.command.Command;
+import com.technototes.library.command.ParallelCommandGroup;
+import com.technototes.library.command.SequentialCommandGroup;
+import com.technototes.library.command.WaitCommand;
 import com.technototes.path.command.TrajectorySequenceCommand;
 import org.firstinspires.ftc.sixteen750.AutoConstants;
 import org.firstinspires.ftc.sixteen750.Robot;
+import org.firstinspires.ftc.sixteen750.commands.slides.HorizontalSlidesCommands;
+import org.firstinspires.ftc.sixteen750.commands.slides.VerticalSlidesCommands;
+import org.firstinspires.ftc.sixteen750.commands.slides.VerticalSlidesSequentials;
 
 public class Paths {
 
@@ -13,27 +19,150 @@ public static Command splineTestCommand(Robot r) {
 
     public static Command SampleScoring(Robot r) {
         return new TrajectorySequenceCommand(r.drivebase, AutoConstants.START_TO_NETSCORING)
+            //First Intake and Scoring
             .andThen(
-                new TrajectorySequenceCommand(r.drivebase, AutoConstants.NETSCORING_TO_INTAKE1)
+                new ParallelCommandGroup(
+                    new TrajectorySequenceCommand(r.drivebase, AutoConstants.NETSCORING_TO_INTAKE1),
+                    HorizontalSlidesCommands.intake(r)
+                )
             )
+            .andThen(HorizontalSlidesCommands.clawChomp(r))
             .andThen(
-                new TrajectorySequenceCommand(r.drivebase, AutoConstants.INTAKE1_TO_NETSCORING)
+                new ParallelCommandGroup(
+                    new TrajectorySequenceCommand(r.drivebase, AutoConstants.INTAKE1_TO_NETSCORING),
+                    HorizontalSlidesCommands.transferring(r),
+                    VerticalSlidesCommands.BucketTransfer(r),
+                    VerticalSlidesCommands.ArmTransfer(r)
+                )
             )
+            .andThen(HorizontalSlidesCommands.clawOpen(r))
+            .andThen(VerticalSlidesSequentials.HighBasket(r))
+            .andThen(new WaitCommand(2))
+            .andThen(VerticalSlidesSequentials.transferVertical(r))
+            //Second Intake and Scoring
             .andThen(
-                new TrajectorySequenceCommand(r.drivebase, AutoConstants.NETSCORING_TO_INTAKE2)
+                new ParallelCommandGroup(
+                    new TrajectorySequenceCommand(r.drivebase, AutoConstants.NETSCORING_TO_INTAKE2),
+                    HorizontalSlidesCommands.intake(r)
+                )
             )
+            .andThen(HorizontalSlidesCommands.clawChomp(r))
             .andThen(
-                new TrajectorySequenceCommand(r.drivebase, AutoConstants.INTAKE2_TO_NETSCORING)
+                new ParallelCommandGroup(
+                    new TrajectorySequenceCommand(r.drivebase, AutoConstants.INTAKE2_TO_NETSCORING),
+                    HorizontalSlidesCommands.transferring(r),
+                    VerticalSlidesCommands.BucketTransfer(r),
+                    VerticalSlidesCommands.ArmTransfer(r)
+                )
             )
+            .andThen(HorizontalSlidesCommands.clawOpen(r))
+            .andThen(VerticalSlidesSequentials.HighBasket(r))
+            .andThen(new WaitCommand(2))
+            .andThen(VerticalSlidesSequentials.transferVertical(r))
+            //Third Intake and Scoring
             .andThen(
-                new TrajectorySequenceCommand(r.drivebase, AutoConstants.NETSCORING_TO_INTAKE3)
+                new ParallelCommandGroup(
+                    new TrajectorySequenceCommand(r.drivebase, AutoConstants.NETSCORING_TO_INTAKE3),
+                    HorizontalSlidesCommands.intake(r)
+                )
             )
+            .andThen(HorizontalSlidesCommands.clawChomp(r))
             .andThen(
-                new TrajectorySequenceCommand(r.drivebase, AutoConstants.INTAKE3_TO_NETSCORING)
+                new ParallelCommandGroup(
+                    new TrajectorySequenceCommand(r.drivebase, AutoConstants.INTAKE3_TO_NETSCORING),
+                    HorizontalSlidesCommands.transferring(r),
+                    VerticalSlidesCommands.BucketTransfer(r),
+                    VerticalSlidesCommands.ArmTransfer(r)
+                )
+            )
+            .andThen(HorizontalSlidesCommands.clawOpen(r))
+            .andThen(VerticalSlidesSequentials.HighBasket(r))
+            .andThen(new WaitCommand(2))
+            .andThen(VerticalSlidesSequentials.transferVertical(r))
+            .andThen(
+                new TrajectorySequenceCommand(r.drivebase, AutoConstants.NETSCORING_TO_ASCENT_CLEAR)
+            )
+            .andThen(
+                new ParallelCommandGroup(
+                    new TrajectorySequenceCommand(
+                        r.drivebase,
+                        AutoConstants.ASCENT_CLEAR_TO_ASCENT
+                    ),
+                    VerticalSlidesCommands.ArmScore(r),
+                    VerticalSlidesCommands.BucketEmpty(r)
+                )
             );
     }
 
-    public static TrajectorySequenceCommand Obs_Parking(Robot r) {
+    public static Command Obs_Parking(Robot r) {
         return new TrajectorySequenceCommand(r.drivebase, AutoConstants.OBS_START_TO_OBS_PARK);
     }
+
+    public static Command Net_Parking(Robot r) {
+        return new TrajectorySequenceCommand(
+            r.drivebase,
+            AutoConstants.NET_START_TO_ASCENT_CLEAR
+        ).andThen(new TrajectorySequenceCommand(r.drivebase, AutoConstants.ASCENT_CLEAR_TO_ASCENT));
+    }
+
+    public static Command ObservationScoring(Robot r) {
+        return new TrajectorySequenceCommand(
+            r.drivebase,
+            AutoConstants.PUSH_BOT_OBSERVATION_SIDE_AUTO1
+        )
+            .andThen(
+                new TrajectorySequenceCommand(
+                    r.drivebase,
+                    AutoConstants.PUSH_BOT_OBSERVATION_SIDE_AUTO2
+                )
+            )
+            .andThen(
+                new TrajectorySequenceCommand(
+                    r.drivebase,
+                    AutoConstants.PUSH_BOT_OBSERVATION_SIDE_AUTO4
+                )
+            )
+            .andThen(
+                new TrajectorySequenceCommand(
+                    r.drivebase,
+                    AutoConstants.PUSH_BOT_OBSERVATION_SIDE_AUTO4HALF
+                )
+            )
+            .andThen(
+                new TrajectorySequenceCommand(
+                    r.drivebase,
+                    AutoConstants.PUSH_BOT_OBSERVATION_SIDE_AUTO5
+                )
+            )
+            .andThen(
+                new TrajectorySequenceCommand(
+                    r.drivebase,
+                    AutoConstants.PUSH_BOT_OBSERVATION_SIDE_AUTO6
+                )
+            )
+            .andThen(
+                new TrajectorySequenceCommand(
+                    r.drivebase,
+                    AutoConstants.PUSH_BOT_OBSERVATION_SIDE_AUTO7
+                )
+            )
+            .andThen(
+                new TrajectorySequenceCommand(
+                    r.drivebase,
+                    AutoConstants.PUSH_BOT_OBSERVATION_SIDE_AUTO8
+                )
+            )
+            .andThen(
+                new TrajectorySequenceCommand(
+                    r.drivebase,
+                    AutoConstants.PUSH_BOT_OBSERVATION_SIDE_AUTO9
+                )
+            )
+            .andThen(
+                new TrajectorySequenceCommand(
+                    r.drivebase,
+                    AutoConstants.PUSH_BOT_OBSERVATION_SIDE_AUTO10
+                )
+            );
+    }
 }

Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/slides/HorizontalSlidesCommands.java

@@ -40,11 +40,9 @@ public static Command wristTransfer(Robot r) {
 
     public static SequentialCommandGroup transferring(Robot r) {
         return new SequentialCommandGroup(
-            Command.create(
-                r.horizontalSlidesSubsystem::ClawWristServoTransfer,
-                r.horizontalSlidesSubsystem
-            ),
-            horizontalRetract(r)
+            wristTransfer(r),
+            horizontalRetract(r),
+            VerticalSlidesSequentials.transferVertical(r)
             // commands for vertical slide bucket transfer position first, then wrist transferring
         );
     }

Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/commands/slides/VerticalSlidesCommands.java

@@ -1,39 +1,64 @@
 package org.firstinspires.ftc.sixteen750.commands.slides;
 
 import com.technototes.library.command.Command;
-
+import com.technototes.library.command.ParallelCommandGroup;
 import org.firstinspires.ftc.sixteen750.Robot;
 
 public class VerticalSlidesCommands {
 
     public static Command HighBasket(Robot r) {
-        return Command.create(r.verticalSlidesSubsystem::slideBasketHigh, r.verticalSlidesSubsystem);
+        return Command.create(
+            r.verticalSlidesSubsystem::slideBasketHigh,
+            r.verticalSlidesSubsystem
+        );
     }
+
     public static Command LowBasket(Robot r) {
         return Command.create(r.verticalSlidesSubsystem::slideBasketLow, r.verticalSlidesSubsystem);
     }
+
     public static Command HighChamber(Robot r) {
-        return Command.create(r.verticalSlidesSubsystem::slideChamberHigh, r.verticalSlidesSubsystem);
+        return Command.create(
+            r.verticalSlidesSubsystem::slideChamberHigh,
+            r.verticalSlidesSubsystem
+        );
     }
+
     public static Command LowChamber(Robot r) {
-        return Command.create(r.verticalSlidesSubsystem::slideChamberLow, r.verticalSlidesSubsystem);
+        return Command.create(
+            r.verticalSlidesSubsystem::slideChamberLow,
+            r.verticalSlidesSubsystem
+        );
     }
+
     public static Command BucketTransfer(Robot r) {
-        return Command.create(r.verticalSlidesSubsystem::bucketServoTransfer, r.verticalSlidesSubsystem);
+        return Command.create(
+            r.verticalSlidesSubsystem::bucketServoTransfer,
+            r.verticalSlidesSubsystem
+        );
     }
+
     public static Command BucketEmpty(Robot r) {
-        return Command.create(r.verticalSlidesSubsystem::bucketServoEmpty, r.verticalSlidesSubsystem);
+        return Command.create(
+            r.verticalSlidesSubsystem::bucketServoEmpty,
+            r.verticalSlidesSubsystem
+        );
     }
+
     public static Command ArmTransfer(Robot r) {
-        return Command.create(r.verticalSlidesSubsystem::armServoTransfer, r.verticalSlidesSubsystem);
+        return Command.create(
+            r.verticalSlidesSubsystem::armServoTransfer,
+            r.verticalSlidesSubsystem
+        );
     }
+
     public static Command ArmScore(Robot r) {
         return Command.create(r.verticalSlidesSubsystem::armServoEmpty, r.verticalSlidesSubsystem);
     }
+
     public static Command SlidesZero(Robot r) {
         return Command.create(r.verticalSlidesSubsystem::slidesDown, r.verticalSlidesSubsystem);
     }
-
     //transfer
     //inc/dec
 

Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/ForwardBackward.java

@@ -38,4 +38,9 @@ public void uponInit() {
             OpModeState.RUN
         );
     }
+
+    public void uponStart() {
+        robot.horizontalSlidesSubsystem.slidesin();
+        robot.horizontalSlidesSubsystem.ClawWristServoTransfer();
+    }
 }

Sixteen750/src/main/java/org/firstinspires/ftc/sixteen750/opmodes/auto/NetScoring.java

@@ -36,4 +36,9 @@ public void uponInit() {
             OpModeState.RUN
         );
     }
+
+    public void uponStart() {
+        robot.horizontalSlidesSubsystem.slidesin();
+        robot.horizontalSlidesSubsystem.ClawWristServoTransfer();
+    }
 }