a

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

@@ -0,0 +1,77 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.util.Range;
+
+import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+
+@TeleOp(name="AutoShot TeleOp")
+public class AutoShotTeleOp extends LinearOpMode {
+    SampleMecanumDrive drive;
+    Hware robot;
+    ShooterSubsystem shooterMap;
+
+    @Override
+    public void runOpMode() {
+        // Initialization
+        drive = new SampleMecanumDrive(hardwareMap);
+        robot = new Hware();
+        robot.initialize(hardwareMap);
+        shooterMap = new ShooterSubsystem();
+
+        waitForStart();
+
+        while (opModeIsActive()) {
+            // 1. Update Roadrunner Localization
+            drive.update();
+            Pose2d myPose = drive.getPoseEstimate();
+
+            // 2. Get Base Targets from Interpolation Table
+            double[] targets = shooterMap.getInterpolatedShot(myPose.getX(), myPose.getY());
+            double targetVelocity = targets[0];
+            double baseHoodPos = targets[1];
+
+            // 3. Calculate Velocity Error and Hood Correction
+            // Get current speed (averaging both flywheels for accuracy)
+            double currentVel = (robot.topFly.getVelocity() + robot.bottomFly.getVelocity()) / 2.0;
+            double velocityError = targetVelocity - currentVel;
+
+            /* Logic: Decrease hood by 0.05 for every 100 ticks/sec the motor is off.
+               If velocityError is 200 (motor is too slow), adjustment is (200/100) * 0.05 = 0.10.
+               We subtract this from the base hood position.
+            */
+            double hoodAdjustment = (velocityError / 100.0) * 0.03;
+            double finalHoodPos = baseHoodPos - hoodAdjustment;
+
+            // 4. Ensure the hood stays within physical servo limits [0.0 to 1.0]
+            finalHoodPos = Range.clip(finalHoodPos, 0.0, 1.0);
+
+            // 5. Apply Power and Positions
+            robot.bottomFly.setVelocity(targetVelocity);
+            robot.topFly.setVelocity(targetVelocity);
+            robot.launchHood.setPosition(finalHoodPos);
+
+            robot.intake.set(1);
+
+            if (gamepad1.dpadUpWasPressed()) {
+                robot.activeTransfer.setPosition(.95);
+            }
+
+            if (gamepad1.dpadDownWasPressed()) {
+                robot.activeTransfer.setPosition(.7);
+            }
+
+            // Telemetry for real-time tuning
+            telemetry.addData("Robot X", myPose.getX());
+            telemetry.addData("Robot Y", myPose.getY());
+            telemetry.addData("Target Velocity", targetVelocity);
+            telemetry.addData("Actual Velocity", currentVel);
+            telemetry.addData("Base Hood", baseHoodPos);
+            telemetry.addData("Corrected Hood", finalHoodPos);
+            telemetry.update();
+        }
+    }
+}

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

@@ -1,215 +1,285 @@
 package org.firstinspires.ftc.teamcode.Auton;//package org.firstinspires.ftc.teamcode.Auton;
-//
+
-//import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.config.Config;
-//import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
-//import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.hardware.limelightvision.LLResult;
-//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.hardware.limelightvision.Limelight3A;
-//import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-//
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-//import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
+import com.qualcomm.robotcore.hardware.DcMotor;
-//import org.firstinspires.ftc.teamcode.drive.DriveConstants;
+
-//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
-//import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
+import org.firstinspires.ftc.teamcode.drive.DriveConstants;
-//import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
-//
+import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
-//@Config
+import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
-//@Autonomous(name = "Blue Depot Cycle")
+import org.firstinspires.ftc.teamcode.util.PersonalPID;
-//public class blueDepot extends LinearOpMode {
+
-//
+@Config
-//    Hware robot = new Hware();
+@Autonomous(name = "Blue Depot Cycle")
-//    public static double TICKS_PER_DEGREE = 9.738888;
+public class blueDepot extends LinearOpMode {
-//
+
-//    public void turretPower(double power) {
+    Hware robot = new Hware();
-//        robot.turret.setPower(power);
+    public static double TICKS_PER_DEGREE = 9.738888 * 0.29969;
-//    }
+
-//
+    //turret
-//    public void turretPos(int position) {
+    public static double p = 0.0000003, i = 0, d = 0.00001, f = 0.007;
-//        turretPower(0);
+    public static double target = 0;
-//        robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
-//        robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+    public static double ticksperdegree = 9.738888 * 0.29969;
-//        robot.turret.setTargetPosition(position * (int) TICKS_PER_DEGREE);
+
-//        robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+    public int minTicks = (int) (ticksperdegree * (-90));
-//        turretPower(1);
+    public int maxTicks = (int) (ticksperdegree * (75));
-//    }
+    PersonalPID pid;
-//
+    Limelight3A limelight;
-//    public void spintakeStart(){
+    LLResult result;
-//        robot.intake.set(1);
+
-//    }
+    boolean lock = true;
-//
+
-//    public void spintakeEnd(){
+    public void turretPower(double power) {
-//        robot.intake.set(0);
+        robot.turret.setPower(power);
-//    }
+    }
-//
+
-//    public void shoot(){
+    public void turretPos(int position) {
-//        robot.topFly.set(0.85);
+        turretPower(0);
-//        robot.bottomFly.set(0.85);
+        robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-//        robot.launchHood.setPosition(0.4);
+        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);
-//    public void reset(){
+        turretPower(1);
-//        robot.intake.set(0);
+    }
-////        robot.topFly.set(0);
+
-////        robot.bottomFly.set(0);
+    public void spintakeStart(){
-//        robot.launchHood.setPosition(0.35);
+        robot.intake.set(1);
-//        robot.activeTransfer.setPosition(0.8);
+    }
-//    }
+
-//
+    public void spintakeEnd(){
-//
+        robot.intake.set(0);
-//    public void runOpMode() throws InterruptedException {
+    }
-//
+
-//        //webcam1 = hardwareMap.get(WebcamName.class, "Webcam 2");
+    public void shoot(){
-//        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+        robot.topFly.setVelocity(1800);
-//        robot.initialize(hardwareMap);
+        robot.bottomFly.setVelocity(1800);
-//
+        robot.launchHood.setPosition(0.55);
-//        Pose2d startPose = new Pose2d(0, 0, Math.toRadians(0));
+    }
-//
+
-//        drive.setPoseEstimate(startPose);
+    public void reset(){
-//
+        robot.intake.set(0);
-//        TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose)
+//        robot.topFly.setVelocity(1200);
-//                .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
+//        robot.bottomFly.setVelocity(1200);
-//                    reset();
+//        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));
+
+        drive.setPoseEstimate(startPose);
+
+        TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose)
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                    reset();
 //                    turretPos(5);
+                })
+                .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);
 //                })
-//                .strafeLeft(35)
+                .waitSeconds(1.5)
-//                .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
-//                    shoot();
+                    reset();
+                })
+
+                //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);
+                })
+
+//                .UNSTABLE_addTemporalMarkerOffset(2.2, () -> {
+//                    robot.launchHood.setPosition(0.5);
 //                })
-//                .UNSTABLE_addTemporalMarkerOffset(1.5, () -> {
+                .waitSeconds(1.5)
-//                    robot.intake.set(0.9);
+
-//                })
+                //9ball
-//                .UNSTABLE_addTemporalMarkerOffset(2, () -> {
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
-//                    robot.activeTransfer.setPosition(1);
+                    reset();
-//                })
+                })
-////                .UNSTABLE_addTemporalMarkerOffset(2.2, () -> {
+                .lineToLinearHeading(new Pose2d(-19,27, Math.toRadians(90)))
-////                    robot.launchHood.setPosition(0.5);
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
-////                })
+                    spintakeStart();
-//                .waitSeconds(3)
+                })
-//                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                .forward(18, SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
-//                    reset();
+                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
-//                })
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
-//                .lineToLinearHeading(new Pose2d(12,50, Math.toRadians(-45)))
+                    spintakeEnd();
-//                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                })
-//                    spintakeStart();
+                .lineToLinearHeading(new Pose2d(-12,54, Math.toRadians(90)))
-//                })
+                .lineToLinearHeading(new Pose2d(26,14, Math.toRadians(52)))
-//                .forward(35,  SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
-//                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                    reset();
-//                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                })
-//                    spintakeEnd();
+                .UNSTABLE_addTemporalMarkerOffset(.5, () -> {
-//                })
+                    robot.intake.set(1);
-//                .lineToLinearHeading(new Pose2d(0,35, Math.toRadians(0)))
+                    robot.activeTransfer.setPosition(0.85);
-//                .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
+                })
-//                    shoot();
+                .waitSeconds(1.5)
-//                })
+
-//                .UNSTABLE_addTemporalMarkerOffset(1.5, () -> {
+                //12ball
-//                    robot.intake.set(0.9);
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
-//                })
+                    reset();
-//                .UNSTABLE_addTemporalMarkerOffset(2, () -> {
+                })
-//                    robot.activeTransfer.setPosition(1);
+                .lineToLinearHeading(new Pose2d(-40,22, Math.toRadians(90)))
-//                })
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
-////                .UNSTABLE_addTemporalMarkerOffset(2.2, () -> {
+                    spintakeStart();
-////                    robot.launchHood.setPosition(0.5);
+                })
-////                })
+                .forward(33, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
-//                .waitSeconds(3)
+                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
-//                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
-//                    reset();
+                    spintakeEnd();
-//                })
+                })
-//                .lineToLinearHeading(new Pose2d(35,60, Math.toRadians(-45)))
+                .lineToLinearHeading(new Pose2d(35,14, Math.toRadians(52)))
-//                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
-//                    spintakeStart();
+                    shoot();
-//                })
+                })
-//                .forward(35,  SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+                .UNSTABLE_addTemporalMarkerOffset(.5, () -> {
-//                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                    robot.intake.set(0.8);
-//                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                    robot.activeTransfer.setPosition(0.85);
-//                    spintakeEnd();
+                })
-//                })
+                .waitSeconds(1.5)
-//                .lineToLinearHeading(new Pose2d(0,35, Math.toRadians(0)))
+                  //     .lineToLinearHeading(new Pose2d(55.5,68, Math.toRadians(-45)))
-//                .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
+
-//                    shoot();
-//                })
-//                .UNSTABLE_addTemporalMarkerOffset(1.5, () -> {
-//                    robot.intake.set(0.9);
-//                })
-//                .UNSTABLE_addTemporalMarkerOffset(2, () -> {
-//                    robot.activeTransfer.setPosition(1);
-//                })
-////                .UNSTABLE_addTemporalMarkerOffset(2.2, () -> {
-////                    robot.launchHood.setPosition(0.5);
-////                })
-//                .waitSeconds(3)
 //                .UNSTABLE_addTemporalMarkerOffset(0.5, () -> {
 //                    turretPos(84);
 //                })
-//                .lineToLinearHeading(new Pose2d(35,60, Math.toRadians(-45)))
+                .waitSeconds(1)
-//                //     .lineToLinearHeading(new Pose2d(55.5,68, Math.toRadians(-45)))
+                .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();
-//
+
-////        TrajectorySequence oneCycle = drive.trajectorySequenceBuilder(startPose)
+        waitForStart();
-////                .strafeLeft(25)
+        if (opModeIsActive() && !isStopRequested()) {
-////                .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
+//          while (opModeIsActive()) {
-////                    shoot();
+            drive.followTrajectorySequenceAsync(threeBall);
-////                })
+            PoseStorage.currentPose = drive.getPoseEstimate();
-////                .waitSeconds(1)
+
-////                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+            while(opModeIsActive()){
-////                    reset();
+                robot.leftPTO.setPosition(0.2);
-////                })
+                robot.rightPTO.setPosition(0.14);
-////                .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
+                drive.update();
-////                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                result = limelight.getLatestResult();
-////                    spintakeStart();
+                double vision = result.getTx() - 3;
-////                })
+
-////                .forward(30,  SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+                double target = ticksperdegree * vision;
-////                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                double targetTicks = pid.calculate(robot.turret.getCurrentPosition(), target);
-////                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                pid.setPIDF(p, i, d, f);
-////                    spintakeEnd();
+
-////                })
+                if((robot.turret.getCurrentPosition() > maxTicks) && (targetTicks > 0)){
-////                .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
+                    targetTicks = targetTicks;
-////                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                } else if((robot.turret.getCurrentPosition() < maxTicks) && (targetTicks < 0)){
-////                    shoot();
+                    targetTicks = targetTicks;
-////                })
+
-////                .waitSeconds(1)
+                } else if((robot.turret.getCurrentPosition() > maxTicks) || (robot.turret.getCurrentPosition() < minTicks) ){
-////                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                    targetTicks = 0;
-////                    reset();
+                }
-////                })
+                PoseStorage.currentPose = drive.getPoseEstimate();
-////                .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
+                 robot.turret.setPower(- targetTicks);
-////                .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.followTrajectorySequence(threeBall);
-//            PoseStorage.currentPose = drive.getPoseEstimate();
-////            }
 //            }
-//    }
+        }
-//}
+    }
+}

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

