yayyyyy

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java

@@ -1,6 +1,7 @@
 package org.firstinspires.ftc.teamcode.autonomous;
 
 import static org.firstinspires.ftc.teamcode.constants.Poses.*;
+
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
 import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*;
 import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.*;
@@ -11,19 +12,20 @@ import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+import com.acmerobotics.roadrunner.Action;
 import com.acmerobotics.roadrunner.ParallelAction;
 import com.acmerobotics.roadrunner.Pose2d;
 import com.acmerobotics.roadrunner.SequentialAction;
 import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
 import com.acmerobotics.roadrunner.Vector2d;
+import com.acmerobotics.roadrunner.ftc.Actions;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.acmerobotics.roadrunner.Action;
-import com.acmerobotics.roadrunner.ftc.Actions;
 
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
 import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam;
+import org.firstinspires.ftc.teamcode.utils.Flywheel;
 import org.firstinspires.ftc.teamcode.utils.Robot;
 
 @Config
@@ -38,9 +40,13 @@ public class Blue_V2 extends LinearOpMode {
 
     AprilTagWebcam aprilTag;
 
-    public static double intake1Time = 4.0;
+    Flywheel flywheel;
 
-    public static double intake2Time = 5.5;
+    double velo = 0.0;
+    double targetVelocity = 0.0;
+    public static double intake1Time = 6.5;
+
+    public static double intake2Time = 6.5;
 
     public static double colorDetect = 3.0;
 
@@ -59,6 +65,7 @@ public class Blue_V2 extends LinearOpMode {
     int b3 = 0;// 0 = no ball, 1 = green, 2 = purple
 
     boolean spindexPosEqual(double spindexer) {
+        TELE.addData("Velocity", velo);
         TELE.addLine("spindex equal");
         TELE.update();
         return (scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) > spindexer - 0.01 &&
@@ -67,7 +74,6 @@ public class Blue_V2 extends LinearOpMode {
 
     public Action initShooter(int vel) {
         return new Action() {
-            double velo = 0.0;
             double initPos = 0.0;
             double stamp = 0.0;
             double stamp1 = 0.0;
@@ -80,34 +86,15 @@ public class Blue_V2 extends LinearOpMode {
                     stamp2 = getRuntime();
                 }
 
+                targetVelocity = (double) vel;
                 ticker++;
                 if (ticker % 16 == 0) {
-                    currentPos = (double) robot.shooter1.getCurrentPosition() / 2048;
                     stamp = getRuntime();
-                    velo = -60 * ((currentPos - initPos) / (stamp - stamp1));
-                    initPos = currentPos;
                     stamp1 = stamp;
                 }
 
-                if (vel - velo > 500) {
+                powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
-                    powPID = 1.0;
+                velo = flywheel.getVelo();
-                } else {
-                    double feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18;
-
-                    // --- PROPORTIONAL CORRECTION ---
-                    double error = vel - velo;
-                    double correction = kP * error;
-
-                    // limit how fast power changes (prevents oscillation)
-                    correction = Math.max(-maxStep, Math.min(maxStep, correction));
-
-                    // --- FINAL MOTOR POWER ---
-                    powPID = feed + correction;
-
-                    // clamp to allowed range
-                    powPID = Math.max(0, Math.min(1, powPID));
-                }
-
                 robot.shooter1.setPower(powPID);
                 robot.shooter2.setPower(powPID);
                 robot.transfer.setPower(1);
@@ -119,6 +106,7 @@ public class Blue_V2 extends LinearOpMode {
                     stamp2 = getRuntime();
                     return true;
                 } else if (steady && getRuntime() - stamp2 > 1.5){
+                    TELE.addData("Velocity", velo);
                     TELE.addLine("finished init");
                     TELE.update();
                     return false;
@@ -131,49 +119,12 @@ public class Blue_V2 extends LinearOpMode {
 
     public Action steadyShooter(int vel, boolean last) {
         return new Action() {
-            double velo = 0.0;
-            double initPos = 0.0;
             double stamp = 0.0;
-            double stamp1 = 0.0;
-            double ticker = 0.0;
-            double stamp2 = 0.0;
-            double currentPos = 0.0;
             boolean steady = false;
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
-                if (ticker == 0) {
+                powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
-                    stamp2 = getRuntime();
+                velo = flywheel.getVelo();
-                }
+                steady = flywheel.getSteady();
-
-                ticker++;
-                if (ticker % 8 == 0) {
-                    currentPos = (double) robot.shooter1.getCurrentPosition() / 2048;
-                    stamp = getRuntime();
-                    velo = -60 * ((currentPos - initPos) / (stamp - stamp1));
-                    initPos = currentPos;
-                    stamp1 = stamp;
-                }
-
-                if (vel - velo > 500) {
-                    powPID = 1.0;
-                } else if (velo - vel > 500){
-                    powPID = 0.0;
-                } else {
-                    double feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18;
-
-                    // --- PROPORTIONAL CORRECTION ---
-                    double error = vel - velo;
-                    double correction = kP * error;
-
-                    // limit how fast power changes (prevents oscillation)
-                    correction = Math.max(-maxStep, Math.min(maxStep, correction));
-
-                    // --- FINAL MOTOR POWER ---
-                    powPID = feed + correction;
-
-                    // clamp to allowed range
-                    powPID = Math.max(0, Math.min(1, powPID));
-                }
-
                 robot.shooter1.setPower(powPID);
                 robot.shooter2.setPower(powPID);
                 robot.transfer.setPower(1);
@@ -181,11 +132,14 @@ public class Blue_V2 extends LinearOpMode {
                 TELE.addData("Velocity", velo);
                 TELE.update();
 
-                if (Math.abs(vel - velo) < 100 && last && !steady){
+
+                if (last && !steady){
                     stamp = getRuntime();
-                    steady = true;
+                    drive.updatePoseEstimate();
+
+                    teleStart = drive.localizer.getPose();
                     return false;
-                } else if (steady && getRuntime() - stamp > 1.0) {
+                } else if (steady) {
                     stamp = getRuntime();
                     return true;
                 } else {
@@ -216,6 +170,7 @@ public class Blue_V2 extends LinearOpMode {
                 }
                 aprilTag.update();
 
+                TELE.addData("Velocity", velo);
                 TELE.addData("21", gpp);
                 TELE.addData("22", pgp);
                 TELE.addData("23", ppg);
@@ -236,7 +191,6 @@ public class Blue_V2 extends LinearOpMode {
         return new Action() {
             double currentPos = 0.0;
             double stamp = 0.0;
-            double velo = 0.0;
             double initPos = 0.0;
             double stamp1 = 0.0;
             int ticker = 0;
@@ -272,12 +226,20 @@ public class Blue_V2 extends LinearOpMode {
                     powPID = Math.max(0, Math.min(1, powPID));
                 }
 
+                powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
+                velo = flywheel.getVelo();
                 robot.shooter1.setPower(powPID);
                 robot.shooter2.setPower(powPID);
                 robot.spin1.setPosition(spindexer);
                 robot.spin2.setPosition(1-spindexer);
+                TELE.addData("Velocity", velo);
                 TELE.addLine("spindex");
                 TELE.update();
+
+                drive.updatePoseEstimate();
+
+                teleStart = drive.localizer.getPose();
+
                 return !spindexPosEqual(spindexer);
             }
         };
@@ -290,13 +252,14 @@ public class Blue_V2 extends LinearOpMode {
             boolean transferIn = false;
             double currentPos = 0.0;
             double stamp = 0.0;
-            double velo = 0.0;
             double initPos = 0.0;
             double stamp1 = 0.0;
             @Override
             public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+                TELE.addData("Velocity", velo);
                 TELE.addLine("shooting");
                 TELE.update();
+
                 if (ticker % 8 == 0) {
                     currentPos = (double) robot.shooter1.getCurrentPosition() / 2048;
                     stamp = getRuntime();
@@ -326,9 +289,15 @@ public class Blue_V2 extends LinearOpMode {
                     powPID = Math.max(0, Math.min(1, powPID));
                 }
 
+                powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
+                velo = flywheel.getVelo();
                 robot.shooter1.setPower(powPID);
                 robot.shooter2.setPower(powPID);
 
+                drive.updatePoseEstimate();
+
+                teleStart = drive.localizer.getPose();
+
 
                 if (ticker == 1) {
                     transferStamp = getRuntime();
@@ -336,12 +305,14 @@ public class Blue_V2 extends LinearOpMode {
                 }
                 if (getRuntime() - transferStamp > waitTransfer && !transferIn) {
                     robot.transferServo.setPosition(transferServo_in);
+                    TELE.addData("Velocity", velo);
                     TELE.addData("ticker", ticker);
                     TELE.update();
                     transferIn = true;
                     return true;
                 } else if (getRuntime() - transferStamp > waitTransfer+waitTransferOut && transferIn){
                     robot.transferServo.setPosition(transferServo_out);
+                    TELE.addData("Velocity", velo);
                     TELE.addLine("shot once");
                     TELE.update();
                     return false;
@@ -378,9 +349,14 @@ public class Blue_V2 extends LinearOpMode {
                 robot.spin1.setPosition(position);
                 robot.spin2.setPosition(1 - position);
 
+                TELE.addData("Velocity", velo);
                 TELE.addLine("Intaking");
                 TELE.update();
 
+                drive.updatePoseEstimate();
+
+                teleStart = drive.localizer.getPose();
+
                 robot.intake.setPower(1);
                 if ((s1D < 40.0 && s2D < 40.0 && s3D < 40.0) || getRuntime() - stamp > intakeTime) {
                     robot.intake.setPower(0);
@@ -416,6 +392,10 @@ public class Blue_V2 extends LinearOpMode {
                 double s2D = robot.color2.getDistance(DistanceUnit.MM);
                 double s3D = robot.color3.getDistance(DistanceUnit.MM);
 
+                drive.updatePoseEstimate();
+
+                teleStart = drive.localizer.getPose();
+
                 if (s1D < 40) {
 
                     double green = robot.color1.getNormalizedColors().green;
@@ -463,6 +443,7 @@ public class Blue_V2 extends LinearOpMode {
                     }
                 }
 
+                TELE.addData("Velocity", velo);
                 TELE.addLine("Detecting");
                 TELE.addData("Distance 1", s1D);
                 TELE.addData("Distance 2", s2D);
@@ -482,6 +463,8 @@ public class Blue_V2 extends LinearOpMode {
 
         robot = new Robot(hardwareMap);
 
+        flywheel = new Flywheel();
+
         TELE = new MultipleTelemetry(
                 telemetry, FtcDashboard.getInstance().getTelemetry()
         );
@@ -495,7 +478,7 @@ public class Blue_V2 extends LinearOpMode {
         TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
                 .strafeToLinearHeading(new Vector2d(bx1, by1), bh1);
 
-        TrajectoryActionBuilder pickup1 = drive.actionBuilder(new Pose2d(rx1, by1, bh1))
+        TrajectoryActionBuilder pickup1 = drive.actionBuilder(new Pose2d(bx1, by1, bh1))
                 .strafeToLinearHeading(new Vector2d(bx2a, by2a), bh2a)
                 .strafeToLinearHeading(new Vector2d(bx2b, by2b), bh2b);
 
@@ -516,14 +499,14 @@ public class Blue_V2 extends LinearOpMode {
         while (opModeInInit()) {
 
             if (gamepad2.dpadUpWasPressed()) {
-                hoodDefault -= 0.01;
+                hoodAuto-= 0.01;
             }
 
             if (gamepad2.dpadDownWasPressed()) {
-                hoodDefault += 0.01;
+                hoodAuto += 0.01;
             }
 
-            robot.hood.setPosition(hoodDefault);
+            robot.hood.setPosition(hoodAuto);
 
             robot.turr1.setPosition(turret_detectBlue);
             robot.turr2.setPosition(1 - turret_detectBlue);
@@ -534,7 +517,7 @@ public class Blue_V2 extends LinearOpMode {
             robot.spin2.setPosition(1 - spindexer_intakePos1);
 
             aprilTag.update();
-
+            TELE.addData("Velocity", velo);
             TELE.update();
         }
 
@@ -544,7 +527,7 @@ public class Blue_V2 extends LinearOpMode {
 
         if (opModeIsActive()) {
 
-            robot.hood.setPosition(hoodStart);
+            robot.hood.setPosition(hoodAuto);
 
             Actions.runBlocking(
                     new ParallelAction(
@@ -553,13 +536,22 @@ public class Blue_V2 extends LinearOpMode {
                             Obelisk()
                     )
             );
+            drive.updatePoseEstimate();
 
+            teleStart = drive.localizer.getPose();
+
+            powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
+            velo = flywheel.getVelo();
             robot.shooter1.setPower(powPID);
             robot.shooter2.setPower(powPID);
 
             shootingSequence();
 
-            robot.hood.setPosition(hoodDefault);
+            robot.hood.setPosition(hoodAuto);
+
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
 
             Actions.runBlocking(
                     new ParallelAction(
@@ -567,6 +559,9 @@ public class Blue_V2 extends LinearOpMode {
                             intake(intake1Time)
                     )
             );
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
 
             Actions.runBlocking(
                     new ParallelAction(
@@ -576,10 +571,19 @@ public class Blue_V2 extends LinearOpMode {
                     )
             );
 
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
+
+            powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
+            velo = flywheel.getVelo();
             robot.shooter1.setPower(powPID);
             robot.shooter2.setPower(powPID);
 
             shootingSequence();
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
 
             Actions.runBlocking(
                     new ParallelAction(
@@ -587,6 +591,9 @@ public class Blue_V2 extends LinearOpMode {
                             intake(intake2Time)
                     )
             );
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
 
             Actions.runBlocking(
                     new ParallelAction(
@@ -596,6 +603,8 @@ public class Blue_V2 extends LinearOpMode {
                     )
             );
 
+            powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
+            velo = flywheel.getVelo();
             robot.shooter1.setPower(powPID);
             robot.shooter2.setPower(powPID);
 
@@ -605,8 +614,8 @@ public class Blue_V2 extends LinearOpMode {
 
             teleStart = drive.localizer.getPose();
 
+            TELE.addData("Velocity", velo);
             TELE.addLine("finished");
-
             TELE.update();
 
             sleep(2000);
@@ -616,6 +625,7 @@ public class Blue_V2 extends LinearOpMode {
     }
 
     public void shootingSequence() {
+        TELE.addData("Velocity", velo);
         if (gpp) {
             if (b1 + b2 + b3 == 4) {
                 if (b1 == 2 && b2 - b3 == 0) {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java

@@ -131,8 +131,12 @@ public class Red_V2 extends LinearOpMode {
                 TELE.addData("Velocity", velo);
                 TELE.update();
 
+
                 if (last && !steady){
                     stamp = getRuntime();
+                    drive.updatePoseEstimate();
+
+                    teleStart = drive.localizer.getPose();
                     return false;
                 } else if (steady) {
                     stamp = getRuntime();
@@ -230,6 +234,11 @@ public class Red_V2 extends LinearOpMode {
                 TELE.addData("Velocity", velo);
                 TELE.addLine("spindex");
                 TELE.update();
+
+                drive.updatePoseEstimate();
+
+                teleStart = drive.localizer.getPose();
+
                 return !spindexPosEqual(spindexer);
             }
         };
@@ -284,6 +293,10 @@ public class Red_V2 extends LinearOpMode {
                 robot.shooter1.setPower(powPID);
                 robot.shooter2.setPower(powPID);
 
+                drive.updatePoseEstimate();
+
+                teleStart = drive.localizer.getPose();
+
 
                 if (ticker == 1) {
                     transferStamp = getRuntime();
@@ -339,6 +352,10 @@ public class Red_V2 extends LinearOpMode {
                 TELE.addLine("Intaking");
                 TELE.update();
 
+                drive.updatePoseEstimate();
+
+                teleStart = drive.localizer.getPose();
+
                 robot.intake.setPower(1);
                 if ((s1D < 40.0 && s2D < 40.0 && s3D < 40.0) || getRuntime() - stamp > intakeTime) {
                     robot.intake.setPower(0);
@@ -374,6 +391,10 @@ public class Red_V2 extends LinearOpMode {
                 double s2D = robot.color2.getDistance(DistanceUnit.MM);
                 double s3D = robot.color3.getDistance(DistanceUnit.MM);
 
+                drive.updatePoseEstimate();
+
+                teleStart = drive.localizer.getPose();
+
                 if (s1D < 40) {
 
                     double green = robot.color1.getNormalizedColors().green;
@@ -477,14 +498,14 @@ public class Red_V2 extends LinearOpMode {
         while (opModeInInit()) {
 
             if (gamepad2.dpadUpWasPressed()) {
-                hoodDefault -= 0.01;
+                hoodAuto-= 0.01;
             }
 
             if (gamepad2.dpadDownWasPressed()) {
-                hoodDefault += 0.01;
+                hoodAuto += 0.01;
             }
 
-            robot.hood.setPosition(hoodDefault);
+            robot.hood.setPosition(hoodAuto);
 
             robot.turr1.setPosition(turret_detectRed);
             robot.turr2.setPosition(1 - turret_detectRed);
@@ -499,13 +520,14 @@ public class Red_V2 extends LinearOpMode {
             TELE.update();
         }
 
+
         waitForStart();
 
         if (isStopRequested()) return;
 
         if (opModeIsActive()) {
 
-            robot.hood.setPosition(hoodStart);
+            robot.hood.setPosition(hoodAuto);
 
             Actions.runBlocking(
                     new ParallelAction(
@@ -514,6 +536,9 @@ public class Red_V2 extends LinearOpMode {
                             Obelisk()
                     )
             );
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
 
             powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
             velo = flywheel.getVelo();
@@ -522,7 +547,11 @@ public class Red_V2 extends LinearOpMode {
 
             shootingSequence();
 
-            robot.hood.setPosition(hoodDefault);
+            robot.hood.setPosition(hoodAuto);
+
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
 
             Actions.runBlocking(
                     new ParallelAction(
@@ -530,6 +559,9 @@ public class Red_V2 extends LinearOpMode {
                             intake(intake1Time)
                     )
             );
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
 
             Actions.runBlocking(
                     new ParallelAction(
@@ -539,12 +571,19 @@ public class Red_V2 extends LinearOpMode {
                     )
             );
 
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
+
             powPID = flywheel.manageFlywheel(AUTO_CLOSE_VEL, (double) robot.shooter1.getCurrentPosition());
             velo = flywheel.getVelo();
             robot.shooter1.setPower(powPID);
             robot.shooter2.setPower(powPID);
 
             shootingSequence();
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
 
             Actions.runBlocking(
                     new ParallelAction(
@@ -552,6 +591,9 @@ public class Red_V2 extends LinearOpMode {
                             intake(intake2Time)
                     )
             );
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
 
             Actions.runBlocking(
                     new ParallelAction(

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java

@@ -26,7 +26,7 @@ public class Poses {
 
 
 
-    public static double rx1 = 50, ry1 = -4, rh1 = 0;
+    public static double rx1 = 46, ry1 = -4, rh1 = 0;
 
     public static double rx2a = 45, ry2a = 5, rh2a = Math.toRadians(140);
 
@@ -36,7 +36,7 @@ public class Poses {
 
     public static double rx3b = 34, ry3b = 58, rh3b = Math.toRadians(140);
 
-    public static double bx1 = 50, by1 = 4, bh1 = 0;
+    public static double bx1 = 46, by1 = 4, bh1 = 0;
 
     public static double bx2a = 45, by2a = -5, bh2a = Math.toRadians(-140);
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java

@@ -1,6 +1,5 @@
 package org.firstinspires.ftc.teamcode.constants;
 
-
 import com.acmerobotics.dashboard.config.Config;
 
 @Config
@@ -21,24 +20,22 @@ public class ServoPositions {
 
     public static double transferServo_in = 0.38;
 
-
-
     public static double turret_range = 0.9;
 
     public static double hoodDefault = 0.6;
 
-    public static double hoodStart = 0.6;
+    public static double hoodAuto = 0.59;
 
     public static double hoodHigh = 0.21;
 
     public static double hoodLow = 1.0;
 
     public static double turret_red = 0.4;
-    public static double turret_blue = 0.4;
+    public static double turret_blue = 0.38;
 
     public static double turret_detectRed = 0.2;
 
     public static double turret_detectBlue = 0.6;
-
+    public static double turrDefault = 0.40;
 
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java

@@ -19,5 +19,5 @@ public class ShooterVars {
     public static double maxStep = 0.06;         // prevents sudden jumps
 
     // VELOCITY CONSTANTS
-    public static int AUTO_CLOSE_VEL = 3050; //3300;
+    public static int AUTO_CLOSE_VEL = 3025; //3300;
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java

@@ -7,6 +7,7 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in;
 import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.turrDefault;
 import static org.firstinspires.ftc.teamcode.constants.ShooterVars.waitTransfer;
 import static org.firstinspires.ftc.teamcode.constants.ShooterVars.waitTransferOut;
 import static org.firstinspires.ftc.teamcode.tests.ShooterTest.kP;
@@ -14,26 +15,20 @@ import static org.firstinspires.ftc.teamcode.tests.ShooterTest.maxStep;
 import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.restPos;
 import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.scalar;
 
-
-
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.Pose2d;
-import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
-import com.acmerobotics.roadrunner.Vector2d;
-import com.acmerobotics.roadrunner.ftc.Actions;
 import com.qualcomm.hardware.lynx.LynxModule;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
-import org.firstinspires.ftc.teamcode.utils.Robot;
 import org.firstinspires.ftc.teamcode.utils.AprilTagWebcam;
+import org.firstinspires.ftc.teamcode.utils.Robot;
 import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
 
-
 import java.util.ArrayList;
 import java.util.List;
 
@@ -46,10 +41,21 @@ public class TeleopV2 extends LinearOpMode {
     public static double hoodDefaultPos = 0.5;
     public static double desiredTurretAngle = 180;
     public static double velMultiplier = 20;
+    public static double shootStamp2 = 0.0;
+
+
     public double vel = 3000;
     public boolean autoVel = true;
     public double manualOffset = 0.0;
     public boolean autoHood = true;
+    public boolean green1 = false;
+    public boolean green2 = false;
+    public boolean green3 = false;
+    public double shootStamp = 0.0;
+    public boolean circle = false;
+    public boolean square = false;
+    public boolean triangle = false;
+    double autoHoodOffset = 0.0;
     Robot robot;
     MultipleTelemetry TELE;
     boolean intake = false;
@@ -67,12 +73,8 @@ public class TeleopV2 extends LinearOpMode {
     List<Boolean> s1 = new ArrayList<>();
     List<Boolean> s2 = new ArrayList<>();
     List<Boolean> s3 = new ArrayList<>();
-
     boolean oddBallColor = false;
-
     AprilTagWebcam aprilTagWebcam = new AprilTagWebcam();
-
-
     MecanumDrive drive;
     double hoodOffset = 0.0;
     boolean shoot1 = false;
@@ -82,11 +84,14 @@ public class TeleopV2 extends LinearOpMode {
     boolean shootC = true;
     boolean manualTurret = false;
 
-    public boolean green1 = false;
+    boolean outtake1 = false;
-    public boolean green2 = false;
+    boolean outtake2 = false;
-    public boolean green3 = false;
+    boolean outtake3 = false;
+    boolean overrideTurr = false;
+
 
     List<Integer> shootOrder = new ArrayList<>();
+    boolean emergency = false;
     private double lastEncoderRevolutions = 0.0;
     private double lastTimeStamp = 0.0;
     private double velo1, velo;
@@ -95,11 +100,23 @@ public class TeleopV2 extends LinearOpMode {
     private double transferStamp = 0.0;
     private int tickerA = 1;
     private boolean transferIn = false;
-    public double shootStamp = 0.0;
-    public boolean circle = false;
-    public boolean square = false;
-    public boolean triangle = false;
 
+    public static double velPrediction(double distance) {
+
+        if (distance < 30) {
+            return 2750;
+        } else if (distance > 100) {
+            if (distance > 160) {
+                return 4200;
+            }
+            return 3700;
+        } else {
+            // linear interpolation between 40->2650 and 120->3600
+            double slope = (3700.0 - 2750.0) / (100.0 - 30);
+            return (int) Math.round(2750 + slope * (distance - 30));
+        }
+
+    }
 
     @Override
     public void runOpMode() throws InterruptedException {
@@ -122,10 +139,7 @@ public class TeleopV2 extends LinearOpMode {
         aprilTagWebcam.init(new Robot(hardwareMap), TELE);
 
         robot.turr1.setPosition(0.4);
-        robot.turr2.setPosition(1-0.4);
+        robot.turr2.setPosition(1 - 0.4);
-
-
-
 
         waitForStart();
         if (isStopRequested()) return;
@@ -133,7 +147,6 @@ public class TeleopV2 extends LinearOpMode {
 
             //DRIVETRAIN:
 
-
             double y = -gamepad1.right_stick_y; // Remember, Y stick value is reversed
             double x = gamepad1.right_stick_x * 1.1; // Counteract imperfect strafing
             double rx = gamepad1.left_stick_x;
@@ -144,21 +157,20 @@ public class TeleopV2 extends LinearOpMode {
             double frontRightPower = (y - x - rx) / denominator;
             double backRightPower = (y + x - rx) / denominator;
 
-
-
             robot.frontLeft.setPower(frontLeftPower);
             robot.backLeft.setPower(backLeftPower);
             robot.frontRight.setPower(frontRightPower);
             robot.backRight.setPower(backRightPower);
 
-
-
             //INTAKE:
 
             if (gamepad1.rightBumperWasPressed()) {
                 intake = !intake;
                 reject = false;
                 shootAll = false;
+                emergency = false;
+                overrideTurr = false;
+
 
             }
 
@@ -166,6 +178,8 @@ public class TeleopV2 extends LinearOpMode {
                 intake = false;
                 reject = true;
                 shootAll = false;
+                overrideTurr = false;
+
 
             }
 
@@ -259,7 +273,6 @@ public class TeleopV2 extends LinearOpMode {
                 s3T.add(getRuntime());
             }
 
-
             if (!s1.isEmpty()) {
                 green1 = checkGreen(s1, s1T);
             }
@@ -271,7 +284,6 @@ public class TeleopV2 extends LinearOpMode {
                 green3 = checkGreen(s3, s3T);
             }
 
-
             //SHOOTER:
 
             double penguin = 0;
@@ -320,7 +332,6 @@ public class TeleopV2 extends LinearOpMode {
 
             robot.transfer.setPower(1);
 
-
             //TURRET:
 
             double offset;
@@ -347,7 +358,7 @@ public class TeleopV2 extends LinearOpMode {
                 offset -= 360;
             }
 
-            double pos = 0.4;
+            double pos = turrDefault;
 
             TELE.addData("offset", offset);
 
@@ -359,12 +370,15 @@ public class TeleopV2 extends LinearOpMode {
                 pos = 0.97;
             }
 
-            if (manualTurret){
+            if (manualTurret) {
-                pos = 0.4 + (manualOffset/100);
+                pos = turrDefault + (manualOffset / 100);
             }
 
+            if (!overrideTurr) {
+
                 robot.turr1.setPosition(pos);
                 robot.turr2.setPosition(1 - pos);
+            }
 
             if (gamepad2.dpad_right) {
                 manualOffset -= 2;
@@ -372,8 +386,6 @@ public class TeleopV2 extends LinearOpMode {
                 manualOffset += 2;
             }
 
-        
-
             //VELOCITY AUTOMATIC
 
             if (autoVel) {
@@ -390,12 +402,10 @@ public class TeleopV2 extends LinearOpMode {
             } else if (gamepad2.right_stick_y > 0.5) {
                 autoVel = false;
                 manualVel = 2700;
-            }
+            } else if (gamepad2.right_stick_x > 0.5) {
-            else if (gamepad2.right_stick_x > 0.5) {
                 autoVel = false;
                 manualVel = 3600;
-            }
+            } else if (gamepad2.right_stick_x < -0.5) {
-            else if (gamepad2.right_stick_x < -0.5) {
                 autoVel = false;
                 manualVel = 3100;
             }
@@ -403,97 +413,143 @@ public class TeleopV2 extends LinearOpMode {
             //HOOD:
 
             if (autoHood) {
-                robot.hood.setPosition(hoodAnglePrediction(distanceToGoal));
+                robot.hood.setPosition(hoodAnglePrediction(distanceToGoal) + autoHoodOffset);
             } else {
                 robot.hood.setPosition(hoodDefaultPos + hoodOffset);
             }
 
             if (gamepad2.dpadUpWasPressed()) {
                 hoodOffset -= 0.03;
+                autoHoodOffset -= 0.02;
+
             } else if (gamepad2.dpadDownWasPressed()) {
                 hoodOffset += 0.03;
+                autoHoodOffset += 0.02;
+
             }
 
-            if (gamepad2.left_stick_x>0.5){
+            if (gamepad2.left_stick_x > 0.5) {
                 manualTurret = false;
-            } else if (gamepad2.left_stick_x<-0.5){
+            } else if (gamepad2.left_stick_x < -0.5) {
                 manualTurret = true;
                 manualOffset = 0;
-                if (gamepad2.left_bumper){
+                autoHoodOffset = 0;
+                if (gamepad2.left_bumper) {
                     xOffset = robotX;
                     yOffset = robotY;
                     headingOffset = robotHeading;
                 }
             }
 
-            if (gamepad2.left_stick_y<-0.5){
+            if (gamepad2.left_stick_y < -0.5) {
                 autoHood = true;
-            } else if (gamepad2.left_stick_y>0.5){
+            } else if (gamepad2.left_stick_y > 0.5) {
                 autoHood = false;
                 hoodOffset = 0;
-                if (gamepad2.left_bumper){
+                if (gamepad2.left_bumper) {
                     xOffset = robotX;
                     yOffset = robotY;
                     headingOffset = robotHeading;
                 }
             }
 
-            //SHOOT ALL:
+            //SHOOT ALL:]
 
+            if (emergency) {
+                intake = false;
+                reject = false;
 
-            if (shootAll) {
+                if (getRuntime() % 3 > 1.5) {
+                    robot.spin1.setPosition(0);
+                    robot.spin2.setPosition(1);
+                } else {
 
-                TELE.addData("100% works",shootOrder);
+                    robot.spin1.setPosition(1);
-                TELE.update();
+                    robot.spin2.setPosition(0);
+                }
 
+                if (getRuntime() % 1 < 0.5) {
+                    robot.transferServo.setPosition(transferServo_in);
+                } else {
+                    robot.transferServo.setPosition(transferServo_out);
 
+                }
 
+                robot.transfer.setPower(1);
 
+            } else if (shootAll) {
 
+                TELE.addData("100% works", shootOrder);
 
                 intake = false;
                 reject = false;
 
-                if (!shootOrder.isEmpty() && (getRuntime()-shootStamp < 10)) {
+                if (!shootOrder.isEmpty() && (getRuntime() - shootStamp < 12)) {
                     int currentSlot = shootOrder.get(0); // Peek, do NOT remove yet
                     boolean shootingDone = false;
 
                     AprilTagDetection d20 = aprilTagWebcam.getTagById(20);
                     AprilTagDetection d24 = aprilTagWebcam.getTagById(24);
 
+                    if (d20 != null) {
+                        overrideTurr = true;
+                        double bearing = d20.ftcPose.bearing;
+                        double finalPos =robot.turr1.getPosition() -  (bearing/1300);
+                        robot.turr1.setPosition(finalPos);
+                        robot.turr2.setPosition(1-finalPos);
+
+                        TELE.addData("Bear", bearing);
+
+
+
+
 
-                    if (d20!=null){
-                        //TODO: Add logic here and below for webcam if using
                     }
 
-                    if (d24!=null){
+                    if (d24 != null) {
+                        overrideTurr = true;
 
+                        double bearing = d24.ftcPose.bearing;
+                        double finalPos = turretPos() +  (bearing/720);
+                        robot.turr1.setPosition(finalPos);
+                        robot.turr2.setPosition(1-finalPos);
+
+                    }
+
+                    if (!outtake1) {
+                        outtake1 = (spindexPosEqual(spindexer_outtakeBall1));
+                    }
+                    if (!outtake2) {
+                        outtake2 = (spindexPosEqual(spindexer_outtakeBall2));
+                    }
+                    if (!outtake3) {
+                        outtake3 = (spindexPosEqual(spindexer_outtakeBall3));
                     }
 
                     switch (currentSlot) {
                         case 1:
-                            shootA = shootTeleop(spindexer_outtakeBall1);
+                            shootA = shootTeleop(spindexer_outtakeBall1, outtake1, shootStamp2);
                             TELE.addData("shootA", shootA);
 
-                            if ((getRuntime()- shootStamp) < 3* (4-shootOrder.size())){
+                            if ((getRuntime() - shootStamp) < 4 * (4 - shootOrder.size())) {
                                 shootingDone = !shootA;
                             } else {
                                 shootingDone = true;
                             }
                             break;
                         case 2:
-                            shootB = shootTeleop(spindexer_outtakeBall2);
+                            shootB = shootTeleop(spindexer_outtakeBall2, outtake2, shootStamp2);
                             TELE.addData("shootB", shootB);
-                            if ((getRuntime()- shootStamp) < 3* (4-shootOrder.size())){
+                            if ((getRuntime() - shootStamp) < 4 * (4 - shootOrder.size())) {
                                 shootingDone = !shootB;
                             } else {
                                 shootingDone = true;
                             }
                             break;
                         case 3:
-                            shootC = shootTeleop(spindexer_outtakeBall3);
+                            shootC = shootTeleop(spindexer_outtakeBall3, outtake3, shootStamp2);
                             TELE.addData("shootC", shootC);
-                            if ((getRuntime()- shootStamp) < 3* (4-shootOrder.size())){
+                            if ((getRuntime() - shootStamp) < 4 * (4 - shootOrder.size())) {
                                 shootingDone = !shootC;
                             } else {
                                 shootingDone = true;
@@ -501,10 +557,11 @@ public class TeleopV2 extends LinearOpMode {
                             break;
                     }
 
-
                     // Remove from the list only if shooting is complete
                     if (shootingDone) {
                         shootOrder.remove(0);
+                        shootStamp2 = getRuntime();
+
                     }
 
                 } else {
@@ -517,31 +574,48 @@ public class TeleopV2 extends LinearOpMode {
                     reject = false;
                     intake = true;
                     shootAll = false;
-                }
+                    outtake1 = false;
+                    outtake2 = false;
+                    outtake3 = false;
+                    overrideTurr = false;
+
 
 
                 }
 
-            if (gamepad2.squareWasPressed()){
+            }
+
+            if (gamepad2.squareWasPressed()) {
                 square = true;
                 shootStamp = getRuntime();
+                shootStamp2 = getRuntime();
+                outtake1 = false;
+                outtake2 = false;
+                outtake3 = false;
             }
 
-
+            if (gamepad2.circleWasPressed()) {
-            if (gamepad2.circleWasPressed()){
                 circle = true;
                 shootStamp = getRuntime();
+                shootStamp2 = getRuntime();
+
+                outtake1 = false;
+                outtake2 = false;
+                outtake3 = false;
 
             }
 
-
+            if (gamepad2.triangleWasPressed()) {
-            if (gamepad2.triangleWasPressed()){
                 triangle = true;
                 shootStamp = getRuntime();
+                shootStamp2 = getRuntime();
+
+                outtake1 = false;
+                outtake2 = false;
+                outtake3 = false;
 
             }
 
-
             if (square || circle || triangle) {
 
                 // Count green balls
@@ -573,7 +647,6 @@ public class TeleopV2 extends LinearOpMode {
                 square = false;
                 triangle = false;
 
-
             }
 
             // Right bumper shoots all balls fastest, ignoring colors
@@ -581,19 +654,76 @@ public class TeleopV2 extends LinearOpMode {
                 shootOrder.clear();
                 shootStamp = getRuntime();
 
+                outtake1 = false;
+                outtake2 = false;
+                outtake3 = false;
+
                 // Fastest order (example: slot 3 → 2 → 1)
+
+                if (ballIn(3)){
                     shootOrder.add(3);
-                shootOrder.add(2);
-                shootOrder.add(1);
-                shootAll = true;
-                shootPos = drive.localizer.getPose();
-
-
 
                 }
 
-            if (gamepad2.x){
+                if (ballIn(2)){
-                shootAll = false;
+                    shootOrder.add(2);
+
+                }
+
+                if (ballIn(1)){
+                    shootOrder.add(1);
+
+                }
+
+                if (!shootOrder.contains(3)) {
+
+                    shootOrder.add(3);
+                }
+
+                if (!shootOrder.contains(2)) {
+
+                    shootOrder.add(2);
+                }
+
+                if (!shootOrder.contains(1)) {
+
+                    shootOrder.add(1);
+                }
+
+
+                shootAll = true;
+                shootPos = drive.localizer.getPose();
+
+            }
+
+            // Right bumper shoots all balls fastest, ignoring colors
+            if (gamepad2.leftBumperWasPressed()) {
+                shootOrder.clear();
+                shootStamp = getRuntime();
+
+                outtake1 = false;
+                outtake2 = false;
+                outtake3 = false;
+
+                // Fastest order (example: slot 3 → 2 → 1)
+
+                if (ballIn(3)) {
+                    shootOrder.add(3);
+                }
+
+                if (ballIn(2)) {
+                    shootOrder.add(2);
+                }
+                if (ballIn(1)) {
+                    shootOrder.add(1);
+                }
+                shootAll = true;
+                shootPos = drive.localizer.getPose();
+
+            }
+
+            if (gamepad2.crossWasPressed()) {
+                emergency = !emergency;
 
             }
 
@@ -605,9 +735,9 @@ public class TeleopV2 extends LinearOpMode {
                 hub.clearBulkCache();
             }
 
-            TELE.addData("Spin1Green", green1);
+            TELE.addData("Spin1Green", green1 + ": " + ballIn(1));
-            TELE.addData("Spin2Green", green2);
+            TELE.addData("Spin2Green", green2 + ": " + ballIn(2));
-            TELE.addData("Spin3Green", green3);
+            TELE.addData("Spin3Green", green3 + ": " + ballIn(3));
 
             TELE.addData("pose", drive.localizer.getPose());
             TELE.addData("heading", drive.localizer.getPose().heading.toDouble());
@@ -620,7 +750,6 @@ public class TeleopV2 extends LinearOpMode {
 
             aprilTagWebcam.update();
 
-
             TELE.update();
 
             ticker++;
@@ -654,13 +783,13 @@ public class TeleopV2 extends LinearOpMode {
                 scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3) < spindexer + 0.01);
     }
 
-    public boolean shootTeleop(double spindexer) {
+    public boolean shootTeleop(double spindexer, boolean spinOk, double stamp) {
         // Set spin positions
         robot.spin1.setPosition(spindexer);
         robot.spin2.setPosition(1 - spindexer);
 
         // Check if spindexer has reached the target position
-        if (spindexPosEqual(spindexer)) {
+        if (spinOk || getRuntime() - stamp > 1.5) {
             if (tickerA == 1) {
                 transferStamp = getRuntime();
                 tickerA++;
@@ -696,9 +825,22 @@ public class TeleopV2 extends LinearOpMode {
         }
     }
 
-    public double hoodAnglePrediction(double distance) {
+    public double hoodAnglePrediction(double x) {
+        if (x < 34) {
+            double L = 1.04471;
+            double U = 0.711929;
+            double Q = 120.02263;
+            double B = 0.780982;
+            double M = 20.61191;
+            double v = 10.40506;
 
-        return 0.4;
+            double inner = 1 + Q * Math.exp(-B * (x - M));
+            return L + (U - L) / Math.pow(inner, 1.0 / v);
+
+        } else {
+            // x >= 34
+            return 1.94372 * Math.exp(-0.0528731 * x) + 0.39;
+        }
     }
 
     void addOddThenRest(List<Integer> order, boolean oddColor) {
@@ -710,8 +852,6 @@ public class TeleopV2 extends LinearOpMode {
         TELE.addData("oddBall", oddColor);
         shootAll = true;
 
-
-
     }
 
     void addOddInMiddle(List<Integer> order, boolean oddColor) {
@@ -750,6 +890,7 @@ public class TeleopV2 extends LinearOpMode {
         shootAll = true;
 
     }
+
     void addOddLast(List<Integer> order, boolean oddColor) {
         // Odd ball last
         for (int i = 1; i <= 3; i++) if (getBallColor(i) != oddColor) order.add(i);
@@ -759,35 +900,50 @@ public class TeleopV2 extends LinearOpMode {
         TELE.addData("oddBall", oddColor);
         shootAll = true;
 
-
     }
 
     // Returns color of ball in slot i (1-based)
     boolean getBallColor(int slot) {
-        switch(slot) {
+        switch (slot) {
-            case 1: return green1;
+            case 1:
-            case 2: return green2;
+                return green1;
-            case 3: return green3;
+            case 2:
+                return green2;
+            case 3:
+                return green3;
         }
         return false; // default
     }
 
-        public static double velPrediction(double distance) {
+    boolean ballIn(int slot) {
+        switch (slot) {
+            case 1:
 
-        if (distance < 30) {
+                if (!s1T.isEmpty()) {
-            return 2750;
+
-        } else if (distance > 100) {
+                    return !(s1T.get(s1T.size() - 1) < (getRuntime()) - 3);
-            if (distance > 160) {
-                return 4200;
-            }
-            return 3700;
-        } else {
-            // linear interpolation between 40->2650 and 120->3600
-            double slope = (3700.0 - 2750.0) / (100.0 - 30);
-            return (int) Math.round(2750 + slope * (distance - 30));
                 }
 
+            case 2:
+
+                if (!s2T.isEmpty()) {
+
+                    return !(s2T.get(s2T.size() - 1) < (getRuntime()) - 3);
                 }
 
+            case 3:
+
+                if (!s3T.isEmpty()) {
+
+                    return !(s3T.get(s3T.size() - 1) < (getRuntime()) - 3);
+
+                }
+        }
+        return true; // default
+    }
+
+    public double turretPos() {
+        return (scalar*((robot.turr1Pos.getVoltage()-restPos) /3.3 ));
+    }
 
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java

@@ -17,7 +17,7 @@ public class PositionalServoProgrammer extends LinearOpMode {
     public static double hoodPos = 0.501;
 
     public static double scalar = 1.112;
-    public static double restPos = 0.158;
+    public static double restPos = 0.15;
     @Override
     public void runOpMode() throws InterruptedException {
         robot = new Robot(hardwareMap);