a

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/redDepot.java

@@ -1,110 +1,285 @@
-package org.firstinspires.ftc.teamcode.Auton;
+package org.firstinspires.ftc.teamcode.Auton;//package org.firstinspires.ftc.teamcode.Auton;
 
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.hardware.limelightvision.Limelight3A;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
+import org.firstinspires.ftc.teamcode.drive.DriveConstants;
 import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
 import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
+import org.firstinspires.ftc.teamcode.util.PersonalPID;
 
 @Config
-@Autonomous(name = "Clean Red Depot")
+@Autonomous(name = "Blue Depot Cycle")
 public class redDepot extends LinearOpMode {
 
     Hware robot = new Hware();
     public static double TICKS_PER_DEGREE = 9.738888 * 0.29969;
 
-    // --- Action Helpers ---
-    public void setTurret(int degrees) {
-        robot.turret.setTargetPosition((int) (degrees * TICKS_PER_DEGREE));
+    //turret
+    public static double p = 0.0000003, i = 0, d = 0.00001, f = 0.007;
+    public static double target = 0;
+
+    public static double ticksperdegree = 9.738888 * 0.29969;
+
+    public int minTicks = (int) (ticksperdegree * (-90));
+    public int maxTicks = (int) (ticksperdegree * (75));
+    PersonalPID pid;
+    Limelight3A limelight;
+    LLResult result;
+
+    boolean lock = true;
+
+    public void turretPower(double power) {
+        robot.turret.setPower(power);
+    }
+
+    public void turretPos(int position) {
+        turretPower(0);
+        robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.turret.setTargetPosition(position * (int) TICKS_PER_DEGREE);
         robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        robot.turret.setPower(1.0);
+        turretPower(1);
     }
 
-    public void startShooter(double velocity) {
-        robot.topFly.setVelocity(velocity);
-        robot.bottomFly.setVelocity(velocity);
-        robot.launchHood.setPosition(0.54);
+    public void spintakeStart(){
+        robot.intake.set(1);
     }
 
-    public void stopEverything() {
-//        robot.intake.set(0);
-//        robot.topFly.setVelocity(0);
-//        robot.bottomFly.setVelocity(0);
-//        robot.activeTransfer.setPosition(0.8);
+    public void spintakeEnd(){
+        robot.intake.set(0);
     }
 
-    @Override
+    public void shoot(){
+        robot.topFly.setVelocity(1800);
+        robot.bottomFly.setVelocity(1800);
+        robot.launchHood.setPosition(0.55);
+    }
+
+    public void reset(){
+        robot.intake.set(0);
+//        robot.topFly.setVelocity(1200);
+//        robot.bottomFly.setVelocity(1200);
+//        robot.topFly.set(0);
+//        robot.bottomFly.set(0);
+//        robot.launchHood.setPosition(0.6);
+        robot.activeTransfer.setPosition(1);
+    }
+
+
     public void runOpMode() throws InterruptedException {
+
+        //webcam1 = hardwareMap.get(WebcamName.class, "Webcam 2");
         SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
         robot.initialize(hardwareMap);
+        limelight = hardwareMap.get(Limelight3A.class, "limelight");
+        pid = new PersonalPID(p, i, d, f);
+        limelight.pipelineSwitch(1);
+        limelight.start();
         robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.turret.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        pid = new PersonalPID(p, i, d, f);
+
+        Pose2d startPose = new Pose2d(54, -46, Math.toRadians(-52));
 
-        Pose2d startPose = new Pose2d(54, -52, Math.toRadians(205));
         drive.setPoseEstimate(startPose);
 
-        // --- TRAJECTORY 1: Initial Score ---
-        TrajectorySequence traj1 = drive.trajectorySequenceBuilder(startPose)
-                .addDisplacementMarker(() -> {
-                    setTurret(-5);
-                    startShooter(1660);
-                    robot.intake.set(.8);
+        TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose)
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                    reset();
+//                    turretPos(5);
                 })
-                .strafeRight(35)
-                .addDisplacementMarker(() -> robot.activeTransfer.setPosition(1.0))
+                .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
+                    shoot();
+                })
+                .back(47)
+                .waitSeconds(0.5)
+                .UNSTABLE_addTemporalMarkerOffset(0.3, () -> {
+                    robot.intake.set(1);
+                    robot.activeTransfer.setPosition(0.85);
+                })
+
+//                .UNSTABLE_addTemporalMarkerOffset(2.2, () -> {
+//                    robot.launchHood.setPosition(0.5);
+//                })
                 .waitSeconds(1.5)
-                .addDisplacementMarker(this::stopEverything)
-                .build();
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                    reset();
+                })
 