@@ -1,80 +0,0 @@
-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.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.SampleMecanumDrive;
-//import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
-//import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
-//
-//@Config
-//@Autonomous(name = "Blue Depot Far NO USE")
-//public class blueDepotFar extends LinearOpMode {
-//
-//    Hware robot = new Hware();
-//    public static double TICKS_PER_DEGREE = 9.738888;
-//
-//    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);
-//        turretPower(1);
-//    }
-//
-//    public void spintakeStart(){
-//        robot.intake.set(1);
-//    }
-//
-//    public void spintakeEnd(){
-//        robot.intake.set(0);
-//    }
-//
-//    public void shoot(){
-//        robot.topFly.set(1);
-//        robot.bottomFly.set(1);
-//        robot.launchHood.setPosition(0.58);
-//    }
-//
-//    public void reset(){
-//        robot.intake.set(0);
-////        robot.topFly.set(0);
-////        robot.bottomFly.set(0);
-//        robot.launchHood.setPosition(0.35);
-//        robot.activeTransfer.setPosition(0.8);
-//    }
-//
-//
-//    public void runOpMode() throws InterruptedException {
-//
-//        //webcam1 = hardwareMap.get(WebcamName.class, "Webcam 2");
-//        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
-//        robot.initialize(hardwareMap);
-//
-//        Pose2d startPose = new Pose2d(0, 0, Math.toRadians(0));
-//
-//        drive.setPoseEstimate(startPose);
-//
-//        TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose)
-//                .waitSeconds(3)
-//                .forward(5)
-//                .build();
-//
-//        waitForStart();
-//        if (opModeIsActive() && !isStopRequested()) {
-////            while (opModeIsActive()) {
-//            drive.followTrajectorySequence(threeBall);
-//            PoseStorage.currentPose = drive.getPoseEstimate();
-////            }
-//        }
-//    }
-//}

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

@@ -0,0 +1,166 @@
+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 = "Blue Far Cycle")
+public class blueFar extends LinearOpMode {
+
+    Hware robot = new Hware();
+    public static double TICKS_PER_DEGREE = 9.738888 * 0.29969;
+
+    //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);
+        turretPower(1);
+    }
+
+    public void spintakeStart(){
+        robot.intake.set(1);
+    }
+
+    public void spintakeEnd(){
+        robot.intake.set(0);
+    }
+
+    public void shoot(){
+        robot.topFly.setVelocity(2250);
+        robot.bottomFly.setVelocity(2250);
+        robot.launchHood.setPosition(0.65);
+    }
+
+    public void reset(){
+        robot.intake.set(0);
+        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(-63, 16, Math.toRadians(0));
+
+        drive.setPoseEstimate(startPose);
+
+        TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose)
+
+                //first shot
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                    reset();
+                })
+                .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
+                    shoot();
+                })
+                .forward(5)
+                .turn(Math.toRadians(15))
+                .waitSeconds(4)
+                .UNSTABLE_addTemporalMarkerOffset(0.3, () -> {
+                    robot.intake.set(0.6);
+                    robot.activeTransfer.setPosition(0.6);
+                })
+                .waitSeconds(1.5)
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                    reset();
+                })
+
+                //intaking more
+                .lineToLinearHeading(new Pose2d(-64,53, Math.toRadians(90)))
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                    spintakeStart();
+                })
+                .forward(5, SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                .back(5, SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                    spintakeEnd();
+                    shoot();
+                })
+
+                //moving to shoot
+                .lineToLinearHeading(new Pose2d(-58, 16, Math.toRadians(0)))
+                //shoot position
+                .UNSTABLE_addTemporalMarkerOffset(.5, () -> {
+                    robot.intake.set(0.52);
+                    robot.activeTransfer.setPosition(0.5);
+                })
+                .waitSeconds(1.5)
+                .strafeLeft(20)
+                .waitSeconds(1)
+                .build();
+
+        waitForStart();
+        if (opModeIsActive() && !isStopRequested()) {
+//          while (opModeIsActive()) {
+            drive.followTrajectorySequenceAsync(threeBall);
+            PoseStorage.currentPose = drive.getPoseEstimate();
+
+            while(opModeIsActive()){
+                robot.leftPTO.setPosition(0.15);
+                robot.rightPTO.setPosition(0.15);
+                drive.update();
+                result = limelight.getLatestResult();
+                double vision = result.getTx();
+
+                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/Auton/redDepot.java

@@ -1,215 +1,285 @@
 package org.firstinspires.ftc.teamcode.Auton;//package org.firstinspires.ftc.teamcode.Auton;
-//
+
-//import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.config.Config;
-//import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
-//import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.hardware.limelightvision.LLResult;
-//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.hardware.limelightvision.Limelight3A;
-//import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-//
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-//import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
+import com.qualcomm.robotcore.hardware.DcMotor;
-//import org.firstinspires.ftc.teamcode.drive.DriveConstants;
+
-//import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
-//import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
+import org.firstinspires.ftc.teamcode.drive.DriveConstants;
-//import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
-//
+import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
-//@Config
+import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
-//@Autonomous(name = "Red Depot Cycle")
+import org.firstinspires.ftc.teamcode.util.PersonalPID;
-//public class redDepot extends LinearOpMode {
+
-//
+@Config
-//    Hware robot = new Hware();
+@Autonomous(name = "Blue Depot Cycle")
-//    public static double TICKS_PER_DEGREE = 9.738888;
+public class redDepot extends LinearOpMode {
-//
+
-//    public void turretPower(double power) {
+    Hware robot = new Hware();
-//        robot.turret.setPower(power);
+    public static double TICKS_PER_DEGREE = 9.738888 * 0.29969;
-//    }
+
-//
+    //turret
-//    public void turretPos(int position) {
+    public static double p = 0.0000003, i = 0, d = 0.00001, f = 0.007;
-//        turretPower(0);
+    public static double target = 0;
-//        robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
-//        robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+    public static double ticksperdegree = 9.738888 * 0.29969;
-//        robot.turret.setTargetPosition(position * (int) TICKS_PER_DEGREE);
+
-//        robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+    public int minTicks = (int) (ticksperdegree * (-90));
-//        turretPower(1);
+    public int maxTicks = (int) (ticksperdegree * (75));
-//    }
+    PersonalPID pid;
-//
+    Limelight3A limelight;
-//    public void spintakeStart(){
+    LLResult result;
-//        robot.intake.set(1);
+
-//    }
+    boolean lock = true;
-//
+
-//    public void spintakeEnd(){
+    public void turretPower(double power) {
-//        robot.intake.set(0);
+        robot.turret.setPower(power);
-//    }
+    }
-//
+
-//    public void shoot(){
+    public void turretPos(int position) {
-//        robot.topFly.set(0.85);
+        turretPower(0);
-//        robot.bottomFly.set(0.85);
+        robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-//        robot.launchHood.setPosition(0.4);
+        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);
-//    public void reset(){
+        turretPower(1);
-//        robot.intake.set(0);
+    }
-////        robot.topFly.set(0);
+
-////        robot.bottomFly.set(0);
+    public void spintakeStart(){
-//        robot.launchHood.setPosition(0.35);
+        robot.intake.set(1);
-//        robot.activeTransfer.setPosition(0.8);
+    }
-//    }
+
-//
+    public void spintakeEnd(){
-//
+        robot.intake.set(0);
-//    public void runOpMode() throws InterruptedException {
+    }
-//
+
-//        //webcam1 = hardwareMap.get(WebcamName.class, "Webcam 2");
+    public void shoot(){
-//        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+        robot.topFly.setVelocity(1800);
-//        robot.initialize(hardwareMap);
+        robot.bottomFly.setVelocity(1800);
-//
+        robot.launchHood.setPosition(0.55);
-//        Pose2d startPose = new Pose2d(0, 0, Math.toRadians(0));
+    }
-//
+
-//        drive.setPoseEstimate(startPose);
+    public void reset(){
-//
+        robot.intake.set(0);
-//        TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose)
+//        robot.topFly.setVelocity(1200);
-//                .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
+//        robot.bottomFly.setVelocity(1200);
-//                    reset();
+//        robot.topFly.set(0);
-//                    turretPos(-10);
+//        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));
+
+        drive.setPoseEstimate(startPose);
+
+        TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose)
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                    reset();
+//                    turretPos(5);
+                })
+                .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);
 //                })
-//                .strafeRight(35)
+                .waitSeconds(1.5)
-//                .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
-//                    shoot();
+                    reset();
+                })
+
+                //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);
+                })
+
+//                .UNSTABLE_addTemporalMarkerOffset(2.2, () -> {
+//                    robot.launchHood.setPosition(0.5);
 //                })
-//                .UNSTABLE_addTemporalMarkerOffset(1.5, () -> {
+                .waitSeconds(1.5)
-//                    robot.intake.set(0.9);
+
-//                })
+                //9ball
-//                .UNSTABLE_addTemporalMarkerOffset(2, () -> {
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
-//                    robot.activeTransfer.setPosition(1);
+                    reset();
-//                })
+                })
-////                .UNSTABLE_addTemporalMarkerOffset(2.2, () -> {
+                .lineToLinearHeading(new Pose2d(-19,-27, Math.toRadians(-90)))
-////                    robot.launchHood.setPosition(0.5);
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
-////                })
+                    spintakeStart();
-//                .waitSeconds(3)
+                })
-//                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                .forward(18, SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
-//                    reset();
+                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
-//                })
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
-//                .lineToLinearHeading(new Pose2d(12,-50, Math.toRadians(45)))
+                    spintakeEnd();
-//                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                })
-//                    spintakeStart();
+                .lineToLinearHeading(new Pose2d(-12,-54, Math.toRadians(-90)))
-//                })
+                .lineToLinearHeading(new Pose2d(26,-14, Math.toRadians(-52)))
-//                .forward(35,  SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
-//                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                    reset();
-//                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                })
-//                    spintakeEnd();
+                .UNSTABLE_addTemporalMarkerOffset(.5, () -> {
-//                })
+                    robot.intake.set(1);
-//                .lineToLinearHeading(new Pose2d(0,-35, Math.toRadians(0)))
+                    robot.activeTransfer.setPosition(0.85);
-//                .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
+                })
-//                    shoot();
+                .waitSeconds(1.5)
-//                })
+
-//                .UNSTABLE_addTemporalMarkerOffset(1.5, () -> {
+                //12ball
-//                    robot.intake.set(0.9);
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
-//                })
+                    reset();
-//                .UNSTABLE_addTemporalMarkerOffset(2, () -> {
+                })
-//                    robot.activeTransfer.setPosition(1);
+                .lineToLinearHeading(new Pose2d(-40,-22, Math.toRadians(-90)))
-//                })
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
-////                .UNSTABLE_addTemporalMarkerOffset(2.2, () -> {
+                    spintakeStart();
-////                    robot.launchHood.setPosition(0.5);
+                })
-////                })
+                .forward(33, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
-//                .waitSeconds(3)
+                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
-//                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
-//                    reset();
+                    spintakeEnd();
-//                })
+                })
-//                .lineToLinearHeading(new Pose2d(35,-60, Math.toRadians(45)))
+                .lineToLinearHeading(new Pose2d(35,-14, Math.toRadians(-52)))
-//                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
-//                    spintakeStart();
+                    shoot();
-//                })
+                })
-//                .forward(35,  SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+                .UNSTABLE_addTemporalMarkerOffset(.5, () -> {
-//                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                    robot.intake.set(0.8);
-//                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                    robot.activeTransfer.setPosition(0.85);
-//                    spintakeEnd();
+                })
-//                })
+                .waitSeconds(1.5)
-//                .lineToLinearHeading(new Pose2d(0,-35, Math.toRadians(0)))
+                //     .lineToLinearHeading(new Pose2d(55.5,68, Math.toRadians(-45)))
-//                .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
+
-//                    shoot();
-//                })
-//                .UNSTABLE_addTemporalMarkerOffset(1.5, () -> {
-//                    robot.intake.set(0.9);
-//                })
-//                .UNSTABLE_addTemporalMarkerOffset(2, () -> {
-//                    robot.activeTransfer.setPosition(1);
-//                })
-////                .UNSTABLE_addTemporalMarkerOffset(2.2, () -> {
-////                    robot.launchHood.setPosition(0.5);
-////                })
-//                .waitSeconds(3)
 //                .UNSTABLE_addTemporalMarkerOffset(0.5, () -> {
