2/11 changes

2026-02-19 18:04:41 -06:00
parent 97f5332c71
commit 045ef98262
6 changed files with 476 additions and 153 deletions
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepot.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepot.java
@@ -2,6 +2,8 @@ package org.firstinspires.ftc.teamcode.Auton;//package org.firstinspires.ftc.tea

 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;
@@ -11,6 +13,7 @@ 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 Depot Cycle")
@@ -19,6 +22,20 @@ public class blueDepot 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);
    }
@@ -41,19 +58,19 @@ public class blueDepot extends LinearOpMode {
    }

    public void shoot(){
-        robot.topFly.setVelocity(1610);
-        robot.bottomFly.setVelocity(1610);
-        robot.launchHood.setPosition(0.6);
+        robot.topFly.setVelocity(1800);
+        robot.bottomFly.setVelocity(1800);
+        robot.launchHood.setPosition(0.55);
    }

    public void reset(){
        robot.intake.set(0);
-//        robot.topFly.setVelocity(1500);
-//        robot.bottomFly.setVelocity(1500);
+//        robot.topFly.setVelocity(1200);
+//        robot.bottomFly.setVelocity(1200);
 //        robot.topFly.set(0);
 //        robot.bottomFly.set(0);
 //        robot.launchHood.setPosition(0.6);
-        robot.activeTransfer.setPosition(0.8);
+        robot.activeTransfer.setPosition(1);
    }


@@ -62,107 +79,114 @@ public class blueDepot extends LinearOpMode {
        //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, 52, Math.toRadians(135));
+        Pose2d startPose = new Pose2d(54, 46, Math.toRadians(52));

        drive.setPoseEstimate(startPose);

        TrajectorySequence threeBall = drive.trajectorySequenceBuilder(startPose)
                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
                    reset();
-                    turretPos(5);
+//                    turretPos(5);
                })
                .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
                    shoot();
                })
-                .strafeLeft(35)
-                .UNSTABLE_addTemporalMarkerOffset(.5, () -> {
-                    robot.intake.set(0.6);
-                    robot.activeTransfer.setPosition(1);
+                .back(47)
+                .waitSeconds(0.5)
+                .UNSTABLE_addTemporalMarkerOffset(0.3, () -> {
+                    robot.intake.set(1);
+                    robot.activeTransfer.setPosition(0.85);
                })

 //                .UNSTABLE_addTemporalMarkerOffset(2.2, () -> {
 //                    robot.launchHood.setPosition(0.5);
 //                })
-                .waitSeconds(2)
+                .waitSeconds(1.5)
                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
                    reset();
                })

                //6ball
-                .lineToLinearHeading(new Pose2d(11,28, Math.toRadians(90)))
+                .lineToLinearHeading(new Pose2d(8,28, Math.toRadians(90)))
                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
                    spintakeStart();
                })
-                .forward(35, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+                .forward(30, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
                    spintakeEnd();
-                })
-                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
                    shoot();
                })
-                .lineToLinearHeading(new Pose2d(24,23, Math.toRadians(135)))
+                .lineToLinearHeading(new Pose2d(24,14, Math.toRadians(52)))
                .UNSTABLE_addTemporalMarkerOffset(.5, () -> {
-                    robot.intake.set(0.6);
-                    robot.activeTransfer.setPosition(1);
+                    robot.intake.set(1);
+                    robot.activeTransfer.setPosition(0.85);
                })

 //                .UNSTABLE_addTemporalMarkerOffset(2.2, () -> {
 //                    robot.launchHood.setPosition(0.5);
 //                })
-                .waitSeconds(2)
+                .waitSeconds(1.5)

                //9ball
                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
                    reset();
                })
-                .lineToLinearHeading(new Pose2d(-14,27, Math.toRadians(90)))
+                .lineToLinearHeading(new Pose2d(-19,27, Math.toRadians(90)))
                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
                    spintakeStart();
                })
-                .forward(35,  SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+                .forward(18, SampleMecanumDrive.getVelocityConstraint(15, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
                    spintakeEnd();
                })
-                .lineToLinearHeading(new Pose2d(24,23, Math.toRadians(135)))
+                .lineToLinearHeading(new Pose2d(-12,54, Math.toRadians(90)))
+                .lineToLinearHeading(new Pose2d(26,14, Math.toRadians(52)))
                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
                    reset();
                })
                .UNSTABLE_addTemporalMarkerOffset(.5, () -> {
-                    robot.intake.set(0.6);
-                    robot.activeTransfer.setPosition(1);
+                    robot.intake.set(1);
+                    robot.activeTransfer.setPosition(0.85);
                })
-                .waitSeconds(2)
+                .waitSeconds(1.5)

                //12ball
                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
                    reset();
                })
-                .lineToLinearHeading(new Pose2d(-33,27, Math.toRadians(90)))
+                .lineToLinearHeading(new Pose2d(-40,22, Math.toRadians(90)))
                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
                    spintakeStart();
                })
-                .forward(35,  SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+                .forward(33, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
                        SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
                    spintakeEnd();
                })
-                .lineToLinearHeading(new Pose2d(24,23, Math.toRadians(135)))
+                .lineToLinearHeading(new Pose2d(35,14, Math.toRadians(52)))
                .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
                    shoot();
                })
                .UNSTABLE_addTemporalMarkerOffset(.5, () -> {
-                    robot.intake.set(0.6);
-                    robot.activeTransfer.setPosition(1);
+                    robot.intake.set(0.8);
+                    robot.activeTransfer.setPosition(0.85);
                })
-                .waitSeconds(2)
+                .waitSeconds(1.5)
                  //     .lineToLinearHeading(new Pose2d(55.5,68, Math.toRadians(-45)))