-        // --- TRAJECTORY 2: Intake 6-Ball ---
-        TrajectorySequence traj2 = drive.trajectorySequenceBuilder(traj1.end())
-                .lineToLinearHeading(new Pose2d(11, -28, Math.toRadians(265)))
-                .addDisplacementMarker(() -> robot.intake.set(1.0))
-                .forward(35)
-                .addDisplacementMarker(() -> robot.intake.set(0))
-                .build();
+                //6ball
+                .lineToLinearHeading(new Pose2d(8,-28, Math.toRadians(-90)))
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                    spintakeStart();
+                })
+                .forward(30, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                    spintakeEnd();
+                    shoot();
+                })
+                .lineToLinearHeading(new Pose2d(24,-14, Math.toRadians(-52)))
+                .UNSTABLE_addTemporalMarkerOffset(.5, () -> {
+                    robot.intake.set(1);
+                    robot.activeTransfer.setPosition(0.85);
+                })
 
-        // --- TRAJECTORY 3: Score 6-Ball ---
-        TrajectorySequence traj3 = drive.trajectorySequenceBuilder(traj2.end())
-                .lineToLinearHeading(new Pose2d(22, -20, Math.toRadians(205)))
-                .addDisplacementMarker(0.5, () -> startShooter(1660)) // Trigger half-way through
-                .addDisplacementMarker(() -> robot.activeTransfer.setPosition(1.0))
+//                .UNSTABLE_addTemporalMarkerOffset(2.2, () -> {
+//                    robot.launchHood.setPosition(0.5);
+//                })
                 .waitSeconds(1.5)
-                .addDisplacementMarker(this::stopEverything)
-                .build();
 
-        // --- TRAJECTORY 4: Intake 9-Ball ---
-        TrajectorySequence traj4 = drive.trajectorySequenceBuilder(traj3.end())
-                .lineToLinearHeading(new Pose2d(-14, -27, Math.toRadians(265)))
-                .addDisplacementMarker(() -> robot.intake.set(1.0))
-                .forward(35)
-                .addDisplacementMarker(() -> robot.intake.set(0))
-                .build();
-
-        // --- TRAJECTORY 5: Score 9-Ball & Park ---
-        TrajectorySequence traj5 = drive.trajectorySequenceBuilder(traj4.end())
-                .lineToLinearHeading(new Pose2d(22, -20, Math.toRadians(205)))
-                .addDisplacementMarker(() -> {
-                    startShooter(1660);
-                    robot.activeTransfer.setPosition(1.0);
+                //9ball
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                    reset();
+                })
+                .lineToLinearHeading(new Pose2d(-19,-27, Math.toRadians(-90)))
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                    spintakeStart();
+                })
+                .forward(18, SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                    spintakeEnd();
+                })
+                .lineToLinearHeading(new Pose2d(-12,-54, Math.toRadians(-90)))
+                .lineToLinearHeading(new Pose2d(26,-14, Math.toRadians(-52)))
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                    reset();
+                })
+                .UNSTABLE_addTemporalMarkerOffset(.5, () -> {
+                    robot.intake.set(1);
+                    robot.activeTransfer.setPosition(0.85);
                 })
                 .waitSeconds(1.5)