-//                    turretPos(-84);
+//                    turretPos(84);
 //                })
-//                .lineToLinearHeading(new Pose2d(35,-60, Math.toRadians(45)))
+                .waitSeconds(1)
-//                //     .lineToLinearHeading(new Pose2d(55.5,68, Math.toRadians(-45)))
+                .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();
-//
+
-////        TrajectorySequence oneCycle = drive.trajectorySequenceBuilder(startPose)
+        waitForStart();
-////                .strafeLeft(25)
+        if (opModeIsActive() && !isStopRequested()) {
-////                .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
+//          while (opModeIsActive()) {
-////                    shoot();
+            drive.followTrajectorySequenceAsync(threeBall);
-////                })
+            PoseStorage.currentPose = drive.getPoseEstimate();
-////                .waitSeconds(1)
+
-////                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+            while(opModeIsActive()){
-////                    reset();
+                robot.leftPTO.setPosition(0.2);
-////                })
+                robot.rightPTO.setPosition(0.14);
-////                .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
+                drive.update();
-////                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                result = limelight.getLatestResult();
-////                    spintakeStart();
+                double vision = result.getTx() + 3;
-////                })
+
-////                .forward(30,  SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+                double target = ticksperdegree * vision;
-////                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+                double targetTicks = pid.calculate(robot.turret.getCurrentPosition(), target);
-////                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                pid.setPIDF(p, i, d, f);
-////                    spintakeEnd();
+
-////                })
+                if((robot.turret.getCurrentPosition() > maxTicks) && (targetTicks > 0)){
-////                .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
+                    targetTicks = targetTicks;
-////                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                } else if((robot.turret.getCurrentPosition() < maxTicks) && (targetTicks < 0)){
-////                    shoot();
+                    targetTicks = targetTicks;
-////                })
+
-////                .waitSeconds(1)
+                } else if((robot.turret.getCurrentPosition() > maxTicks) || (robot.turret.getCurrentPosition() < minTicks) ){
-////                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                    targetTicks = 0;
-////                    reset();
+                }
-////                })
+                PoseStorage.currentPose = drive.getPoseEstimate();
-////                .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
+                robot.turret.setPower(- targetTicks);
-////                .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.followTrajectorySequence(threeBall);
-//            PoseStorage.currentPose = drive.getPoseEstimate();
-////            }
 //            }
-//    }
+        }
-//}
+    }
+}

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

@@ -0,0 +1,257 @@
+package org.firstinspires.ftc.teamcode;
+
+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.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+import com.qualcomm.robotcore.util.ElapsedTime;
+import com.qualcomm.robotcore.util.Range;
+
+import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
+import org.firstinspires.ftc.teamcode.util.PersonalPID;
+
+@Config
+@TeleOp(name = "TeleOp BLUE - Final Optimized")
+public class DriverControlsV1Blue extends LinearOpMode {
+
+    // --- DASHBOARD TUNING ---
+    public static double flywheelTuner = 1.0;
+    private final double P_BASE = 1.09;
+    private final double F_BASE = 14.12;
+
+    public static double HOOD_MIN = 0.3;
+    public static double HOOD_MAX = 1.0;
+    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, START_SHOOT_FAR }
+
+    //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));
+    public int offsetCam = 3;
+    PersonalPID pid;
+    Limelight3A limelight;
+    LLResult result;
+
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        Hware robot = new Hware();
+        robot.initialize(hardwareMap);
+        limelight = hardwareMap.get(Limelight3A.class, "limelight");
+
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+        if (PoseStorage.currentPose != null) drive.setPoseEstimate(PoseStorage.currentPose);
+
+        ShooterSubsystem shooterMap = new ShooterSubsystem();
+        ShootState shootStatus = ShootState.IDLE;
+        ElapsedTime shootTimer = new ElapsedTime();
+
+        boolean adjust = false;
+        boolean shooting = false;
+
+        // Initial Hardware Setup
+//        robot.turret.setTargetPosition(8);
+//        robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+//        robot.turret.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+//        robot.turret.setPower(0.2);
+        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);
+
+        waitForStart();
+
+        while (opModeIsActive()) {
+             result = limelight.getLatestResult();
+             double vision = result.getTx()-offsetCam;
+
+            // 1. INPUTS & TURRET
+//            if (gamepad2.yWasPressed()) {
+//                robot.turret.setTargetPosition(8);
+//            } else if (gamepad2.xWasPressed()) {
+//                robot.turret.setTargetPosition(88);
+//            } else if (gamepad2.bWasPressed()) {
+//                robot.turret.setTargetPosition(-88);
+//            }
+            double target = ticksperdegree * vision;
+            double targetTicks = pid.calculate(robot.turret.getCurrentPosition(), target);
+            pid.setPIDF(p, i, d, f);
+
+            //double targetTicks =  turretLock.update(pose, result, vision, robot.turret.getCurrentPosition());
+//            robot.turret.setTargetPosition((int) (robot.turret.getCurrentPosition() - targetTicks));
+//            robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+
+            telemetry.addData("PID power", - targetTicks);
+            telemetry.addData("Angle", result.getTx());
+
+
+            // 2. PTO OPTIMIZATION (Only write if value is new)
+            double targetPTO = 0.15;
+            if (targetPTO != lastPTOPosition) {
+                robot.leftPTO.setPosition(targetPTO);
+                robot.rightPTO.setPosition(targetPTO);
+                lastPTOPosition = targetPTO;
+            }
+
+            // 3. FLYWHEEL PIDF OPTIMIZATION (Crucial for preventing lag/DC)
+            if (flywheelTuner != lastTuner) {
+                PIDFCoefficients dynamicPIDF = new PIDFCoefficients(
+                        P_BASE * flywheelTuner, 0, 0, F_BASE * flywheelTuner
+                );
+                robot.bottomFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, dynamicPIDF);
+                robot.topFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, dynamicPIDF);
+                lastTuner = flywheelTuner;
+            }
+
+            // 4. DRIVETRAIN
+            drive.update();
+            Pose2d myPose = drive.getPoseEstimate();
+            double speedControl = gamepad1.right_bumper ? 0.5 : 1.0;
+
+            drive.setWeightedDrivePower(new Pose2d(
+                    -gamepad1.left_stick_y * speedControl,
+                    -gamepad1.left_stick_x * speedControl,
+                    -gamepad1.right_stick_x * speedControl
+            ));
+
+            // 5. INTAKE / TRANSFER
+            if (!shooting) {
+                if (gamepad1.right_trigger > 0.3) {
+                    robot.intake.set(1.0);
+                    robot.activeTransfer.setPosition(1.0);
+                } else if (gamepad1.left_trigger > 0.5) {
+                    robot.intake.set(-1.0);
+                    robot.activeTransfer.setPosition(1.0);
+                } else {
+                    robot.intake.set(0);
+                    robot.activeTransfer.setPosition(1.0);
+                }
+            }
+
+            // 6. SHOOTER & HOOD LOGIC
+            double[] targets = shooterMap.getInterpolatedShot(myPose.getX(), myPose.getY());
+            double targetVelocity = targets[0];
+            double baseHood = targets[1];
+
+//            targetVelocity = targetVelocity + 40 ;
+
+            double currentVel = (robot.topFly.getVelocity() + robot.bottomFly.getVelocity()) / 2.0;
+            double velocityError = targetVelocity - currentVel;
+
+            double hoodAdjustment = (velocityError / 50.0) * HOOD_CORRECTION_SENSITIVITY;
+            double correctedHood = Range.clip(baseHood - hoodAdjustment, HOOD_MIN, HOOD_MAX);
+
+            if (gamepad2.dpad_up) adjust = true;
+            else if (gamepad2.dpad_down) adjust = false;
+
+            if (adjust || shooting) {
+                robot.bottomFly.setVelocity(targetVelocity);
+                robot.topFly.setVelocity(targetVelocity);
+                robot.launchHood.setPosition(correctedHood);
+            } else {
+                robot.bottomFly.setVelocity(1700);
+                robot.topFly.setVelocity(1700);
+            }
+
+            // 7. SHOOTING STATE MACHINE
+            if (gamepad2.dpadRightWasPressed()) {
+                shooting = true;
+                shootStatus = ShootState.START_SHOOT;
+                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.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;
+                        shooting = false;
+                    }
+                    break;
+                case IDLE:
+                    // Do nothing
+                    break;
+            }
+
+            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;
+            }
+            robot.turret.setPower(- targetTicks);
+
+
+            if (gamepad2.dpadLeftWasPressed()) {
+                shooting = false;
+                shootStatus = ShootState.IDLE;
+                robot.activeTransfer.setPosition(1.0);
+            }
+
+            // 8. TELEMETRY (Keep it clean to save bandwidth)
+            telemetry.addData("Status", "Running");
+            telemetry.addData("Vel Error", (int)velocityError);
+            telemetry.addData("Hood", "%.2f", correctedHood);
+            telemetry.addData("TunerMult", flywheelTuner);
+            telemetry.update();
+        }
+    }
+}

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

@@ -0,0 +1,257 @@
+package org.firstinspires.ftc.teamcode;
+
+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.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+import com.qualcomm.robotcore.util.ElapsedTime;
+import com.qualcomm.robotcore.util.Range;
+
+import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
+import org.firstinspires.ftc.teamcode.util.PersonalPID;
+
+@Config
+@TeleOp(name = "TeleOp BLUE - Final Optimized")
+public class DriverControlsV1Red extends LinearOpMode {
+
+    // --- DASHBOARD TUNING ---
+    public static double flywheelTuner = 1.0;
+    private final double P_BASE = 1.09;
+    private final double F_BASE = 14.12;
+
+    public static double HOOD_MIN = 0.3;
+    public static double HOOD_MAX = 1.0;
+    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, START_SHOOT_FAR }
+
+    //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));
+    public int offsetCam = 3;
+    PersonalPID pid;
+    Limelight3A limelight;
+    LLResult result;
+
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        Hware robot = new Hware();
+        robot.initialize(hardwareMap);
+        limelight = hardwareMap.get(Limelight3A.class, "limelight");
+
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+        if (PoseStorage.currentPose != null) drive.setPoseEstimate(PoseStorage.currentPose);
+
+        ShooterSubsystem shooterMap = new ShooterSubsystem();
+        ShootState shootStatus = ShootState.IDLE;
+        ElapsedTime shootTimer = new ElapsedTime();
+
+        boolean adjust = false;
+        boolean shooting = false;
+
+        // Initial Hardware Setup
+//        robot.turret.setTargetPosition(8);
+//        robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+//        robot.turret.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+//        robot.turret.setPower(0.2);
+        pid = new PersonalPID(p, i, d, f);
+        limelight.pipelineSwitch(2);
+        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);
+
+        waitForStart();
+
+        while (opModeIsActive()) {
+            result = limelight.getLatestResult();
+            double vision = result.getTx()+offsetCam;
+
+            // 1. INPUTS & TURRET
+//            if (gamepad2.yWasPressed()) {
+//                robot.turret.setTargetPosition(8);
+//            } else if (gamepad2.xWasPressed()) {
+//                robot.turret.setTargetPosition(88);
+//            } else if (gamepad2.bWasPressed()) {
+//                robot.turret.setTargetPosition(-88);
+//            }
+            double target = ticksperdegree * vision;
+            double targetTicks = pid.calculate(robot.turret.getCurrentPosition(), target);
+            pid.setPIDF(p, i, d, f);
+
+            //double targetTicks =  turretLock.update(pose, result, vision, robot.turret.getCurrentPosition());
+//            robot.turret.setTargetPosition((int) (robot.turret.getCurrentPosition() - targetTicks));
+//            robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+
+            telemetry.addData("PID power", - targetTicks);
+            telemetry.addData("Angle", result.getTx());
+
+
+            // 2. PTO OPTIMIZATION (Only write if value is new)
+            double targetPTO = 0.15;
+            if (targetPTO != lastPTOPosition) {
+                robot.leftPTO.setPosition(targetPTO);
+                robot.rightPTO.setPosition(targetPTO);
+                lastPTOPosition = targetPTO;
+            }
+
+            // 3. FLYWHEEL PIDF OPTIMIZATION (Crucial for preventing lag/DC)
+            if (flywheelTuner != lastTuner) {
+                PIDFCoefficients dynamicPIDF = new PIDFCoefficients(
+                        P_BASE * flywheelTuner, 0, 0, F_BASE * flywheelTuner
+                );
+                robot.bottomFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, dynamicPIDF);
+                robot.topFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, dynamicPIDF);
+                lastTuner = flywheelTuner;
+            }
+
+            // 4. DRIVETRAIN
+            drive.update();
+            Pose2d myPose = drive.getPoseEstimate();
+            double speedControl = gamepad1.right_bumper ? 0.5 : 1.0;
+
+            drive.setWeightedDrivePower(new Pose2d(
+                    -gamepad1.left_stick_y * speedControl,
+                    -gamepad1.left_stick_x * speedControl,
+                    -gamepad1.right_stick_x * speedControl
+            ));
+
+            // 5. INTAKE / TRANSFER
+            if (!shooting) {
+                if (gamepad1.right_trigger > 0.3) {
+                    robot.intake.set(1.0);
+                    robot.activeTransfer.setPosition(1.0);
+                } else if (gamepad1.left_trigger > 0.5) {
+                    robot.intake.set(-1.0);
+                    robot.activeTransfer.setPosition(1.0);
+                } else {
+                    robot.intake.set(0);
+                    robot.activeTransfer.setPosition(1.0);
+                }
+            }
+
+            // 6. SHOOTER & HOOD LOGIC
+            double[] targets = shooterMap.getInterpolatedShot(myPose.getX(), myPose.getY());
+            double targetVelocity = targets[0];
+            double baseHood = targets[1];
+
+//            targetVelocity = targetVelocity + 40 ;
+
+            double currentVel = (robot.topFly.getVelocity() + robot.bottomFly.getVelocity()) / 2.0;
+            double velocityError = targetVelocity - currentVel;
+
+            double hoodAdjustment = (velocityError / 50.0) * HOOD_CORRECTION_SENSITIVITY;
+            double correctedHood = Range.clip(baseHood - hoodAdjustment, HOOD_MIN, HOOD_MAX);
+
+            if (gamepad2.dpad_up) adjust = true;
+            else if (gamepad2.dpad_down) adjust = false;
+
+            if (adjust || shooting) {
+                robot.bottomFly.setVelocity(targetVelocity);
+                robot.topFly.setVelocity(targetVelocity);
+                robot.launchHood.setPosition(correctedHood);
+            } else {
+                robot.bottomFly.setVelocity(1700);
+                robot.topFly.setVelocity(1700);
+            }
+
+            // 7. SHOOTING STATE MACHINE
+            if (gamepad2.dpadRightWasPressed()) {
+                shooting = true;
+                shootStatus = ShootState.START_SHOOT;
+                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.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;
+                        shooting = false;
+                    }
+                    break;
+                case IDLE:
+                    // Do nothing
+                    break;
+            }
+
+            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;
+            }
+            robot.turret.setPower(- targetTicks);
+
+
+            if (gamepad2.dpadLeftWasPressed()) {
+                shooting = false;
+                shootStatus = ShootState.IDLE;
+                robot.activeTransfer.setPosition(1.0);
+            }
+
+            // 8. TELEMETRY (Keep it clean to save bandwidth)
+            telemetry.addData("Status", "Running");
+            telemetry.addData("Vel Error", (int)velocityError);
+            telemetry.addData("Hood", "%.2f", correctedHood);
+            telemetry.addData("TunerMult", flywheelTuner);
+            telemetry.update();
+        }
+    }
+}

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

