2/11 changes

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.topFly.setVelocity(1800);
-        robot.bottomFly.setVelocity(1610);
+        robot.bottomFly.setVelocity(1800);
-        robot.launchHood.setPosition(0.6);
+        robot.launchHood.setPosition(0.55);
     }
 
     public void reset(){
         robot.intake.set(0);
-//        robot.topFly.setVelocity(1500);
+//        robot.topFly.setVelocity(1200);
-//        robot.bottomFly.setVelocity(1500);
+//        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)
+                .back(47)
-                .UNSTABLE_addTemporalMarkerOffset(.5, () -> {
+                .waitSeconds(0.5)
-                    robot.intake.set(0.6);
+                .UNSTABLE_addTemporalMarkerOffset(0.3, () -> {
-                    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)
                 .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.intake.set(1);
-                    robot.activeTransfer.setPosition(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.intake.set(1);
-                    robot.activeTransfer.setPosition(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.intake.set(0.8);
-                    robot.activeTransfer.setPosition(1);
+                    robot.activeTransfer.setPosition(0.85);
                 })
-                .waitSeconds(2)
+                .waitSeconds(1.5)
                   //     .lineToLinearHeading(new Pose2d(55.5,68, Math.toRadians(-45)))
 
-                .UNSTABLE_addTemporalMarkerOffset(0.5, () -> {
+//                .UNSTABLE_addTemporalMarkerOffset(0.5, () -> {
-                    turretPos(84);
+//                    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);
+            }
 //            }
         }
     }

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();
-            }
-        }
-    }
-}

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/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.bottomFly.setVelocity(1700);
-                robot.topFly.setVelocity(0);
+                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.activeTransfer.setPosition(0.85);
-                    robot.intake.set(1.0);
+                    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();
         }
     }

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

@@ -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);
+
         }
     }
 }