-                .addDisplacementMarker(this::stopEverything)
-                .lineToLinearHeading(new Pose2d(11, -28, Math.toRadians(-90)))
+
+                //12ball
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                    reset();
+                })
+                .lineToLinearHeading(new Pose2d(-40,-22, Math.toRadians(-90)))
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                    spintakeStart();
+                })
+                .forward(33, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                    spintakeEnd();
+                })
+                .lineToLinearHeading(new Pose2d(35,-14, Math.toRadians(-52)))
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                    shoot();
+                })
+                .UNSTABLE_addTemporalMarkerOffset(.5, () -> {
+                    robot.intake.set(0.8);
+                    robot.activeTransfer.setPosition(0.85);
+                })
+                .waitSeconds(1.5)
+                //     .lineToLinearHeading(new Pose2d(55.5,68, Math.toRadians(-45)))
+
+//                .UNSTABLE_addTemporalMarkerOffset(0.5, () -> {
+//                    turretPos(84);
+//                })
+                .waitSeconds(1)
                 .build();
 
+//        TrajectorySequence oneCycle = drive.trajectorySequenceBuilder(startPose)
+//                .strafeLeft(25)
+//                .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
+//                    shoot();
+//                })
+//                .waitSeconds(1)
+//                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+//                    reset();
+//                })
+//                .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
+//                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+//                    spintakeStart();
+//                })
+//                .forward(30,  SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+//                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+//                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+//                    spintakeEnd();
+//                })
+//                .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
+//                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+//                    shoot();
+//                })
+//                .waitSeconds(1)
+//                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+//                    reset();
+//                })
+//                .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
+//                .forward(10)
+//                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+//                    spintakeStart();
+//                })
+//                .strafeLeft(10)
+//                .forward(3)
+//                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+//                    spintakeEnd();
+//                })
+//                .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
+//                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+//                    shoot();
+//                })
+//                .waitSeconds(1)
+//                .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
+//                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+//                    spintakeStart();
+//                })
+//                .forward(30,  SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+//                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+//                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+//                    spintakeEnd();
+//                })
+//                .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
+//                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+//                    shoot();
+//                })
+//                .waitSeconds(1)
+//                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+//                    reset();
+//                })
+//                .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
+//                .build();
+
         waitForStart();
+        if (opModeIsActive() && !isStopRequested()) {
+//          while (opModeIsActive()) {
+            drive.followTrajectorySequenceAsync(threeBall);
+            PoseStorage.currentPose = drive.getPoseEstimate();
 
-        if (isStopRequested()) return;
+            while(opModeIsActive()){
+                robot.leftPTO.setPosition(0.2);
+                robot.rightPTO.setPosition(0.14);
+                drive.update();
+                result = limelight.getLatestResult();
+                double vision = result.getTx() + 3;
 
-        // Execute sequentially
-        drive.followTrajectorySequence(traj1);
-        drive.followTrajectorySequence(traj2);
-        drive.followTrajectorySequence(traj3);
-        drive.followTrajectorySequence(traj4);
-        drive.followTrajectorySequence(traj5);
+                double target = ticksperdegree * vision;
+                double targetTicks = pid.calculate(robot.turret.getCurrentPosition(), target);
+                pid.setPIDF(p, i, d, f);
+
+                if((robot.turret.getCurrentPosition() > maxTicks) && (targetTicks > 0)){
+                    targetTicks = targetTicks;
+                } else if((robot.turret.getCurrentPosition() < maxTicks) && (targetTicks < 0)){
+                    targetTicks = targetTicks;
+
+                } else if((robot.turret.getCurrentPosition() > maxTicks) || (robot.turret.getCurrentPosition() < minTicks) ){
+                    targetTicks = 0;
+                }
+                PoseStorage.currentPose = drive.getPoseEstimate();
+                robot.turret.setPower(- targetTicks);
+            }
+//            }
+        }
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1Blue.java

@@ -18,7 +18,7 @@ import org.firstinspires.ftc.teamcode.util.PersonalPID;
 
 @Config
 @TeleOp(name = "TeleOp BLUE - Final Optimized")