@@ -0,0 +1,166 @@
+package org.firstinspires.ftc.teamcode;
+
+import static org.firstinspires.ftc.teamcode.TeleOpTurretLock.MANUAL_TRIM_DEG_PER_LOOP;
+
+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.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
+
+@Config
+@TeleOp (name = "TeleOp V2")
+public class DriverControlsV2 extends LinearOpMode {
+    enum ShootState { IDLE, START_SHOOT, LOWER_HOOD, RESET }
+    Limelight3A limelight;
+    LLResult result = null;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        Hware robot = new Hware();
+        robot.initialize(hardwareMap);
+
+
+        double SPEEDCONTROL = 1;
+
+        Boolean adjust = false;
+        Boolean shooting = false;
+
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+        drive.setPoseEstimate(PoseStorage.currentPose);
+        drive.getLocalizer().setPoseEstimate(PoseStorage.currentPose);
+
+        Pose2d driveDirection;
+
+        ShooterSubsystem shooterMap = new ShooterSubsystem();
+
+        ShootState shootStatus = ShootState.IDLE;
+        ElapsedTime shootTimer = new ElapsedTime();
+
+        PIDFCoefficients pidf = new PIDFCoefficients(3, 0, 0, 14);
+        waitForStart();
+
+
+
+        while (opModeIsActive()) {
+            robot.leftPTO.setPosition(0.2);
+            robot.rightPTO.setPosition(0.2);
+
+            Pose2d myPose = drive.getPoseEstimate();
+
+            if (gamepad1.right_bumper) {
+                SPEEDCONTROL= 0.5;
+            }
+            if (!shooting) {
+                if (gamepad1.right_trigger > 0.3) {
+                    robot.intake.set(0.8);
+                    robot.activeTransfer.setPosition(0.7);
+                } else if (gamepad1.left_trigger > 0.5) {
+                    robot.intake.set(-0.8);
+                    robot.activeTransfer.setPosition(0.7);
+                } else {
+                    robot.intake.set(0);
+                    robot.activeTransfer.setPosition(0.7);
+                }
+            }
+
+            driveDirection = new Pose2d(
+                    -gamepad1.left_stick_y * SPEEDCONTROL,
+                    -gamepad1.left_stick_x * SPEEDCONTROL,
+                    -gamepad1.right_stick_x * SPEEDCONTROL
+            );
+
+            // Arm
+
+
+            // 2. Get Targets based on current X, Y
+            double[] targets = shooterMap.getInterpolatedShot(myPose.getX(), myPose.getY());
+            double targetVelocity = targets[0];
+            double targetHood = targets[1];
+
+
+            if (shooting) {
+                robot.launchHood.setPosition(targetHood);
+            }
+
+            if(gamepad2.dpadUpWasPressed()) {
+                adjust = true;
+            } else if (gamepad2.dpadDownWasPressed()){
+                adjust = false;
+            }
+
+            if (adjust) {
+                robot.bottomFly.setVelocity(targetVelocity);
+                robot.topFly.setVelocity(targetVelocity);
+            } else {
+                robot.bottomFly.setVelocity(0);
+                robot.topFly.setVelocity(0);
+            }
+
+
+            // 1. TRIGGER: Start the sequence
+            if (gamepad2.xWasPressed()) {
+                shooting = true;
+                shootStatus = ShootState.START_SHOOT;
+                shootTimer.reset();
+            }
+
+            switch (shootStatus) {
+                case START_SHOOT:
+                    // Initial actions
+                    robot.activeTransfer.setPosition(0.95);
+                    robot.intake.set(0.55);
+                    robot.launchHood.setPosition(targetHood);
+
+                    // Wait 250ms for the shot to physically fire before moving the hood
+                    if (shootTimer.milliseconds() > 100) {
+                        shootStatus = ShootState.LOWER_HOOD;
+                    }
+                    break;
+
+                case LOWER_HOOD:
+                    targetHood = Math.max(0.34, targetHood - 0.2);
+                    targetVelocity = targetVelocity + 100;
+                    robot.launchHood.setPosition(targetHood);
+                    robot.bottomFly.setVelocity(targetVelocity);
+                    robot.topFly.setVelocity(targetVelocity);
+                    shootStatus = ShootState.RESET;
+                    break;
+
+                case RESET:
+                    if (shootTimer.milliseconds() > 3000) {
+                        shootStatus = ShootState.IDLE;
+                        shooting = false;
+                    }
+                    break;
+            }
+
+            if (gamepad2.yWasPressed()) {
+                shooting = false;
+                robot.activeTransfer.setPosition(.7);
+            }
+
+            if (gamepad2.right_trigger > 0.5) {
+                robot.turret.setPower(.05);
+            } else if (gamepad2.left_trigger > 0.5) {
+                robot.turret.setPower(.05);
+            } else {
+                robot.turret.setPower(0);
+            }
+
+            drive.setWeightedDrivePower(driveDirection);
+            drive.update();
+
+            telemetry.update();
+        }
+
+    }
+}

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

