I added the new door to the macros and a potential blue right auto drive path.

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

@@ -65,7 +65,7 @@ public class Arm {
         }
     }
 
-    public void openDoor(DoorPos.DoorPosition pos) {
+    public void setDoor(DoorPos.DoorPosition pos) {
         if (pos == DoorPos.DoorPosition.OPEN) {
             dServo.setPosition(doorOpenpos);
         } else if (pos == DoorPos.DoorPosition.CLOSE) {

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

@@ -1,6 +1,6 @@
 package org.firstinspires.ftc.teamcode.hardware;
 
-public class Hieght {
+public class Height {
     public enum height {
         ASCEND, HOLD, DESCEND, RESET
     }

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

@@ -76,6 +76,10 @@ public class Intake {
         dcMotor.setPower(pwr);
     }
 
+    public double getPower() {
+        return dcMotor.getPower();
+    }
+
     public String getTelemetry() {
         return "lServo: "+lServo.getPosition()+"rServo: "+rServo.getPosition()+"dcMotor: "+dcMotor.getPower();
     }

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

@@ -1,6 +1,8 @@
 package org.firstinspires.ftc.teamcode.hardware;
 
 import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.UP;
+import static org.firstinspires.ftc.teamcode.hardware.DoorPos.DoorPosition.CLOSE;
+import static org.firstinspires.ftc.teamcode.hardware.DoorPos.DoorPosition.OPEN;
 
 import com.acmerobotics.dashboard.config.Config;
 import com.qualcomm.robotcore.hardware.HardwareMap;
@@ -41,6 +43,7 @@ public class Robot {
             case(0):
                 macroStartTime = runTime;
                 slides.setTarget(slidePos);
+                arm.setDoor(CLOSE);
                 macroState ++;
                 break;
             case(1):
@@ -63,16 +66,27 @@ public class Robot {
         switch(macroState) {
             case(0):
                 macroStartTime = runTime;
-                arm.setArmPos(Arm.Position.INTAKE);
+                arm.setDoor(OPEN);
-                arm.setWristPos(Arm.Position.INTAKE);
                 macroState++;
                 break;
+                //Ind_sleeper
             case(1):
-                if (runTime > macroStartTime + 1) {
+                if (runTime > macroStartTime + 2) {
                     macroState ++;
                 }
                 break;
             case(2):
+                macroStartTime = runTime;
+                arm.setArmPos(Arm.Position.INTAKE);
+                arm.setWristPos(Arm.Position.INTAKE);
+                macroState++;
+                break;
+            case(3):
+                if (runTime > macroStartTime + 1.1) {
+                    macroState ++;
+                }
+                break;
+            case(4):
                 macroStartTime = runTime;
                 slides.setTarget(pos);
                 macroState = 0;

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

@@ -0,0 +1,79 @@
+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(-24, 61.5, Math.toRadians(-90));
+
+//        Pose2d dropLeft = new Pose2d(24, 60, Math.toRadians(-90));
+        Pose2d dropMiddle = new Pose2d(-24, 40.5, Math.toRadians(-180));
+//        Pose2d dropRight = new Pose2d(24, 60, Math.toRadians(-90));
+
+        Pose2d stack1 = new Pose2d(-68, 40.5, Math.toRadians(-180));
+
+        Pose2d dropMiddle2 = new Pose2d(-24, 40.5, Math.toRadians(-180));
+
+        Pose2d bmid = new Pose2d(-24, 5, Math.toRadians(-180));
+
+        Pose2d bmid2 = new Pose2d(24, 5, Math.toRadians(-180));
+
+        Pose2d alimb = new Pose2d(60, 40.5, Math.toRadians(-180));
+
+        Pose2d score = new Pose2d(60, 36, Math.toRadians(180));
+
+        drive.setPoseEstimate(start);
+//      add this per trajectories
+        Trajectory scorePurple = drive.trajectoryBuilder(start)
+                .lineToLinearHeading(dropMiddle)
+                .build();
+
+        Trajectory stack= 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 about_to_score= drive.trajectoryBuilder(bmid2)
+
+
+                .lineToLinearHeading(alimb)
+                .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.followTrajectory(stack);
+        drive.followTrajectory(back_to_mid);
+        drive.followTrajectory(front_gate);
+        drive.followTrajectory(front_gate_pt_2);
+        drive.followTrajectory(about_to_score);
+        drive.followTrajectory(scoreYellow);
+    }
+}

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

@@ -8,11 +8,11 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
 
 @Autonomous(name = "Simple Blue Left Auto")
-public class SimpleBlueLeftAuto extends LinearOpMode {
+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));
@@ -22,11 +22,11 @@ public class SimpleBlueLeftAuto extends LinearOpMode {
         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();
@@ -34,7 +34,7 @@ public class SimpleBlueLeftAuto extends LinearOpMode {
         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/teleop/NewTeleop.java

@@ -1,10 +1,12 @@
 package org.firstinspires.ftc.teamcode.opmode.teleop;
 
+import static org.firstinspires.ftc.teamcode.hardware.DoorPos.DoorPosition.CLOSE;
+import static org.firstinspires.ftc.teamcode.hardware.DoorPos.DoorPosition.OPEN;
+
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import com.qualcomm.robotcore.util.ElapsedTime;
 
 import org.firstinspires.ftc.teamcode.controller.Controller;
 import org.firstinspires.ftc.teamcode.hardware.Intake;
@@ -63,7 +65,7 @@ public class NewTeleop extends OpMode {
 
         // macros
         switch (robot.runningMacro) {
-            case(0): // manual mode
+            case (0): // manual mode
                 robot.slides.increaseTarget(controller2.getLeftStick().getY());
                 if (controller2.getX().isJustPressed()) {
                     robot.runningMacro = 1;
@@ -74,17 +76,22 @@ public class NewTeleop extends OpMode {
                 } else if (controller2.getA().isJustPressed()) {
                     robot.runningMacro = 4;
                 }
+                if (robot.intake.getPower() >= 0.01) {
+                    robot.arm.setDoor(OPEN);
+                } else {
+                    robot.arm.setDoor(CLOSE);
+                }
                 break;
-            case(1):
+            case (1):
                 robot.extendMacro(Slides.Position.TIER1, getRuntime());
                 break;
-            case(2):
+            case (2):
                 robot.extendMacro(Slides.Position.TIER2, getRuntime());
                 break;
-            case(3):
+            case (3):
                 robot.extendMacro(Slides.Position.TIER3, getRuntime());
                 break;
-            case(4):
+            case (4):
                 robot.resetMacro(Slides.Position.DOWN, getRuntime());
                 break;
         }