READY CODE LM3

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/roadrunner/drive/MecanumDrive.java

@@ -76,7 +76,7 @@ public class    MecanumDrive extends com.acmerobotics.roadrunner.drive.MecanumDr
 
     private TrajectoryFollower follower;
 
-    private DcMotorEx leftFront, leftRear, rightRear, rightFront;
+    public DcMotorEx leftFront, leftRear, rightRear, rightFront;
     private List<DcMotorEx> motors;
 
     private IMU imu;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlue.java

@@ -1,111 +1,96 @@
 package org.firstinspires.ftc.teamcode.opmodes;
 
 import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.trajectory.Trajectory;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.teamcode.hardware.Robot;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
 
+@Config
 @Autonomous(name = "autoBlue")
 public class AutoBlue extends LinearOpMode {
     protected Pose2d initialPosition;
-
-    protected Trajectory preloadOne;
-    protected Trajectory scoreOne;
-    protected Trajectory boardOne;
-    protected Trajectory backOffOne;
-
-
-    protected Trajectory preloadTwo;
-    protected Trajectory scoreTwo;
-    protected Trajectory backOffTwo;
-
-
-    protected Trajectory preloadThree;
-    protected Trajectory scoreThree;
-    protected Trajectory backOffThree;
-
-
-    protected Trajectory park1;
-    protected Trajectory park2;
-
-
     private Robot robot;
     private String randomization;
-    private int random;
+
+    //Pose2ds
+    //Preloads
+    final static Pose2d RIGHT_PRELOAD_ONE = new Pose2d(-40, -37.5, Math.toRadians(270));
+    final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-29.5, -29, Math.toRadians(180));
+    final static Pose2d CENTER_PRELOAD = new Pose2d(-33, -28, Math.toRadians(270));
+    final static Pose2d LEFT_PRELOAD = new Pose2d(-43, -35, Math.toRadians(270));
+    //Board Scores
+    final static Pose2d RIGHT_BOARD = new Pose2d(-75.3, -24.5, Math.toRadians(185));
+    final static Pose2d CENTER_BOARD = new Pose2d(-75.3, -35, Math.toRadians(185));
+    final static Pose2d LEFT_BOARD = new Pose2d(-75.3, -42, Math.toRadians(185));
+    //Park
+    final static Pose2d BACK_OFF =  new Pose2d(-60,-58,Math.toRadians(180));
+    final static Pose2d PARK = new Pose2d(-80, -60, Math.toRadians(180));
+
+    protected void scorePreloadOne() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        switch (randomization) {
+            case "RIGHT":
+                builder.lineToLinearHeading(RIGHT_PRELOAD_ONE);
+                builder.lineToLinearHeading(RIGHT_PRELOAD_TWO);
+                break;
+            case "CENTER":
+                builder.lineToLinearHeading(CENTER_PRELOAD);
+                break;
+            case "LEFT":
+                builder.lineToLinearHeading(LEFT_PRELOAD);
+                break;
+        }
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+    protected void boardScore() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        switch (randomization) {
+            case "RIGHT":
+                builder.lineToLinearHeading(RIGHT_BOARD);
+                break;
+            case "CENTER":
+                builder.lineToLinearHeading(CENTER_BOARD);
+                break;
+            case "LEFT":
+                builder.lineToLinearHeading(LEFT_BOARD);
+                break;
+        }
+        builder.addTemporalMarker(.2, robot.getArm()::armScore);
+        builder.addTemporalMarker(.2, robot.getSlides()::slideFirstLayer);
+        builder.addTemporalMarker(.2, robot.getWrist()::wristScore);
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+    protected void park() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToLinearHeading(BACK_OFF);
+        builder.lineToLinearHeading(PARK);
+        builder.addTemporalMarker(.1, robot.getArm()::armRest);
+        builder.addTemporalMarker(.1, robot.getClaw()::close);
+        builder.addTemporalMarker(.1, robot.getWrist()::wristPickup);
+        builder.addTemporalMarker(.1, robot.getSlides()::slideDown);
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
 
     @Override
     public void runOpMode() throws InterruptedException {
+
         this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
         this.robot = new Robot().init(hardwareMap);
         this.robot.getCamera().initTargetingCamera();
-
-        //Trajectories
         this.initialPosition = new Pose2d(-34, -59.5, Math.toRadians(270));
         this.robot.getDrive().setPoseEstimate(initialPosition);
 
-        //Randomization One
+//        this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end())
-        this.preloadOne = this.robot.getDrive().trajectoryBuilder(initialPosition)
+//                .lineToLinearHeading(new Pose2d(80, -57, Math.toRadians(360)))
-                .lineToLinearHeading(new Pose2d(-40, -37.5, Math.toRadians(270)))
+//                .build();
-                .build();
-
-        this.scoreOne = this.robot.getDrive().trajectoryBuilder(preloadOne.end())
-                .lineToLinearHeading(new Pose2d(-29, -32, Math.toRadians(180)))
-                .build();
-
-        this.boardOne = this.robot.getDrive().trajectoryBuilder(scoreOne.end())
-                .lineToLinearHeading(new Pose2d(-72, -26.3, Math.toRadians(180)))
-                .addTemporalMarker(.2, robot.getArm()::armSecondaryScore)
-                .addTemporalMarker(.2, robot.getWrist()::wristScore)
-                .build();
-
-        this.backOffOne = this.robot.getDrive().trajectoryBuilder(boardOne.end())
-                .lineToLinearHeading(new Pose2d(-60, -26, Math.toRadians(180)))
-                .build();
-
-
-        //Randomization Two
-        this.preloadTwo = this.robot.getDrive().trajectoryBuilder(initialPosition)
-                .lineToLinearHeading(new Pose2d(-35, -28, Math.toRadians(270)))
-                .build();
-
-        this.scoreTwo = this.robot.getDrive().trajectoryBuilder(preloadTwo.end())
-                .lineToLinearHeading(new Pose2d(-70, -34, Math.toRadians(180)))
-                .addTemporalMarker(.2, robot.getArm()::armSecondaryScore)
-                .addTemporalMarker(.2, robot.getWrist()::wristScore)
-                .build();
-
-        this.backOffTwo = this.robot.getDrive().trajectoryBuilder(scoreTwo.end())
-                .lineToLinearHeading(new Pose2d(-60, -34, Math.toRadians(180)))
-                .build();
-
-        //Randomization Three
-        this.preloadThree = this.robot.getDrive().trajectoryBuilder(initialPosition)
-                .lineToLinearHeading(new Pose2d(-42, -35, Math.toRadians(270)))
-                .build();
-
-        this.scoreThree = this.robot.getDrive().trajectoryBuilder(preloadThree.end())
-                .lineToLinearHeading(new Pose2d(-71.5, -41.3, Math.toRadians(180)))
-                .addTemporalMarker(.2, robot.getArm()::armSecondaryScore)
-                .addTemporalMarker(.2, robot.getWrist()::wristScore)
-                .build();
-
-        this.backOffThree = this.robot.getDrive().trajectoryBuilder(scoreThree.end())
-                .lineToLinearHeading(new Pose2d(-60, -40, Math.toRadians(180)))
-                .build();
-
-        //Park
-        this.park1 = this.robot.getDrive().trajectoryBuilder(backOffTwo.end())
-                .lineToLinearHeading(new Pose2d(-65, -55, Math.toRadians(180)))
-                .addTemporalMarker(.3, robot.getArm()::armRest)
-                .addTemporalMarker(.3, robot.getWrist()::wristPickup)
-                .build();
-        this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end())
-                .lineToLinearHeading(new Pose2d(-80, -60, Math.toRadians(180)))
-                .build();
 
         // Do super fancy chinese shit
         while (!this.isStarted()) {
@@ -114,41 +99,12 @@ public class AutoBlue extends LinearOpMode {
             this.telemetry.update();
         }
 
-        switch (randomization) {
+        scorePreloadOne();
-            case "RIGHT":
+        boardScore();
-                this.robot.getDrive().followTrajectory(preloadOne);
+        sleep(250);
-                this.robot.getDrive().followTrajectory(scoreOne);
+        this.robot.getClaw().open();
-                this.robot.getDrive().followTrajectory(boardOne);
+        sleep(250);
-                this.robot.getClaw().open();
+        park();
-                sleep(500);
-                this.robot.getDrive().followTrajectory(backOffOne);
-                sleep(500);
-                this.robot.getDrive().followTrajectory(park1);
-                this.robot.getDrive().followTrajectory(park2);
-                break;
-            case "CENTER":
-                this.robot.getDrive().followTrajectory(preloadTwo);
-                this.robot.getDrive().followTrajectory(scoreTwo);
-                this.robot.getClaw().open();
-                sleep(500);
-                this.robot.getDrive().followTrajectory(backOffTwo);
-                sleep(500);
-                this.robot.getDrive().followTrajectory(park1);
-                this.robot.getDrive().followTrajectory(park2);
-                break;
-            case "LEFT":
-                this.robot.getDrive().followTrajectory(preloadThree);
-                this.robot.getDrive().followTrajectory(scoreThree);
-                this.robot.getClaw().open();
-                sleep(500);
-                this.robot.getDrive().followTrajectory(backOffThree);
-                sleep(500);
-                this.robot.getDrive().followTrajectory(park1);
-                this.robot.getDrive().followTrajectory(park2);
-                break;
-        }
-
-
     }
 
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueFar.java