@@ -16,7 +16,7 @@ public class FlywheelTuner extends LinearOpMode {
         robot.initialize(hardwareMap);
 
         // ticks/sec targets (NOT rpm)
-        double highVelocity = 1500;
+        double highVelocity = 1700;
         double lowVelocity  = -900;
         double curTargetVelocity = highVelocity;
 
@@ -52,7 +52,8 @@ public class FlywheelTuner extends LinearOpMode {
             // keep P/F non-negative (usually)
             if (P < 0) P = 0;
             if (F < 0) F = 0;
-
+// P - 1.09
+            // F - 14.12
             // ---- apply PIDF LIVE ----
             PIDFCoefficients pidf = new PIDFCoefficients(P, 0, 0, F);
             robot.bottomFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf);

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/AutoLock.java

@@ -1,337 +1,337 @@
-package org.firstinspires.ftc.teamcode.OUTDATED;
+//package org.firstinspires.ftc.teamcode.OUTDATED;
-
+//
-import com.acmerobotics.dashboard.config.Config;
+//import com.acmerobotics.dashboard.config.Config;
-import com.arcrobotics.ftclib.drivebase.MecanumDrive;
+//import com.arcrobotics.ftclib.drivebase.MecanumDrive;
-import com.arcrobotics.ftclib.gamepad.GamepadEx;
+//import com.arcrobotics.ftclib.gamepad.GamepadEx;
-import com.arcrobotics.ftclib.gamepad.GamepadKeys;
+//import com.arcrobotics.ftclib.gamepad.GamepadKeys;
-import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+//import com.qualcomm.robotcore.eventloop.opmode.OpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+//import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import com.qualcomm.robotcore.hardware.DcMotor;
+//import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.DcMotorEx;
+//import com.qualcomm.robotcore.hardware.DcMotorEx;
-import com.qualcomm.robotcore.util.ElapsedTime;
+//import com.qualcomm.robotcore.util.ElapsedTime;
-
+//
-import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+//import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
-import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+//import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
-import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+//import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
-import org.firstinspires.ftc.robotcore.external.navigation.Position;
+//import org.firstinspires.ftc.robotcore.external.navigation.Position;
-import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+//import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
-import org.firstinspires.ftc.vision.VisionPortal;
+//import org.firstinspires.ftc.vision.VisionPortal;
-import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
+//import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
-import org.firstinspires.ftc.vision.apriltag.AprilTagGameDatabase;
+//import org.firstinspires.ftc.vision.apriltag.AprilTagGameDatabase;
-import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
+//import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
-
+//
-import java.util.List;
+//import java.util.List;
-@Config
+//@Config
-@TeleOp(name = "Auto-aiming turret (Webcam AprilTag)", group = "Iterative Opmode")
+//@TeleOp(name = "Auto-aiming turret (Webcam AprilTag)", group = "Iterative Opmode")
-public class AutoLock extends OpMode {
+//public class AutoLock extends OpMode {
-
+//
-    // Hardware
+//    // Hardware
-    private DcMotorEx turretMotor;
+//    private DcMotorEx turretMotor;
-
+//
-    // Vision
+//    // Vision
-    private VisionPortal visionPortal;
+//    private VisionPortal visionPortal;
-    private AprilTagProcessor aprilTag;
+//    private AprilTagProcessor aprilTag;
-
+//
-    // PID gains
+//    // PID gains
-    public double kP = 0.0012;
+//    public double kP = 0.0012;
-    public static double kI = 0.001;
+//    public static double kI = 0.001;
-    public static double kD = 0.002;
+//    public static double kD = 0.002;
-
+//
-    // Target center (degrees). 0 means center of camera.
+//    // Target center (degrees). 0 means center of camera.
-    private double targetX = 0.0;
+//    private double targetX = 0.0;
-
+//
-    private double integral = 0.0;
+//    private double integral = 0.0;
-    private double lastError = 0.0;
+//    private double lastError = 0.0;
-
+//
-    GamepadEx driverControl = null;
+//    GamepadEx driverControl = null;
-    GamepadEx armControl = null;
+//    GamepadEx armControl = null;
-
+//
-    private final ElapsedTime loopTimer = new ElapsedTime();
+//    private final ElapsedTime loopTimer = new ElapsedTime();
-    private final ElapsedTime targetLostTimer = new ElapsedTime();
+//    private final ElapsedTime targetLostTimer = new ElapsedTime();
-
+//
-    // Tuning
+//    // Tuning
-    private static final double POSITION_TOLERANCE_DEG = 1.5;
+//    private static final double POSITION_TOLERANCE_DEG = 1.5;
-    private static final double MIN_POWER = 0.05;
+//    private static final double MIN_POWER = 0.05;
-    private static final double MAX_POWER = 0.4;
+//    private static final double MAX_POWER = 0.4;
-    private static final double TARGET_LOST_TIMEOUT_SEC = 0.5;
+//    private static final double TARGET_LOST_TIMEOUT_SEC = 0.5;
-
+//
-    // Flip if turret moves the wrong way
+//    // Flip if turret moves the wrong way
-    private static final boolean INVERT_MOTOR = true;
+//    private static final boolean INVERT_MOTOR = true;
-
+//
-    // OPTIONAL: only lock onto certain tag IDs. Leave empty to accept any.
+//    // OPTIONAL: only lock onto certain tag IDs. Leave empty to accept any.
-    // Example: new int[]{1,2,3}
+//    // Example: new int[]{1,2,3}
-    private static final int[] DESIRED_TAG_IDS = new int[]{23, 24}; // empty = any
+//    private static final int[] DESIRED_TAG_IDS = new int[]{23, 24}; // empty = any
-
+//
-    private boolean targetWasVisible = false;
+//    private boolean targetWasVisible = false;
-    private Position cameraPosition = new Position(DistanceUnit.INCH,
+//    private Position cameraPosition = new Position(DistanceUnit.INCH,
-            0, 0, 0, 0);
+//            0, 0, 0, 0);
-    private YawPitchRollAngles cameraOrientation = new YawPitchRollAngles(AngleUnit.DEGREES,
+//    private YawPitchRollAngles cameraOrientation = new YawPitchRollAngles(AngleUnit.DEGREES,
-            0, -90, 0, 0);
+//            0, -90, 0, 0);
-
+//
-    // Drive Variables
+//    // Drive Variables
-    double x, y, xy;
+//    double x, y, xy;
-    double SPEEDCONTROL;
+//    double SPEEDCONTROL;
-    boolean driveMode = true;
+//    boolean driveMode = true;
-    MecanumDrive drive = null;
+//    MecanumDrive drive = null;
-    Hware robot = null;
+//    Hware robot = null;
-
+//
-    @Override
+//    @Override
-    public void init() {
+//    public void init() {
-        try {
+//        try {
-
+//
-            // Hardware Map Setup
+//            // Hardware Map Setup
-            robot = new Hware();
+//            robot = new Hware();
-            robot.initialize(hardwareMap);
+//            robot.initialize(hardwareMap);
-
+//
-            driverControl = new GamepadEx(gamepad1);
+//            driverControl = new GamepadEx(gamepad1);
-            armControl = new GamepadEx(gamepad2);
+//            armControl = new GamepadEx(gamepad2);
-
+//
-            // DriveBase Setup
+//            // DriveBase Setup
-            drive = new MecanumDrive(true, robot.frontLeft, robot.frontRight, robot.backLeft, robot.backRight);
+////            drive = new MecanumDrive(true, robot.frontLeft, robot.frontRight, robot.backLeft, robot.backRight);
-
+//
-
+//
-            turretMotor = hardwareMap.get(DcMotorEx.class, "turret");
+//            turretMotor = hardwareMap.get(DcMotorEx.class, "turret");
-            turretMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+//            turretMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-
+//
-            // AprilTag processor
+//            // AprilTag processor
-            aprilTag = new AprilTagProcessor.Builder()
+//            aprilTag = new AprilTagProcessor.Builder()
-                    .setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11)
+//                    .setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11)
-                    .setTagLibrary(AprilTagGameDatabase.getDecodeTagLibrary())
+//                    .setTagLibrary(AprilTagGameDatabase.getDecodeTagLibrary())
-                    .setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES)
+//                    .setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES)
-                    // If you have a Logitech C920/C270 etc, you can usually leave defaults.
+//                    // If you have a Logitech C920/C270 etc, you can usually leave defaults.
-                    // If you have calibrated intrinsics, you can set them here (advanced).
+//                    // If you have calibrated intrinsics, you can set them here (advanced).
-                    .build();
+//                    .build();
-
+//
-            // Vision portal with webcam
+//            // Vision portal with webcam
-            visionPortal = new VisionPortal.Builder()
+//            visionPortal = new VisionPortal.Builder()
-                    .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"))
+//                    .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"))
-                    .addProcessor(aprilTag)
+//                    .addProcessor(aprilTag)
-                    .build();
+//                    .build();
-
+//
-
+//
-            loopTimer.reset();
+//            loopTimer.reset();
-            targetLostTimer.reset();
+//            targetLostTimer.reset();
-
+//
-            telemetry.addData("Status", "Initialized (Webcam + AprilTag)");
+//            telemetry.addData("Status", "Initialized (Webcam + AprilTag)");
-            telemetry.addData("Motor Inverted", INVERT_MOTOR);
+//            telemetry.addData("Motor Inverted", INVERT_MOTOR);
-        } catch (Exception e) {
+//        } catch (Exception e) {
-            telemetry.addData("Init Error", e.getMessage());
+//            telemetry.addData("Init Error", e.getMessage());
-        }
+//        }
-    }
+//    }
-
+//
-    @Override
+//    @Override
-    public void loop() {
+//    public void loop() {
-        try {
+//        try {
-            // Get current detections
+//            // Get current detections
-            List<AprilTagDetection> detections = aprilTag.getDetections();
+//            List<AprilTagDetection> detections = aprilTag.getDetections();
-
+//
-            AprilTagDetection best = pickBestDetection(detections);
+//            AprilTagDetection best = pickBestDetection(detections);
-
+//
-            telemetry.addData("Detections", detections.size());
+//            telemetry.addData("Detections", detections.size());
-            for (AprilTagDetection d : detections) {
+//            for (AprilTagDetection d : detections) {
-                    telemetry.addData("ID", d.id);
+//                    telemetry.addData("ID", d.id);
-                    telemetry.addData("Metadata null?", d.metadata == null);
+//                    telemetry.addData("Metadata null?", d.metadata == null);
-                    telemetry.addData("Pose null?", d.ftcPose == null);
+//                    telemetry.addData("Pose null?", d.ftcPose == null);
-
+//
-            }
+//            }
-
+//
-            /** DRIVER CONTROLS **/
+//            /** DRIVER CONTROLS **/
-
+//
-            if (gamepad2.x) { driveMode = true; } else if (gamepad2.y) { driveMode = false;}
+//            if (gamepad2.x) { driveMode = true; } else if (gamepad2.y) { driveMode = false;}
-
+//
-            // Chassis
+//            // Chassis
-            if (!driveMode) { SPEEDCONTROL = 0;} else if (driverControl.getButton(GamepadKeys.Button.RIGHT_BUMPER) || driverControl.getButton(GamepadKeys.Button.LEFT_BUMPER)) { SPEEDCONTROL = .5; } else { SPEEDCONTROL = 1; }
+//            if (!driveMode) { SPEEDCONTROL = 0;} else if (driverControl.getButton(GamepadKeys.Button.RIGHT_BUMPER) || driverControl.getButton(GamepadKeys.Button.LEFT_BUMPER)) { SPEEDCONTROL = .5; } else { SPEEDCONTROL = 1; }
-            x = driverControl.getLeftX();
+//            x = driverControl.getLeftX();
-            y = driverControl.getLeftY();
+//            y = driverControl.getLeftY();
-            xy = driverControl.getRightX();
+//            xy = driverControl.getRightX();
-
+//
-            drive.driveRobotCentric(
+//            drive.driveRobotCentric(
-                    -x * SPEEDCONTROL,
+//                    -x * SPEEDCONTROL,
-                    -y * SPEEDCONTROL,
+//                    -y * SPEEDCONTROL,
-                    -xy * SPEEDCONTROL
+//                    -xy * SPEEDCONTROL
-            );
+//            );
-
+//
-            // Intake
+//            // Intake
-                if (driverControl.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) { robot.intake.set(1); } else if (driverControl.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) { robot.intake.set(-1); } else { robot.intake.set(0); }
+//                if (driverControl.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) { robot.intake.set(1); } else if (driverControl.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) { robot.intake.set(-1); } else { robot.intake.set(0); }
-
+//
-            /** Arm Controls **/
+//            /** Arm Controls **/
-//                if (armControl.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) { robot.turret.set(.3); } else if (armControl.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) { robot.turret.set(-0.3); } else { robot.turret.set(0); }
+////                if (armControl.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) { robot.turret.set(.3); } else if (armControl.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) { robot.turret.set(-0.3); } else { robot.turret.set(0); }
-//                if (armControl.getButton(GamepadKeys.Button.X)) { robot.launchLeft.set(1); robot.launchRight.set(1); } else {robot.launchLeft.set(0); robot.launchRight.set(0);}
+////                if (armControl.getButton(GamepadKeys.Button.X)) { robot.launchLeft.set(1); robot.launchRight.set(1); } else {robot.launchLeft.set(0); robot.launchRight.set(0);}
-//                if (!driveMode) {robot.rightPTO.setPosition(1); robot.leftPTO.setPosition(1);} else {robot.rightPTO.setPosition(0); robot.rightPTO.setPosition(0);}
+////                if (!driveMode) {robot.rightPTO.setPosition(1); robot.leftPTO.setPosition(1);} else {robot.rightPTO.setPosition(0); robot.rightPTO.setPosition(0);}
-
+//
-
+//
-            if (best != null) {
+//            if (best != null) {
-                targetWasVisible = true;
+//                targetWasVisible = true;
-                targetLostTimer.reset();
+//                targetLostTimer.reset();
-
+//
-                // ----- "tx" equivalent -----
+//                // ----- "tx" equivalent -----
-                // AprilTag gives pose relative to camera. We want horizontal angle offset.
+//                // AprilTag gives pose relative to camera. We want horizontal angle offset.
-                // Use bearing/yaw style value if available from metadata or pose.
+//                // Use bearing/yaw style value if available from metadata or pose.
-                //
+//                //
-                // Most reliable: compute from pose X/Z (left/right vs forward):
+//                // Most reliable: compute from pose X/Z (left/right vs forward):
-                // angle = atan2(x, z)
+//                // angle = atan2(x, z)
-                //
+//                //
-                // In FTC pose:
+//                // In FTC pose:
-                // - pose.x: left/right (inches or meters depending on config; typically inches)
+//                // - pose.x: left/right (inches or meters depending on config; typically inches)
-                // - pose.z: forward distance
+//                // - pose.z: forward distance
-                //
+//                //
-                // If your coordinate axes differ, flip sign once using INVERT_MOTOR or sign flip here.
+//                // If your coordinate axes differ, flip sign once using INVERT_MOTOR or sign flip here.
-                double x = best.ftcPose.x;  // left(+) / right(-) depending on convention
+//                double x = best.ftcPose.x;  // left(+) / right(-) depending on convention
-                double z = best.ftcPose.z;  // forward distance
+//                double z = best.ftcPose.z;  // forward distance
-                double txDeg = Math.toDegrees(Math.atan2(x, z));
+//                double txDeg = Math.toDegrees(Math.atan2(x, z));
-
+//
-                // Timing
+//                // Timing
-                double dt = loopTimer.seconds();
+//                double dt = loopTimer.seconds();
-                loopTimer.reset();
+//                loopTimer.reset();
-
+//
-                if (dt < 0.001) dt = 0.001;
+//                if (dt < 0.001) dt = 0.001;
-                if (dt > 1.0) dt = 1.0;
+//                if (dt > 1.0) dt = 1.0;
-
+//
-                // Error (positive tx means tag is to one side)
+//                // Error (positive tx means tag is to one side)
-                double error = txDeg - targetX;
+//                double error = txDeg - targetX;
-
+//
-                // PID
+//                // PID
-                integral += error * dt;
+//                integral += error * dt;
-                integral = Math.max(-50, Math.min(50, integral)); // anti-windup
+//                integral = Math.max(-50, Math.min(50, integral)); // anti-windup
-
+//
-                double derivative = (error - lastError) / dt;
+//                double derivative = (error - lastError) / dt;
-
+//
-                double pidOutput = (kP * error) + (kI * integral) + (kD * derivative);
+//                double pidOutput = (kP * error) + (kI * integral) + (kD * derivative);
-
+//
-                // Deadband
+//                // Deadband
-                if (Math.abs(error) < POSITION_TOLERANCE_DEG) {
+//                if (Math.abs(error) < POSITION_TOLERANCE_DEG) {
-                    pidOutput = 0;
+//                    pidOutput = 0;
-                    integral = 0;
+//                    integral = 0;
-                }
+//                }
-
+//
-                // Min power to overcome friction
+//                // Min power to overcome friction
-                if (pidOutput != 0 && Math.abs(pidOutput) < MIN_POWER) {
+//                if (pidOutput != 0 && Math.abs(pidOutput) < MIN_POWER) {
-                    pidOutput = MIN_POWER * Math.signum(pidOutput);
+//                    pidOutput = MIN_POWER * Math.signum(pidOutput);
-                }
+//                }
-
+//
-                // Clamp
+//                // Clamp
-                double motorPower = clamp(pidOutput, -MAX_POWER, MAX_POWER);
+//                double motorPower = clamp(pidOutput, -MAX_POWER, MAX_POWER);
-
+//
-                if (INVERT_MOTOR) motorPower = -motorPower;
+//                if (INVERT_MOTOR) motorPower = -motorPower;
-
+//
-                if (!Double.isFinite(motorPower)) {
+//                if (!Double.isFinite(motorPower)) {
-                    motorPower = 0;
+//                    motorPower = 0;
-                    resetPID();
+//                    resetPID();
-                }
+//                }
-
+//
-                turretMotor.setPower(motorPower);
+//                turretMotor.setPower(motorPower);
-                lastError = error;
+//                lastError = error;
-
+//
-                telemetry.addData("Status", "LOCKED ON");
+//                telemetry.addData("Status", "LOCKED ON");
-                telemetry.addData("Tag ID", best.id);
+//                telemetry.addData("Tag ID", best.id);
-                telemetry.addData("tx (deg)", "%.2f", txDeg);
+//                telemetry.addData("tx (deg)", "%.2f", txDeg);
-                telemetry.addData("Error", "%.2f", error);
+//                telemetry.addData("Error", "%.2f", error);
-                telemetry.addData("PID Output", "%.3f", pidOutput);
+//                telemetry.addData("PID Output", "%.3f", pidOutput);
-                telemetry.addData("Motor Power", "%.3f", motorPower);
+//                telemetry.addData("Motor Power", "%.3f", motorPower);
-
+//
-                // Extra pose telemetry (helpful for debugging)
+//                // Extra pose telemetry (helpful for debugging)
-                telemetry.addData("Pose x", "%.2f", best.ftcPose.x);
+//                telemetry.addData("Pose x", "%.2f", best.ftcPose.x);
-                telemetry.addData("Pose z", "%.2f", best.ftcPose.z);
+//                telemetry.addData("Pose z", "%.2f", best.ftcPose.z);
-                telemetry.addData("Range: ", best.ftcPose.range);
+//                telemetry.addData("Range: ", best.ftcPose.range);
-                telemetry.addData("Bearing", "%.2f deg", best.ftcPose.bearing);
+//                telemetry.addData("Bearing", "%.2f deg", best.ftcPose.bearing);
-                telemetry.addData("Yaw", "%.2f deg", best.ftcPose.yaw);
+//                telemetry.addData("Yaw", "%.2f deg", best.ftcPose.yaw);
-
+//
-                if(best.ftcPose.range < 40){
+//                if(best.ftcPose.range < 40){
-                    kP = 0.008;
+//                    kP = 0.008;
-                } else if (best.ftcPose.range < 70) {
+//                } else if (best.ftcPose.range < 70) {
-                    kP = 0.005;
+//                    kP = 0.005;
-                } else if (best.ftcPose.range < 90){
+//                } else if (best.ftcPose.range < 90){
-                    kP = 0.004;
+//                    kP = 0.004;
-                } else if (best.ftcPose.range < 110) {
+//                } else if (best.ftcPose.range < 110) {
-                    kP = 0.003;
+//                    kP = 0.003;
-                } else if (best.ftcPose.range < 130) {
+//                } else if (best.ftcPose.range < 130) {
-                    kP = 0.002;
+//                    kP = 0.002;
-                } else if (best.ftcPose.range < 150) {
+//                } else if (best.ftcPose.range < 150) {
-                    kP = 0.001;
+//                    kP = 0.001;
-                } else {
+//                } else {
-                    kP = 0.0005;
+//                    kP = 0.0005;
-                }
+//                }
-                telemetry.addData("kP: ", kP);
+//                telemetry.addData("kP: ", kP);
-
+//
-            } else {
+//            } else {
-                handleNoTarget();
+//                handleNoTarget();
-            }
+//            }
-
+//
-        } catch (Exception e) {
+//        } catch (Exception e) {
-            telemetry.addData("Error", e.getMessage());
+//            telemetry.addData("Error", e.getMessage());
-            telemetry.addData("Error Type", e.getClass().getSimpleName());
+//            telemetry.addData("Error Type", e.getClass().getSimpleName());
-            stopTurret();
+//            stopTurret();
-        }
+//        }
-
+//
-        telemetry.update();
+//        telemetry.update();
-    }
+//    }
-
+//
-    /**
+//    /**
-     * Pick "best" detection. Simple approach:
+//     * Pick "best" detection. Simple approach:
-     * - If DESIRED_TAG_IDS set: only accept those
+//     * - If DESIRED_TAG_IDS set: only accept those
-     * - Choose closest (smallest range) among candidates
+//     * - Choose closest (smallest range) among candidates
-     */
+//     */
-    private AprilTagDetection pickBestDetection(List<AprilTagDetection> detections) {
+//    private AprilTagDetection pickBestDetection(List<AprilTagDetection> detections) {
-        if (detections == null || detections.isEmpty()) return null;
+//        if (detections == null || detections.isEmpty()) return null;
-
+//
-        AprilTagDetection best = null;
+//        AprilTagDetection best = null;
-        double bestRange = Double.POSITIVE_INFINITY;
+//        double bestRange = Double.POSITIVE_INFINITY;
-
+//
-        for (AprilTagDetection d : detections) {
+//        for (AprilTagDetection d : detections) {
-            if (d == null) continue;
+//            if (d == null) continue;
-
+//
-            if (!isDesiredId(d.id)) continue;
+//            if (!isDesiredId(d.id)) continue;
-
+//
-            // Use range as "quality" metric
+//            // Use range as "quality" metric
-            double r = d.ftcPose.range;
+//            double r = d.ftcPose.range;
-            if (r < bestRange) {
+//            if (r < bestRange) {
-                bestRange = r;
+//                bestRange = r;
-                best = d;
+//                best = d;
-            }
+//            }
-        }
+//        }
-        return best;
+//        return best;
-    }
+//    }
-
+//
-    private boolean isDesiredId(int id) {
+//    private boolean isDesiredId(int id) {
-        if (DESIRED_TAG_IDS == null || DESIRED_TAG_IDS.length == 0) return true;
+//        if (DESIRED_TAG_IDS == null || DESIRED_TAG_IDS.length == 0) return true;
-        for (int x : DESIRED_TAG_IDS) {
+//        for (int x : DESIRED_TAG_IDS) {
-            if (x == id) return true;
+//            if (x == id) return true;
-        }
+//        }
-        return false;
+//        return false;
-    }
+//    }
-
+//
-    private void handleNoTarget() {
+//    private void handleNoTarget() {
-        if (targetWasVisible && targetLostTimer.seconds() < TARGET_LOST_TIMEOUT_SEC) {
+//        if (targetWasVisible && targetLostTimer.seconds() < TARGET_LOST_TIMEOUT_SEC) {
-            telemetry.addData("Status", "TRACKING LOST - Coasting");
+//            telemetry.addData("Status", "TRACKING LOST - Coasting");
-            telemetry.addData("Time Lost", "%.2f s", targetLostTimer.seconds());
+//            telemetry.addData("Time Lost", "%.2f s", targetLostTimer.seconds());
-            // You could also keep last motor power for a brief time if you store it.
+//            // You could also keep last motor power for a brief time if you store it.
-        } else {
+//        } else {
-            stopTurret();
+//            stopTurret();
-            telemetry.addData("Status", "SEARCHING");
+//            telemetry.addData("Status", "SEARCHING");
-            telemetry.addData("Target Visible", "No");
+//            telemetry.addData("Target Visible", "No");
-        }
+//        }
-    }
+//    }
-
+//
-    private void stopTurret() {
+//    private void stopTurret() {
-        if (turretMotor != null) turretMotor.setPower(0);
+//        if (turretMotor != null) turretMotor.setPower(0);
-        resetPID();
+//        resetPID();
-        targetWasVisible = false;
+//        targetWasVisible = false;
-    }
+//    }
-
+//
-    private void resetPID() {
+//    private void resetPID() {
-        integral = 0;
+//        integral = 0;
-        lastError = 0;
+//        lastError = 0;
-    }
+//    }
-
+//
-    private static double clamp(double v, double lo, double hi) {
+//    private static double clamp(double v, double lo, double hi) {
-        return Math.max(lo, Math.min(hi, v));
+//        return Math.max(lo, Math.min(hi, v));
-    }
+//    }
-
+//
-    @Override
+//    @Override
-    public void stop() {
+//    public void stop() {
-        try {
+//        try {
-            if (turretMotor != null) turretMotor.setPower(0);
+//            if (turretMotor != null) turretMotor.setPower(0);
-            if (visionPortal != null) visionPortal.close();
+//            if (visionPortal != null) visionPortal.close();
-        } catch (Exception ignored) {}
+//        } catch (Exception ignored) {}
-    }
+//    }
-}
+//}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/Hware.java