-public class DriverControlsV1 extends LinearOpMode {
+public class DriverControlsV1Blue extends LinearOpMode {
 
     // --- DASHBOARD TUNING ---
     public static double flywheelTuner = 1.0;
@@ -34,7 +34,7 @@ public class DriverControlsV1 extends LinearOpMode {
     private double lastTuner = -1.0;
     private double lastPTOPosition = -1.0;
 
-    enum ShootState { IDLE, START_SHOOT, RESET }
+    enum ShootState { IDLE, START_SHOOT, RESET, START_SHOOT_FAR }
 
     //turret
     public static double p = 0.0000003, i = 0, d = 0.00001, f = 0.007;
@@ -44,6 +44,7 @@ public class DriverControlsV1 extends LinearOpMode {
 
     public int minTicks = (int) (ticksperdegree * (-90));
     public int maxTicks = (int) (ticksperdegree * (75));
+    public int offsetCam = 3;
     PersonalPID pid;
     Limelight3A limelight;
     LLResult result;
@@ -81,7 +82,7 @@ public class DriverControlsV1 extends LinearOpMode {
 
         while (opModeIsActive()) {
              result = limelight.getLatestResult();
-             double vision = result.getTx()-3;
+             double vision = result.getTx()-offsetCam;
 
             // 1. INPUTS & TURRET
 //            if (gamepad2.yWasPressed()) {
@@ -178,6 +179,12 @@ public class DriverControlsV1 extends LinearOpMode {
                 shootTimer.reset();
             }
 
+            if (gamepad2.dpadLeftWasPressed()) {
+                shooting = true;
+                shootStatus = ShootState.START_SHOOT_FAR;
+                shootTimer.reset();
+            }
+
             if (gamepad2.yWasPressed()) {
                 flywheelTuner+=.01;
             }
@@ -186,6 +193,12 @@ public class DriverControlsV1 extends LinearOpMode {
                 flywheelTuner-=.01;
             }
 
+            if (gamepad2.xWasPressed()) {
+                offsetCam = 0;
+            } else if (gamepad2.bWasPressed()) {
+                offsetCam = 3;
+            }
+
             switch (shootStatus) {
                 case START_SHOOT:
                     robot.activeTransfer.setPosition(0.85);
@@ -195,6 +208,16 @@ public class DriverControlsV1 extends LinearOpMode {
                         shootStatus = ShootState.RESET;
                     }
                     break;
+
+                case START_SHOOT_FAR:
+                    robot.activeTransfer.setPosition(0.85);
+                    robot.intake.set(.6);
+                    targetTicks = 0;
+                    if (shootTimer.milliseconds() > 2000) {
+                        shootStatus = ShootState.RESET;
+                    }
+                    break;
+
                 case RESET:
                     if (shootTimer.milliseconds() > 2500) {
                         shootStatus = ShootState.IDLE;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1Red.java

@@ -17,7 +17,7 @@ import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
 import org.firstinspires.ftc.teamcode.util.PersonalPID;
 
 @Config
-@TeleOp(name = "TeleOp RED - Final Optimized")
+@TeleOp(name = "TeleOp BLUE - Final Optimized")
 public class DriverControlsV1Red extends LinearOpMode {
 
     // --- DASHBOARD TUNING ---
@@ -27,14 +27,14 @@ public class DriverControlsV1Red extends LinearOpMode {
 
     public static double HOOD_MIN = 0.3;
     public static double HOOD_MAX = 1.0;
-    public static double HOOD_CORRECTION_SENSITIVITY = 0.04;
+    public static double HOOD_CORRECTION_SENSITIVITY = 0.01;
     // -------------------------
 
     // State tracking to prevent redundant hardware writes (The Fix)
     private double lastTuner = -1.0;
     private double lastPTOPosition = -1.0;
 
-    enum ShootState { IDLE, START_SHOOT, RESET }
+    enum ShootState { IDLE, START_SHOOT, RESET, START_SHOOT_FAR }
 
     //turret
     public static double p = 0.0000003, i = 0, d = 0.00001, f = 0.007;
@@ -44,6 +44,7 @@ public class DriverControlsV1Red extends LinearOpMode {
 
     public int minTicks = (int) (ticksperdegree * (-90));
     public int maxTicks = (int) (ticksperdegree * (75));
+    public int offsetCam = 3;
     PersonalPID pid;
     Limelight3A limelight;
     LLResult result;
@@ -58,7 +59,7 @@ public class DriverControlsV1Red extends LinearOpMode {
         SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
         if (PoseStorage.currentPose != null) drive.setPoseEstimate(PoseStorage.currentPose);
 
-        ShooterSubsystemRed shooterMap = new ShooterSubsystemRed();
+        ShooterSubsystem shooterMap = new ShooterSubsystem();
         ShootState shootStatus = ShootState.IDLE;
         ElapsedTime shootTimer = new ElapsedTime();
 
@@ -81,7 +82,7 @@ public class DriverControlsV1Red extends LinearOpMode {
 
         while (opModeIsActive()) {
             result = limelight.getLatestResult();
-            double vision = result.getTx();
+            double vision = result.getTx()+offsetCam;
 
             // 1. INPUTS & TURRET
 //            if (gamepad2.yWasPressed()) {
@@ -104,7 +105,7 @@ public class DriverControlsV1Red extends LinearOpMode {
 
 
             // 2. PTO OPTIMIZATION (Only write if value is new)
-            double targetPTO = 0.2;
+            double targetPTO = 0.15;
             if (targetPTO != lastPTOPosition) {
                 robot.leftPTO.setPosition(targetPTO);
                 robot.rightPTO.setPosition(targetPTO);
@@ -156,7 +157,7 @@ public class DriverControlsV1Red extends LinearOpMode {
             double currentVel = (robot.topFly.getVelocity() + robot.bottomFly.getVelocity()) / 2.0;
             double velocityError = targetVelocity - currentVel;
 
-            double hoodAdjustment = (velocityError / 80.0) * HOOD_CORRECTION_SENSITIVITY;
+            double hoodAdjustment = (velocityError / 50.0) * HOOD_CORRECTION_SENSITIVITY;
             double correctedHood = Range.clip(baseHood - hoodAdjustment, HOOD_MIN, HOOD_MAX);
 
             if (gamepad2.dpad_up) adjust = true;
@@ -167,8 +168,8 @@ public class DriverControlsV1Red extends LinearOpMode {
                 robot.topFly.setVelocity(targetVelocity);
                 robot.launchHood.setPosition(correctedHood);
             } else {
-                robot.bottomFly.setVelocity(0);
-                robot.topFly.setVelocity(0);
+                robot.bottomFly.setVelocity(1700);
+                robot.topFly.setVelocity(1700);
             }
 
             // 7. SHOOTING STATE MACHINE
@@ -178,15 +179,45 @@ public class DriverControlsV1Red extends LinearOpMode {
                 shootTimer.reset();
             }
 
+            if (gamepad2.dpadLeftWasPressed()) {
+                shooting = true;
+                shootStatus = ShootState.START_SHOOT_FAR;
+                shootTimer.reset();
+            }
+
+            if (gamepad2.yWasPressed()) {
+                flywheelTuner+=.01;
+            }
+
+            if (gamepad2.aWasPressed()) {
+                flywheelTuner-=.01;
+            }
+
+            if (gamepad2.xWasPressed()) {
+                offsetCam = 0;
+            } else if (gamepad2.bWasPressed()) {
+                offsetCam = 3;
+            }
+
             switch (shootStatus) {
                 case START_SHOOT:
-                    robot.activeTransfer.setPosition(0.81);
-                    robot.intake.set(1.0);
+                    robot.activeTransfer.setPosition(0.85);
+                    robot.intake.set(1);
                     targetTicks = 0;
                     if (shootTimer.milliseconds() > 2000) {
                         shootStatus = ShootState.RESET;
                     }
                     break;
+
+                case START_SHOOT_FAR:
+                    robot.activeTransfer.setPosition(0.85);
+                    robot.intake.set(.6);
+                    targetTicks = 0;
+                    if (shootTimer.milliseconds() > 2000) {
+                        shootStatus = ShootState.RESET;
+                    }
+                    break;
+
                 case RESET:
                     if (shootTimer.milliseconds() > 2500) {
                         shootStatus = ShootState.IDLE;
@@ -219,6 +250,7 @@ public class DriverControlsV1Red extends LinearOpMode {
             telemetry.addData("Status", "Running");
             telemetry.addData("Vel Error", (int)velocityError);
             telemetry.addData("Hood", "%.2f", correctedHood);
+            telemetry.addData("TunerMult", flywheelTuner);
             telemetry.update();
         }
     }