Robby is dumb and stupid

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java

@@ -42,7 +42,7 @@ public class Robot {
         camEnabled = true;
     }
 
-    public void extendMacro(Slides.Position slidePos, double runTime) {
+    public void extendMacro(int slidePos, double runTime) {
         switch(macroState) {
             case(0):
                 macroStartTime = runTime;
@@ -58,14 +58,19 @@ public class Robot {
             case(2):
                 arm.setArmPos(Arm.Position.SCORE);
                 arm.setWristPos(Arm.Position.SCORE);
-                macroState = 0;
+                macroState++;
-                lastMacro = runningMacro;
+                break;
-                runningMacro = 0;
+            case (3):
+                if(arm.armAtPosition()){
+                    macroState = 0;
+                    lastMacro = runningMacro;
+                    runningMacro = 0;
+                }
                 break;
         }
     }
 
-    public void resetMacro(Slides.Position pos, double runTime) {
+    public void resetMacro(int slidePos, double runTime) {
         switch(macroState) {
             case(0):
                 macroStartTime = runTime;
@@ -75,7 +80,7 @@ public class Robot {
                 break;
             case(1):
                 if (runTime > macroStartTime + scoreWait) {
-                    macroState ++;
+                        macroState ++;
                 }
                 break;
             case(2):
@@ -90,10 +95,16 @@ public class Robot {
                 }
                 break;
             case(4):
-                slides.setTarget(pos);
+                slides.setTarget(slidePos);
-                macroState = 0;
+                macroState++;
-                lastMacro = runningMacro;
+                break;
-                runningMacro = 0;
+            case (5):
+                if (slides.atTarget()){
+                    macroState = 0;
+                    lastMacro = runningMacro;
+                    runningMacro = 0;
+                }
+                break;
         }
     }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java

@@ -22,6 +22,7 @@ public class Slides {
     public static int targetMax = 830;
 
     public static int down = 0;
+    public static int mini_tier1 = 155;
     public static int tier1 = 350;
     public static int tier2 = 500;
     public static int tier3 = 760;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AbstractAuto.java

@@ -1,338 +0,0 @@
-package org.firstinspires.ftc.teamcode.opmode.autonomous;
-
-import com.acmerobotics.roadrunner.trajectory.Trajectory;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-
-import org.firstinspires.ftc.teamcode.hardware.Arm;
-import org.firstinspires.ftc.teamcode.hardware.Intake;
-import org.firstinspires.ftc.teamcode.hardware.Robot;
-import org.firstinspires.ftc.teamcode.hardware.Slides;
-import org.firstinspires.ftc.teamcode.util.CameraPosition;
-
-import java.util.ArrayList;
-import java.util.Locale;
-
-public abstract class AbstractAuto extends LinearOpMode {
-    public Robot robot;
-
-    private int teamElementLocation = 2;
-    private ArrayList<Step> steps;
-    private double currentRuntime;
-    public CameraPosition cameraPosition;
-
-    // Main method to run all the steps for autonomous
-    @Override
-    public void runOpMode() {
-//        telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
-
-        telemetry.addLine("Initializing...");
-        telemetry.update();
-
-        setCameraPosition();
-
-        robot = new Robot(hardwareMap);
-
-        makeTrajectories();
-
-        while (robot.camera.getFrameCount() < 1) {
-            idle();
-        }
-
-        // wait for start
-        while (!(isStarted() || isStopRequested())) {
-            currentRuntime = getRuntime();
-            robot.update(currentRuntime);
-
-//            int newLocation = robot.camera.getMarkerId();
-//            if (newLocation != -1) {
-//                teamElementLocation = newLocation;
-//            }
-
-            telemetry.addLine("Initialized");
-            telemetry.addLine("Randomization: "+teamElementLocation);
-            telemetry.addLine(robot.getTelemetry());
-            telemetry.update();
-        }
-        resetRuntime();
-
-        // build the first step
-        steps = new ArrayList<>();
-//        stopCamera();
-        initializeSteps(teamElementLocation);
-
-        int stepNumber = 0;
-        double stepTimeout;
-        Step step = steps.get(stepNumber);
-        stepTimeout = step.getTimeout() != -1 ? currentRuntime + step.getTimeout() : Double.MAX_VALUE;
-        step.start();
-
-        // run the remaining steps
-        while (opModeIsActive()) {
-            currentRuntime = getRuntime();
-            // once a step finishes
-            if (step.isFinished() || currentRuntime >= stepTimeout) {
-                // do the finishing move
-                step.end();
-                stepNumber++;
-                // if it was the last step break out of the while loop
-                if (stepNumber > steps.size() - 1) {
-                    break;
-                }
-                // else continue to the next step
-                step = steps.get(stepNumber);
-                stepTimeout = step.getTimeout() != -1 ? currentRuntime + step.getTimeout() : Double.MAX_VALUE;
-                step.start();
-            }
-
-            // while the step is running display telemetry
-            step.whileRunning();
-            robot.update(currentRuntime);
-
-            telemetry.addLine(String.format(Locale.US, "Runtime: %.0f", currentRuntime));
-            telemetry.addLine("Step " + (stepNumber + 1) + " of " + steps.size() + ", " + step.getTelemetry() + "\n");
-            telemetry.addLine(robot.getTelemetry());
-            telemetry.update();
-        }
-    }
-
-    // Load up all of the steps for the autonomous: to be overridden with the specific steps in the specific auto
-    public abstract void initializeSteps(int location);
-
-    //methods to be implemented in the specific autos
-    public abstract void setAlliance();
-
-    public abstract void setCameraPosition();
-
-    public abstract boolean useCamera();
-
-    public abstract void makeTrajectories();
-
-
-//    public abstract void setArm(Arm.Position armPos, Claw.Position clawPos);
-
-    //other methods that do certain tasks
-
-    public void turn(double degrees) {
-        steps.add(new Step("Following a trajectory") {
-            @Override
-            public void start() {
-                robot.drive.turn(degrees);
-            }
-
-            @Override
-            public void whileRunning() {
-                robot.drive.update();
-            }
-
-            @Override
-            public void end() {
-            }
-
-            @Override
-            public boolean isFinished() {
-                return !robot.drive.isBusy();
-            }
-        });
-    }
-
-    public void followTrajectory(Trajectory trajectory) {
-        steps.add(new Step("Following a trajectory") {
-            @Override
-            public void start() {
-                robot.drive.followTrajectoryAsync(trajectory);
-            }
-
-            @Override
-            public void whileRunning() {
-                robot.drive.update();
-            }
-
-            @Override
-            public void end() {
-            }
-
-            @Override
-            public boolean isFinished() {
-                return !robot.drive.isBusy();
-            }
-        });
-    }
-
-    public void followAndExtend(Trajectory trajectory, Slides.Position slidePos) {
-        steps.add(new Step("Following a trajectory") {
-            @Override
-            public void start() {
-                robot.drive.followTrajectoryAsync(trajectory);
-                if (slidePos == Slides.Position.TIER1) {
-                    robot.runningMacro = 1;
-                } else if (slidePos == Slides.Position.TIER2) {
-                    robot.runningMacro = 2;
-                } else if (slidePos == Slides.Position.TIER3) {
-                    robot.runningMacro = 3;
-                }
-                robot.extendMacro(slidePos, currentRuntime);
-            }
-
-            @Override
-            public void whileRunning() {
-                if (robot.runningMacro != 0) {
-                    robot.extendMacro(slidePos, currentRuntime);
-                }
-                robot.drive.update();
-            }
-
-            @Override
-            public void end() {
-            }
-
-            @Override
-            public boolean isFinished() {
-                return !robot.drive.isBusy() && robot.runningMacro == 0;
-            }
-        });
-    }
-
-    public void followAndRetract(Trajectory trajectory, Slides.Position slidePos) {
-        steps.add(new Step("Following a trajectory") {
-            @Override
-            public void start() {
-                robot.drive.followTrajectoryAsync(trajectory);
-                robot.runningMacro = 4;
-                robot.resetMacro(slidePos, currentRuntime);
-            }
-
-            @Override
-            public void whileRunning() {
-                if (robot.runningMacro != 0) {
-                    robot.resetMacro(slidePos, currentRuntime);
-                }
-                robot.drive.update();
-            }
-
-            @Override
-            public void end() {
-            }
-
-            @Override
-            public boolean isFinished() {
-                return !robot.drive.isBusy() && robot.runningMacro == 0;
-            }
-        });
-    }
-
-    public void intakeStack(Intake.Position stackHeight1, Intake.Position stackHeight2) {
-        steps.add(new Step("Intaking Stack") {
-            @Override
-            public void start() {
-                robot.intake.setDcMotor(0.5);
-                robot.arm.setDoor(Arm.DoorPosition.OPEN);
-            }
-
-            @Override
-            public void whileRunning() {
-                switch(macroState) {
-                    case 0:
-                        macroStart = currentRuntime;
-                        robot.intake.setpos(stackHeight1);
-                        macroState++;
-                        break;
-                    case 1:
-                        if (currentRuntime > macroStart + 1.0) {
-                            macroState++;
-                        }
-                        break;
-                    case 2:
-                        robot.intake.setpos(stackHeight2);
-                        macroState++;
-                        break;
-                    case 3:
-                        if (currentRuntime > macroStart + 1.0) {
-                            macroState++;
-                        }
-                        break;
-                    case 4:
-                        macroState = 0;
-                }
-            }
-
-            @Override
-            public void end() {
-                robot.intake.setDcMotor(0);
-                robot.intake.setpos(Intake.Position.UP);
-                robot.arm.setDoor(Arm.DoorPosition.CLOSE);
-            }
-
-            @Override
-            public boolean isFinished() {
-                return macroState == 0;
-            }
-        });
-    }
-
-    public void runIntake(double power, double timeout) {
-        steps.add(new Step("Running Intake" + timeout + " seconds", timeout) {
-            @Override
-            public void start() {
-                robot.intake.setDcMotor(power);
-            }
-
-            @Override
-            public void whileRunning() {
-            }
-
-            @Override
-            public void end() {
-                robot.intake.setDcMotor(0);
-            }
-
-            @Override
-            public boolean isFinished() {
-                return false;
-            }
-        });
-    }
-
-    // Functions to add steps
-    public void addDelay(double timeout) {
-        steps.add(new Step("Waiting for " + timeout + " seconds", timeout) {
-            @Override
-            public void start() {
-            }
-
-            @Override
-            public void whileRunning() {
-            }
-
-            @Override
-            public void end() {
-            }
-
-            @Override
-            public boolean isFinished() {
-                return false;
-            }
-        });
-    }
-
-    public void stopCamera() {
-        steps.add(new Step("Stopping Signal Camera") {
-            @Override
-            public void start() {
-                robot.camera.stopTargetingCamera();
-            }
-
-            @Override
-            public void whileRunning() {
-            }
-
-            @Override
-            public void end() {
-            }
-
-            @Override
-            public boolean isFinished() {
-                return true;
-            }
-        });
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AutoBase.java

@@ -31,12 +31,12 @@ public abstract class AutoBase extends LinearOpMode {
         createTrajectories();
 
         // wait for start
-        while (!isStarted() || !isStopRequested()) {
+        while (!isStarted() && !isStopRequested()) {
             if (robot.camera.getFrameCount() < 1) {
                 telemetry.addLine("Initializing...");
             } else {
                 telemetry.addLine("Initialized");
-                teamPropLocation = robot.camera.getMarkerId(); // or whatever method you end up using
+                //teamPropLocation = robot.camera.getMarkerId(); // or whatever method you end up using
                 telemetry.addData("Team Prop Location", teamPropLocation);
             }
             telemetry.update();
@@ -51,6 +51,7 @@ public abstract class AutoBase extends LinearOpMode {
             robot.update(getRuntime());
 
             // update telemetry
+            telemetry.addData("Macro State", macroState);
             telemetry.addLine(robot.getTelemetry());
             telemetry.update();
         }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueLeftAuto.java

@@ -1,59 +0,0 @@
-package org.firstinspires.ftc.teamcode.opmode.autonomous;
-
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.trajectory.Trajectory;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-
-import org.firstinspires.ftc.teamcode.hardware.Slides;
-
-@Autonomous(name = "Blue Left Auto")
-public class BlueLeftAuto extends AbstractAuto {
-    public Trajectory scorePurple;
-    public Trajectory scoreYellow;
-    public Trajectory parkRobot;
-
-    @Override
-    public void initializeSteps(int location) {
-       followTrajectory(scorePurple);
-       runIntake(-0.3, 1);
-       followAndExtend(scoreYellow, Slides.Position.TIER1);
-       followAndRetract(parkRobot, Slides.Position.DOWN);
-    }
-
-    @Override
-    public void setAlliance() {
-    }
-
-    @Override
-    public void setCameraPosition() {
-    }
-
-    @Override
-    public boolean useCamera() {
-        return false;
-    }
-
-    @Override
-    public void makeTrajectories() {
-        Pose2d start = new Pose2d(24, 61.5, Math.toRadians(-90));
-//        Pose2d dropLeft = new Pose2d(24, 60, Math.toRadians(-90));
-        Pose2d dropMiddle = new Pose2d(24, 40.5, Math.toRadians(-90));
-//        Pose2d dropRight = new Pose2d(24, 60, Math.toRadians(-90));
-        Pose2d score = new Pose2d(62, 36, Math.toRadians(180));
-        Pose2d park = new Pose2d(62, 60, Math.toRadians(180));
-
-        scorePurple = robot.drive.trajectoryBuilder(start)
-                .lineToLinearHeading(dropMiddle)
-                .build();
-
-        scoreYellow = robot.drive.trajectoryBuilder(scorePurple.end())
-                .lineToLinearHeading(score)
-                .build();
-
-        parkRobot = robot.drive.trajectoryBuilder(scoreYellow.end())
-                .lineToLinearHeading(park)
-                .build();
-
-        robot.drive.setPoseEstimate(start);
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueRightAuto_Drivepathonly_.java

@@ -1,79 +0,0 @@
-package org.firstinspires.ftc.teamcode.opmode.autonomous;
-
-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.roadrunner.drive.SampleMecanumDrive;
-
-@Autonomous(name = "BlueRightAuto(Drive only)")
-public class BlueRightAuto_Drivepathonly_ extends LinearOpMode {
-    @Override
-    public void runOpMode() throws InterruptedException {
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
-//      add more trajectories here
-        Pose2d start = new Pose2d(-36, 61.5, Math.toRadians(-90));
-
-//        Pose2d dropLeft = new Pose2d(24, 60, Math.toRadians(-90));
-        Pose2d dropMiddle = new Pose2d(-36, 40.5, Math.toRadians(-90));
-//
-    //    Pose2d nine = new Pose2d(-36, 40.5, Math.toRadians(-180));
-
-        Pose2d stack1 = new Pose2d(-50, 40.5, Math.toRadians(-180));
-
-        Pose2d dropMiddle2 = new Pose2d(-36, 40.5, Math.toRadians(-180));
-
-        Pose2d bmid = new Pose2d(-36, 5, Math.toRadians(-180));
-
-        Pose2d bmid2 = new Pose2d(36, 5, Math.toRadians(-180));
-
-        Pose2d alimb = new Pose2d(54, 40.5, Math.toRadians(-180));
-
-        Pose2d score = new Pose2d(54, 36, Math.toRadians(180));
-
-        drive.setPoseEstimate(start);
-//      add this per trajectories
-        Trajectory scorePurple = drive.trajectoryBuilder(start)
-                .lineToLinearHeading(dropMiddle)
-                .build();
-
-        Trajectory mid_drop = drive.trajectoryBuilder(dropMiddle)
-                .lineToLinearHeading(stack1)
-                .build();
-        Trajectory back_to_mid = drive.trajectoryBuilder(stack1)
-                .lineToLinearHeading(dropMiddle2)
-                .build();
-        Trajectory front_gate = drive.trajectoryBuilder(dropMiddle2)
-                .lineToLinearHeading(bmid)
-                .build();
-        Trajectory front_gate_pt_2 = drive.trajectoryBuilder(bmid)
-                .lineToLinearHeading(bmid2)
-                .build();
-        Trajectory front_gate_pt_3 = drive.trajectoryBuilder(bmid2)
-                .lineToLinearHeading(alimb)
-                .build();
-        Trajectory about_to_score = drive.trajectoryBuilder(alimb)
-                .lineToLinearHeading(score)
-                .build();
-// the scorePurple. should be whatever the start pose2d thing was
-        Trajectory scoreYellow = drive.trajectoryBuilder(scorePurple.end())
-                .lineToLinearHeading(score)
-                .build();
-
-        waitForStart();
-
-        if(isStopRequested()) return;
-//      add this per trajectories
-        drive.followTrajectory(scorePurple);
-        drive.turn(-90);
-        drive.followTrajectory(mid_drop);
-    //    drive.followTrajectory(stack);
-        drive.followTrajectory(back_to_mid);
-        drive.followTrajectory(front_gate);
-        drive.followTrajectory(front_gate_pt_2);
-        drive.followTrajectory(front_gate_pt_3);
-        drive.followTrajectory(about_to_score);
-        drive.followTrajectory(scoreYellow);
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java

@@ -29,13 +29,13 @@ public class RedBackStageAuto extends AutoBase {
         // create pose2d variables
         // you might not need 3 instances of the deposit position, for example, however based on localization accuracy
         // you might need them for each one to be slightly different
-        Pose2d drop1 = new Pose2d(12, -40.5, Math.toRadians(90));
+        Pose2d drop1 = new Pose2d(12, -39.5, Math.toRadians(90));
-        Pose2d drop2 = new Pose2d(12, -40.5, Math.toRadians(90));
+        Pose2d drop2 = new Pose2d(12, -39.5, Math.toRadians(90));
-        Pose2d drop3 = new Pose2d(12, -40.5, Math.toRadians(90));
+        Pose2d drop3 = new Pose2d(12, -39.5, Math.toRadians(90));
 
-        Pose2d depositPreload1 = new Pose2d(46, -36, Math.toRadians(180));
+        Pose2d depositPreload1 = new Pose2d(50.5, -32, Math.toRadians(187));
-        Pose2d depositPreload2 = new Pose2d(46, -36, Math.toRadians(180));
+        Pose2d depositPreload2 = new Pose2d(50.5, -32, Math.toRadians(187));
-        Pose2d depositPreload3 = new Pose2d(46, -36, Math.toRadians(180));
+        Pose2d depositPreload3 = new Pose2d(50.5, -32, Math.toRadians(187));
 
         Pose2d park1 = new Pose2d(48, -12, Math.toRadians(180));
         Pose2d park2 = new Pose2d(48, -12, Math.toRadians(180));
@@ -77,49 +77,59 @@ public class RedBackStageAuto extends AutoBase {
     public void followTrajectories() {
         switch (macroState) {
             case 0:
-                // start drive to tape
                 robot.drive.followTrajectoryAsync(teamPropLocation==1?scorePurple1:(teamPropLocation==2?scorePurple2:scorePurple3));
                 macroState++;
                 break;
+            // DRIVE TO TAPE
             case 1:
-                // wait until robot is at tape
+                // if drive is done move on
                 if (!robot.drive.isBusy()) {
                     macroTime = getRuntime();
                     macroState++;
                 }
+                break;
+            // RUN INTAKE
             case 2:
-                // run intake
+                // intake
                 if (getRuntime() < macroTime + 0.5) {
-                    robot.intake.setDcMotor(-0.3);
+                    robot.intake.setDcMotor(-0.26);
                 }
-                // start drive to backdrop
+                // if intake is done move on
                 else {
-                    robot.extendMacro(Slides.Position.TIER1, getRuntime());
+                    robot.intake.setDcMotor(0);
+                    robot.extendMacro(Slides.mini_tier1, getRuntime());
                     robot.drive.followTrajectoryAsync(teamPropLocation==1?scoreYellow1:(teamPropLocation==2?scoreYellow2:scoreYellow3));
                     macroState++;
                 }
                 break;
+            // EXTEND AND MOVE TO BACKBOARD
             case 3:
                 // extend macro
                 if (robot.macroState != 0) {
-                    robot.extendMacro(Slides.Position.TIER1, getRuntime());
+                    robot.extendMacro(Slides.mini_tier1, getRuntime());
                 }
-                // if macro and drive are done, start park
+                // if macro and drive are done, move on
-                else if (!robot.drive.isBusy()) {
+                if (robot.macroState == 0 && !robot.drive.isBusy()) {
-                    robot.resetMacro(Slides.Position.DOWN, getRuntime());
+                    robot.resetMacro(0, getRuntime());
+                    macroState++;
+                }
+                break;
+            case 4:
+                robot.resetMacro(0, getRuntime());
+                if (robot.macroState >= 2){
                     robot.drive.followTrajectoryAsync(teamPropLocation==1?parkRobot1:(teamPropLocation==2?parkRobot2:parkRobot3));
                     macroState++;
                 }
                 break;
-            // park robot
+            // PARK ROBOT
-            case 4:
+            case 5:
-                // reset macro
+                // reset macro'
                 if (robot.macroState != 0) {
-                    robot.resetMacro(Slides.Position.DOWN, getRuntime());
+                    robot.resetMacro(0, getRuntime());
                 }
                 // if macro and drive are done, end auto
-                if (!robot.drive.isBusy()) {
+                if (robot.macroState == 0 && !robot.drive.isBusy()) {
-                    macroState = -1;
+                    macroState=-1;
                 }
                 break;
         }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedRightAuto.java

@@ -1,135 +0,0 @@
-package org.firstinspires.ftc.teamcode.opmode.autonomous;
-
-import com.acmerobotics.roadrunner.geometry.Pose2d;
-import com.acmerobotics.roadrunner.trajectory.Trajectory;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-
-import org.firstinspires.ftc.teamcode.hardware.Intake;
-import org.firstinspires.ftc.teamcode.hardware.Slides;
-import org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants;
-import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
-
-@Autonomous(name = "Red Right Auto")
-public class RedRightAuto extends AbstractAuto {
-    public Trajectory scorePurple;
-    public Trajectory scoreYellow;
-    public Trajectory load1;
-    public Trajectory score1;
-    public Trajectory load2;
-    public Trajectory score2;
-    public Trajectory parkRobot;
-
-    @Override
-    public void makeTrajectories() {
-        Pose2d start = new Pose2d(12, -61.5, Math.toRadians(90));
-
-        Pose2d dropMiddle = new Pose2d(12, -40.5, Math.toRadians(90));
-
-        Pose2d depositPreload = new Pose2d(46, -36, Math.toRadians(180));
-
-        Pose2d lineup1 = new Pose2d(12, -14, Math.toRadians(180));
-        Pose2d pickup1 = new Pose2d(-58, -14, Math.toRadians(180));
-
-        Pose2d driveup1 = new Pose2d(12-6, -14, Math.toRadians(180));
-        Pose2d deposit1 = new Pose2d(48-6, -36-4, Math.toRadians(180));
-
-        Pose2d lineup2 = new Pose2d(12, -14, Math.toRadians(180));
-        Pose2d pickup2 = new Pose2d(-58, -14, Math.toRadians(180));
-
-        Pose2d driveup2 = new Pose2d(12-6, -14, Math.toRadians(180));
-        Pose2d deposit2 = new Pose2d(48-6, -36-4, Math.toRadians(180));
-
-        Pose2d park = new Pose2d(48, -12, Math.toRadians(180));
-
-        scorePurple = robot.drive.trajectoryBuilder(start)
-                .lineToLinearHeading(dropMiddle)
-                .build();
-
-        scoreYellow = robot.drive.trajectoryBuilder(scorePurple.end())
-                .lineToLinearHeading(depositPreload)
-                .build();
-
-        int driveSpeed = 45;
-
-        load1 = robot.drive.trajectoryBuilder(scoreYellow.end())
-                .splineToConstantHeading(lineup1.vec(), lineup1.getHeading(),
-                        SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
-                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
-                )
-                .lineToLinearHeading(pickup1,
-                        SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
-                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
-                )
-                .build();
-
-        score1 = robot.drive.trajectoryBuilder(load1.end(), true)
-                .lineToLinearHeading(driveup1,
-                        SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
-                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
-                )
-                .splineToConstantHeading(deposit1.vec(), deposit1.getHeading(),
-                        SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
-                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
-                )
-                .build();
-
-        load2 = robot.drive.trajectoryBuilder(score1.end())
-                .splineToConstantHeading(lineup1.vec(), lineup1.getHeading(),
-                        SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
-                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
-                )
-                .lineToLinearHeading(pickup1,
-                        SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
-                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
-                )
-                .build();
-
-        score2 = robot.drive.trajectoryBuilder(load2.end(), true)
-                .lineToLinearHeading(driveup1,
-                        SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
-                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
-                )
-                .splineToConstantHeading(deposit1.vec(), deposit1.getHeading(),
-                        SampleMecanumDrive.getVelocityConstraint(driveSpeed, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
-                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL)
-                )
-                .build();
-
-        parkRobot = robot.drive.trajectoryBuilder(score2.end())
-                .lineToLinearHeading(park)
-                .build();
-
-        robot.drive.setPoseEstimate(start);
-    }
-
-    @Override
-    public void initializeSteps(int location) {
-        // score preloads
-        followTrajectory(scorePurple);
-        runIntake(-0.4, 0.5);
-        followAndExtend(scoreYellow, Slides.Position.TIER1);
-
-        // cycle
-        followAndRetract(load1, Slides.Position.DOWN);
-        intakeStack(Intake.Position.STACK5, Intake.Position.STACK4);
-        followAndExtend(score1, Slides.Position.TIER1);
-        followAndRetract(load2, Slides.Position.DOWN);
-        intakeStack(Intake.Position.STACK3, Intake.Position.STACK2);
-        followAndExtend(score2, Slides.Position.TIER1);
-
-        followAndRetract(parkRobot, Slides.Position.DOWN);
-    }
-
-    @Override
-    public void setAlliance() {
-    }
-
-    @Override
-    public void setCameraPosition() {
-    }
-
-    @Override
-    public boolean useCamera() {
-        return false;
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/SimpleBlueLeftAuto_ignore.java

@@ -1,42 +0,0 @@
-package org.firstinspires.ftc.teamcode.opmode.autonomous;
-
-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.roadrunner.drive.SampleMecanumDrive;
-
-@Autonomous(name = "Simple Blue Left Auto")
-public class SimpleBlueLeftAuto_ignore extends LinearOpMode {
-    @Override
-    public void runOpMode() throws InterruptedException {
-        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
-//      add more trajectories here
-        Pose2d start = new Pose2d(24, 61.5, Math.toRadians(-90));
-
-//        Pose2d dropLeft = new Pose2d(24, 60, Math.toRadians(-90));
-        Pose2d dropMiddle = new Pose2d(24, 40.5, Math.toRadians(-90));
-//        Pose2d dropRight = new Pose2d(24, 60, Math.toRadians(-90));
-
-        Pose2d score = new Pose2d(60, 36, Math.toRadians(180));
-
-        drive.setPoseEstimate(start);
-//      add this per trajectories
-        Trajectory scorePurple = drive.trajectoryBuilder(start)
-                .lineToLinearHeading(dropMiddle)
-                .build();
-// the scorePurple. should be whatever the start pose@d thing was
-        Trajectory scoreYellow = drive.trajectoryBuilder(scorePurple.end())
-                .lineToLinearHeading(score)
-                .build();
-
-        waitForStart();
-
-        if(isStopRequested()) return;
-//      add this per trajectories
-        drive.followTrajectory(scorePurple);
-//        drive.wait(1);
-        drive.followTrajectory(scoreYellow);
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/Step.java

@@ -1,48 +0,0 @@
-package org.firstinspires.ftc.teamcode.opmode.autonomous;
-
-
-import org.firstinspires.ftc.teamcode.vision.Detection;
-
-// Class for every step that the autonomous program will take
-public abstract class Step {
-    private final double timeout;
-    private String telemetry;
-
-    // macro variables
-    public double macroStart;
-    public int macroState;
-
-    // Constructors
-    public Step(String telemetry) {
-        this.telemetry = telemetry;
-        this.timeout = -1;
-    }
-
-    public Step(String telemetry, double timeout) {
-        this.telemetry = telemetry;
-        this.timeout = timeout;
-    }
-
-    // Abstract methods to be overrode
-    public abstract void start();
-
-    public abstract void whileRunning();
-
-    public abstract void end();
-
-    public abstract boolean isFinished();
-
-    // Return the timeout for the step
-    public double getTimeout() {
-        return timeout;
-    }
-
-    public void setTelemetry(String telemetry) {
-        this.telemetry = telemetry;
-    }
-
-    // Return the Telemetry for the step
-    public String getTelemetry() {
-        return telemetry;
-    }
-}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java

@@ -119,16 +119,16 @@ public class MainTeleOp extends OpMode {
                 }
                 break;
             case (1):
-                robot.extendMacro(Slides.Position.TIER1, getRuntime());
+                robot.extendMacro(Slides.tier1, getRuntime());
                 break;
             case (2):
-                robot.extendMacro(Slides.Position.TIER2, getRuntime());
+                robot.extendMacro(Slides.tier2, getRuntime());
                 break;
             case (3):
-                robot.extendMacro(Slides.Position.TIER3, getRuntime());
+                robot.extendMacro(Slides.tier3, getRuntime());
                 break;
             case (4):
-                robot.resetMacro(Slides.Position.DOWN, getRuntime());
+                robot.resetMacro(0, getRuntime());
                 break;
         }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/FollowerPIDTuner.java

@@ -22,10 +22,10 @@ import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySe
  * These coefficients can be tuned live in dashboard.
  */
 //@Config
-@Disabled
+
 @Autonomous(group = "drive")
 public class FollowerPIDTuner extends LinearOpMode {
-    public static double DISTANCE = 48; // in
+    public static double DISTANCE = 36; // in
 
     @Override
     public void runOpMode() throws InterruptedException {