@@ -8,16 +8,20 @@ import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
 
 import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
 
 public class Hware {
 
     HardwareMap hardwareMap;
-    public Motor frontRight, frontLeft, backRight, backLeft, intake;
+    public Motor intake;
-    public DcMotorEx turret, bottomFly, topFly;
+    public DcMotorEx frontRight, frontLeft, backRight, backLeft, turret, bottomFly, topFly;
     public SimpleServo activeTransfer, rightPTO, leftPTO, launchHood;
-    public Limelight3A limelight;
+   // public Limelight3A limelight;
+    public VoltageSensor batteryVoltageSensor;
+
+
     WebcamName webcam;
     public void initialize(HardwareMap hwMap) {
         hardwareMap = hwMap;
@@ -28,10 +32,17 @@ public class Hware {
 
 
 //         Chassis Motors
-        frontRight = new Motor(hardwareMap, "frontRight", Motor.GoBILDA.RPM_435);
+
-        frontLeft = new Motor(hardwareMap, "frontLeft", Motor.GoBILDA.RPM_435);
+
-        backRight = new Motor(hardwareMap, "backRight", Motor.GoBILDA.RPM_435);
+        frontRight = hardwareMap.get(DcMotorEx.class, "frontRight");
-        backLeft = new Motor(hardwareMap, "backLeft", Motor.GoBILDA.RPM_435);
+        frontLeft = hardwareMap.get(DcMotorEx.class, "frontLeft");
+        backRight = hardwareMap.get(DcMotorEx.class, "backRight");
+        backLeft = hardwareMap.get(DcMotorEx.class, "backLeft");
+//
+//        frontRight = new Motor(hardwareMap, "frontRight", Motor.GoBILDA.RPM_435);
+//        frontLeft = new Motor(hardwareMap, "frontLeft", Motor.GoBILDA.RPM_435);
+//        backRight = new Motor(hardwareMap, "backRight", Motor.GoBILDA.RPM_435);
+//        backLeft = new Motor(hardwareMap, "backLeft", Motor.GoBILDA.RPM_435);
 
 //         Intake
         intake = new Motor(hardwareMap, "intake", Motor.GoBILDA.RPM_1620);
@@ -57,20 +68,37 @@ public class Hware {
 //        webcam = hardwareMap.get(WebcamName.class, "cam");
 
 //         Zero Power Behaviors
-        frontRight.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
+        frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        frontLeft.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
+        frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        backLeft.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
+        backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        backRight.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
+        backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         intake.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
 
-        PIDFCoefficients pidf = new PIDFCoefficients(3, 0, 0, 14
+        batteryVoltageSensor = hardwareMap.get(VoltageSensor.class, "Control Hub"); // Or "Expansion Hub"
+
+        // To get the actual number:
+
+
+        // P - 1.09
+        // F - 14.12
+        PIDFCoefficients pidf = new PIDFCoefficients(1.09, 0, 0, 14.12
         );
+
         bottomFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf);
         topFly.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf);
 
         bottomFly.setDirection(DcMotorSimple.Direction.REVERSE);
 
+        turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+        // Turret Positions
+        // Center = 8
+        // 45 deg = -88
+        // -45 deg = 88
+        // 90 deg = -240
+        // -90 deg = 240
+
         //Vision
-        limelight = hardwareMap.get(Limelight3A.class, "limelight");
+ //       limelight = hardwareMap.get(Limelight3A.class, "limelight");
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/blueDepotFar.java

@@ -0,0 +1,204 @@
+package org.firstinspires.ftc.teamcode.OUTDATED;//package org.firstinspires.ftc.teamcode.Auton;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+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.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
+import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
+
+@Config
+@Autonomous(name = "Blue / Red Far")
+public class blueDepotFar extends LinearOpMode {
+
+    Hware robot = new Hware();
+    public static double TICKS_PER_DEGREE = 9.738888;
+
+    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);
+        turretPower(1);
+    }
+
+    public void spintakeStart(){
+        robot.intake.set(1);
+    }
+
+    public void spintakeEnd(){
+        robot.intake.set(0);
+    }
+
+    public void shoot(){
+        robot.topFly.setVelocity(2200);
+        robot.bottomFly.setVelocity(2200);
+        robot.launchHood.setPosition(0.66);
+    }
+
+    public void reset(){
+        robot.intake.set(0);
+        robot.topFly.setVelocity(0);
+        robot.bottomFly.setVelocity(0);
+        robot.launchHood.setPosition(0.35);
+        robot.activeTransfer.setPosition(0.8);
+    }
+
+
+    public void runOpMode() throws InterruptedException {
+
+        //webcam1 = hardwareMap.get(WebcamName.class, "Webcam 2");
+        SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+        robot.initialize(hardwareMap);
+
+        Pose2d startPose = new Pose2d(-64, -20, Math.toRadians(25));
+
+        drive.setPoseEstimate(startPose);
+
+        TrajectorySequence threeBall1 = drive.trajectorySequenceBuilder(startPose)
+                .forward(3)
+                .addDisplacementMarker(()-> {
+                    shoot();
+                })
+                .build();
+
+        TrajectorySequence letGo = drive.trajectorySequenceBuilder(threeBall1.end())
+                        .forward(.01)
+                        .addDisplacementMarker(()-> {
+                                    robot.activeTransfer.setPosition(1);
+                                    robot.intake.set(.5);
+                        })
+                .build();
+
+        TrajectorySequence waitTime = drive.trajectorySequenceBuilder(startPose)
+                .waitSeconds(4)
+                .build();
+
+        TrajectorySequence goOut = drive.trajectorySequenceBuilder(startPose)
+                .forward(4)
+                .build();
+
+        TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose)
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                    reset();
+//                    turretPos(5);
+                })
+                .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
+                    shoot();
+                })
+                .back(42)
+                .waitSeconds(0.3)
+                .UNSTABLE_addTemporalMarkerOffset(0.3, () -> {
+                    robot.intake.set(1);
+                    robot.activeTransfer.setPosition(0.6);
+                })
+
+//                .UNSTABLE_addTemporalMarkerOffset(2.2, () -> {
+//                    robot.launchHood.setPosition(0.5);
+//                })
+                .waitSeconds(1.5)
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                    reset();
+                })
+
+                //6ball
+                .lineToLinearHeading(new Pose2d(8,28, Math.toRadians(90)))
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                    spintakeStart();
+                })
+                .forward(20, SampleMecanumDrive.getVelocityConstraint(15, 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.5);
+                })
+
+//                .UNSTABLE_addTemporalMarkerOffset(2.2, () -> {
+//                    robot.launchHood.setPosition(0.5);
+//                })
+                .waitSeconds(1.5)
+
+                //9ball
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                    reset();
+                })
+                .lineToLinearHeading(new Pose2d(-19,27, Math.toRadians(90)))
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                    spintakeStart();
+                })
+                .forward(18, SampleMecanumDrive.getVelocityConstraint(30, 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(24,14, Math.toRadians(52)))
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                    reset();
+                })
+                .UNSTABLE_addTemporalMarkerOffset(.5, () -> {
+                    robot.intake.set(1);
+                    robot.activeTransfer.setPosition(0.5);
+                })
+                .waitSeconds(1.5)
+
+                //12ball
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                    reset();
+                })
+                .lineToLinearHeading(new Pose2d(-40,27, 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();
+                })
+                .lineToLinearHeading(new Pose2d(35,14, Math.toRadians(52)))
+                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+                    shoot();
+                })
+                .UNSTABLE_addTemporalMarkerOffset(.5, () -> {
+                    robot.intake.set(0.8);
+                    robot.activeTransfer.setPosition(0.6);
+                })
+                .waitSeconds(1.5)
+                //     .lineToLinearHeading(new Pose2d(55.5,68, Math.toRadians(-45)))
+
+//                .UNSTABLE_addTemporalMarkerOffset(0.5, () -> {
+//                    turretPos(84);
+//                })
+                .waitSeconds(1)
+                .build();
+
+        waitForStart();
+        if (opModeIsActive() && !isStopRequested()) {
+            while (opModeIsActive()) {
+                robot.leftPTO.setPosition(0.2);
+                robot.rightPTO.setPosition(0.2);
+            drive.followTrajectorySequence(threeBall);
+            drive.followTrajectorySequence(waitTime);
+            drive.followTrajectorySequence(letGo);
+            drive.followTrajectorySequence(waitTime);
+            drive.followTrajectorySequence(goOut);
+
+            PoseStorage.currentPose = drive.getPoseEstimate();
+            }
+        }
+    }
+}

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