-                .UNSTABLE_addTemporalMarkerOffset(0.5, () -> {
-                    turretPos(84);
-                })
+//                .UNSTABLE_addTemporalMarkerOffset(0.5, () -> {
+//                    turretPos(84);
+//                })
                .waitSeconds(1)
                .build();

@@ -230,8 +254,31 @@ public class blueDepot extends LinearOpMode {
        waitForStart();
        if (opModeIsActive() && !isStopRequested()) {
 //          while (opModeIsActive()) {
-            drive.followTrajectorySequence(threeBall);
+            drive.followTrajectorySequenceAsync(threeBall);
            PoseStorage.currentPose = drive.getPoseEstimate();
+
+            while(opModeIsActive()){
+                robot.leftPTO.setPosition(0.2);
+                robot.rightPTO.setPosition(0.14);
+                drive.update();
+                result = limelight.getLatestResult();
+                double vision = result.getTx() - 3;
+
+                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);
+            }
 //            }
        }
    }
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepotFar.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepotFar.java
@@ -1,106 +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;
-import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceBuilder;
-
-@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 threeBall = drive.trajectorySequenceBuilder(startPose)
-                .forward(3)
-                .addDisplacementMarker(()-> {
-                    shoot();
-                })
-                .build();
-
-        TrajectorySequence letGo = drive.trajectorySequenceBuilder(threeBall.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();
-
-        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();
-            }
-        }
-    }
-}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueFar.java
+++ b/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);
+            }
+//            }
+        }
+    }
+}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsV1.java
@@ -27,7 +27,7 @@ public class DriverControlsV1 extends LinearOpMode {

    public static double HOOD_MIN = 0.3;
    public static double HOOD_MAX = 1.0;
-    public static double HOOD_CORRECTION_SENSITIVITY = 0.04;
+    public static double HOOD_CORRECTION_SENSITIVITY = 0.01;
    // -------------------------

    // State tracking to prevent redundant hardware writes (The Fix)
@@ -81,7 +81,7 @@ public class DriverControlsV1 extends LinearOpMode {

        while (opModeIsActive()) {
             result = limelight.getLatestResult();
-             double vision = result.getTx();
+             double vision = result.getTx()-3;

            // 1. INPUTS & TURRET
 //            if (gamepad2.yWasPressed()) {
@@ -104,7 +104,7 @@ public class DriverControlsV1 extends LinearOpMode {


            // 2. PTO OPTIMIZATION (Only write if value is new)
-            double targetPTO = 0.2;
+            double targetPTO = 0.15;
            if (targetPTO != lastPTOPosition) {
                robot.leftPTO.setPosition(targetPTO);
                robot.rightPTO.setPosition(targetPTO);
@@ -156,7 +156,7 @@ public class DriverControlsV1 extends LinearOpMode {
            double currentVel = (robot.topFly.getVelocity() + robot.bottomFly.getVelocity()) / 2.0;
            double velocityError = targetVelocity - currentVel;

-            double hoodAdjustment = (velocityError / 80.0) * HOOD_CORRECTION_SENSITIVITY;
+            double hoodAdjustment = (velocityError / 50.0) * HOOD_CORRECTION_SENSITIVITY;
            double correctedHood = Range.clip(baseHood - hoodAdjustment, HOOD_MIN, HOOD_MAX);

            if (gamepad2.dpad_up) adjust = true;
@@ -167,8 +167,8 @@ public class DriverControlsV1 extends LinearOpMode {
                robot.topFly.setVelocity(targetVelocity);
                robot.launchHood.setPosition(correctedHood);
            } else {
-                robot.bottomFly.setVelocity(0);
-                robot.topFly.setVelocity(0);
+                robot.bottomFly.setVelocity(1700);
+                robot.topFly.setVelocity(1700);
            }

            // 7. SHOOTING STATE MACHINE
@@ -178,10 +178,18 @@ public class DriverControlsV1 extends LinearOpMode {
                shootTimer.reset();
            }

+            if (gamepad2.yWasPressed()) {
+                flywheelTuner+=.01;
+            }
+
+            if (gamepad2.aWasPressed()) {
+                flywheelTuner-=.01;
+            }
+
            switch (shootStatus) {
                case START_SHOOT:
-                    robot.activeTransfer.setPosition(0.81);
-                    robot.intake.set(1.0);
+                    robot.activeTransfer.setPosition(0.85);
+                    robot.intake.set(1);
                    targetTicks = 0;
                    if (shootTimer.milliseconds() > 2000) {
                        shootStatus = ShootState.RESET;
@@ -219,6 +227,7 @@ public class DriverControlsV1 extends LinearOpMode {
            telemetry.addData("Status", "Running");
            telemetry.addData("Vel Error", (int)velocityError);
            telemetry.addData("Hood", "%.2f", correctedHood);
+            telemetry.addData("TunerMult", flywheelTuner);
            telemetry.update();
        }
    }
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OUTDATED/blueDepotFar.java
+++ b/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();
+            }
+        }
+    }
+}
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PTOTest.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PTOTest.java
@@ -11,7 +11,8 @@ import org.firstinspires.ftc.teamcode.OUTDATED.Hware;
@Config

 public class PTOTest extends LinearOpMode {
-    public static double activeTransfer = 0;
+    public static double launchHood = 0.3;
+    public static double activeTransfer = 0.3;

    @Override
    public void runOpMode() throws InterruptedException {
@@ -20,7 +21,9 @@ public class PTOTest extends LinearOpMode {

        waitForStart();
        while (opModeIsActive()) {
+            robot.launchHood.setPosition(launchHood);
            robot.activeTransfer.setPosition(activeTransfer);
+
        }
    }
 }