@@ -1,150 +1,60 @@
 package org.firstinspires.ftc.teamcode.opmodes;
 
 import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.trajectory.Trajectory;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.teamcode.hardware.Robot;
-
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
+@Config
 @Autonomous(name = "autoBlueFar")
 public class AutoBlueFar extends LinearOpMode {
     protected Pose2d initialPosition;
-
-    protected Trajectory preloadOne;
-    protected Trajectory scoreOne;
-    protected Trajectory boardOne;
-    protected Trajectory backOffOne;
-    protected Trajectory goGate1;
-    protected Trajectory passGate;
-
-    protected Trajectory preloadTwo;
-    protected Trajectory scoreTwo;
-    protected Trajectory backOffTwo;
-    protected Trajectory tokyoDrift;
-    protected Trajectory tokyoDrift2;
-    protected Trajectory tokyoDrift3;
-
-
-    protected Trajectory preloadThree;
-    protected Trajectory boardThree;
-    protected Trajectory scoreThree;
-    protected Trajectory backOffThree;
-    protected Trajectory goGate3;
-    protected Trajectory goGate3Again;
-
-
-    protected Trajectory park1;
-    protected Trajectory park2;
-
     private Robot robot;
     private String randomization;
-    private int random;
+
+    //Pose2ds
+    //Preloads
+    final static Pose2d LEFT_PRELOAD_ONE = new Pose2d(40, -37.5, Math.toRadians(270));
+    final static Pose2d LEFT_PRELOAD_TWO = new Pose2d(29.5, -29, Math.toRadians(360));
+    final static Pose2d CENTER_PRELOAD = new Pose2d(33, -28, Math.toRadians(270));
+    final static Pose2d RIGHT_PRELOAD = new Pose2d(43, -35, Math.toRadians(270));
+
+    protected void scorePreloadOne() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        switch (randomization) {
+            case "LEFT":
+                builder.lineToLinearHeading(LEFT_PRELOAD_ONE);
+                builder.lineToLinearHeading(LEFT_PRELOAD_TWO);
+                break;
+            case "CENTER":
+                builder.lineToLinearHeading(CENTER_PRELOAD);
+                break;
+            case "RIGHT":
+                builder.lineToLinearHeading(RIGHT_PRELOAD);
+                break;
+        }
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+    protected void goBackToWhereYouCameFrom() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToLinearHeading(initialPosition);
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
 
     @Override
     public void runOpMode() throws InterruptedException {
+
         this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
         this.robot = new Robot().init(hardwareMap);
         this.robot.getCamera().initTargetingCamera();
-
-        //Trajectories
         this.initialPosition = new Pose2d(34, -59.5, Math.toRadians(270));
         this.robot.getDrive().setPoseEstimate(initialPosition);
 
-        //Randomization One
-        this.preloadOne = this.robot.getDrive().trajectoryBuilder(initialPosition)
-                .lineToLinearHeading(new Pose2d(40, -37.5, Math.toRadians(270)))
-                .build();
-
-        this.scoreOne = this.robot.getDrive().trajectoryBuilder(preloadOne.end())
-                .lineToLinearHeading(new Pose2d(35, -20, Math.toRadians(150)))
-                .build();
-
-        this.goGate1 = this.robot.getDrive().trajectoryBuilder(scoreOne.end())
-                .lineToLinearHeading(new Pose2d(31, -10, Math.toRadians(180)))
-                .build();
-
-        this.passGate = this.robot.getDrive().trajectoryBuilder(goGate1.end())
-                .lineToLinearHeading(new Pose2d(-40, -12, Math.toRadians(180)))
-                .build();
-
-        this.boardOne = this.robot.getDrive().trajectoryBuilder(passGate.end())
-                .lineToLinearHeading(new Pose2d(-50, -28, Math.toRadians(180)))
-                .addTemporalMarker(.2, robot.getArm()::armSecondaryScore)
-                .addTemporalMarker(.2, robot.getWrist()::wristScore)
-                .build();
-
-        this.backOffOne = this.robot.getDrive().trajectoryBuilder(boardOne.end())
-                .lineToLinearHeading(new Pose2d(-40, -25, Math.toRadians(180)))
-                .build();
-
-        //Randomization Two
-        this.preloadTwo = this.robot.getDrive().trajectoryBuilder(initialPosition)
-                .lineToLinearHeading(new Pose2d(36, -28, Math.toRadians(290)))
-                .build();
-
-        this.tokyoDrift = this.robot.getDrive().trajectoryBuilder(preloadTwo.end())
-                .lineToLinearHeading(new Pose2d(50, -38, Math.toRadians(270)))
-                .build();
-
-        this.tokyoDrift2 = this.robot.getDrive().trajectoryBuilder(tokyoDrift.end())
-                .lineToLinearHeading(new Pose2d(50, -9, Math.toRadians(180)))
-                .build();
-
-        this.tokyoDrift3 = this.robot.getDrive().trajectoryBuilder(tokyoDrift2.end())
-                .lineToLinearHeading(new Pose2d(35, -9, Math.toRadians(180)))
-                .build();
-
-        this.scoreTwo = this.robot.getDrive().trajectoryBuilder(passGate.end())
-                .lineToLinearHeading(new Pose2d(-50, -33, Math.toRadians(180)))
-                .addTemporalMarker(.02, robot.getArm()::armSecondaryScore)
-                .addTemporalMarker(.02, robot.getWrist()::wristScore)
-                .build();
-
-        this.backOffTwo = this.robot.getDrive().trajectoryBuilder(scoreTwo.end())
-                .lineToLinearHeading(new Pose2d(-40, -33, Math.toRadians(180)))
-                .build();
-
-        //Randomization Three
-        this.preloadThree = this.robot.getDrive().trajectoryBuilder(initialPosition)
-                .lineToLinearHeading(new Pose2d(43, -37.5, Math.toRadians(270)))
-                .build();
-
-        this.scoreThree = this.robot.getDrive().trajectoryBuilder(preloadThree.end())
-                .lineToLinearHeading(new Pose2d(29, -32, Math.toRadians(335)))
-                .build();
-
-        this.goGate3 = this.robot.getDrive().trajectoryBuilder(scoreThree.end())
-                .lineToLinearHeading(new Pose2d(40, -32, Math.toRadians(335)))
-                .build();
-
-        this.goGate3Again = this.robot.getDrive().trajectoryBuilder(goGate3.end())
-                .lineToLinearHeading(new Pose2d(35, -10, Math.toRadians(180)))
-                .build();
-
-        this.boardThree = this.robot.getDrive().trajectoryBuilder(passGate.end())
-                .lineToLinearHeading(new Pose2d(-50.5, -39, Math.toRadians(180)))
-                .addTemporalMarker(.2, robot.getArm()::armSecondaryScore)
-                .addTemporalMarker(.2, robot.getWrist()::wristScore)
-                .build();
-
-        this.backOffThree = this.robot.getDrive().trajectoryBuilder(boardThree.end())
-                .lineToLinearHeading(new Pose2d(-40, -40, Math.toRadians(180)))
-                .build();
-
-
-        //Park
-        this.park1 = this.robot.getDrive().trajectoryBuilder(backOffOne.end())
-                .lineToLinearHeading(new Pose2d(-40, -10, Math.toRadians(180)))
-                .addTemporalMarker(.3, robot.getArm()::armRest)
-                .addTemporalMarker(.3, robot.getWrist()::wristPickup)
-                .build();
-        this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end())
-                .lineToLinearHeading(new Pose2d(-60, -10, Math.toRadians(170)))
-                .build();
-
         // Do super fancy chinese shit
         while (!this.isStarted()) {
             this.telemetry.addData("Starting Position", this.robot.getCamera().getStartingPositionBlue());
@@ -152,52 +62,8 @@ public class AutoBlueFar extends LinearOpMode {
             this.telemetry.update();
         }
 
-        sleep(5000);
+        scorePreloadOne();
-
+        goBackToWhereYouCameFrom();
-        switch (randomization) {
-            case "RIGHT":
-                this.robot.getDrive().followTrajectory(preloadOne);
-                this.robot.getDrive().followTrajectory(scoreOne);
-                this.robot.getDrive().followTrajectory(goGate1);
-                this.robot.getDrive().followTrajectory(passGate);
-                this.robot.getDrive().followTrajectory(boardOne);
-                sleep(500);
-                this.robot.getClaw().open();
-                sleep(500);
-                this.robot.getDrive().followTrajectory(backOffOne);
-                sleep(300);
-                break;
-            case "CENTER":
-                this.robot.getDrive().followTrajectory(preloadTwo);
-                this.robot.getDrive().followTrajectory(tokyoDrift);
-                this.robot.getDrive().followTrajectory(tokyoDrift2);
-                this.robot.getDrive().followTrajectory(passGate);
-                this.robot.getDrive().followTrajectory(scoreTwo);
-                sleep(500);
-                this.robot.getClaw().open();
-                sleep(500);
-                this.robot.getDrive().followTrajectory(backOffTwo);
-                sleep(300);
-                break;
-            case "LEFT":
-                this.robot.getDrive().followTrajectory(preloadThree);
-                this.robot.getDrive().followTrajectory(scoreThree);
-                this.robot.getDrive().followTrajectory(goGate3);
-                this.robot.getDrive().followTrajectory(goGate3Again);
-                this.robot.getDrive().followTrajectory(passGate);
-                this.robot.getDrive().followTrajectory(boardThree);
-                sleep(500);
-                this.robot.getClaw().open();
-                sleep(500);
-                this.robot.getDrive().followTrajectory(backOffThree);
-                sleep(300);
-                break;
-        }
-        //Cycle
-        this.robot.getDrive().followTrajectory(park1);
-        this.robot.getDrive().followTrajectory(park2);
-
-
     }
 
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoBlueTwoPlusTwo.java

@@ -8,10 +8,12 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.teamcode.hardware.Robot;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
 import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
 
 @Config
-@Autonomous(name = "autoBLue2+2")
+@Autonomous(name = "autoBlue2+2")
 public class AutoBlueTwoPlusTwo extends LinearOpMode {
     protected Pose2d initialPosition;
     private Robot robot;
@@ -20,19 +22,19 @@ public class AutoBlueTwoPlusTwo extends LinearOpMode {
     //Pose2ds
     //Preloads
     final static Pose2d RIGHT_PRELOAD_ONE = new Pose2d(-40, -37.5, Math.toRadians(270));
-    final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-29.5, -29, Math.toRadians(180));
+    final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-29.5, -31, Math.toRadians(180));
     final static Pose2d CENTER_PRELOAD = new Pose2d(-33, -28, Math.toRadians(270));
     final static Pose2d LEFT_PRELOAD = new Pose2d(-43, -35, Math.toRadians(270));
     //Board Scores
-    final static Pose2d RIGHT_BOARD = new Pose2d(-75.3, -24.5, Math.toRadians(185));
+    final static Pose2d RIGHT_BOARD = new Pose2d(-75.7, -25.7, Math.toRadians(185));
-    final static Pose2d CENTER_BOARD = new Pose2d(-75.3, -35, Math.toRadians(185));
+    final static Pose2d CENTER_BOARD = new Pose2d(-75.7, -35, Math.toRadians(185));
-    final static Pose2d LEFT_BOARD = new Pose2d(-75.3, -42, Math.toRadians(185));
+    final static Pose2d LEFT_BOARD = new Pose2d(-75.7, -42, Math.toRadians(185));
     //Stack Cycle
     final static Pose2d LEAVE_BOARD = new Pose2d(-65, -10, Math.toRadians(180));
     final static Pose2d TO_STACK = new Pose2d(40.75, -7.25, Math.toRadians(180));
     final static Pose2d BACK_THROUGH_GATE = new Pose2d(-50, -10, Math.toRadians(180));
     final static Pose2d APPROACHING_BOARD = new Pose2d(-70, -28, Math.toRadians(180));
-    final static Pose2d SCORE_STACK = new Pose2d(-74.5, -28, Math.toRadians(180));
+    final static Pose2d SCORE_STACK = new Pose2d(-73, -31, Math.toRadians(180));
     //Park
     final static Pose2d BACK_OFF =  new Pose2d(-60,-58,Math.toRadians(180));
     final static Pose2d PARK = new Pose2d(-80, -60, Math.toRadians(180));
@@ -76,10 +78,11 @@ public class AutoBlueTwoPlusTwo extends LinearOpMode {
     protected void toStack() {
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
         builder.lineToLinearHeading(LEAVE_BOARD);
-        builder.addTemporalMarker(.3, robot.getArm()::armPickupStack);
+        builder.addTemporalMarker(.3, robot.getArm()::armRest);
         builder.addTemporalMarker(.3, robot.getWrist()::wristPickup);
         builder.addTemporalMarker(.1, robot.getSlides()::slideDown);
         builder.addTemporalMarker(1.5,robot.getClaw()::openStack);
+        builder.addTemporalMarker(1.5, robot.getArm()::pickup);
         builder.lineToLinearHeading(TO_STACK);
         this.robot.getDrive().followTrajectorySequence(builder.build());
     }
@@ -95,6 +98,15 @@ public class AutoBlueTwoPlusTwo extends LinearOpMode {
         this.robot.getDrive().followTrajectorySequence(builder.build());
     }
 
+    protected void scoreTest() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToLinearHeading(new Pose2d(-77, -31, Math.toRadians(180)),
+                MecanumDrive.getVelocityConstraint(20, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+                MecanumDrive.getAccelerationConstraint(20));
+        builder.addTemporalMarker(.2,this::clawSlowOpen);
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
     protected void clawSlowOpen() {
         double currentPos = .86;
         double targetPos = .8;
@@ -138,25 +150,21 @@ public class AutoBlueTwoPlusTwo extends LinearOpMode {
             randomization = String.valueOf(this.robot.getCamera().getStartingPositionBlue());
             this.telemetry.update();
         }
-
         scorePreloadOne();
         boardScore();
 
-        sleep(250);
+        sleep(100);
         this.robot.getClaw().open();
-        sleep(250);
 
         toStack();
 
         sleep(500);
         this.robot.getClaw().close();
-        sleep(500);
+        sleep(250);
         this.robot.getArm().armRest();
         scoreStack();
         this.robot.getClaw().setPos(.86);
-        sleep(150);
+        scoreTest();
-        clawSlowOpen();
-        sleep(300);
         park();
     }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRed.java

@@ -1,121 +1,96 @@
 package org.firstinspires.ftc.teamcode.opmodes;
 
 import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.trajectory.Trajectory;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.teamcode.hardware.Robot;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
 
+@Config
 @Autonomous(name = "autoRed")
 public class AutoRed extends LinearOpMode {
     protected Pose2d initialPosition;
-
-    protected Trajectory preloadOne;
-    protected Trajectory scoreOne;
-    protected Trajectory boardOne;
-    protected Trajectory backOffOne;
-
-
-    protected Trajectory preloadTwo;
-    protected Trajectory scoreTwo;
-    protected Trajectory backOffTwo;
-
-
-    protected Trajectory preloadThree;
-    protected Trajectory scoreThree;
-    protected Trajectory boardThree;
-    protected Trajectory backOffThree;
-
-
-    protected Trajectory park1;
-    protected Trajectory park2;
-
-    protected Trajectory goGate;
-    protected Trajectory goStack;
-    protected Trajectory backGate;
-    protected Trajectory approachBoard;
-    protected Trajectory scoreStack;
-
-
     private Robot robot;
     private String randomization;
-    private int random;
+
+    //Pose2ds
+    //Preloads
+    final static Pose2d LEFT_PRELOAD_ONE = new Pose2d(40, -37.5, Math.toRadians(270));
+    final static Pose2d LEFT_PRELOAD_TWO = new Pose2d(29.5, -29, Math.toRadians(360));
+    final static Pose2d CENTER_PRELOAD = new Pose2d(33, -28, Math.toRadians(270));
+    final static Pose2d RIGHT_PRELOAD = new Pose2d(43, -35, Math.toRadians(270));
+    //Board Scores
+    final static Pose2d LEFT_BOARD = new Pose2d(74.3, -26.5, Math.toRadians(355));
+    final static Pose2d CENTER_BOARD = new Pose2d(74.7, -36.3, Math.toRadians(355));
+    final static Pose2d RIGHT_BOARD = new Pose2d(74.3, -40, Math.toRadians(355));
+
+    //Park
+    final static Pose2d BACK_OFF =  new Pose2d(60,-58,Math.toRadians(360));
+    final static Pose2d PARK = new Pose2d(80, -60, Math.toRadians(360));
+
+    protected void scorePreloadOne() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        switch (randomization) {
+            case "LEFT":
+                builder.lineToLinearHeading(LEFT_PRELOAD_ONE);
+                builder.lineToLinearHeading(LEFT_PRELOAD_TWO);
+                break;
+            case "CENTER":
+                builder.lineToLinearHeading(CENTER_PRELOAD);
+                break;
+            case "RIGHT":
+                builder.lineToLinearHeading(RIGHT_PRELOAD);
+                break;
+        }
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+    protected void boardScore() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        switch (randomization) {
+            case "LEFT":
+                builder.lineToLinearHeading(LEFT_BOARD);
+                break;
+            case "CENTER":
+                builder.lineToLinearHeading(CENTER_BOARD);
+                break;
+            case "RIGHT":
+                builder.lineToLinearHeading(RIGHT_BOARD);
+                break;
+        }
+        builder.addTemporalMarker(.2, robot.getArm()::armScore);
+        builder.addTemporalMarker(.2, robot.getSlides()::slideFirstLayer);
+        builder.addTemporalMarker(.2, robot.getWrist()::wristScore);
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+    protected void park() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToLinearHeading(BACK_OFF);
+        builder.lineToLinearHeading(PARK);
+        builder.addTemporalMarker(.1, robot.getArm()::armRest);
+        builder.addTemporalMarker(.1, robot.getWrist()::wristPickup);
+        builder.addTemporalMarker(.1, robot.getSlides()::slideDown);
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
 
     @Override
     public void runOpMode() throws InterruptedException {
+
         this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
         this.robot = new Robot().init(hardwareMap);
         this.robot.getCamera().initTargetingCamera();
-
-        //Trajectories
         this.initialPosition = new Pose2d(34, -59.5, Math.toRadians(270));
         this.robot.getDrive().setPoseEstimate(initialPosition);
 
-        //Randomization One
+//        this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end())
-        this.preloadOne = this.robot.getDrive().trajectoryBuilder(initialPosition)
+//                .lineToLinearHeading(new Pose2d(80, -57, Math.toRadians(360)))
-                .lineToLinearHeading(new Pose2d(40, -37.5, Math.toRadians(270)))
+//                .build();
-                .build();
-
-        this.scoreOne = this.robot.getDrive().trajectoryBuilder(preloadOne.end())
-                .lineToLinearHeading(new Pose2d(29.5, -29, Math.toRadians(360)))
-                .build();
-
-        this.boardOne = this.robot.getDrive().trajectoryBuilder(scoreOne.end())
-                .lineToLinearHeading(new Pose2d(76, -26.5, Math.toRadians(360)))
-                .addTemporalMarker(.2, robot.getArm()::armScore)
-                .addTemporalMarker(.2,robot.getSlides()::slideFirstLayer)
-                .addTemporalMarker(.2, robot.getWrist()::wristScore)
-                .build();
-
-        this.backOffOne = this.robot.getDrive().trajectoryBuilder(boardOne.end())
-                .lineToLinearHeading(new Pose2d(60, -45, Math.toRadians(360)))
-                .build();
-
-
-        //Randomization Two
-        this.preloadTwo = this.robot.getDrive().trajectoryBuilder(initialPosition)
-                .lineToLinearHeading(new Pose2d(33, -28, Math.toRadians(270)))
-                .build();
-
-        this.scoreTwo = this.robot.getDrive().trajectoryBuilder(preloadTwo.end())
-                .lineToLinearHeading(new Pose2d(75, -36.3, Math.toRadians(360)))
-                .addTemporalMarker(.2, robot.getArm()::armScore)
-                .addTemporalMarker(.2,robot.getSlides()::slideFirstLayer)
-                .addTemporalMarker(.2, robot.getWrist()::wristScore)
-                .build();
-
-        this.backOffTwo = this.robot.getDrive().trajectoryBuilder(scoreTwo.end())
-                .lineToLinearHeading(new Pose2d(60, -35, Math.toRadians(360)))
-                .build();
-
-        //Randomization Three
-        this.preloadThree = this.robot.getDrive().trajectoryBuilder(initialPosition)
-                .lineToLinearHeading(new Pose2d(46, -35, Math.toRadians(270)))
-                .build();
-
-        this.scoreThree = this.robot.getDrive().trajectoryBuilder(preloadThree.end())
-                .lineToLinearHeading(new Pose2d(75, -42, Math.toRadians(360)))
-                .addTemporalMarker(.2, robot.getArm()::armScore)
-                .addTemporalMarker(.2,robot.getSlides()::slideFirstLayer)
-                .addTemporalMarker(.2, robot.getWrist()::wristScore)
-                .build();
-
-        this.backOffThree = this.robot.getDrive().trajectoryBuilder(scoreThree.end())
-                .lineToLinearHeading(new Pose2d(60, -45, Math.toRadians(360)))
-                .build();
-
-        //Park
-        this.park1 = this.robot.getDrive().trajectoryBuilder(backOffTwo.end())
-                .lineToLinearHeading(new Pose2d(65, -55, Math.toRadians(360)))
-                .addTemporalMarker(.3, robot.getArm()::armRest)
-                .addTemporalMarker(.3, robot.getWrist()::wristPickup)
-                .build();
-        this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end())
-                .lineToLinearHeading(new Pose2d(80, -57, Math.toRadians(360)))
-                .build();
 
         // Do super fancy chinese shit
         while (!this.isStarted()) {
@@ -124,68 +99,12 @@ public class AutoRed extends LinearOpMode {
             this.telemetry.update();
         }
 
-        switch (randomization) {
+        scorePreloadOne();
-            case "LEFT":
+        boardScore();
-                this.robot.getDrive().followTrajectory(preloadOne);
+        sleep(250);
-                this.robot.getDrive().followTrajectory(scoreOne);
+        this.robot.getClaw().open();
-                this.robot.getDrive().followTrajectory(boardOne);
+        sleep(250);
-                sleep(500);
+        park();
-                this.robot.getClaw().open();
-                sleep(500);
-                this.robot.getDrive().followTrajectory(backOffOne);
-                this.robot.getSlides().slideDown();
-                sleep(300);
-                break;
-            case "CENTER":
-                this.robot.getDrive().followTrajectory(preloadTwo);
-                this.robot.getDrive().followTrajectory(scoreTwo);
-                sleep(500);
-                this.robot.getClaw().open();
-                sleep(500);
-                this.robot.getDrive().followTrajectory(backOffTwo);
-                this.robot.getSlides().slideDown();
-                sleep(300);
-                break;
-            case "RIGHT":
-                this.robot.getDrive().followTrajectory(preloadThree);
-                this.robot.getDrive().followTrajectory(scoreThree);
-                sleep(500);
-                this.robot.getClaw().open();
-                sleep(500);
-                this.robot.getDrive().followTrajectory(backOffThree);
-                this.robot.getSlides().slideDown();
-                sleep(300);
-                break;
-        }
-        //Cycle
-        this.robot.getDrive().followTrajectory(park1);
-        this.robot.getDrive().followTrajectory(park2);
-//        this.robot.getDrive().followTrajectory(goGate);
-//        sleep(120);
-//        this.robot.getClaw().close();
-//        sleep(120);
-//        this.robot.getDrive().followTrajectory(backGate);
-//        this.robot.getDrive().followTrajectory(approachBoard);
-//        sleep(120);
-//        this.robot.getClaw().open();
-//        sleep(120);
-//
-//        this.robot.getDrive().followTrajectory(park1);
-//        this.robot.getDrive().followTrajectory(goGate);
-//        sleep(120);
-//        this.robot.getClaw().close();
-//        sleep(120);
-//        this.robot.getDrive().followTrajectory(backGate);
-//        this.robot.getDrive().followTrajectory(approachBoard);
-//        sleep(120);
-//        this.robot.getClaw().open();
-//        sleep(120);
-//        this.robot.getDrive().followTrajectory(park1);
-//        this.robot.getDrive().followTrajectory(park2);
-
-
-
-
     }
 
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedFar.java

@@ -1,151 +1,64 @@
 package org.firstinspires.ftc.teamcode.opmodes;
 
 import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.trajectory.Trajectory;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.teamcode.hardware.Robot;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
 
-@Autonomous(name = "autoRedFar")
+@Config
+@Autonomous(name = "autRedFar")
 public class AutoRedFar extends LinearOpMode {
     protected Pose2d initialPosition;
-
-    protected Trajectory preloadOne;
-    protected Trajectory scoreOne;
-    protected Trajectory boardOne;
-    protected Trajectory backOffOne;
-    protected Trajectory goGate1;
-    protected Trajectory passGate;
-
-    protected Trajectory preloadTwo;
-    protected Trajectory scoreTwo;
-    protected Trajectory backOffTwo;
-    protected Trajectory tokyoDrift;
-    protected Trajectory tokyoDrift2;
-    protected Trajectory tokyoDrift3;
-
-    protected Trajectory preloadThree;
-    protected Trajectory boardThree;
-    protected Trajectory scoreThree;
-    protected Trajectory backOffThree;
-    protected Trajectory goGate3;
-    protected Trajectory goGate3Again;
-
-
-    protected Trajectory park1;
-    protected Trajectory park2;
-
     private Robot robot;
     private String randomization;
-    private int random;
+
+    //Pose2ds
+    //Preloads
+    final static Pose2d RIGHT_PRELOAD_ONE = new Pose2d(-40, -37.5, Math.toRadians(270));
+    final static Pose2d RIGHT_PRELOAD_TWO = new Pose2d(-29.5, -29, Math.toRadians(180));
+    final static Pose2d CENTER_PRELOAD = new Pose2d(-33, -28, Math.toRadians(270));
+    final static Pose2d LEFT_PRELOAD = new Pose2d(-43, -35, Math.toRadians(270));
+
+    protected void scorePreloadOne() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        switch (randomization) {
+            case "RIGHT":
+                builder.lineToLinearHeading(RIGHT_PRELOAD_ONE);
+                builder.lineToLinearHeading(RIGHT_PRELOAD_TWO);
+                break;
+            case "CENTER":
+                builder.lineToLinearHeading(CENTER_PRELOAD);
+                break;
+            case "LEFT":
+                builder.lineToLinearHeading(LEFT_PRELOAD);
+                break;
+        }
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
+    protected void goBackToWhereYouCameFrom() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToLinearHeading(initialPosition);
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
 
     @Override
     public void runOpMode() throws InterruptedException {
+
         this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
         this.robot = new Robot().init(hardwareMap);
         this.robot.getCamera().initTargetingCamera();
-
-        //Trajectories
         this.initialPosition = new Pose2d(-34, -59.5, Math.toRadians(270));
         this.robot.getDrive().setPoseEstimate(initialPosition);
-        //niggler
-        //Randomization One
-        this.preloadOne = this.robot.getDrive().trajectoryBuilder(initialPosition)
-                .lineToLinearHeading(new Pose2d(-40, -37.5, Math.toRadians(270)))
-                .build();
 
-        this.scoreOne = this.robot.getDrive().trajectoryBuilder(preloadOne.end())
+//        this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end())
-                .lineToLinearHeading(new Pose2d(-35, -25, Math.toRadians(30)))
+//                .lineToLinearHeading(new Pose2d(80, -57, Math.toRadians(360)))
-                .build();
+//                .build();
-
-        this.goGate1 = this.robot.getDrive().trajectoryBuilder(scoreOne.end())
-                .lineToLinearHeading(new Pose2d(-32, -10, Math.toRadians(360)))
-                .build();
-
-        this.passGate = this.robot.getDrive().trajectoryBuilder(goGate1.end())
-                .lineToLinearHeading(new Pose2d(40, -11, Math.toRadians(360)))
-                .addTemporalMarker(2, robot.getArm()::armScore)
-                .addTemporalMarker(2, robot.getWrist()::wristScore)
-                .build();
-
-        this.boardOne = this.robot.getDrive().trajectoryBuilder(passGate.end())
-                .lineToLinearHeading(new Pose2d(50, -28, Math.toRadians(360)))
-                .addTemporalMarker(.02, robot.getArm()::armSecondaryScore)
-                .addTemporalMarker(.02, robot.getWrist()::wristScore)
-                .build();
-
-        this.backOffOne = this.robot.getDrive().trajectoryBuilder(boardOne.end())
-                .lineToLinearHeading(new Pose2d(40, -25, Math.toRadians(360)))
-                .build();
-
-
-        //Randomization Two
-        this.preloadTwo = this.robot.getDrive().trajectoryBuilder(initialPosition)
-                .lineToLinearHeading(new Pose2d(-36, -28, Math.toRadians(240)))
-                .build();
-
-        this.tokyoDrift = this.robot.getDrive().trajectoryBuilder(preloadTwo.end())
-                .lineToLinearHeading(new Pose2d(-50, -38, Math.toRadians(270)))
-                .build();
-
-        this.tokyoDrift2 = this.robot.getDrive().trajectoryBuilder(tokyoDrift.end())
-                .lineToLinearHeading(new Pose2d(-50, -9, Math.toRadians(360)))
-                .build();
-
-        this.tokyoDrift3 = this.robot.getDrive().trajectoryBuilder(tokyoDrift2.end())
-                .lineToLinearHeading(new Pose2d(-35, -9, Math.toRadians(360)))
-                .build();
-
-        this.scoreTwo = this.robot.getDrive().trajectoryBuilder(passGate.end())
-                .lineToLinearHeading(new Pose2d(50, -34, Math.toRadians(360)))
-                .addTemporalMarker(.02, robot.getArm()::armSecondaryScore)
-                .addTemporalMarker(.02, robot.getWrist()::wristScore)
-                .build();
-
-        this.backOffTwo = this.robot.getDrive().trajectoryBuilder(scoreTwo.end())
-                .lineToLinearHeading(new Pose2d(40, -33, Math.toRadians(360)))
-                .build();
-
-        //Randomization Three
-        this.preloadThree = this.robot.getDrive().trajectoryBuilder(initialPosition)
-                .lineToLinearHeading(new Pose2d(-43, -37.5, Math.toRadians(270)))
-                .build();
-
-        this.scoreThree = this.robot.getDrive().trajectoryBuilder(preloadThree.end())
-                .lineToLinearHeading(new Pose2d(-28, -32, Math.toRadians(180)))
-                .build();
-
-        this.goGate3 = this.robot.getDrive().trajectoryBuilder(scoreThree.end())
-                .lineToSplineHeading(new Pose2d(-40, -32, Math.toRadians(180)))
-                .build();
-
-        this.goGate3Again = this.robot.getDrive().trajectoryBuilder(goGate3.end())
-                .lineToSplineHeading(new Pose2d(-33, -10, Math.toRadians(360)))
-                .build();
-
-        this.boardThree = this.robot.getDrive().trajectoryBuilder(passGate.end())
-                .lineToLinearHeading(new Pose2d(50, -39, Math.toRadians(360)))
-                .addTemporalMarker(.02, robot.getArm()::armSecondaryScore)
-                .addTemporalMarker(.02, robot.getWrist()::wristScore)
-                .build();
-
-        this.backOffThree = this.robot.getDrive().trajectoryBuilder(boardThree.end())
-                .lineToLinearHeading(new Pose2d(40, -42, Math.toRadians(360)))
-                .build();
-
-
-        //Park
-        this.park1 = this.robot.getDrive().trajectoryBuilder(backOffOne.end())
-                .lineToLinearHeading(new Pose2d(40, -12, Math.toRadians(360)))
-                .addTemporalMarker(.3, robot.getArm()::armRest)
-                .addTemporalMarker(.3, robot.getWrist()::wristPickup)
-                .build();
-        this.park2 = this.robot.getDrive().trajectoryBuilder(park1.end())
-                .lineToLinearHeading(new Pose2d(60, -10, Math.toRadians(10)))
-                .build();
 
         // Do super fancy chinese shit
         while (!this.isStarted()) {
@@ -153,52 +66,9 @@ public class AutoRedFar extends LinearOpMode {
             randomization = String.valueOf(this.robot.getCamera().getStartingPosition());
             this.telemetry.update();
         }
-        sleep(5000);
-        switch (randomization) {
-            case "LEFT":
-                this.robot.getDrive().followTrajectory(preloadOne);
-                this.robot.getDrive().followTrajectory(scoreOne);
-                this.robot.getDrive().followTrajectory(goGate1);
-                this.robot.getDrive().followTrajectory(passGate);
-                this.robot.getDrive().followTrajectory(boardOne);
-                sleep(500);
-                this.robot.getClaw().open();
-                sleep(500);
-                this.robot.getDrive().followTrajectory(backOffOne);
-                sleep(300);
-                break;
-            case "CENTER":
-                this.robot.getDrive().followTrajectory(preloadTwo);
-                this.robot.getDrive().followTrajectory(tokyoDrift);
-                this.robot.getDrive().followTrajectory(tokyoDrift2);
-                this.robot.getDrive().followTrajectory(tokyoDrift3);
-                this.robot.getDrive().followTrajectory(passGate);
-                this.robot.getDrive().followTrajectory(scoreTwo);
-                sleep(500);
-                this.robot.getClaw().open();
-                sleep(500);
-                this.robot.getDrive().followTrajectory(backOffTwo);
-                sleep(300);
-                break;
-            case "RIGHT":
-                this.robot.getDrive().followTrajectory(preloadThree);
-                this.robot.getDrive().followTrajectory(scoreThree);
-                this.robot.getDrive().followTrajectory(goGate3);
-                this.robot.getDrive().followTrajectory(goGate3Again);
-                this.robot.getDrive().followTrajectory(passGate);
-                this.robot.getDrive().followTrajectory(boardThree);
-                sleep(500);
-                this.robot.getClaw().open();
-                sleep(500);
-                this.robot.getDrive().followTrajectory(backOffThree);
-                sleep(300);
-                break;
-        }
-        //Cycle
-        this.robot.getDrive().followTrajectory(park1);
-        this.robot.getDrive().followTrajectory(park2);
-
 
+        scorePreloadOne();
+        goBackToWhereYouCameFrom();
     }
 
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmodes/AutoRedTwoPlusTwo.java

@@ -8,6 +8,8 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.teamcode.hardware.Robot;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.hardware.roadrunner.drive.MecanumDrive;
 import org.firstinspires.ftc.teamcode.hardware.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
 @Config
 @Autonomous(name = "autoRed2+2")
@@ -19,19 +21,19 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
     //Pose2ds
     //Preloads
     final static Pose2d LEFT_PRELOAD_ONE = new Pose2d(40, -37.5, Math.toRadians(270));
-    final static Pose2d LEFT_PRELOAD_TWO = new Pose2d(29.5, -29, Math.toRadians(360));
+    final static Pose2d LEFT_PRELOAD_TWO = new Pose2d(29.5, -32, Math.toRadians(360));
     final static Pose2d CENTER_PRELOAD = new Pose2d(33, -28, Math.toRadians(270));
     final static Pose2d RIGHT_PRELOAD = new Pose2d(43, -35, Math.toRadians(270));
     //Board Scores
-    final static Pose2d LEFT_BOARD = new Pose2d(74.3, -26.5, Math.toRadians(355));
+    final static Pose2d LEFT_BOARD = new Pose2d(75.3, -26.5, Math.toRadians(360));
-    final static Pose2d CENTER_BOARD = new Pose2d(74.7, -36.3, Math.toRadians(355));
+    final static Pose2d CENTER_BOARD = new Pose2d(75.3, -36.3, Math.toRadians(360));
-    final static Pose2d RIGHT_BOARD = new Pose2d(74.3, -40, Math.toRadians(355));
+    final static Pose2d RIGHT_BOARD = new Pose2d(75.3, -40, Math.toRadians(355));
     //Stack Cycle
     final static Pose2d LEAVE_BOARD = new Pose2d(65, -10, Math.toRadians(360));
-    final static Pose2d TO_STACK = new Pose2d(-40, -8, Math.toRadians(360));
+    final static Pose2d TO_STACK = new Pose2d(-40, -8.4, Math.toRadians(360));
     final static Pose2d BACK_THROUGH_GATE = new Pose2d(50, -10, Math.toRadians(360));
-    final static Pose2d APPROACHING_BOARD = new Pose2d(70, -28, Math.toRadians(360));
+    final static Pose2d APPROACHING_BOARD = new Pose2d(70, -31, Math.toRadians(360));
-    final static Pose2d SCORE_STACK = new Pose2d(74.45, -28, Math.toRadians(360));
+    final static Pose2d SCORE_STACK = new Pose2d(73.5, -31, Math.toRadians(360));
     //Park
     final static Pose2d BACK_OFF =  new Pose2d(60,-58,Math.toRadians(360));
     final static Pose2d PARK = new Pose2d(80, -60, Math.toRadians(360));
@@ -75,10 +77,11 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
     protected void toStack() {
         TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
         builder.lineToLinearHeading(LEAVE_BOARD);
-        builder.addTemporalMarker(.3, robot.getArm()::armPickupStack);
+        builder.addTemporalMarker(.3, robot.getArm()::armRest);
         builder.addTemporalMarker(.3, robot.getWrist()::wristPickup);
         builder.addTemporalMarker(.1, robot.getSlides()::slideDown);
         builder.addTemporalMarker(1.5,robot.getClaw()::openStack);
+        builder.addTemporalMarker(1.5, robot.getArm()::pickup);
         builder.lineToLinearHeading(TO_STACK);
         this.robot.getDrive().followTrajectorySequence(builder.build());
     }
@@ -94,6 +97,15 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
         this.robot.getDrive().followTrajectorySequence(builder.build());
     }
 
+    protected void scoreTest() {
+        TrajectorySequenceBuilder builder = this.robot.getTrajectorySequenceBuilder();
+        builder.lineToLinearHeading(new Pose2d(75, -31, Math.toRadians(360)),
+                MecanumDrive.getVelocityConstraint(20, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+                MecanumDrive.getAccelerationConstraint(20));
+        builder.addTemporalMarker(.2,this::clawSlowOpen);
+        this.robot.getDrive().followTrajectorySequence(builder.build());
+    }
+
     protected void clawSlowOpen() {
         double currentPos = .86;
         double targetPos = .78;
@@ -138,21 +150,18 @@ public class AutoRedTwoPlusTwo extends LinearOpMode {
         scorePreloadOne();
         boardScore();
 
-        sleep(250);
+        sleep(100);
         this.robot.getClaw().open();
-        sleep(250);
 
         toStack();
 
         sleep(500);
         this.robot.getClaw().close();
-        sleep(500);
+        sleep(250);
         this.robot.getArm().armRest();
         scoreStack();
         this.robot.getClaw().setPos(.86);
-        sleep(500);
+        scoreTest();
-        clawSlowOpen();
-        sleep(300);
         park();
     }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java

@@ -17,7 +17,7 @@ public class Configurables {
     public static double ARMREST = 0.8;
     public static double ARMSCORE = 0.39;
     public static double ARMACCSCORE = .38;
-    public static double ARMPICKUPSTACK = 0.843;
+    public static double ARMPICKUPSTACK = 0.8415;
     public static double PICKUP = 0.84;
     public static double WRISTPICKUP = 0.28;
     public static double WRISTSCORE = .96;
@@ -30,15 +30,15 @@ public class Configurables {
 
     //Drive Speed
     public static double SPEED = 1;
-    public static double SLOWMODE_SPEED = 0.5;
+    public static double SLOWMODE_SPEED = 0.3;
-    public static double TURN = 1;
+    public static double TURN = .75;
     public static double SLOWMODE_TURN = 0.3;
 
     //Motor Positions
     public static double SLIDE_POWER_UP = 1;
     public static double SLIDE_POWER_DOWN = .7;
     public static int SLIDELAYERONE = 150;
-    public static int SLIDEAUTOSTACKS = 225;
+    public static int SLIDEAUTOSTACKS = 350;
     public static int SLIDEUP = 1150;
     public static int HANGRELEASE = 2500;
     public static int HANG = 0;