@@ -0,0 +1,38 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.apache.commons.math3.geometry.euclidean.twod.Line;
+import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
+
+@TeleOp
+@Config
+
+public class PTOTest extends LinearOpMode {
+    public static double launchHood = 0.3;
+    public static double activeTransfer = 0.3;
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        Hware robot = new Hware();
+        robot.initialize(hardwareMap);
+
+        waitForStart();
+        while (opModeIsActive()) {
+            robot.launchHood.setPosition(launchHood);
+            robot.activeTransfer.setPosition(activeTransfer);
+
+        }
+    }
+}
+
+ // Left PTO
+// Engage - 0
+// Disengage - 0.2
+
+
+// Right PTO
+// Engage - 0
+// Disengage - 0.2

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

@@ -0,0 +1,76 @@
+package org.firstinspires.ftc.teamcode;
+
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+
+public class ShooterSubsystem {
+    public static class ShotPoint {
+        double x, y, velocity, hood;
+        public ShotPoint(double x, double y, double v, double h) {
+            this.x = x; this.y = y; this.velocity = v; this.hood = h;
+        }
+    }
+
+    private final List<ShotPoint> shotData = new ArrayList<>();
+
+    public ShooterSubsystem() {
+        // Top Row (Y = 60)
+        shotData.add(new ShotPoint(60, 36, 1500, 0.32)); // Field (-36, 60)
+        shotData.add(new ShotPoint(60, 12, 1700, 0.57)); // Field (-12, 60)
+        shotData.add(new ShotPoint(60,  -12, 1800, 0.63)); // Field (12, 60)
+        shotData.add(new ShotPoint(60,  -36, 2000, 0.68)); // Field (36, 60)
+
+        // Field Middle Row (Field Y = 36)
+        shotData.add(new ShotPoint(36, 36, 1570, 0.42)); // Field (-36, 36)
+        shotData.add(new ShotPoint(36, 12, 1680, 0.55)); // Field (-12, 36)
+        shotData.add(new ShotPoint(36,  -12, 1800, 0.65)); // Field (12, 36)
+        shotData.add(new ShotPoint(36,  -36, 1900, 0.65)); // Field (36, 36)
+
+        // Field Bottom "V" Points (Field Y = 12)
+        shotData.add(new ShotPoint(12, 12, 1750, 0.62)); // Field (-12, 12)
+        shotData.add(new ShotPoint(12,  -12, 1900, 0.65)); // Field (12, 12)
+        shotData.add(new ShotPoint(0,  0, 1900, 0.65)); // Field (12, 12)
+
+        shotData.add(new ShotPoint(-60, 12, 2300, 0.72));
+        shotData.add(new ShotPoint(-60, -12, 2300, 0.7));
+
+    }
+
+    public double[] getInterpolatedShot(double robotX, double robotY) {
+        // Calculate distances and store in a temporary list
+        class Node {
+            ShotPoint p; double dist;
+            Node(ShotPoint p, double d) { this.p = p; this.dist = d; }
+        }
+
+        List<Node> nodes = new ArrayList<>();
+        for (ShotPoint p : shotData) {
+            nodes.add(new Node(p, Math.hypot(p.x - robotX, p.y - robotY)));
+        }
+
+        // Sort by distance
+        Collections.sort(nodes, (a, b) -> Double.compare(a.dist, b.dist));
+
+        // If we are basically on top of the point, return it exactly
+        if (nodes.get(0).dist < 1.0) {
+            return new double[]{nodes.get(0).p.velocity, nodes.get(0).p.hood};
+        }
+
+        double totalWeight = 0;
+        double weightedVel = 0;
+        double weightedHood = 0;
+
+        // Take the 3 closest points for local averaging
+        for (int i = 0; i < 3; i++) {
+            Node n = nodes.get(i);
+            // Inverse Square Weighting: weight = 1 / d^2
+            double weight = 1.0 / (n.dist * n.dist);
+            weightedVel += n.p.velocity * weight;
+            weightedHood += n.p.hood * weight;
+            totalWeight += weight;
+        }
+
+        return new double[]{weightedVel / totalWeight, weightedHood / totalWeight};
+    }
+}

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

@@ -0,0 +1,71 @@
+package org.firstinspires.ftc.teamcode;
+
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+
+public class ShooterSubsystemRed {
+    public static class ShotPoint {
+        double x, y, velocity, hood;
+        public ShotPoint(double x, double y, double v, double h) {
+            this.x = x; this.y = y; this.velocity = v; this.hood = h;
+        }
+    }
+
+    private final List<ShotPoint> shotData = new ArrayList<>();
+
+    public ShooterSubsystemRed() {
+        // Top Row (X = 60) - Y Reflected
+        shotData.add(new ShotPoint(60, -36, 1500, 0.32));
+        shotData.add(new ShotPoint(60, -12, 1700, 0.57));
+        shotData.add(new ShotPoint(60,  12, 1800, 0.63));
+        shotData.add(new ShotPoint(60,  36, 2000, 0.68));
+
+        // Field Middle Row (X = 36) - Y Reflected
+        shotData.add(new ShotPoint(36, -36, 1570, 0.42));
+        shotData.add(new ShotPoint(36, -12, 1680, 0.55));
+        shotData.add(new ShotPoint(36,  12, 1800, 0.65));
+        shotData.add(new ShotPoint(36,  36, 1900, 0.65));
+
+        // Field Bottom "V" Points (X = 12) - Y Reflected
+        shotData.add(new ShotPoint(12, -12, 1750, 0.62));
+        shotData.add(new ShotPoint(12,  12, 1900, 0.65));
+        shotData.add(new ShotPoint(0,   0, 1900, 0.65));
+
+        // Far Points - Y Reflected
+        shotData.add(new ShotPoint(-60, -12, 2300, 0.72));
+        shotData.add(new ShotPoint(-60,  12, 2300, 0.7));
+    }
+
+    public double[] getInterpolatedShot(double robotX, double robotY) {
+        class Node {
+            ShotPoint p; double dist;
+            Node(ShotPoint p, double d) { this.p = p; this.dist = d; }
+        }
+
+        List<Node> nodes = new ArrayList<>();
+        for (ShotPoint p : shotData) {
+            nodes.add(new Node(p, Math.hypot(p.x - robotX, p.y - robotY)));
+        }
+
+        Collections.sort(nodes, (a, b) -> Double.compare(a.dist, b.dist));
+
+        if (nodes.get(0).dist < 1.0) {
+            return new double[]{nodes.get(0).p.velocity, nodes.get(0).p.hood};
+        }
+
+        double totalWeight = 0;
+        double weightedVel = 0;
+        double weightedHood = 0;
+
+        for (int i = 0; i < 3; i++) {
+            Node n = nodes.get(i);
+            double weight = 1.0 / (n.dist * n.dist);
+            weightedVel += n.p.velocity * weight;
+            weightedHood += n.p.hood * weight;
+            totalWeight += weight;
+        }
+
+        return new double[]{weightedVel / totalWeight, weightedHood / totalWeight};
+    }
+}

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

@@ -1,15 +1,19 @@
 package org.firstinspires.ftc.teamcode;
 
+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.qualcomm.hardware.limelightvision.LLResult;
 import com.qualcomm.hardware.limelightvision.Limelight3A;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
 import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
 import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.util.PersonalPID;
 import org.firstinspires.ftc.vision.VisionPortal;
 import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
 import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
