area code

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

@@ -503,7 +503,7 @@ public class Auto_LT_Close extends LinearOpMode {
 
 
             if (gateCycle) {
-                startAutoGate();
+                startAuto();
                 shoot(0.501, 0.501, 0.501);
                 cycleStackClose();
                 shoot(0.501,0.501, 0.501);

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

@@ -79,8 +79,8 @@ public class Auto_LT_Far extends LinearOpMode {
     public static double shootStackTime = 2;
     public static double shootGateTime = 2.5;
     public static double colorSenseTime = 1;
-    public static double intakeStackTime = 2.5;
+    public static double intakeStackTime = 5;
-    public static double intakeGateTime = 2;
+    public static double intakeGateTime = 3.75;
     double obeliskTurrPos1 = 0.0;
     double obeliskTurrPos2 = 0.0;
     double obeliskTurrPos3 = 0.0;
@@ -122,6 +122,8 @@ public class Auto_LT_Far extends LinearOpMode {
         robot.limelight.pipelineSwitch(1);
 
         turret = new Turret(robot, TELE, robot.limelight);
+        limelightUsed = false;
+
 
 //        Spindexer.teleop = false;
 
@@ -190,6 +192,7 @@ public class Auto_LT_Far extends LinearOpMode {
                     turret.setTurret(turrDefault);
 
                     servos.setSpinPos(spinStartPos);
+                    limelightUsed = true;
 
                     servos.setTransferPos(transferServo_out);
                 }
@@ -231,6 +234,7 @@ public class Auto_LT_Far extends LinearOpMode {
                     gamepad2.rumble(500);
                     sleep(1000);
                     turret.setTurret(turrDefault);
+                    limelightUsed = true;
 
                     servos.setSpinPos(spinStartPos);
 
@@ -259,10 +263,10 @@ public class Auto_LT_Far extends LinearOpMode {
             shootGate = drive.actionBuilder(new Pose2d(pickupGateX, pickupGateY, Math.toRadians(pickupGateH)))
                     .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot));
 
-            limelightUsed = true;
 
             TELE.addData("Red?", redAlliance);
             TELE.addData("Turret Default", turrDefault);
+            TELE.addData("Limelight On?",limelightUsed);
             TELE.addData("Gate Cycle?", gatePickup);
             TELE.addData("Pickup Stack?", stack3);
             TELE.addData("Start Position", autoStart);
@@ -281,11 +285,17 @@ public class Auto_LT_Far extends LinearOpMode {
             robot.transfer.setPower(1);
 
             startAuto();
-            shoot();
+            if (redAlliance){
+                shoot(autoStartRX, autoStartRY, autoStartRH);
+
+            } else {
+                shoot(autoStartBX, autoStartBY, autoStartBH);
+
+            }
 
             if (stack3){
                 cycleStackFar();
-                shoot();
+                shoot(xShoot, yShoot, hShoot);
             }
 
             while (gatePickup && getRuntime() - stamp < endAutoTime){
@@ -294,10 +304,7 @@ public class Auto_LT_Far extends LinearOpMode {
                     break;
                 }
                 cycleGatePrepareShoot();
-                if (getRuntime() - stamp > endAutoTime + shootAllTime + 1){
+                shoot(xShoot, yShoot, hShoot);
-                    break;
-                }
-                shoot();
             }
 
             if (gatePickup || stack3){
@@ -324,28 +331,46 @@ public class Auto_LT_Far extends LinearOpMode {
 
     }
 
-    void shoot(){
+    void shoot(double x, double y, double z){
         Actions.runBlocking(
                 new ParallelAction(
-                        autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease, 0.501, 0.501, 0.501)
+                        autoActions.shootAllAuto(shootAllTime, spindexerSpeedIncrease, x, y, z)
                 )
 
         );
     }
 
     void startAuto(){
-        Actions.runBlocking(
-                new ParallelAction(
-                        autoActions.manageShooterAuto(
-                                flywheel0Time,
-                                0.501,
-                                0.501,
-                                0.501,
-                                true
-                        )
 
-                )
+        if (redAlliance){
-        );
+            Actions.runBlocking(
+                    new ParallelAction(
+                            autoActions.manageShooterAuto(
+                                    flywheel0Time,
+                                    autoStartRX,
+                                    autoStartRY,
+                                    autoStartRH,
+                                    true
+                            )
+
+                    )
+            );
+
+        } else {
+            Actions.runBlocking(
+                    new ParallelAction(
+                            autoActions.manageShooterAuto(
+                                    flywheel0Time,
+                                    autoStartBX,
+                                    autoStartBY,
+                                    autoStartBH,
+                                    true
+                            )
+
+                    )
+            );
+        }
+
     }
 
     void leave3Ball(){
@@ -371,9 +396,13 @@ public class Auto_LT_Far extends LinearOpMode {
                 )
         );
         servos.setSpinPos(spinStartPos);
+        spindexer.setIntakePower(-0.1);
         Actions.runBlocking(
                 new ParallelAction(
-                        shoot3.build()
+                        shoot3.build(),
+                        autoActions.manageShooterAuto(
+                                shootStackTime,xShoot, yShoot, hShoot, false
+                        )
                 )
         );
     }
@@ -383,7 +412,7 @@ public class Auto_LT_Far extends LinearOpMode {
                 new ParallelAction(
                         pickupGate.build(),
                         autoActions.intake(
-                                intakeStackTime,
+                                intakeGateTime,
                                 pickupGateX,
                                 pickupGateY,
                                 pickupGateH
@@ -393,16 +422,16 @@ public class Auto_LT_Far extends LinearOpMode {
     }
 
     void cycleGatePrepareShoot(){
+        spindexer.setIntakePower(-0.1);
         Actions.runBlocking(
                 new ParallelAction(
                         shootGate.build(),
-                        autoActions.prepareShootAll(
+                        autoActions.manageShooterAuto(
-                                colorSenseTime,
+                               shootGateTime,
-                                shootGateTime,
-                                motif,
                                 xShoot,
                                 yShoot,
-                                hShoot
+                                hShoot,
+                                false
                         )
                 )
         );

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

@@ -10,11 +10,11 @@ public class Back_Poses {
     public static double rShootX = 100, rShootY = 60, rShootH = 125.2;
     public static double bShootX = 100, bShootY = -60, bShootH = -125.2;
 
-    public static double rStackPickupFarAX = 73, rStackPickupFarAY = 51, rStackPickupFarAH = 145;
+    public static double rStackPickupFarAX = 75, rStackPickupFarAY = 45, rStackPickupFarAH = 150;
-    public static double bStackPickupFarAX = 73, bStackPickupFarAY = -51, bStackPickupFarAH = -145;
+    public static double bStackPickupFarAX = 75, bStackPickupFarAY = -45, bStackPickupFarAH = -150;
 
-    public static double rStackPickupFarBX = 53, rStackPickupFarBY = 71, rStackPickupFarBH = 145.1;
+    public static double rStackPickupFarBX = 45, rStackPickupFarBY = 80, rStackPickupFarBH = 145.1;
-    public static double bStackPickupFarBX = 55, bStackPickupFarBY = -71, bStackPickupFarBH = -145.1;
+    public static double bStackPickupFarBX = 45, bStackPickupFarBY = -80, bStackPickupFarBH = -145.1;
 
     public static double rStackPickupMiddleAX = 55, rStackPickupMiddleAY = 39, rStackPickupMiddleAH = 145;
     public static double bStackPickupMiddleAX = 55, bStackPickupMiddleAY = -39, bStackPickupMiddleAH = -145;
@@ -22,8 +22,8 @@ public class Back_Poses {
     public static double rStackPickupMiddleBX = 45, rStackPickupMiddleBY = 49, rStackPickupMiddleBH = 145.1;
     public static double bStackPickupMiddleBX = 45, bStackPickupMiddleBY = -49, bStackPickupMiddleBH = -145.1;
 
-    public static double rPickupGateXA = 60, rPickupGateYA = 83, rPickupGateHA = 145;
+    public static double rPickupGateXA = 60, rPickupGateYA = 90, rPickupGateHA = 140;
-    public static double bPickupGateXA = 60, bPickupGateYA = -83, bPickupGateHA = -145;
+    public static double bPickupGateXA = 60, bPickupGateYA = -90, bPickupGateHA = -140;
     public static double rPickupGateXB = 84, rPickupGateYB = 76, rPickupGateHB = 145;
     public static double bPickupGateXB = 84, bPickupGateYB = -76, bPickupGateHB = -145;
     public static double rPickupGateXC = 50, rPickupGateYC = 83, rPickupGateHC = 190;

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

@@ -142,6 +142,62 @@ public class TeleopV3 extends LinearOpMode {
         while (opModeIsActive()) {
 
             //TELE.addData("Is limelight on?", robot.limelight.getStatus());
+            drive.updatePoseEstimate();
+            Pose2d currentPose = drive.localizer.getPose();
+
+            if (enableSpindexerManager) {
+                //if (!shootAll) {
+                spindexer.processIntake();
+                //}
+
+                // RIGHT_BUMPER
+                if (gamepad1.right_bumper && intakeTicker > resetSpinTicks) {
+                    spindexer.setIntakePower(1);
+                } else if (gamepad1.cross) {
+                    spindexer.setIntakePower(-1);
+                } else {
+                    spindexer.setIntakePower(0);
+                }
+
+                // LEFT_BUMPER
+                if (!shootAll && gamepad1.leftBumperWasReleased()) {
+                    shootStamp = getRuntime();
+                    shootAll = true;
+
+                    shooterTicker = 0;
+
+                }
+                intakeTicker++;
+                if (shootAll) {
+                    intakeTicker = 0;
+                    intake = false;
+                    reject = false;
+
+                    if (shooterTicker == 0) {
+                        spindexer.prepareShootAllContinous();
+                        //TELE.addLine("preparing to shoot");
+//                    } else if (shooterTicker == 2) {
+//                        //servo.setTransferPos(transferServo_in);
+//                        spindexer.shootAll();
+//                        TELE.addLine("starting to shoot");
+                    } else if (spindexer.shootAllComplete()) {
+                        //spindexPos = spindexer_intakePos1;
+                        shootAll = false;
+                        spindexer.resetSpindexer();
+                        //spindexer.processIntake();
+                        //TELE.addLine("stop shooting");
+                    }
+                    shooterTicker++;
+                    //spindexer.processIntake();
+                }
+
+                if (gamepad1.left_stick_button) {
+                    servo.setTransferPos(transferServo_out);
+                    //spindexPos = spindexer_intakePos1;
+                    shootAll = false;
+                    spindexer.resetSpindexer();
+                }
+            }
 
             //DRIVETRAIN:
 
@@ -169,9 +225,9 @@ public class TeleopV3 extends LinearOpMode {
 
             //TURRET TRACKING
 
-            double robX = drive.localizer.getPose().position.x;
+            double robX = currentPose.position.x;
-            double robY = drive.localizer.getPose().position.y;
+            double robY = currentPose.position.y;
-            double robH = drive.localizer.getPose().heading.toDouble();
+            double robH = currentPose.heading.toDouble();
 
             double robotX = robX + xOffset;
             double robotY = robY + yOffset;
@@ -278,63 +334,7 @@ public class TeleopV3 extends LinearOpMode {
                 shootAllSpindexerSpeedIncrease = shootAllSpindexerSpeedIncrease+0.01;
             }
 
-
-            if (enableSpindexerManager) {
-                //if (!shootAll) {
-                spindexer.processIntake();
-                //}
-
-                // RIGHT_BUMPER
-                if (gamepad1.right_bumper && intakeTicker > resetSpinTicks) {
-                    spindexer.setIntakePower(1);
-                } else if (gamepad1.cross) {
-                    spindexer.setIntakePower(-1);
-                } else {
-                    spindexer.setIntakePower(0);
-                }
-
-                // LEFT_BUMPER
-                if (!shootAll && gamepad1.leftBumperWasReleased()) {
-                    shootStamp = getRuntime();
-                    shootAll = true;
-
-                    shooterTicker = 0;
-
-                }
-                intakeTicker++;
-                if (shootAll) {
-                    intakeTicker = 0;
-                    intake = false;
-                    reject = false;
-
-                    if (shooterTicker == 0) {
-                        spindexer.prepareShootAllContinous();
-                        //TELE.addLine("preparing to shoot");
-//                    } else if (shooterTicker == 2) {
-//                        //servo.setTransferPos(transferServo_in);
-//                        spindexer.shootAll();
-//                        TELE.addLine("starting to shoot");
-                    } else if (spindexer.shootAllComplete()) {
-                        //spindexPos = spindexer_intakePos1;
-                        shootAll = false;
-                        spindexer.resetSpindexer();
-                        //spindexer.processIntake();
-                        //TELE.addLine("stop shooting");
-                    }
-                    shooterTicker++;
-                    //spindexer.processIntake();
-                }
-
-                if (gamepad1.left_stick_button) {
-                    servo.setTransferPos(transferServo_out);
-                    //spindexPos = spindexer_intakePos1;
-                    shootAll = false;
-                    spindexer.resetSpindexer();
-                }
-            }
-
             //EXTRA STUFFINESS:
-            drive.updatePoseEstimate();
 
             for (LynxModule hub : allHubs) {
                 hub.clearBulkCache();

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

@@ -71,6 +71,8 @@ public class Drivetrain {
             robot.frontRight.setPower(frontRightPower);
             robot.backRight.setPower(backRightPower);
         }
+        Pose2d currentPos = drive.localizer.getPose();
+        brakePos = currentPos;
 
         if (brake > 0.4 && robot.frontLeft.getZeroPowerBehavior() != DcMotor.ZeroPowerBehavior.BRAKE && !autoDrive) {
             robot.frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
@@ -78,15 +80,9 @@ public class Drivetrain {
             robot.backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
             robot.backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
 
-            drive.updatePoseEstimate();
-
-            brakePos = drive.localizer.getPose();
             autoDrive = true;
 
         } else if (brake > 0.4) {
-            drive.updatePoseEstimate();
-
-            Pose2d currentPos = drive.localizer.getPose();
 
             TrajectoryActionBuilder traj2 = drive.actionBuilder(currentPos)
                     .strafeToLinearHeading(new Vector2d(brakePos.position.x, brakePos.position.y), brakePos.heading.toDouble(), VEL_CONSTRAINT, ACCEL_CONSTRAINT);

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

@@ -242,6 +242,7 @@ public class Turret {
         return bearingOffset;
     }
 
+    double targetTurretPos;
     public void trackGoal(Pose2d deltaPos) {
 
         /* ---------------- FIELD → TURRET GEOMETRY ---------------- */
@@ -273,7 +274,7 @@ public class Turret {
 
         limelightRead();
         // Active correction if we see the target
-        if (result.isValid() && !lockOffset && limelightUsed) {
+        if (result.isValid() && !lockOffset && limelightUsed && targetTurretPos > turrMin && targetTurretPos < turrMax) {
             currentTrackOffset += bearingAlign(result);
             currentTrackCount++;
 
@@ -329,7 +330,7 @@ public class Turret {
 
         /* ---------------- ANGLE → SERVO POSITION ---------------- */
 
-        double targetTurretPos = turrDefault + (turretAngleDeg * (turret180Range * 2.0) / 360);
+        targetTurretPos = turrDefault + (turretAngleDeg * (turret180Range * 2.0) / 360);
 
         // Clamp to physical servo limits
         targetTurretPos = Math.max(turrMin, Math.min(targetTurretPos, turrMax));