@@ -25,13 +29,18 @@ public class TeleOpTurretLock extends LinearOpMode {
     // Put the APRIL TAG / BACKDROP location in YOUR RoadRunner coordinate system.
     // You said your start is (0,0). So measure these from that same origin.
     public static double GOAL_X = 75;
-    public static double GOAL_Y = 75;
+    public static double GOAL_Y = -75;
-
-    public static double p = 1, i = 0, d = 0, f = 1;
 
+    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 * (90));
+
+    PersonalPID pid;
+
     Limelight3A limelight;
 
 
@@ -63,7 +72,8 @@ public class TeleOpTurretLock extends LinearOpMode {
 
         // Start pose (you said 0,0)
         drive.setPoseEstimate(new Pose2d(0, 0, 0));
-        TurretLock turretLock = new TurretLock(robot.turret, limelight, telemetry, p, i, d, f);
+//        TurretLock turretLock = new TurretLock(robot.turret, limelight, TurretLock.SIDE.RED);
+
 
         // limelight
         
@@ -77,15 +87,21 @@ public class TeleOpTurretLock extends LinearOpMode {
         boolean autoLockEnabled = true;
         boolean xPrev = false;
 
+        telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+        pid = new PersonalPID(p, i, d, f);
+
         waitForStart();
 
 //        turretLock.cameraInit();
         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);
 
         while (opModeIsActive()) {
-
+            result = limelight.getLatestResult();
-            result = robot.limelight.getLatestResult();
+            double vision = result.getTx();
 
             /* ---------------- DRIVE ---------------- */
             double speed = DRIVE_SPEED;
@@ -103,32 +119,58 @@ public class TeleOpTurretLock extends LinearOpMode {
 
             Pose2d pose = drive.getPoseEstimate();
 
-            /* ---------------- UPDATE TARGETS (Dashboard) ---------------- */
+//            /* ---------------- UPDATE TARGETS (Dashboard) ---------------- */
-            turretLock.goalX = GOAL_X;
+//            turretLock.goalX = GOAL_X;
-            turretLock.goalY = GOAL_Y;
+//            turretLock.goalY = GOAL_Y;
 
             /* ---------------- MANUAL TRIM ---------------- */
             // Left trigger = +trim, Right trigger = -trim (same style as your old code)
-            double trimDeltaDeg = MANUAL_TRIM_DEG_PER_LOOP * (gamepad2.left_trigger - gamepad2.right_trigger);
+//            double trimDeltaDeg = MANUAL_TRIM_DEG_PER_LOOP * (gamepad2.left_trigger - gamepad2.right_trigger);
-            if (Math.abs(trimDeltaDeg) > 1e-6) turretLock.addManualTrimDeg(trimDeltaDeg);
+//            if (Math.abs(trimDeltaDeg) > 1e-6) turretLock.addManualTrimDeg(trimDeltaDeg);
-            if (gamepad2.y) turretLock.resetManualTrim();  // reset driver trim
+//            if (gamepad2.y) turretLock.resetManualTrim();  // reset driver trim
 //            telemetry.update();\
 //
-            turretLock.update(pose, result);
+            double target = ticksperdegree * vision;
+            double targetTicks = pid.calculate(robot.turret.getCurrentPosition(), target);
+            pid.setPIDF(p, i, d, f);
 
-            telemetry.addData("VisionAngle", turretLock.getBearing());
+            //double targetTicks =  turretLock.update(pose, result, vision, robot.turret.getCurrentPosition());
-            telemetry.addData("Angle", turretLock.getFinalDeg());
+//            robot.turret.setTargetPosition((int) (robot.turret.getCurrentPosition() - targetTicks));
-            telemetry.addData("Ticks", Math.round(turretLock.getFinalDeg() * 2.543));
+//            robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-            telemetry.addData("MODE", turretLock.getTYPE());
+
-            telemetry.addData("Tag ID", turretLock.getObeliskID());
+            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;
+            }
+
+            robot.turret.setPower(- targetTicks);
+
+//            int turretPos = robot.turret.getCurrentPosition();
+//            double power = pid.calculate(turretPos, targetTicks);
+//            robot.turret.setPower(power);
+//
+//            telemetry.addData("pid", power);
+//            telemetry.addData("target", targetTicks);
+//            telemetry.addData("currentPos", turretPos);
+
+//            telemetry.addData("VisionAngle", turretLock.getBearing());
+//            telemetry.addData("Angle", turretLock.getFinalDeg());
+//            telemetry.addData("Ticks", Math.round(turretLock.getFinalDeg() * 2.543));
+//            telemetry.addData("MODE", turretLock.getTYPE());
+//            telemetry.addData("Tag ID", turretLock.getObeliskID());
             telemetry.addData("Does Lime have Result", result.isValid());
+            telemetry.addData("pid", targetTicks);
 
-            telemetry.addData("Botpose", result.getBotpose());
+//            telemetry.addData("Botpose", result.getBotpose());
             telemetry.addData("Yaw", result.getTx());
 
-            telemetry.addData("Status", turretLock.getStatus());
+//            telemetry.addData("Status", turretLock.getStatus());
 
-            telemetry.addData("LimelightInfo", robot.limelight.getStatus());
+            telemetry.addData("LimelightInfo", limelight.getStatus());
 
             telemetry.update();
         }

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

@@ -19,7 +19,7 @@ import java.util.List;
 
 public class TurretLock {
 
-    public enum MODE{
+    public enum MODE {
         LIME,
         ODOMETRY
     }
@@ -28,7 +28,7 @@ public class TurretLock {
     /* ------------------- TUNABLES ------------------- */
 
     // Field target (in RoadRunner field units; usually inches)
-    // Set this to the AprilTag/backdrop location you want to aim at.
+    // Set this to the AprilTag/backdrop location 4   you want to aim at.
     public double goalX = 75;
     public double goalY = 75;
 
@@ -36,11 +36,11 @@ public class TurretLock {
 
     // Turret conversion
     // If you already trust your TICKS_PER_DEGREE, keep it.
-    public double ticksPerDegree = 2.543; //9.738888 * 0.29969;
+    public double ticksPerDegree = 9.738888 * 0.29969; //2.543
 
     // Mechanical limits (ticks). Keep your existing numbers.
     public int minTicks = (int) (ticksPerDegree * (-90));
-    public int maxTicks = (int) (ticksPerDegree * ( 90 ));
+    public int maxTicks = (int) (ticksPerDegree * (90));
 
     public double finalDeg = 0;
 
@@ -50,6 +50,11 @@ public class TurretLock {
     public static double maxOffsetChangePerCycle = 5.0; // Degrees per cycle
     public static double cameraBearingEqual = 0.5;      // Deadband
 
+    public enum SIDE {
+        RED,
+        BLUE
+    }
+
     // TODO: tune these values for limelight
     Hware robot;
     Limelight3A webcam;
@@ -78,27 +83,33 @@ public class TurretLock {
 
     // private final ElapsedTime tagTimer = new ElapsedTime();
 
-    public TurretLock(DcMotorEx turretMotor, Limelight3A cam, Telemetry tele, double p, double i, double d, double f) {
+    public TurretLock(DcMotorEx turretMotor, Limelight3A cam, SIDE side) {
         this.turret = turretMotor;
         this.webcam = cam;
-        this.tele = tele;
+        switch (side) {
-        this.p = p;
+            case RED:
-        this.i = i;
+                goalX = 75;
-        this.d = d;
+                goalY = -75;
-        this.f = f;
+                break;
+            case BLUE:
+                goalX = 75;
+                goalY = 75;
+                break;
+        }
     }
 
     public double getTurrPos() {
         return turret.getCurrentPosition();
     }
 
-    public void addManualTrimDeg(double deltaDeg){
+    public void addManualTrimDeg(double deltaDeg) {
         manualTrimDeg += deltaDeg;
     }
 
-    public void resetManualTrim(){
+    public void resetManualTrim() {
         manualTrimDeg = 0;
     }
+
     private void limelightRead(LLResult result) { // only for tracking purposes, not general reads
         if (result != null) {
             if (result.isValid()) {
@@ -155,24 +166,26 @@ public class TurretLock {
 
     private double bearingAlign(LLResult llResult) {
         double targetTx = 0;
-        if(llResult != null && llResult.isValid()){
+        if (llResult != null && llResult.isValid()) {
             targetTx = llResult.getTx();   // How far left or right the target is (degrees)
 
         }
         return llResult.getBotpose().getOrientation().getYaw();
     }
 
-    public void Init(){
+    public void Init() {
         pid = new PersonalPID(p, i, d, f);
     }
 
-    public String getStatus(){
+    public String getStatus() {
         return String.valueOf(webcam.getStatus());
     }
 
 
-    /** Main update: call every loop. */
+    /**
-    public void update(Pose2d robotPose, LLResult result) {
+     * Main update: call every loop.
+     */
+    public double update(Pose2d robotPose, LLResult result, double visionCorrect, double turrPos) {
         // 1) ODOM: compute turret angle needed to face goal (in degrees)
         double dx = goalX - robotPose.getX();
         double dy = goalY - robotPose.getY();
@@ -192,23 +205,25 @@ public class TurretLock {
         if (invertTurret) turretCmdDeg = -turretCmdDeg;
 
         // 2) VISION: find tag error and update vision trim
+        finalDeg = manualTrimDeg + turretCmdDeg;
         limelightRead(result);
-        filteredVisionTrimDeg = result.getTx();
+        filteredVisionTrimDeg = -visionCorrect;
-        switch (TYPE){
+        switch (TYPE) {
             case LIME:
-                finalDeg = turretCmdDeg + manualTrimDeg - filteredVisionTrimDeg;
+                finalDeg = manualTrimDeg + turrPos / ticksPerDegree + filteredVisionTrimDeg;
-                if (!result.isValid()) {
+                if (result != null && (finalDeg > 20 || finalDeg < -20)) {
                     TYPE = MODE.ODOMETRY;
                 }
                 break;
             case ODOMETRY:
                 finalDeg = turretCmdDeg + manualTrimDeg;
-                if (finalDeg < 12 || finalDeg > -12) {
+                if ((finalDeg < 20 || finalDeg > -20) && result.isValid()) {
                     TYPE = MODE.LIME;
                 }
                 break;
         }
 
+//        finalDeg = filteredVisionTrimDeg;
         // Optional: keep within your allowed lock range (like ±90)
         finalDeg = clamp(finalDeg, -90, 90);
 
@@ -216,23 +231,36 @@ public class TurretLock {
         int targetTicks = (int) Math.round(finalDeg * ticksPerDegree);
         targetTicks = clamp(targetTicks, minTicks, maxTicks);
 
+        return targetTicks;
 //        double pow = pid.calculate(targetTicks);
 //        turret.setPower(pow);
 
-        turret.setTargetPosition(targetTicks);
+//        turret.setTargetPosition(targetTicks);
-        turret.setMode(DcMotorEx.RunMode.RUN_TO_POSITION);
+//        turret.setMode(DcMotorEx.RunMode.RUN_TO_POSITION);
-        turret.setPower(1.0);
+//        turret.setPower(1.0);
+    }
+
+    public double updateHeading(Pose2d robotPose, LLResult result, double turrPos) {
+        double robotHeadingDeg = Math.toDegrees(robotPose.getHeading());
+
+        if (result.getTx() < 1 || result.getTy() > -1) {
+            robotHeadingDeg = -(turrPos / ticksPerDegree - 45);
+        }
+
+        return robotHeadingDeg;
     }
 
     public MODE getTYPE() {
         return TYPE;
     }
 
-    public double getFinalDeg(){
+    public double getFinalDeg() {
         return finalDeg;
     }
 
-    /** Returns tag horizontal error in degrees (positive means tag is to the right), or null if no suitable tag. */
+    /**
+     * Returns tag horizontal error in degrees (positive means tag is to the right), or null if no suitable tag.
+     */
 
     private static double normalizeDeg(double deg) {
         while (deg > 180) deg -= 360;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java

@@ -45,7 +45,7 @@ public class DriveConstants {
      * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO.
      */
     public static double WHEEL_RADIUS = 2.047; // in
-    public static double GEAR_RATIO = (24 / 22); // output (wheel) speed / input (motor) speed
+    public static double GEAR_RATIO = (24.0 / 22.0); // output (wheel) speed / input (motor) speed
     public static double TRACK_WIDTH = 12.3; // in
 
     /*
@@ -65,8 +65,8 @@ public class DriveConstants {
      * small and gradually increase them later after everything is working. All distance units are
      * inches.
      */
-    public static double MAX_VEL = 60;
+    public static double MAX_VEL = 72;
-    public static double MAX_ACCEL = 60;
+    public static double MAX_ACCEL = 72;
     public static double MAX_ANG_VEL = Math.toRadians(90);
     public static double MAX_ANG_ACCEL = Math.toRadians(90);